Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
linear_flow_fit.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 Hann Woei Ho
3 * Guido de Croon
4 *
5 *
6 * This file is part of Paparazzi.
7 *
8 * Paparazzi is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2, or (at your option)
11 * any later version.
12 *
13 * Paparazzi is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with Paparazzi; see the file COPYING. If not, write to
20 * the Free Software Foundation, 59 Temple Place - Suite 330,
21 * Boston, MA 02111-1307, USA.
22 */
23
24/*
25 * @file modules/computer_vision/opticflow/linear_flow_fit.c
26 * @brief Takes a set of optical flow vectors and extracts information such as relative velocities and surface roughness.
27 *
28 * A horizontal and vertical linear fit is made with the optical flow vectors, and from the fit parameters information is extracted such as relative velocities (useful for time-to-contact determination), slope angle, and surface roughness.
29 *
30 * Code based on the article:
31 * "Optic-flow based slope estimation for autonomous landing",
32 * de Croon, G.C.H.E., and Ho, H.W., and De Wagter, C., and van Kampen, E., and Remes B., and Chu, Q.P.,
33 * in the International Journal of Micro Air Vehicles, Volume 5, Number 4, pages 287 – 297, (2013)
34 */
35
36#include <stdlib.h>
37#include <stdio.h>
38#include <math.h>
39#include <string.h>
40//#include "defs_and_types.h"
41#include "linear_flow_fit.h"
45#include "math/RANSAC.h"
46#include "std.h"
47
48// Is this still necessary?
49#define MAX_COUNT_PT 50
50
51#define MIN_SAMPLES_FIT 3
52
53#define N_PAR_TR_FIT 6
54
67bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations,
68 int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info)
69{
70 // Are there enough flow vectors to perform a fit?
71 if (count < MIN_SAMPLES_FIT) {
72 // return that no fit was made:
73 return false;
74 }
75
76 // fit linear flow field:
77 float parameters_u[3], parameters_v[3], min_error_u, min_error_v;
78 fit_linear_flow_field(vectors, count, error_threshold, n_iterations, n_samples, parameters_u, parameters_v,
79 &info->fit_error, &min_error_u, &min_error_v, &info->n_inliers_u, &info->n_inliers_v);
80
81 // extract information from the parameters:
82 extract_information_from_parameters(parameters_u, parameters_v, im_width, im_height, info);
83
84 // surface roughness is equal to fit error:
85 info->surface_roughness = info->fit_error;
86 info->divergence = info->relative_velocity_z;
87 float diverg = (fabsf(info->divergence) < 1E-5) ? 1E-5 : info->divergence;
88 info->time_to_contact = 1.0f / diverg;
89
90 // return successful fit:
91 return true;
92}
93
109void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples,
110 float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u,
111 int *n_inliers_v)
112{
113
114 // We will solve systems of the form A x = b,
115 // where A = [nx3] matrix with entries [x, y, 1] for each optic flow location
116 // and b = [nx1] vector with either the horizontal (bu) or vertical (bv) flow.
117 // x in the system are the parameters for the horizontal (pu) or vertical (pv) flow field.
118
119 // local vars for iterating, random numbers:
120 int sam, p, i_rand, si, add_si;
121
122 // ensure that n_samples is high enough to ensure a result for a single fit:
124 // n_samples should not be higher than count:
125 n_samples = (n_samples < count) ? n_samples : count;
126 // at least one iteration, else the best fit below reads uninitialized results:
127 n_iterations = (n_iterations < 1) ? 1 : n_iterations;
128
129 // initialize matrices and vectors for the full point set problem:
130 // this is used for determining inliers
131 float _AA[count][3];
132 MAKE_MATRIX_PTR(AA, _AA, count);
133 float _bu_all[count][1];
134 MAKE_MATRIX_PTR(bu_all, _bu_all, count);
135 float _bv_all[count][1];
136 MAKE_MATRIX_PTR(bv_all, _bv_all, count);
137 for (sam = 0; sam < count; sam++) {
138 AA[sam][0] = (float) vectors[sam].pos.x;
139 AA[sam][1] = (float) vectors[sam].pos.y;
140 AA[sam][2] = 1.0f;
141 bu_all[sam][0] = (float) vectors[sam].flow_x;
142 bv_all[sam][0] = (float) vectors[sam].flow_y;
143 }
144
145 // later used to determine the error of a set of parameters on the whole set:
146 float _bb[count][1];
147 MAKE_MATRIX_PTR(bb, _bb, count);
148 float _C[count][1];
149 MAKE_MATRIX_PTR(C, _C, count);
150
151 // ***************
152 // perform RANSAC:
153 // ***************
154
155 // set up variables for small linear system solved repeatedly inside RANSAC:
156 float _A[n_samples][3];
158 float _bu[n_samples][1];
159 MAKE_MATRIX_PTR(bu, _bu, n_samples);
160 float _bv[n_samples][1];
161 MAKE_MATRIX_PTR(bv, _bv, n_samples);
162 float w[n_samples], _v[3][3];
163 MAKE_MATRIX_PTR(v, _v, 3);
164 float _pu[3][1];
165 MAKE_MATRIX_PTR(pu, _pu, 3);
166 float _pv[3][1];
167 MAKE_MATRIX_PTR(pv, _pv, 3);
168
169 // iterate and store parameters, errors, inliers per fit:
170 float PU[n_iterations * 3];
171 float PV[n_iterations * 3];
172 float errors_pu[n_iterations];
173 errors_pu[0] = 0.0;
174 float errors_pv[n_iterations];
175 errors_pv[0] = 0.0;
176 int n_inliers_pu[n_iterations];
177 int n_inliers_pv[n_iterations];
178 int it, ii;
179 for (it = 0; it < n_iterations; it++) {
180 errors_pu[it] = 0.0;
181 errors_pv[it] = 0.0;
182 n_inliers_pu[it] = 0;
183 n_inliers_pv[it] = 0;
184 }
185 for (it = 0; it < n_iterations; it++) {
186 // select a random sample of n_sample points:
187 int sample_indices[n_samples];
188 i_rand = 0;
189
190 // sampling without replacement:
191 while (i_rand < n_samples) {
192 si = rand() % count;
193 add_si = 1;
194 for (ii = 0; ii < i_rand; ii++) {
195 if (sample_indices[ii] == si) { add_si = 0; }
196 }
197 if (add_si) {
198 sample_indices[i_rand] = si;
199 i_rand ++;
200 }
201 }
202
203 // Setup the system:
204 for (sam = 0; sam < n_samples; sam++) {
205 A[sam][0] = (float) vectors[sample_indices[sam]].pos.x;
206 A[sam][1] = (float) vectors[sample_indices[sam]].pos.y;
207 A[sam][2] = 1.0f;
208 bu[sam][0] = (float) vectors[sample_indices[sam]].flow_x;
209 bv[sam][0] = (float) vectors[sample_indices[sam]].flow_y;
210 //printf("%d,%d,%d,%d,%d\n",A[sam][0],A[sam][1],A[sam][2],bu[sam][0],bv[sam][0]);
211 }
212
213 // Solve the small system:
214
215 // for horizontal flow:
216 // decompose A in u, w, v with singular value decomposition A = u * w * vT.
217 // u replaces A as output:
218 pprz_svd_float(A, w, v, n_samples, 3);
219 pprz_svd_solve_float(pu, A, w, v, bu, n_samples, 3, 1);
220 PU[it * 3] = pu[0][0];
221 PU[it * 3 + 1] = pu[1][0];
222 PU[it * 3 + 2] = pu[2][0];
223
224 // for vertical flow:
225 pprz_svd_solve_float(pv, A, w, v, bv, n_samples, 3, 1);
226 PV[it * 3] = pv[0][0];
227 PV[it * 3 + 1] = pv[1][0];
228 PV[it * 3 + 2] = pv[2][0];
229
230 // count inliers and determine their error on all points:
231 errors_pu[it] = 0;
232 errors_pv[it] = 0;
233 n_inliers_pu[it] = 0;
234 n_inliers_pv[it] = 0;
235
236 // for horizontal flow:
237 // bb = AA * pu:
238 MAT_MUL(count, 3, 1, bb, AA, pu);
239 // subtract bu_all: C = 0 in case of perfect fit:
240 MAT_SUB(count, 1, C, bb, bu_all);
241
242 for (p = 0; p < count; p++) {
243 C[p][0] = fabsf(C[p][0]);
244 if (C[p][0] < error_threshold) {
245 errors_pu[it] += C[p][0];
246 n_inliers_pu[it]++;
247 } else {
248 errors_pu[it] += error_threshold;
249 }
250 }
251
252 // for vertical flow:
253 // bb = AA * pv:
254 MAT_MUL(count, 3, 1, bb, AA, pv);
255 // subtract bv_all: C = 0 in case of perfect fit:
256 MAT_SUB(count, 1, C, bb, bv_all);
257
258 for (p = 0; p < count; p++) {
259 C[p][0] = fabsf(C[p][0]);
260 if (C[p][0] < error_threshold) {
261 errors_pv[it] += C[p][0];
262 n_inliers_pv[it]++;
263 } else {
264 errors_pv[it] += error_threshold;
265 }
266 }
267 }
268
269 // After all iterations:
270 // select the parameters with lowest error:
271 // for horizontal flow:
272 int param;
273 int min_ind = 0;
274 *min_error_u = (float)errors_pu[0];
275 for (it = 1; it < n_iterations; it++) {
276 if (errors_pu[it] < *min_error_u) {
277 *min_error_u = (float)errors_pu[it];
278 min_ind = it;
279 }
280 }
281 for (param = 0; param < 3; param++) {
282 parameters_u[param] = PU[min_ind * 3 + param];
283 }
284 *n_inliers_u = n_inliers_pu[min_ind];
285
286 // for vertical flow:
287 min_ind = 0;
288 *min_error_v = (float)errors_pv[0];
289 for (it = 0; it < n_iterations; it++) {
290 if (errors_pv[it] < *min_error_v) {
291 *min_error_v = (float)errors_pv[it];
292 min_ind = it;
293 }
294 }
295 for (param = 0; param < 3; param++) {
296 parameters_v[param] = PV[min_ind * 3 + param];
297 }
298 *n_inliers_v = n_inliers_pv[min_ind];
299
300 // error has to be determined on the entire set without threshold:
301 // bb = AA * pu:
302 MAT_MUL(count, 3, 1, bb, AA, pu);
303 // subtract bu_all: C = 0 in case of perfect fit:
304 MAT_SUB(count, 1, C, bb, bu_all);
305 *min_error_u = 0;
306 for (p = 0; p < count; p++) {
307 *min_error_u += fabsf(C[p][0]);
308 }
309 // bb = AA * pv:
310 MAT_MUL(count, 3, 1, bb, AA, pv);
311 // subtract bv_all: C = 0 in case of perfect fit:
312 MAT_SUB(count, 1, C, bb, bv_all);
313 *min_error_v = 0;
314 for (p = 0; p < count; p++) {
315 *min_error_v += fabsf(C[p][0]);
316 }
317 *fit_error = (*min_error_u + *min_error_v) / (2 * count);
318
319}
320
328
329void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height,
330 struct linear_flow_fit_info *info)
331{
332 // This method assumes a linear flow field in x- and y- direction according to the formulas:
333 // u = parameters_u[0] * x + parameters_u[1] * y + parameters_u[2]
334 // v = parameters_v[0] * x + parameters_v[1] * y + parameters_v[2]
335 // where u is the horizontal flow at image coordinate (x,y)
336 // and v is the vertical flow at image coordinate (x,y)
337
338 // relative velocities:
339 info->relative_velocity_z = (parameters_u[0] + parameters_v[1]) / 2.0f; // divergence / 2
340
341 // translation orthogonal to the camera axis:
342 // flow in the center of the image:
343 info->relative_velocity_x = -(parameters_u[2] + (im_width / 2.0f) * parameters_u[0] +
344 (im_height / 2.0f) * parameters_u[1]);
345 info->relative_velocity_y = -(parameters_v[2] + (im_width / 2.0f) * parameters_v[0] +
346 (im_height / 2.0f) * parameters_v[1]);
347
348 float arv_x = fabsf(info->relative_velocity_x);
349 float arv_y = fabsf(info->relative_velocity_y);
350
351 // extract inclination from flow field:
352 float threshold_slope = 1.0;
353 float eta = 0.002;
354
355 if (fabsf(parameters_v[1]) < eta && arv_y < threshold_slope && arv_x >= 2 * threshold_slope) {
356 // there is no forward motion and not enough vertical motion, but enough horizontal motion:
357 info->slope_x = parameters_u[0] / info->relative_velocity_x;
358 } else if (arv_y >= 2 * threshold_slope) {
359 // there is sufficient vertical motion:
360 info->slope_x = parameters_v[0] / info->relative_velocity_y;
361 } else {
362 // there may be forward motion, then we can do a quadratic fit:
363 // a linear fit provides no information though
364 info->slope_x = 0.0f;
365 }
366
367 if (fabsf(parameters_u[0]) < eta && arv_x < threshold_slope && arv_y >= 2 * threshold_slope) {
368 // there is no forward motion, little horizontal movement, but sufficient vertical motion:
369 info->slope_y = parameters_v[1] / info->relative_velocity_y;
370 } else if (arv_x >= 2 * threshold_slope) {
371 // there is sufficient horizontal motion:
372 info->slope_y = parameters_u[1] / info->relative_velocity_x;
373 } else {
374 // there could be forward motion, then we can do a quadratic fit:
375 // a linear fit provides no information though
376 info->slope_y = 0.0f;
377 }
378
379 // Focus of Expansion:
380 // the flow planes intersect the flow=0 plane in a line
381 // the FoE is the point where these 2 lines intersect (flow = (0,0))
382 // x:
383 float denominator = parameters_v[0] * parameters_u[1] - parameters_u[0] * parameters_v[1];
384 if (fabsf(denominator) > 1E-5) {
385 info->focus_of_expansion_x = ((parameters_u[2] * parameters_v[1] - parameters_v[2] * parameters_u[1]) / denominator);
386 } else { info->focus_of_expansion_x = 0.0f; }
387 // y:
388 denominator = parameters_u[1];
389 if (fabsf(denominator) > 1E-5) {
390 info->focus_of_expansion_y = (-(parameters_u[0] * (info->focus_of_expansion_x) + parameters_u[2]) / denominator);
391 } else { info->focus_of_expansion_y = 0.0f; }
392}
393
407bool analyze_flow_field(struct flow_t *vectors __attribute__((unused)), int count, float error_threshold __attribute__((unused)), int n_iterations __attribute__((unused)), int n_samples __attribute__((unused)),
408 int im_width __attribute__((unused)), int im_height __attribute__((unused)), float focal_length __attribute__((unused)), struct linear_flow_fit_info *info __attribute__((unused)))
409{
410 // Are there enough flow vectors to perform a fit?
411 if (count < N_PAR_TR_FIT) {
412 // return that no fit was made:
413 return false;
414 }
415 return true;
416
417 /*
418 // fit linear flow field:
419 //float parameters_u[N_PAR_TR_FIT], parameters_v[N_PAR_TR_FIT], min_error_u, min_error_v;
420 float parameters_comb[N_PAR_TR_FIT], min_error_comb;
421
422 // normalize the flow vectors. Is supposed to be better for the fit and the parameters will be easier to interpret:
423 struct flow_t *normalized_vectors = (struct flow_t *) malloc(count *sizeof(struct flow_t));
424
425
426 //float targets_x[count];
427 //float targets_y[count];
428
429 // separate horizontal / vertical fit:
430 //float D_x[count][N_PAR_TR_FIT-1]; // bias is added in the RANSAC routine
431 //float D_y[count][N_PAR_TR_FIT-1];
432
433 // combined fit:
434 int total_count = 2*count;
435 float targets[total_count];
436 float D_comb[total_count][N_PAR_TR_FIT];
437 int index;
438 for (int i = 0; i < count; i++) {
439
440 // normalize vectors:
441 normalized_vectors[i].pos.x = (vectors[i].pos.x - (im_width / 2.0f)) / focal_length;
442 normalized_vectors[i].pos.y = (vectors[i].pos.y - (im_height / 2.0f)) / focal_length;
443 normalized_vectors[i].flow_x = vectors[i].flow_x / focal_length;
444 normalized_vectors[i].flow_y = vectors[i].flow_y / focal_length;
445
446
447 // Combine vertical and horizontal flow in one system:
448 // As in "Estimation of Visual Motion Parameters Used for Bio-inspired Navigation", by Alkowatly et al.
449 index = i*2;
450 targets[index] = normalized_vectors[i].flow_x;
451 D_comb[index][0] = 1.0f;
452 D_comb[index][1] = normalized_vectors[i].pos.x;
453 D_comb[index][2] = normalized_vectors[i].pos.y;
454 D_comb[index][3] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.y;
455 D_comb[index][4] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.x;
456 D_comb[index][5] = 0.0f;
457
458 index++;
459 targets[index] = normalized_vectors[i].flow_y;
460 D_comb[index][0] = 0.0f;
461 D_comb[index][1] = normalized_vectors[i].pos.y;
462 D_comb[index][1] = -normalized_vectors[i].pos.x;
463 D_comb[index][2] = normalized_vectors[i].pos.y * normalized_vectors[i].pos.y;
464 D_comb[index][3] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.y;
465 D_comb[index][4] = 1.0f;
466
467 }
468
469 // Perform RANSAC on the horizontal flow:
470 // RANSAC_linear_model(n_samples, n_iterations, error_threshold, targets_x, N_PAR_TR_FIT, D_x, count, parameters_u, &min_error_u);
471
472 // Perform RANSAC on the combined system:
473 bool use_bias = false;
474 RANSAC_linear_model(n_samples, n_iterations, error_threshold, targets, N_PAR_TR_FIT+1, D_comb, total_count, parameters_comb, &min_error_comb, use_bias);
475
476 // extract information from the parameters:
477 info->rotation_X = parameters_comb[3];
478 info->rotation_Y = -parameters_comb[4];
479 info->rotation_Z = parameters_comb[2];
480 info->divergence = parameters_comb[1];
481 info->relative_velocity_x = -parameters_comb[0] - info->rotation_Y;
482 info->relative_velocity_y = -parameters_comb[5] + info->rotation_X;
483 if(fabs(parameters_comb[1]) > 1E-3) {
484 info->focus_of_expansion_x = info->relative_velocity_x / parameters_comb[1];
485 info->focus_of_expansion_y = info->relative_velocity_y / parameters_comb[1];
486 }
487 else {
488 // FoE in the center of the image:
489 info->focus_of_expansion_x = 0.0f;
490 info->focus_of_expansion_y = 0.0f;
491 }
492
493 // free up allocated memory:
494 free(normalized_vectors);
495
496 // return successful fit:
497 return true;
498 */
499}
Perform Random Sample Consensus (RANSAC), a robust fitting method.
int n_samples
Definition detect_gate.c:85
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
#define E
#define A
Definition image.h:78
static float p[2][2]
#define N_PAR_TR_FIT
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v)
Analyze a linear flow field, retrieving information such as divergence, surface roughness,...
bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info)
Analyze a flow field, retrieving information on relative velocities and rotations,...
#define MIN_SAMPLES_FIT
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info)
Analyze a linear flow field, retrieving information such as divergence, surface roughness,...
void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, struct linear_flow_fit_info *info)
Extract information from the parameters that were fit to the optical flow field.
float slope_x
Slope of the surface in x-direction - given sufficient lateral motion.
float divergence
Basically, relative_velocity_z. Actual divergence of a 2D flow field is 2 * relative_velocity_z.
int n_inliers_v
Number of inliers in the vertical flow fit.
float slope_y
Slope of the surface in y-direction - given sufficient lateral motion.
float focus_of_expansion_x
Image x-coordinate of the focus of expansion (contraction).
float time_to_contact
Basically, 1 / relative_velocity_z.
float relative_velocity_x
Relative velocity in x-direction, i.e., vx / z, where z is the depth in direction of the camera's pri...
int n_inliers_u
Number of inliers in the horizontal flow fit.
float relative_velocity_z
Relative velocity in z-direction, i.e., vz / z, where z is the depth in direction of the camera's pri...
float surface_roughness
The error of the linear fit is a measure of surface roughness.
float focus_of_expansion_y
Image y-coordinate of the focus of expansion (contraction).
float relative_velocity_y
Relative velocity in y-direction, i.e., vy / z, where z is the depth in direction of the camera's pri...
float fit_error
Error of the fit (same as surface roughness).
Paparazzi floating point algebra.
void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l)
SVD based linear solver.
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
Matrix decompositions in floating point.
Simple matrix helper macros.
#define MAT_MUL(_i, _k, _j, C, A, B)
#define MAT_SUB(_i, _j, C, A, B)