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
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  *
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 
31 #include <stdlib.h>
32 #include <stdio.h>
33 #include <math.h>
34 #include <string.h>
35 #include "lucas_kanade.h"
36 
37 
53 struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt,
54  uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points) {
55  // A straightforward one-level implementation of Lucas-Kanade.
56  // For all points:
57  // (1) determine the subpixel neighborhood in the old image
58  // (2) get the x- and y- gradients
59  // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
60  // (4) iterate over taking steps in the image to minimize the error:
61  // [a] get the subpixel neighborhood in the new image
62  // [b] determine the image difference between the two neighborhoods
63  // [c] calculate the 'b'-vector
64  // [d] calculate the additional flow step and possibly terminate the iteration
65 
66  // Allocate some memory for returning the vectors
67  struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
68  uint16_t new_p = 0;
69  uint16_t points_orig = *points_cnt;
70  *points_cnt = 0;
71 
72  // determine patch sizes and initialize neighborhoods
73  uint16_t patch_size = 2 * half_window_size;
74  uint32_t error_threshold = (25 * 25) *(patch_size *patch_size);
75  uint16_t padded_patch_size = patch_size + 2;
76 
77  // Create the window images
78  struct image_t window_I, window_J, window_DX, window_DY, window_diff;
79  image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
80  image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
81  image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
82  image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
83  image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
84 
85  // Calculate the amount of points to skip
86  float skip_points = (points_orig > max_points) ? points_orig / max_points : 1;
87 
88  // Go trough all points
89  for (uint16_t i = 0; i < max_points && i < points_orig; i++) {
90  uint16_t p = i * skip_points;
91 
92  // If the pixel is outside ROI, do not track it
93  if (points[p].x < half_window_size || (old_img->w - points[p].x) < half_window_size
94  || points[p].y < half_window_size || (old_img->h - points[p].y) < half_window_size) {
95  continue;
96  }
97 
98  // Convert the point to a subpixel coordinate
99  vectors[new_p].pos.x = points[p].x * subpixel_factor;
100  vectors[new_p].pos.y = points[p].y * subpixel_factor;
101  vectors[new_p].flow_x = 0;
102  vectors[new_p].flow_y = 0;
103 
104  // (1) determine the subpixel neighborhood in the old image
105  image_subpixel_window(old_img, &window_I, &vectors[new_p].pos, subpixel_factor);
106 
107  // (2) get the x- and y- gradients
108  image_gradients(&window_I, &window_DX, &window_DY);
109 
110  // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
111  int32_t G[4];
112  image_calculate_g(&window_DX, &window_DY, G);
113 
114  // calculate G's determinant in subpixel units:
115  int32_t Det = (G[0] * G[3] - G[1] * G[2]) / subpixel_factor;
116 
117  // Check if the determinant is bigger than 1
118  if (Det < 1) {
119  continue;
120  }
121 
122  // a * (Ax - Bx) + (1-a) * (Ax+1 - Bx+1)
123  // a * Ax - a * Bx + (1-a) * Ax+1 - (1-a) * Bx+1
124  // (a * Ax + (1-a) * Ax+1) - (a * Bx + (1-a) * Bx+1)
125 
126  // (4) iterate over taking steps in the image to minimize the error:
127  bool_t tracked = TRUE;
128  for (uint8_t it = 0; it < max_iterations; it++) {
129  struct point_t new_point = {
130  vectors[new_p].pos.x + vectors[new_p].flow_x,
131  vectors[new_p].pos.y + vectors[new_p].flow_y
132  };
133  // If the pixel is outside ROI, do not track it
134  if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) < half_window_size
135  || new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) < half_window_size) {
136  tracked = FALSE;
137  break;
138  }
139 
140  // [a] get the subpixel neighborhood in the new image
141  image_subpixel_window(new_img, &window_J, &new_point, subpixel_factor);
142 
143  // [b] determine the image difference between the two neighborhoods
144  uint32_t error = image_difference(&window_I, &window_J, &window_diff);
145  if (error > error_threshold && it > max_iterations / 2) {
146  tracked = FALSE;
147  break;
148  }
149 
150  int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
151  int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
152 
153  // [d] calculate the additional flow step and possibly terminate the iteration
154  int16_t step_x = (G[3] * b_x - G[1] * b_y) / Det;
155  int16_t step_y = (G[0] * b_y - G[2] * b_x) / Det;
156  vectors[new_p].flow_x += step_x;
157  vectors[new_p].flow_y += step_y;
158 
159  // Check if we exceeded the treshold
160  if ((abs(step_x) + abs(step_y)) < step_threshold) {
161  break;
162  }
163  }
164 
165  // If we tracked the point we update the index and the count
166  if (tracked) {
167  new_p++;
168  (*points_cnt)++;
169  }
170  }
171 
172  // Free the images
173  image_free(&window_I);
174  image_free(&window_J);
175  image_free(&window_DX);
176  image_free(&window_DY);
177  image_free(&window_diff);
178 
179  // Return the vectors
180  return vectors;
181 }
unsigned short uint16_t
Definition: types.h:16
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:302
uint16_t x
The x coordinate of the point.
Definition: image.h:55
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:357
void image_free(struct image_t *img)
Free the image.
Definition: image.c:63
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
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
int16_t flow_x
The x direction flow in subpixels.
Definition: image.h:62
uint16_t w
Image width.
Definition: image.h:44
unsigned long uint32_t
Definition: types.h:18
uint16_t h
Image height.
Definition: image.h:45
signed short int16_t
Definition: types.h:17
struct point_t pos
The original position the flow comes from.
Definition: image.h:61
Definition: image.h:54
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
efficient fixed-point optical-flow calculation
static float p[2][2]
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:36
uint16_t y
The y coordinate of the point.
Definition: image.h:56
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
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:325
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint16_t subpixel_factor)
This outputs a subpixel window image in grayscale Currently only works with Grayscale images as input...
Definition: image.c:247
An image gradient (int16 per pixel)
Definition: image.h:38
int16_t flow_y
The y direction flow in subpixels.
Definition: image.h:63
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:395