Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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, float *params, float *fit_error __attribute__((unused)))
59 {
60 
61  uint8_t D_1 = D + 1;
62  float err;
63  float errors[n_iterations];
64  int indices_subset[n_samples];
65  float subset_targets[n_samples];
66  float subset_samples[n_samples][D];
67  float subset_params[n_iterations][D_1];
68  bool use_bias = true;
69 
70  // ensure that n_samples is high enough to ensure a result for a single fit:
71  n_samples = (n_samples < D_1) ? D_1 : n_samples;
72  // n_samples should not be higher than count:
73  n_samples = (n_samples < count) ? n_samples : count;
74 
75  // do the RANSAC iterations:
76  for (int i = 0; i < n_iterations; i++) {
77 
78  // get a subset of indices
79  get_indices_without_replacement(indices_subset, n_samples, count);
80 
81  // get the corresponding samples and targets:
82  for (int j = 0; j < n_samples; j++) {
83  subset_targets[j] = targets[indices_subset[j]];
84  for (int k = 0; k < D; k++) {
85  subset_samples[j][k] = samples[indices_subset[j]][k];
86  }
87  }
88 
89  // fit a linear model on the small system:
90  fit_linear_model(subset_targets, D, subset_samples, n_samples, use_bias, subset_params[i], &err);
91  printf("params normal: %f, %f\n", subset_params[i][0], subset_params[i][1]);
92  float priors[2];
93  priors[0] = 1.0f;
94  priors[1] = 10.0f;
95  fit_linear_model_prior(subset_targets, D, subset_samples, n_samples, use_bias, priors, subset_params[i], &err);
96  printf("params prior: %f, %f\n", subset_params[i][0], subset_params[i][1]);
97 
98  // determine the error on the whole set:
99  float err_sum = 0.0f;
100  float prediction;
101  for (int j = 0; j < count; j++) {
102  // predict the sample's value and determine the absolute error:
103  prediction = predict_value(samples[j], subset_params[i], D, use_bias);
104  err = fabsf(prediction - targets[j]);
105  // cap the error with the threshold:
106  err = (err > error_threshold) ? error_threshold : err;
107  err_sum += err;
108  }
109  errors[i] = err_sum;
110  }
111 
112  // determine the minimal error:
113  float min_err = errors[0];
114  int min_ind = 0;
115  for (int i = 1; i < n_iterations; i++) {
116  if (errors[i] < min_err) {
117  min_err = errors[i];
118  min_ind = i;
119  }
120  }
121 
122  // copy the parameters:
123  for (int d = 0; d < D_1; d++) {
124  params[d] = subset_params[min_ind][d];
125  }
126 
127 }
128 
136 float predict_value(float *sample, float *weights, int D, bool use_bias)
137 {
138 
139  float sum = 0.0f;
140 
141  for (int w = 0; w < D; w++) {
142  sum += weights[w] * sample[w];
143  }
144  if (use_bias) {
145  sum += weights[D];
146  }
147 
148  // printf("Prediction = %f\n", sum);
149 
150  return sum;
151 }
152 
160 void get_indices_without_replacement(int *indices_subset, int n_samples, int count)
161 {
162 
163  int index;
164 
165  for (int j = 0; j < n_samples; j++) {
166  bool picked_number = false;
167  while (!picked_number) {
168  index = rand() % count;
169  bool new_val = true;
170  for (int k = 0; k < j; k++) {
171  if (indices_subset[k] == index) {
172  new_val = false;
173  break;
174  }
175  }
176  if (new_val) {
177  indices_subset[j] = index;
178  picked_number = true;
179  }
180  }
181  }
182 }
unsigned short uint16_t
Definition: types.h:16
void get_indices_without_replacement(int *indices_subset, int n_samples, int count)
Get indices without replacement.
Definition: RANSAC.c:160
int n_samples
Definition: detect_gate.c:85
static float D
Definition: trilateration.c:35
Paparazzi floating point algebra.
Perform Random Sample Consensus (RANSAC), a robust fitting method.
void fit_linear_model_prior(float *targets, int D, float(*samples)[D], uint16_t count, bool use_bias, float *priors, float *params, float *fit_error)
Fit a linear model from samples to target values with a prior.
float predict_value(float *sample, float *weights, int D, bool use_bias)
Predict the value of a sample with linear weights.
Definition: RANSAC.c:136
void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D, float(*samples)[D], uint16_t count, float *params, float *fit_error)
Perform RANSAC to fit a linear model.
Definition: RANSAC.c:57
Matrix decompositions in floating point.
unsigned char uint8_t
Definition: types.h:14
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.