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
opticflow_calculator.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Hann Woei Ho
3  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
4  *
5  * This file is part of Paparazzi.
6  *
7  * Paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * Paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with Paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
29 #include "std.h"
30 
31 #include <stdio.h>
32 #include <string.h>
33 #include <stdlib.h>
34 
35 // Own Header
36 #include "opticflow_calculator.h"
37 
38 // Computer Vision
39 #include "lib/vision/image.h"
41 #include "lib/vision/fast_rosten.h"
42 
43 #include "size_divergence.h"
44 #include "linear_flow_fit.h"
45 
46 // What methods are run to determine divergence, lateral flow, etc.
47 // SIZE_DIV looks at line sizes and only calculates divergence
48 #define SIZE_DIV 1
49 // LINEAR_FIT makes a linear optical flow field fit and extracts a lot of information:
50 // relative velocities in x, y, z (divergence / time to contact), the slope of the surface, and the surface roughness.
51 #define LINEAR_FIT 1
52 
53 // Camera parameters (defaults are from an ARDrone 2)
54 #ifndef OPTICFLOW_FOV_W
55 #define OPTICFLOW_FOV_W 0.89360857702
56 #endif
57 PRINT_CONFIG_VAR(OPTICFLOW_FOV_W)
58 
59 #ifndef OPTICFLOW_FOV_H
60 #define OPTICFLOW_FOV_H 0.67020643276
61 #endif
62 PRINT_CONFIG_VAR(OPTICFLOW_FOV_H)
63 
64 #ifndef OPTICFLOW_FX
65 #define OPTICFLOW_FX 343.1211
66 #endif
67 PRINT_CONFIG_VAR(OPTICFLOW_FX)
68 
69 #ifndef OPTICFLOW_FY
70 #define OPTICFLOW_FY 348.5053
71 #endif
72 PRINT_CONFIG_VAR(OPTICFLOW_FY)
73 
74 /* Set the default values */
75 #ifndef OPTICFLOW_MAX_TRACK_CORNERS
76 #define OPTICFLOW_MAX_TRACK_CORNERS 25
77 #endif
78 PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS)
79 
80 #ifndef OPTICFLOW_WINDOW_SIZE
81 #define OPTICFLOW_WINDOW_SIZE 10
82 #endif
83 PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE)
84 
85 #ifndef OPTICFLOW_SUBPIXEL_FACTOR
86 #define OPTICFLOW_SUBPIXEL_FACTOR 10
87 #endif
88 PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR)
89 
90 #ifndef OPTICFLOW_MAX_ITERATIONS
91 #define OPTICFLOW_MAX_ITERATIONS 10
92 #endif
93 PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS)
94 
95 #ifndef OPTICFLOW_THRESHOLD_VEC
96 #define OPTICFLOW_THRESHOLD_VEC 2
97 #endif
98 PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC)
99 
100 #ifndef OPTICFLOW_FAST9_ADAPTIVE
101 #define OPTICFLOW_FAST9_ADAPTIVE TRUE
102 #endif
103 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE)
104 
105 #ifndef OPTICFLOW_FAST9_THRESHOLD
106 #define OPTICFLOW_FAST9_THRESHOLD 20
107 #endif
108 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
109 
110 #ifndef OPTICFLOW_FAST9_MIN_DISTANCE
111 #define OPTICFLOW_FAST9_MIN_DISTANCE 10
112 #endif
113 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE)
114 
115 /* Functions only used here */
116 static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime);
117 static int cmp_flow(const void *a, const void *b);
118 
126 {
127  /* Create the image buffers */
128  image_create(&opticflow->img_gray, w, h, IMAGE_GRAYSCALE);
129  image_create(&opticflow->prev_img_gray, w, h, IMAGE_GRAYSCALE);
130 
131  /* Set the previous values */
132  opticflow->got_first_img = FALSE;
133  opticflow->prev_phi = 0.0;
134  opticflow->prev_theta = 0.0;
135 
136  /* Set the default values */
137  opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
138  opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
139  opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
140  opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
141  opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
142 
143  opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
144  opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
145  opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
146 }
147 
156  struct opticflow_result_t *result)
157 {
158  // variables for size_divergence:
159  float size_divergence; int n_samples;
160 
161  // variables for linear flow fit:
162  float error_threshold; int n_iterations_RANSAC, n_samples_RANSAC, success_fit; struct linear_flow_fit_info fit_info;
163 
164  // Update FPS for information
165  result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
166  memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval));
167 
168  // Convert image to grayscale
169  image_to_grayscale(img, &opticflow->img_gray);
170 
171  // Copy to previous image if not set
172  if (!opticflow->got_first_img) {
173  image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
174  opticflow->got_first_img = TRUE;
175  }
176 
177  // *************************************************************************************
178  // Corner detection
179  // *************************************************************************************
180 
181  // FAST corner detection (TODO: non fixed threshold)
182  struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
183  20, 20, &result->corner_cnt);
184 
185  // Adaptive threshold
186  if (opticflow->fast9_adaptive) {
187 
188  // Decrease and increase the threshold based on previous values
189  if (result->corner_cnt < 40 && opticflow->fast9_threshold > 5) {
190  opticflow->fast9_threshold--;
191  } else if (result->corner_cnt > 50 && opticflow->fast9_threshold < 60) {
192  opticflow->fast9_threshold++;
193  }
194  }
195 
196 #if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_CORNERS
197  image_show_points(img, corners, result->corner_cnt);
198 #endif
199 
200  // Check if we found some corners to track
201  if (result->corner_cnt < 1) {
202  free(corners);
203  image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
204  return;
205  }
206 
207  // *************************************************************************************
208  // Corner Tracking
209  // *************************************************************************************
210 
211  // Execute a Lucas Kanade optical flow
212  result->tracked_cnt = result->corner_cnt;
213  struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
214  opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
215  opticflow->threshold_vec, opticflow->max_track_corners);
216 
217 #if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
218  image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
219 #endif
220 
221  // Estimate size divergence:
222  if (SIZE_DIV) {
223  n_samples = 100;
224  size_divergence = get_size_divergence(vectors, result->tracked_cnt, n_samples);
225  result->div_size = size_divergence;
226  } else {
227  result->div_size = 0.0f;
228  }
229  if (LINEAR_FIT) {
230  // Linear flow fit (normally derotation should be performed before):
231  error_threshold = 10.0f;
232  n_iterations_RANSAC = 20;
233  n_samples_RANSAC = 5;
234  success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
235  n_samples_RANSAC, img->w, img->h, &fit_info);
236 
237  if (!success_fit) {
238  fit_info.divergence = 0.0f;
239  fit_info.surface_roughness = 0.0f;
240  }
241 
242  result->divergence = fit_info.divergence;
243  result->surface_roughness = fit_info.surface_roughness;
244  } else {
245  result->divergence = 0.0f;
246  result->surface_roughness = 0.0f;
247  }
248 
249 
250  // Get the median flow
251  qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow);
252  if (result->tracked_cnt == 0) {
253  // We got no flow
254  result->flow_x = 0;
255  result->flow_y = 0;
256  } else if (result->tracked_cnt > 3) {
257  // Take the average of the 3 median points
258  result->flow_x = vectors[result->tracked_cnt / 2 - 1].flow_x;
259  result->flow_y = vectors[result->tracked_cnt / 2 - 1].flow_y;
260  result->flow_x += vectors[result->tracked_cnt / 2].flow_x;
261  result->flow_y += vectors[result->tracked_cnt / 2].flow_y;
262  result->flow_x += vectors[result->tracked_cnt / 2 + 1].flow_x;
263  result->flow_y += vectors[result->tracked_cnt / 2 + 1].flow_y;
264  result->flow_x /= 3;
265  result->flow_y /= 3;
266  } else {
267  // Take the median point
268  result->flow_x = vectors[result->tracked_cnt / 2].flow_x;
269  result->flow_y = vectors[result->tracked_cnt / 2].flow_y;
270  }
271 
272  // Flow Derotation
273  float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
274  float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
275  result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
276  result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
277  opticflow->prev_phi = state->phi;
278  opticflow->prev_theta = state->theta;
279 
280  // Velocity calculation
281  // Right now this formula is under assumption that the flow only exist in the center axis of the camera.
282  // TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
283  float vel_hor = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
284  float vel_ver = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;
285 
286  // Velocity calculation: uncomment if focal length of the camera is not known or incorrect.
287  // result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
288  // result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
289 
290  // Rotate velocities from camera frame coordinates to body coordinates.
291  result->vel_x = vel_ver;
292  result->vel_y = - vel_hor;
293 
294  // Determine quality of noise measurement for state filter
295  //TODO Experiment with multiple noise measurement models
296  if (result->tracked_cnt < 10) {
297  result->noise_measurement = (float)result->tracked_cnt / (float)opticflow->max_track_corners;
298  } else {
299  result->noise_measurement = 1.0;
300  }
301 
302  // *************************************************************************************
303  // Next Loop Preparation
304  // *************************************************************************************
305  free(corners);
306  free(vectors);
307  image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
308 }
309 
316 static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime)
317 {
318  uint32_t msec;
319  msec = (finishtime->tv_sec - starttime->tv_sec) * 1000;
320  msec += (finishtime->tv_usec - starttime->tv_usec) / 1000;
321  return msec;
322 }
323 
331 static int cmp_flow(const void *a, const void *b)
332 {
333  const struct flow_t *a_p = (const struct flow_t *)a;
334  const struct flow_t *b_p = (const struct flow_t *)b;
335  return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y *
336  b_p->flow_y);
337 }
338 
339 
unsigned short uint16_t
Definition: types.h:16
int16_t flow_y
Flow in y direction from the camera (in subpixels)
uint16_t fast9_min_distance
Minimum distance in pixels between corners.
int16_t flow_der_y
The derotated flow calculation in the y direction (in subpixels)
float div_size
Divergence as determined with the size_divergence script.
uint8_t max_iterations
The maximum amount of iterations the Lucas Kanade algorithm should do.
struct opticflow_t opticflow
Opticflow calculations.
uint16_t window_size
Window size of the Lucas Kanade calculation (needs to be even)
void image_switch(struct image_t *a, struct image_t *b)
This will switch image *a and *b This is faster as image_copy because it doesn't copy the whole image...
Definition: image.c:97
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
Initialize the opticflow calculator.
Calculate velocity from optic flow.
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:38
Definition: image.h:42
Definition: image.h:60
float theta
pitch [rad]
#define OPTICFLOW_THRESHOLD_VEC
#define OPTICFLOW_FAST9_MIN_DISTANCE
void image_copy(struct image_t *input, struct image_t *output)
Copy an image from inut to output This will only work if the formats are the same.
Definition: image.c:77
#define OPTICFLOW_FAST9_ADAPTIVE
float prev_phi
Phi from the previous image frame.
void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt)
Show points in an image by coloring them through giving the pixels the maximum value.
Definition: image.c:431
static int cmp_flow(const void *a, const void *b)
Compare two flow vectors based on flow distance Used for sorting.
Calculate divergence from flow vectors by looking at line sizes beteween the points.
uint8_t threshold_vec
The threshold in x, y subpixels which the algorithm should stop.
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
Shows the flow from a specific point to a new point This works on YUV422 and Grayscale images...
Definition: image.c:456
#define FALSE
Definition: std.h:5
uint8_t max_track_corners
Maximum amount of corners Lucas Kanade should track.
float agl
height above ground [m]
float get_size_divergence(struct flow_t *vectors, int count, int n_samples)
Get divergence from optical flow vectors based on line sizes between corners.
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result)
Run the optical flow on a new image frame.
struct point_t * fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint16_t x_padding, uint16_t y_padding, uint16_t *num_corners)
Do a FAST9 corner detection.
Definition: fast_rosten.c:49
Image helper functions like resizing, color filter, converters...
#define TRUE
Definition: std.h:4
uint16_t tracked_cnt
The amount of tracked corners.
bool_t got_first_img
If we got a image to work with.
int16_t flow_x
The x direction flow in subpixels.
Definition: image.h:62
struct timeval prev_timestamp
Timestamp of the previous frame, used for FPS calculation.
uint16_t w
Image width.
Definition: image.h:44
unsigned long uint32_t
Definition: types.h:18
#define OPTICFLOW_FAST9_THRESHOLD
#define OPTICFLOW_FOV_W
uint16_t h
Image height.
Definition: image.h:45
struct image_t prev_img_gray
Previous gray image frame.
uint8_t fast9_threshold
FAST9 corner detection threshold.
#define OPTICFLOW_FX
#define OPTICFLOW_WINDOW_SIZE
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition: image.c:116
Definition: image.h:54
#define OPTICFLOW_FOV_H
bool_t fast9_adaptive
Whether the FAST9 threshold should be adaptive.
struct image_t img_gray
Current gray image frame.
float vel_x
The velocity in the x direction.
uint8_t subpixel_factor
The amount of subpixels per pixel.
#define SIZE_DIV
uint16_t corner_cnt
The amount of coners found by FAST9.
float vel_y
The velocity in the y direction.
#define OPTICFLOW_FY
float fps
Frames per second of the optical flow calculation.
efficient fixed-point optical-flow calculation
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.
struct timeval ts
The timestamp of creation.
Definition: image.h:46
float divergence
Basically, relative_velocity_z. Actual divergence of a 2D flow field is 2 * relative_velocity_z.
#define LINEAR_FIT
float surface_roughness
The error of the linear fit is a measure of surface roughness.
float prev_theta
Theta from the previous image frame.
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:36
static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime)
Calculate the difference from start till finish.
#define OPTICFLOW_MAX_TRACK_CORNERS
struct flow_t * opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points)
Compute the optical flow of several points using the Lucas-Kanade algorithm by Yves Bouguet The initi...
Definition: lucas_kanade.c:53
#define OPTICFLOW_SUBPIXEL_FACTOR
int16_t flow_x
Flow in x direction from the camera (in subpixels)
#define OPTICFLOW_MAX_ITERATIONS
float noise_measurement
noise of measurement, for state filter
int16_t flow_y
The y direction flow in subpixels.
Definition: image.h:63
float divergence
Divergence as determined with a linear flow fit.
float surface_roughness
Surface roughness as determined with a linear optical flow fit.
struct State state
Definition: state.c:36
float phi
roll [rad]
int16_t flow_der_x
The derotated flow calculation in the x direction (in subpixels)