81 if (pyramid_level == 0) {
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);
88 struct flow_t *vectors = calloc(max_points,
sizeof(
struct flow_t));
95 uint16_t border_size = padded_patch_size / 2 + 2;
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));
102 pyramid_build(old_img, pyramid_old, pyramid_level, border_size);
103 pyramid_build(new_img, pyramid_new, pyramid_level, border_size);
106 struct image_t window_I, window_J, window_DX, window_DY, window_diff;
114 for (
int8_t LVL = pyramid_level; LVL != -1; LVL--) {
120 float skip_points = (points_orig > max_points) ? (
float)points_orig / max_points : 1;
123 for (
uint16_t i = 0; i < max_points && i < points_orig; i++) {
126 if (LVL == 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;
135 vectors[new_p].
pos.
x = vectors[
p].
pos.
x << 1;
136 vectors[new_p].
pos.
y = vectors[
p].
pos.
y << 1;
143 || ((vectors[new_p].
pos.
x + vectors[new_p].
flow_x) > (
uint32_t)((pyramid_new[LVL].
w - 1 - 2 * border_size)*
146 || ((vectors[new_p].
pos.
y + vectors[new_p].
flow_y) > (
uint32_t)((pyramid_new[LVL].
h - 1 - 2 * border_size)*
148 if (keep_bad_points) {
158 image_subpixel_window(&pyramid_old[LVL], &window_I, &vectors[new_p].pos, subpixel_factor, border_size);
168 int32_t Det = (G[0] * G[3] - G[1] * G[2]);
172 if (keep_bad_points) {
183 for (
uint8_t it = max_iterations; 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))) {
204 if (error > error_threshold && it < max_iterations / 2) {
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;
219 vectors[new_p].
error = error;
222 if ((abs(step_x) + abs(step_y)) < step_threshold) {
231 }
else if (keep_bad_points) {
232 vectors[new_p].
flow_x = 0;
233 vectors[new_p].
flow_y = 0;
249 for (
int8_t i = pyramid_level; i != -1; i--) {
291 struct flow_t *vectors = calloc(max_points,
sizeof(
struct flow_t));
302 struct image_t window_I, window_J, window_DX, window_DY, window_diff;
310 float skip_points = (points_orig > max_points) ? (
float)points_orig / max_points : 1;
313 for (
uint16_t i = 0; i < max_points && i < points_orig; i++) {
317 vectors[new_p].
pos.
x = points[
p].
x * subpixel_factor;
318 vectors[new_p].
pos.
y = points[
p].
y * subpixel_factor;
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) {
342 int32_t Det = (G[0] * G[3] - G[1] * G[2]) / subpixel_factor;
346 if (keep_bad_points) {
360 for (
uint8_t it = 0; it < max_iterations; 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) {
380 if (error > error_threshold && it > max_iterations / 2) {
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;
396 if ((abs(step_x) + abs(step_y)) < step_threshold) {
405 }
else if (keep_bad_points) {
406 vectors[new_p].
flow_x = 0;
407 vectors[new_p].
flow_y = 0;
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...
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].
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...
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...
void image_free(struct image_t *img)
Free the image.
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
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.
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...
uint32_t x
The x coordinate of the point.
uint32_t y
The y coordinate of the point.
struct point_t pos
The original position the flow comes from in subpixels.
uint32_t error
The matching error in the tracking process in subpixels.
int32_t flow_y
The y direction flow in subpixels.
int32_t flow_x
The x direction flow in subpixels.
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
@ IMAGE_GRADIENT
An image gradient (int16 per pixel)
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)
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...
efficient fixed-point optical-flow calculation
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.