Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
46 // Is this still necessary?
47 #define MAX_COUNT_PT 50
48 
49 #define MIN_SAMPLES_FIT 3
50 #define NO_FIT 0
51 #define FIT 1
52 
65 int 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)
66 {
67  // Are there enough flow vectors to perform a fit?
68  if (count < MIN_SAMPLES_FIT) {
69  // return that no fit was made:
70  return NO_FIT;
71  }
72 
73  // fit linear flow field:
74  float parameters_u[3], parameters_v[3], min_error_u, min_error_v;
75  fit_linear_flow_field(vectors, count, n_iterations, error_threshold, n_samples, parameters_u, parameters_v, &info->fit_error, &min_error_u, &min_error_v, &info->n_inliers_u, &info->n_inliers_v);
76 
77  // extract information from the parameters:
78  extract_information_from_parameters(parameters_u, parameters_v, im_width, im_height, info);
79 
80  // surface roughness is equal to fit error:
81  info->surface_roughness = info->fit_error;
82  info->divergence = info->relative_velocity_z;
83  float diverg = (abs(info->divergence) < 1E-5) ? 1E-5 : info->divergence;
84  info->time_to_contact = 1.0f / diverg;
85 
86  // return successful fit:
87  return FIT;
88 }
89 
105 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)
106 {
107 
108  // We will solve systems of the form A x = b,
109  // where A = [nx3] matrix with entries [x, y, 1] for each optic flow location
110  // and b = [nx1] vector with either the horizontal (bu) or vertical (bv) flow.
111  // x in the system are the parameters for the horizontal (pu) or vertical (pv) flow field.
112 
113  // local vars for iterating, random numbers:
114  int sam, p, i_rand, si, add_si;
115 
116  // ensure that n_samples is high enough to ensure a result for a single fit:
117  n_samples = (n_samples < MIN_SAMPLES_FIT) ? MIN_SAMPLES_FIT : n_samples;
118  // n_samples should not be higher than count:
119  n_samples = (n_samples < count) ? n_samples : count;
120 
121  // initialize matrices and vectors for the full point set problem:
122  // this is used for determining inliers
123  float _AA[count][3];
124  MAKE_MATRIX_PTR(AA, _AA, count);
125  float _bu_all[count][1];
126  MAKE_MATRIX_PTR(bu_all, _bu_all, count);
127  float _bv_all[count][1];
128  MAKE_MATRIX_PTR(bv_all, _bv_all, count);
129  for (sam = 0; sam < count; sam++) {
130  AA[sam][0] = (float) vectors[sam].pos.x;
131  AA[sam][1] = (float) vectors[sam].pos.y;
132  AA[sam][2] = 1.0f;
133  bu_all[sam][0] = (float) vectors[sam].flow_x;
134  bv_all[sam][0] = (float) vectors[sam].flow_y;
135  }
136 
137  // later used to determine the error of a set of parameters on the whole set:
138  float _bb[count][1];
139  MAKE_MATRIX_PTR(bb, _bb, count);
140  float _C[count][1];
141  MAKE_MATRIX_PTR(C, _C, count);
142 
143  // ***************
144  // perform RANSAC:
145  // ***************
146 
147  // set up variables for small linear system solved repeatedly inside RANSAC:
148  float _A[n_samples][3];
149  MAKE_MATRIX_PTR(A, _A, n_samples);
150  float _bu[n_samples][1];
151  MAKE_MATRIX_PTR(bu, _bu, n_samples);
152  float _bv[n_samples][1];
153  MAKE_MATRIX_PTR(bv, _bv, n_samples);
154  float w[n_samples], _v[3][3];
155  MAKE_MATRIX_PTR(v, _v, 3);
156  float _pu[3][1];
157  MAKE_MATRIX_PTR(pu, _pu, 3);
158  float _pv[3][1];
159  MAKE_MATRIX_PTR(pv, _pv, 3);
160 
161  // iterate and store parameters, errors, inliers per fit:
162  float PU[n_iterations * 3];
163  float PV[n_iterations * 3];
164  float errors_pu[n_iterations];
165  errors_pu[0] = 0.0;
166  float errors_pv[n_iterations];
167  errors_pv[0] = 0.0;
168  int n_inliers_pu[n_iterations];
169  int n_inliers_pv[n_iterations];
170  int it, ii;
171  for (it = 0; it < n_iterations; it++) {
172  // select a random sample of n_sample points:
173  int sample_indices[n_samples];
174  i_rand = 0;
175 
176  // sampling without replacement:
177  while (i_rand < n_samples) {
178  si = rand() % count;
179  add_si = 1;
180  for (ii = 0; ii < i_rand; ii++) {
181  if (sample_indices[ii] == si) { add_si = 0; }
182  }
183  if (add_si) {
184  sample_indices[i_rand] = si;
185  i_rand ++;
186  }
187  }
188 
189  // Setup the system:
190  for (sam = 0; sam < n_samples; sam++) {
191  A[sam][0] = (float) vectors[sample_indices[sam]].pos.x;
192  A[sam][1] = (float) vectors[sample_indices[sam]].pos.y;
193  A[sam][2] = 1.0f;
194  bu[sam][0] = (float) vectors[sample_indices[sam]].flow_x;
195  bv[sam][0] = (float) vectors[sample_indices[sam]].flow_y;
196  //printf("%d,%d,%d,%d,%d\n",A[sam][0],A[sam][1],A[sam][2],bu[sam][0],bv[sam][0]);
197  }
198 
199  // Solve the small system:
200 
201  // for horizontal flow:
202  // decompose A in u, w, v with singular value decomposition A = u * w * vT.
203  // u replaces A as output:
204  pprz_svd_float(A, w, v, n_samples, 3);
205  pprz_svd_solve_float(pu, A, w, v, bu, n_samples, 3, 1);
206  PU[it * 3] = pu[0][0];
207  PU[it * 3 + 1] = pu[1][0];
208  PU[it * 3 + 2] = pu[2][0];
209 
210  // for vertical flow:
211  pprz_svd_solve_float(pv, A, w, v, bv, n_samples, 3, 1);
212  PV[it * 3] = pv[0][0];
213  PV[it * 3 + 1] = pv[1][0];
214  PV[it * 3 + 2] = pv[2][0];
215 
216  // count inliers and determine their error on all points:
217  errors_pu[it] = 0;
218  errors_pv[it] = 0;
219  n_inliers_pu[it] = 0;
220  n_inliers_pv[it] = 0;
221 
222  // for horizontal flow:
223  // bb = AA * pu:
224  MAT_MUL(count, 3, 1, bb, AA, pu);
225  // subtract bu_all: C = 0 in case of perfect fit:
226  MAT_SUB(count, 1, C, bb, bu_all);
227 
228  for (p = 0; p < count; p++) {
229  C[p][0] = abs(C[p][0]);
230  if (C[p][0] < error_threshold) {
231  errors_pu[it] += C[p][0];
232  n_inliers_pu[it]++;
233  } else {
234  errors_pu[it] += error_threshold;
235  }
236  }
237 
238  // for vertical flow:
239  // bb = AA * pv:
240  MAT_MUL(count, 3, 1, bb, AA, pv);
241  // subtract bv_all: C = 0 in case of perfect fit:
242  MAT_SUB(count, 1, C, bb, bv_all);
243 
244  for (p = 0; p < count; p++) {
245  C[p][0] = abs(C[p][0]);
246  if (C[p][0] < error_threshold) {
247  errors_pv[it] += C[p][0];
248  n_inliers_pv[it]++;
249  } else {
250  errors_pv[it] += error_threshold;
251  }
252  }
253  }
254 
255  // After all iterations:
256  // select the parameters with lowest error:
257  // for horizontal flow:
258  int param;
259  int min_ind = 0;
260  *min_error_u = (float)errors_pu[0];
261  for (it = 1; it < n_iterations; it++) {
262  if (errors_pu[it] < *min_error_u) {
263  *min_error_u = (float)errors_pu[it];
264  min_ind = it;
265  }
266  }
267  for (param = 0; param < 3; param++) {
268  parameters_u[param] = PU[min_ind * 3 + param];
269  }
270  *n_inliers_u = n_inliers_pu[min_ind];
271 
272  // for vertical flow:
273  min_ind = 0;
274  *min_error_v = (float)errors_pv[0];
275  for (it = 0; it < n_iterations; it++) {
276  if (errors_pv[it] < *min_error_v) {
277  *min_error_v = (float)errors_pv[it];
278  min_ind = it;
279  }
280  }
281  for (param = 0; param < 3; param++) {
282  parameters_v[param] = PV[min_ind * 3 + param];
283  }
284  *n_inliers_v = n_inliers_pv[min_ind];
285 
286  // error has to be determined on the entire set without threshold:
287  // bb = AA * pu:
288  MAT_MUL(count, 3, 1, bb, AA, pu);
289  // subtract bu_all: C = 0 in case of perfect fit:
290  MAT_SUB(count, 1, C, bb, bu_all);
291  *min_error_u = 0;
292  for (p = 0; p < count; p++) {
293  *min_error_u += abs(C[p][0]);
294  }
295  // bb = AA * pv:
296  MAT_MUL(count, 3, 1, bb, AA, pv);
297  // subtract bv_all: C = 0 in case of perfect fit:
298  MAT_SUB(count, 1, C, bb, bv_all);
299  *min_error_v = 0;
300  for (p = 0; p < count; p++) {
301  *min_error_v += abs(C[p][0]);
302  }
303  *fit_error = (*min_error_u + *min_error_v) / (2 * count);
304 
305 }
315 void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, struct linear_flow_fit_info *info)
316 {
317  // This method assumes a linear flow field in x- and y- direction according to the formulas:
318  // u = parameters_u[0] * x + parameters_u[1] * y + parameters_u[2]
319  // v = parameters_v[0] * x + parameters_v[1] * y + parameters_v[2]
320  // where u is the horizontal flow at image coordinate (x,y)
321  // and v is the vertical flow at image coordinate (x,y)
322 
323  // relative velocities:
324  info->relative_velocity_z = (parameters_u[0] + parameters_v[1]) / 2.0f; // divergence / 2
325 
326  // translation orthogonal to the camera axis:
327  // flow in the center of the image:
328  info->relative_velocity_x = -(parameters_u[2] + (im_width / 2.0f) * parameters_u[0] + (im_height / 2.0f) * parameters_u[1]);
329  info->relative_velocity_y = -(parameters_v[2] + (im_width / 2.0f) * parameters_v[0] + (im_height / 2.0f) * parameters_v[1]);
330 
331  float arv_x = abs(info->relative_velocity_x);
332  float arv_y = abs(info->relative_velocity_y);
333 
334  // extract inclination from flow field:
335  float threshold_slope = 1.0;
336  float eta = 0.002;
337 
338  if (abs(parameters_v[1]) < eta && arv_y < threshold_slope && arv_x >= 2 * threshold_slope) {
339  // there is no forward motion and not enough vertical motion, but enough horizontal motion:
340  info->slope_x = parameters_u[0] / info->relative_velocity_x;
341  } else if (arv_y >= 2 * threshold_slope) {
342  // there is sufficient vertical motion:
343  info->slope_x = parameters_v[0] / info->relative_velocity_y;
344  } else {
345  // there may be forward motion, then we can do a quadratic fit:
346  // a linear fit provides no information though
347  info->slope_x = 0.0f;
348  }
349 
350  if (abs(parameters_u[0]) < eta && arv_x < threshold_slope && arv_y >= 2 * threshold_slope) {
351  // there is no forward motion, little horizontal movement, but sufficient vertical motion:
352  info->slope_y = parameters_v[1] / info->relative_velocity_y;
353  } else if (arv_x >= 2 * threshold_slope) {
354  // there is sufficient horizontal motion:
355  info->slope_y = parameters_u[1] / info->relative_velocity_x;
356  } else {
357  // there could be forward motion, then we can do a quadratic fit:
358  // a linear fit provides no information though
359  info->slope_y = 0.0f;
360  }
361 
362  // Focus of Expansion:
363  // the flow planes intersect the flow=0 plane in a line
364  // the FoE is the point where these 2 lines intersect (flow = (0,0))
365  // x:
366  float denominator = parameters_v[0] * parameters_u[1] - parameters_u[0] * parameters_v[1];
367  if (abs(denominator) > 1E-5) {
368  info->focus_of_expansion_x = ((parameters_u[2] * parameters_v[1] - parameters_v[2] * parameters_u[1]) / denominator);
369  } else { info->focus_of_expansion_x = 0.0f; }
370  // y:
371  denominator = parameters_u[1];
372  if (abs(denominator) > 1E-5) {
373  info->focus_of_expansion_y = (-(parameters_u[0] * (info->focus_of_expansion_x) + parameters_u[2]) / denominator);
374  } else { info->focus_of_expansion_y = 0.0f; }
375 }
376 
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...
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, focus of expansion, etc.
Simple matrix helper macros.
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
float fit_error
Error of the fit (same as surface roughness)
int n_inliers_u
Number of inliers in the horizontal flow fit.
Definition: image.h:60
#define FIT
#define NO_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 time_to_contact
Basically, 1 / relative_velocity_z.
float focus_of_expansion_x
Image x-coordinate of the focus of expansion (contraction)
float focus_of_expansion_y
Image y-coordinate of the focus of expansion (contraction)
Paparazzi floating point algebra.
float slope_y
Slope of the surface in y-direction - given sufficient lateral motion.
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.
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...
#define AA
Definition: LPC21xx.h:179
struct point_t pos
The original position the flow comes from.
Definition: image.h:61
float slope_x
Slope of the surface in x-direction - given sufficient lateral motion.
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.
Matrix decompositions in floating point.
int 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, focus of expansion, etc.
int n_inliers_v
Number of inliers in the vertical flow fit.
float divergence
Basically, relative_velocity_z. Actual divergence of a 2D flow field is 2 * relative_velocity_z.
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
#define MAT_SUB(_i, _j, C, A, B)
float surface_roughness
The error of the linear fit is a measure of surface roughness.
#define E
static float p[2][2]
#define A
uint16_t y
The y coordinate of the point.
Definition: image.h:56
#define MAT_MUL(_i, _k, _j, C, A, B)
#define MIN_SAMPLES_FIT