Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
RANSAC.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Guido de Croon
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
36 #include "RANSAC.h"
39 #include <math.h>
40 #include <string.h>
41 #include <stdlib.h>
42 #include "stdio.h"
43 
57 void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D,
58  float (*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error __attribute__((unused)))
59 {
60 
61  uint8_t D_1 = D;
62  if (use_bias) {
63  D_1++;
64  }
65  float err;
66  float errors[n_iterations];
67  int indices_subset[n_samples];
68  float subset_targets[n_samples];
69  float subset_samples[n_samples][D];
70  float subset_params[n_iterations][D_1];
71 
72  // ensure that n_samples is high enough to ensure a result for a single fit:
73  n_samples = (n_samples < D_1) ? D_1 : n_samples;
74  // n_samples should not be higher than count:
75  n_samples = (n_samples < count) ? n_samples : count;
76 
77  // do the RANSAC iterations:
78  for (int i = 0; i < n_iterations; i++) {
79 
80  // get a subset of indices
81  get_indices_without_replacement(indices_subset, n_samples, count);
82 
83  // get the corresponding samples and targets:
84  for (int j = 0; j < n_samples; j++) {
85  subset_targets[j] = targets[indices_subset[j]];
86  for (int k = 0; k < D; k++) {
87  subset_samples[j][k] = samples[indices_subset[j]][k];
88  }
89  }
90 
91  // fit a linear model on the small system:
92  fit_linear_model(subset_targets, D, subset_samples, n_samples, use_bias, subset_params[i], &err);
93 
94 
95  // determine the error on the whole set:
96  float err_sum = 0.0f;
97  float prediction;
98  for (int j = 0; j < count; j++) {
99  // predict the sample's value and determine the absolute error:
100  prediction = predict_value(samples[j], subset_params[i], D, use_bias);
101  err = fabsf(prediction - targets[j]);
102  // cap the error with the threshold:
103  err = (err > error_threshold) ? error_threshold : err;
104  err_sum += err;
105  }
106  errors[i] = err_sum;
107  }
108 
109  // determine the minimal error:
110  float min_err = errors[0];
111  int min_ind = 0;
112  for (int i = 1; i < n_iterations; i++) {
113  if (errors[i] < min_err) {
114  min_err = errors[i];
115  min_ind = i;
116  }
117  }
118 
119  // copy the parameters:
120  for (int d = 0; d < D_1; d++) {
121  params[d] = subset_params[min_ind][d];
122  }
123 
124 }
125 
133 float predict_value(float *sample, float *weights, int D, bool use_bias)
134 {
135 
136  float sum = 0.0f;
137 
138  for (int w = 0; w < D; w++) {
139  sum += weights[w] * sample[w];
140  }
141  if (use_bias) {
142  sum += weights[D];
143  }
144 
145  // printf("Prediction = %f\n", sum);
146 
147  return sum;
148 }
149 
157 void get_indices_without_replacement(int *indices_subset, int n_samples, int count)
158 {
159 
160  int index;
161 
162  for (int j = 0; j < n_samples; j++) {
163  bool picked_number = false;
164  while (!picked_number) {
165  index = rand() % count;
166  bool new_val = true;
167  for (int k = 0; k < j; k++) {
168  if (indices_subset[k] == index) {
169  new_val = false;
170  break;
171  }
172  }
173  if (new_val) {
174  indices_subset[j] = index;
175  picked_number = true;
176  }
177  }
178  }
179 }
float predict_value(float *sample, float *weights, int D, bool use_bias)
Predict the value of a sample with linear weights.
Definition: RANSAC.c:133
void get_indices_without_replacement(int *indices_subset, int n_samples, int count)
Get indices without replacement.
Definition: RANSAC.c:157
void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D, float(*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error)
Perform RANSAC to fit a linear model.
Definition: RANSAC.c:57
Perform Random Sample Consensus (RANSAC), a robust fitting method.
int n_samples
Definition: detect_gate.c:85
float * weights
Paparazzi floating point algebra.
void fit_linear_model(float *targets, int D, float(*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error)
Fit a linear model from samples to target values.
Matrix decompositions in floating point.
static float D
Definition: trilateration.c:35
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98