Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
lucas_kanade.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 G. de Croon
3  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
4  * 2016 Hrvoje Brezak <hrvoje.brezak@gmail.com>
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, see
20  * <http://www.gnu.org/licenses/>.
21  */
22 
32 #include <stdlib.h>
33 #include <stdio.h>
34 #include <math.h>
35 #include <string.h>
36 #include "lucas_kanade.h"
37 
38 
74 struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points,
75  uint16_t *points_cnt, uint16_t half_window_size,
76  uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_level,
77  uint8_t keep_bad_points)
78 {
79 
80  // if no pyramids, use the old code:
81  if (pyramid_level == 0) {
82  // use the old code in this case:
83  return opticFlowLK_flat(new_img, old_img, points, points_cnt, half_window_size, subpixel_factor, max_iterations,
84  step_threshold, max_points, keep_bad_points);
85  }
86 
87  // Allocate some memory for returning the vectors
88  struct flow_t *vectors = calloc(max_points, sizeof(struct flow_t));
89 
90  // Determine patch sizes and initialize neighborhoods
91  uint16_t patch_size = 2 * half_window_size + 1;
92  // TODO: Feature management shows that this threshold rejects corners maybe too often, maybe another formula could be chosen
93  uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
94  uint16_t padded_patch_size = patch_size + 2;
95  uint16_t border_size = padded_patch_size / 2 + 2; // amount of padding added to images
96 
97  // Allocate memory for image pyramids
98  struct image_t *pyramid_old = malloc(sizeof(struct image_t) * (pyramid_level + 1));
99  struct image_t *pyramid_new = malloc(sizeof(struct image_t) * (pyramid_level + 1));
100 
101  // Build pyramid levels
102  pyramid_build(old_img, pyramid_old, pyramid_level, border_size);
103  pyramid_build(new_img, pyramid_new, pyramid_level, border_size);
104 
105  // Create the window images
106  struct image_t window_I, window_J, window_DX, window_DY, window_diff;
107  image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
112 
113  // Iterate through pyramid levels
114  for (int8_t LVL = pyramid_level; LVL != -1; LVL--) {
115  uint16_t points_orig = *points_cnt;
116  *points_cnt = 0;
117  uint16_t new_p = 0;
118 
119  // Calculate the amount of points to skip
120  float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1;
121 
122  // Go through all points
123  for (uint16_t i = 0; i < max_points && i < points_orig; i++) {
124  uint16_t p = i * skip_points;
125 
126  if (LVL == pyramid_level) {
127  // Convert point position on original image to a subpixel coordinate on the top pyramid level
128  vectors[new_p].pos.x = (points[p].x * subpixel_factor) >> pyramid_level;
129  vectors[new_p].pos.y = (points[p].y * subpixel_factor) >> pyramid_level;
130  vectors[new_p].flow_x = 0;
131  vectors[new_p].flow_y = 0;
132 
133  } else {
134  // (5) use calculated flow as initial flow estimation for next level of pyramid
135  vectors[new_p].pos.x = vectors[p].pos.x << 1;
136  vectors[new_p].pos.y = vectors[p].pos.y << 1;
137  vectors[new_p].flow_x = vectors[p].flow_x << 1;
138  vectors[new_p].flow_y = vectors[p].flow_y << 1;
139  }
140 
141  // If the pixel is outside original image, do not track it
142  if ((((int32_t) vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
143  || ((vectors[new_p].pos.x + vectors[new_p].flow_x) > (uint32_t)((pyramid_new[LVL].w - 1 - 2 * border_size)*
144  subpixel_factor))
145  || (((int32_t) vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
146  || ((vectors[new_p].pos.y + vectors[new_p].flow_y) > (uint32_t)((pyramid_new[LVL].h - 1 - 2 * border_size)*
147  subpixel_factor))) {
148  if (keep_bad_points) {
149  vectors[new_p].error = LARGE_FLOW_ERROR;
150  new_p++;
151  (*points_cnt)++;
152  }
153  continue;
154  }
155 
156 
157  // (1) determine the subpixel neighborhood in the old image
158  image_subpixel_window(&pyramid_old[LVL], &window_I, &vectors[new_p].pos, subpixel_factor, border_size);
159 
160  // (2) get the x- and y- gradients
161  image_gradients(&window_I, &window_DX, &window_DY);
162 
163  // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
164  int32_t G[4];
165  image_calculate_g(&window_DX, &window_DY, G);
166 
167  // calculate G's determinant in subpixel units:
168  int32_t Det = (G[0] * G[3] - G[1] * G[2]);
169 
170  // Check if the determinant is bigger than 1
171  if (Det < 1) {
172  if (keep_bad_points) {
173  vectors[new_p].error = LARGE_FLOW_ERROR;
174  new_p++;
175  (*points_cnt)++;
176  }
177  continue;
178  }
179 
180  // (4) iterate over taking steps in the image to minimize the error:
181  bool tracked = true;
182 
183  for (uint8_t it = max_iterations; it--;) {
184  struct point_t new_point = { vectors[new_p].pos.x + vectors[new_p].flow_x,
185  vectors[new_p].pos.y + vectors[new_p].flow_y,
186  0, 0, 0
187  };
188 
189  // If the pixel is outside original image, do not track it
190  if ((((int32_t)vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
191  || (new_point.x > (uint32_t)((pyramid_new[LVL].w - 1 - 2 * border_size)*subpixel_factor))
192  || (((int32_t)vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
193  || (new_point.y > (uint32_t)((pyramid_new[LVL].h - 1 - 2 * border_size)*subpixel_factor))) {
194  tracked = false;
195  break;
196  }
197 
198  // [a] get the subpixel neighborhood in the new image
199  image_subpixel_window(&pyramid_new[LVL], &window_J, &new_point, subpixel_factor, border_size);
200 
201  // [b] determine the image difference between the two neighborhoods
202  uint32_t error = image_difference(&window_I, &window_J, &window_diff);
203 
204  if (error > error_threshold && it < max_iterations / 2) {
205  tracked = false;
206  break;
207  }
208 
209  int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
210  int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
211 
212 
213  // [d] calculate the additional flow step and possibly terminate the iteration
214  int16_t step_x = (((int64_t) G[3] * b_x - G[1] * b_y) * subpixel_factor) / Det;
215  int16_t step_y = (((int64_t) G[0] * b_y - G[2] * b_x) * subpixel_factor) / Det;
216 
217  vectors[new_p].flow_x = vectors[new_p].flow_x + step_x;
218  vectors[new_p].flow_y = vectors[new_p].flow_y + step_y;
219  vectors[new_p].error = error;
220 
221  // Check if we exceeded the treshold CHANGED made this better for 0.03
222  if ((abs(step_x) + abs(step_y)) < step_threshold) {
223  break;
224  }
225  } // lucas kanade step iteration
226 
227  // If we tracked the point we update the index and the count
228  if (tracked) {
229  new_p++;
230  (*points_cnt)++;
231  } else if (keep_bad_points) {
232  vectors[new_p].flow_x = 0;
233  vectors[new_p].flow_y = 0;
234  vectors[new_p].error = LARGE_FLOW_ERROR;
235  new_p++;
236  (*points_cnt)++;
237  }
238  } // go through all points
239 
240  } // LVL of pyramid
241 
242  // Free the images
243  image_free(&window_I);
244  image_free(&window_J);
245  image_free(&window_DX);
246  image_free(&window_DY);
247  image_free(&window_diff);
248 
249  for (int8_t i = pyramid_level; i != -1; i--) {
250  image_free(&pyramid_old[i]);
251  image_free(&pyramid_new[i]);
252  }
253  pyramid_old = NULL;
254  pyramid_new = NULL;
255 
256  // Return the vectors
257  return vectors;
258 }
259 
275 struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt,
276  uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold,
277  uint16_t max_points, uint8_t keep_bad_points)
278 {
279  // A straightforward one-level implementation of Lucas-Kanade.
280  // For all points:
281  // (1) determine the subpixel neighborhood in the old image
282  // (2) get the x- and y- gradients
283  // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
284  // (4) iterate over taking steps in the image to minimize the error:
285  // [a] get the subpixel neighborhood in the new image
286  // [b] determine the image difference between the two neighborhoods
287  // [c] calculate the 'b'-vector
288  // [d] calculate the additional flow step and possibly terminate the iteration
289 
290  // Allocate some memory for returning the vectors
291  struct flow_t *vectors = calloc(max_points, sizeof(struct flow_t));
292  uint16_t new_p = 0;
293  uint16_t points_orig = *points_cnt;
294  *points_cnt = 0;
295 
296  // determine patch sizes and initialize neighborhoods
297  uint16_t patch_size = 2 * half_window_size;
298  uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
299  uint16_t padded_patch_size = patch_size + 2;
300 
301  // Create the window images
302  struct image_t window_I, window_J, window_DX, window_DY, window_diff;
303  image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
308 
309  // Calculate the amount of points to skip
310  float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1;
311 
312  // Go through all points
313  for (uint16_t i = 0; i < max_points && i < points_orig; i++) {
314  uint16_t p = i * skip_points;
315 
316  // Convert the point to a subpixel coordinate
317  vectors[new_p].pos.x = points[p].x * subpixel_factor;
318  vectors[new_p].pos.y = points[p].y * subpixel_factor;
319 
320  // If the pixel is outside ROI, do not track it
321  if (points[p].x < half_window_size || (old_img->w - points[p].x) < half_window_size
322  || points[p].y < half_window_size || (old_img->h - points[p].y) < half_window_size) {
323  if (keep_bad_points) {
324  vectors[new_p].error = LARGE_FLOW_ERROR;
325  new_p++;
326  (*points_cnt)++;
327  }
328  continue;
329  }
330 
331  // (1) determine the subpixel neighborhood in the old image
332  image_subpixel_window(old_img, &window_I, &vectors[new_p].pos, subpixel_factor, 0);
333 
334  // (2) get the x- and y- gradients
335  image_gradients(&window_I, &window_DX, &window_DY);
336 
337  // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
338  int32_t G[4];
339  image_calculate_g(&window_DX, &window_DY, G);
340 
341  // calculate G's determinant in subpixel units:
342  int32_t Det = (G[0] * G[3] - G[1] * G[2]) / subpixel_factor;
343 
344  // Check if the determinant is bigger than 1
345  if (Det < 1) {
346  if (keep_bad_points) {
347  vectors[new_p].error = LARGE_FLOW_ERROR;
348  new_p++;
349  (*points_cnt)++;
350  }
351  continue;
352  }
353 
354  // a * (Ax - Bx) + (1-a) * (Ax+1 - Bx+1)
355  // a * Ax - a * Bx + (1-a) * Ax+1 - (1-a) * Bx+1
356  // (a * Ax + (1-a) * Ax+1) - (a * Bx + (1-a) * Bx+1)
357 
358  // (4) iterate over taking steps in the image to minimize the error:
359  bool tracked = TRUE;
360  for (uint8_t it = 0; it < max_iterations; it++) {
361  struct point_t new_point = {
362  vectors[new_p].pos.x + vectors[new_p].flow_x,
363  vectors[new_p].pos.y + vectors[new_p].flow_y,
364  0, 0, 0
365  };
366  // If the pixel is outside ROI, do not track it
367  if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) <= half_window_size
368  || new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) <= half_window_size
369  || new_point.x / subpixel_factor > old_img->w || new_point.y / subpixel_factor > old_img->h) {
370  tracked = FALSE;
371  break;
372  }
373 
374  // [a] get the subpixel neighborhood in the new image
375  image_subpixel_window(new_img, &window_J, &new_point, subpixel_factor, 0);
376 
377  // [b] determine the image difference between the two neighborhoods
378  // TODO: also give this error back, so that it can be used for reliability
379  uint32_t error = image_difference(&window_I, &window_J, &window_diff);
380  if (error > error_threshold && it > max_iterations / 2) {
381  tracked = FALSE;
382  break;
383  }
384 
385  int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
386  int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
387 
388  // [d] calculate the additional flow step and possibly terminate the iteration
389  int16_t step_x = (G[3] * b_x - G[1] * b_y) / Det;
390  int16_t step_y = (G[0] * b_y - G[2] * b_x) / Det;
391  vectors[new_p].flow_x += step_x;
392  vectors[new_p].flow_y += step_y;
393  vectors[new_p].error = error;
394 
395  // Check if we exceeded the threshold
396  if ((abs(step_x) + abs(step_y)) < step_threshold) {
397  break;
398  }
399  }
400 
401  // If we tracked the point we update the index and the count
402  if (tracked) {
403  new_p++;
404  (*points_cnt)++;
405  } else if (keep_bad_points) {
406  vectors[new_p].flow_x = 0;
407  vectors[new_p].flow_y = 0;
408  vectors[new_p].error = LARGE_FLOW_ERROR;
409  new_p++;
410  (*points_cnt)++;
411  }
412  }
413 
414  // Free the images
415  image_free(&window_I);
416  image_free(&window_J);
417  image_free(&window_DX);
418  image_free(&window_DY);
419  image_free(&window_diff);
420 
421  // Return the vectors
422  return vectors;
423 }
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size)
This outputs a subpixel window image in grayscale Currently only works with Grayscale images as input...
Definition: image.c:491
void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy)
Calculate the gradients using the following matrix: [0 -1 0; -1 0 1; 0 1 0].
Definition: image.c:549
void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size)
This function populates given array of image_t structs with wanted number of padded pyramids based on...
Definition: image.c:464
uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff)
Calculate the difference between two images and return the error This will only work with grayscale i...
Definition: image.c:605
void image_free(struct image_t *img)
Free the image.
Definition: image.c:75
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
Calculate the G vector of an image gradient This is used for optical flow calculation.
Definition: image.c:573
int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult)
Calculate the multiplication between two images and return the error This will only work with image g...
Definition: image.c:643
uint32_t x
The x coordinate of the point.
Definition: image.h:59
uint32_t y
The y coordinate of the point.
Definition: image.h:60
uint16_t h
Image height.
Definition: image.h:47
uint16_t w
Image width.
Definition: image.h:46
struct point_t pos
The original position the flow comes from in subpixels.
Definition: image.h:79
uint32_t error
The matching error in the tracking process in subpixels.
Definition: image.h:82
int32_t flow_y
The y direction flow in subpixels.
Definition: image.h:81
int32_t flow_x
The x direction flow in subpixels.
Definition: image.h:80
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:37
@ IMAGE_GRADIENT
An image gradient (int16 per pixel)
Definition: image.h:39
Definition: image.h:78
Definition: image.h:44
Definition: image.h:58
static float p[2][2]
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, uint8_t max_points, uint8_t pyramid_level, uint8_t keep_bad_points)
Definition: lucas_kanade.c:74
struct flow_t * opticFlowLK_flat(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, uint8_t keep_bad_points)
Compute the optical flow of several points using the Lucas-Kanade algorithm by Yves Bouguet The initi...
Definition: lucas_kanade.c:275
efficient fixed-point optical-flow calculation
#define LARGE_FLOW_ERROR
Definition: lucas_kanade.h:37
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5
uint8_t patch_size
Definition: textons.c:123
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103