Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merged with master

Browse Source
This commit is contained in:
hrvoje
2016-03-29 21:07:18 +02:00
parent 0b13661a13
commit 3aaefbc4de
6 changed files with 296 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/computer_vision/lib/vision/image.c
+119 -13
View File
@@ -235,6 +235,108 @@ void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint
}
}
/**
* This function adds padding to input image by mirroring the edge image elements.
* @param[in] *input - input image (grayscale only)
* @param[out] *output - the output image
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
* Example: f e d c b a | a b c d e f | f e d c b a
*/
void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
{
image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
uint8_t *input_buf = (uint8_t *)input->buf;
uint8_t *output_buf = (uint8_t *)output->buf;
// Skip first `border_size` rows, iterate through next input->h rows
for (uint16_t i = border_size; i != (output->h - border_size); i++){
// Mirror first `border_size` columns
for (uint8_t j = 0; j != border_size; j++)
output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
// Copy corresponding row values from input image
memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
// Mirror last `border_size` columns
for (uint8_t j = 0; j != border_size; j++)
output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size -1 - j];
}
// Mirror first `border_size` and last `border_size` rows
for (uint8_t i = 0; i != border_size; i++){
memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w], sizeof(uint8_t) * output->w);
memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w], &output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
}
}
/**
* This function takes previous padded pyramid level and outputs next level of pyramid without padding.
* For calculating new pixel value 5x5 filter matrix suggested by Bouguet is used in decimal number form:
* [1/16 1/8 3/4 1/8 1/16]' x [1/16 1/8 3/4 1/8 1/16]
*
* @param[in] *input - input image (grayscale only)
* @param[out] *output - the output image
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
* Example: f e d c b a | a b c d e f | f e d c b a
*/
void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
{
// Create output image, new image size is half the size of input image without padding (border)
image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size ) / 2, input->type);
uint8_t *input_buf = (uint8_t *)input->buf;
uint8_t *output_buf = (uint8_t *)output->buf;
uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
uint16_t w = input->w;
int32_t sum = 0;
for (uint16_t i = 0; i != output->h; i++){
for (uint16_t j = 0; j != output->w; j++){
row = border_size + 2 * i; // First skip border, then every second pixel
col = border_size + 2 * j;
sum = 39 * ( input_buf[(row -2)*w + (col -2)] + input_buf[(row -2)*w + (col +2)] + input_buf[(row +2)*w + (col -2)] + input_buf[(row +2)*w + (col +2)]);
sum += 156 * ( input_buf[(row -2)*w + (col -1)] + input_buf[(row -2)*w + (col +1)] + input_buf[(row -1)*w + (col +2)] + input_buf[(row +1)*w + (col -2)]
+ input_buf[(row +1)*w + (col +2)] + input_buf[(row +2)*w + (col -1)] + input_buf[(row +2)*w + (col +1)] + input_buf[(row -1)*w + (col -2)]);
sum += 234 * ( input_buf[(row -2)*w + (col)] + input_buf[(row)*w + (col -2)] + input_buf[(row)*w + (col +2)] + input_buf[(row +2)*w + (col)]);
sum += 625 * ( input_buf[(row -1)*w + (col -1)] + input_buf[(row -1)*w + (col +1)] + input_buf[(row +1)*w + (col -1)] + input_buf[(row +1)*w + (col +1)]);
sum += 938 * ( input_buf[(row -1)*w + (col)] + input_buf[(row)*w + (col -1)] + input_buf[(row)*w + (col +1)] + input_buf[(row +1)*w + (col)]);
sum += 1406 * input_buf[(row)*w + (col)];
output_buf[i*output->w + j] = sum / 10000;
}
}
}
/**
* This function populates given array of image_t structs with wanted number of padded pyramids based on given input.
* @param[in] *input - input image (grayscale only)
* @param[out] *output - array of image_t structs containing image pyiramid levels. Level zero contains original image,
* followed by `pyr_level` of pyramid.
* @param[in] pyr_level - number of pyramids to be built. If 0, original image is padded and outputed.
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
* Example: f e d c b a | a b c d e f | f e d c b a
*/
void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint8_t border_size)
{
// Pad input image and save it as '0' pyramid level
image_add_border(input, &output_array[0], border_size);
// Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
struct image_t temp;
for (uint8_t i = 1; i != pyr_level + 1; i++){
pyramid_next_level(&output_array[i-1], &temp, border_size);
image_add_border(&temp, &output_array[i], border_size);
image_free(&temp);
}
}
/**
* This outputs a subpixel window image in grayscale
* Currently only works with Grayscale images as input but could be upgraded to
@@ -243,41 +345,45 @@ void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint
* @param[out] *output Window output (width and height is used to calculate the window size)
* @param[in] *center Center point in subpixel coordinates
* @param[in] subpixel_factor The subpixel factor per pixel
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
* Example: f e d c b a | a b c d e f | f e d c b a
*/
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint16_t subpixel_factor)
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size)
{
uint8_t *input_buf = (uint8_t *)input->buf;
uint8_t *output_buf = (uint8_t *)output->buf;
// Calculate the window size
uint16_t half_window = output->w / 2;
uint16_t subpixel_w = input->w * subpixel_factor;
uint16_t subpixel_h = input->h * subpixel_factor;
uint32_t subpixel_w = input->w * subpixel_factor;
uint32_t subpixel_h = input->h * subpixel_factor;
// Go through the whole window size in normal coordinates
for (uint16_t i = 0; i < output->w; i++) {
for (uint16_t j = 0; j < output->h; j++) {
// Calculate the subpixel coordinate
uint16_t x = center->x + (i - half_window) * subpixel_factor;
uint16_t y = center->y + (j - half_window) * subpixel_factor;
BoundUpper(x, subpixel_w);
BoundUpper(y, subpixel_h);
uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor ;
uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor ;
BoundUpper(x, subpixel_w - 1);
BoundUpper(y, subpixel_h - 1);
// Calculate the original pixel coordinate
uint16_t orig_x = x / subpixel_factor;
uint16_t orig_y = y / subpixel_factor;
// Calculate top left (in subpixel coordinates)
uint16_t tl_x = orig_x * subpixel_factor;
uint16_t tl_y = orig_y * subpixel_factor;
uint32_t tl_x = orig_x * subpixel_factor;
uint32_t tl_y = orig_y * subpixel_factor;
// Check if it is the top left pixel
if (tl_x == x && tl_y == y) {
output_buf[output->w * j + i] = input_buf[input->w * orig_y + orig_x];
} else {
// Calculate the difference from the top left
uint16_t alpha_x = (x - tl_x);
uint16_t alpha_y = (y - tl_y);
uint32_t alpha_x = (x - tl_x);
uint32_t alpha_y = (y - tl_y);
// Blend from the 4 surrounding pixels
uint32_t blend = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + orig_x];
@@ -339,7 +445,7 @@ void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
}
}
// ouput the G vector
// output the G vector
g[0] = sum_dxx / 255;
g[1] = sum_dxy / 255;
g[2] = g[1];
@@ -407,7 +513,7 @@ int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct imag
// Calculate the multiplication
for (uint16_t x = 0; x < img_a->w; x++) {
for (uint16_t y = 0; y < img_a->h; y++) {
int16_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
int32_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
sum += mult_c;
// Set the difference image
sw/airborne/modules/computer_vision/lib/vision/image.h
+6 -3
View File
@@ -52,8 +52,8 @@ struct image_t {
/* Image point structure */
struct point_t {
uint16_t x; ///< The x coordinate of the point
uint16_t y; ///< The y coordinate of the point
uint32_t x; ///< The x coordinate of the point // CHANGED 16 -> 32
uint32_t y; ///< The y coordinate of the point // CHANGED 16 -> 32
};
/* Vector structure for point differences */
@@ -64,6 +64,7 @@ struct flow_t {
};
/* Usefull image functions */
void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size);
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type);
void image_free(struct image_t *img);
void image_copy(struct image_t *input, struct image_t *output);
@@ -71,7 +72,7 @@ void image_switch(struct image_t *a, struct image_t *b);
void image_to_grayscale(struct image_t *input, struct image_t *output);
uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint16_t downsample);
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint16_t subpixel_factor);
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size);
void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy);
void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g);
uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff);
@@ -79,5 +80,7 @@ int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct imag
void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt);
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor);
void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to);
void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size);
void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint8_t border_size);
#endif
sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
+158 -107
View File
@@ -1,6 +1,14 @@
/*
* lucas_kanade.c
*
* Created on: Jan 11, 2016
* Author: hrvoje
*/
/*
* Copyright (C) 2014 G. de Croon
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
* 2016 Hrvoje Brezak <hrvoje.brezak@gmail.com>
*
* This file is part of Paparazzi.
*
@@ -37,7 +45,7 @@
/**
* Compute the optical flow of several points using the Lucas-Kanade algorithm by Yves Bouguet
* The initial fixed-point implementation is doen by G. de Croon and is adapted by
* The initial fixed-point implementation is done by G. de Croon and is adapted by
* Freek van Tienen for the implementation in Paparazzi.
* @param[in] *new_img The newest grayscale image (TODO: fix YUV422 support)
* @param[in] *old_img The old grayscale image (TODO: fix YUV422 support)
@@ -48,134 +56,177 @@
* @param[in] max_iterations Maximum amount of iterations to find the new point
* @param[in] step_threshold The threshold at which the iterations should stop
* @param[in] max_points The maximum amount of points to track, we skip x points and then take a point.
* @param[in] pyramid_level Level of pyramid used in computation (0 == no pyramids used)
* @return The vectors from the original *points in subpixels
*/
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) {
// A straightforward one-level implementation of Lucas-Kanade.
// For all points:
// (1) determine the subpixel neighborhood in the old image
// (2) get the x- and y- gradients
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
// (4) iterate over taking steps in the image to minimize the error:
// [a] get the subpixel neighborhood in the new image
// [b] determine the image difference between the two neighborhoods
// [c] calculate the 'b'-vector
// [d] calculate the additional flow step and possibly terminate the iteration
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) {
// Allocate some memory for returning the vectors
struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
uint16_t new_p = 0;
uint16_t points_orig = *points_cnt;
*points_cnt = 0;
// determine patch sizes and initialize neighborhoods
uint16_t patch_size = 2 * half_window_size;
uint32_t error_threshold = (25 * 25) *(patch_size *patch_size);
uint16_t padded_patch_size = patch_size + 2;
// Pyramidal implementation of Lucas-Kanade feature tracker.
// For all points:
// (1) determine the subpixel neighborhood in the old image
// (2) get the x- and y- gradients
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
// (4) iterate over taking steps in the image to minimize the error:
// [a] get the subpixel neighborhood in the new image
// [b] determine the image difference between the two neighborhoods
// [c] calculate the 'b'-vector
// [d] calculate the additional flow step and possibly terminate the iteration
// Create the window images
struct image_t window_I, window_J, window_DX, window_DY, window_diff;
image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
// Allocate some memory for returning the vectors
struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
// Calculate the amount of points to skip
float skip_points = (points_orig > max_points) ? points_orig / max_points : 1;
// Determine patch sizes and initialize neighborhoods
uint16_t patch_size = 2 * half_window_size + 1;
uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
uint16_t padded_patch_size = patch_size + 2;
uint8_t border_size = padded_patch_size / 2 + 2;
step_threshold = step_threshold*(subpixel_factor/100);
// Go trough all points
for (uint16_t i = 0; i < max_points && i < points_orig; i++) {
uint16_t p = i * skip_points;
// Allocate memory for image pyramids
struct image_t *pyramid_old = malloc(sizeof(struct image_t) * (pyramid_level+1));
struct image_t *pyramid_new = malloc(sizeof(struct image_t) * (pyramid_level+1));
// If the pixel is outside ROI, do not track it
if (points[p].x < half_window_size || (old_img->w - points[p].x) < half_window_size
|| points[p].y < half_window_size || (old_img->h - points[p].y) < half_window_size) {
continue;
}
// Build pyramid levels
pyramid_build(old_img, pyramid_old, pyramid_level, border_size);
pyramid_build(new_img, pyramid_new, pyramid_level, border_size);
// Convert the point to a subpixel coordinate
vectors[new_p].pos.x = points[p].x * subpixel_factor;
vectors[new_p].pos.y = points[p].y * subpixel_factor;
vectors[new_p].flow_x = 0;
vectors[new_p].flow_y = 0;
// Create the window images
struct image_t window_I, window_J, window_DX, window_DY, window_diff;
image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
// (1) determine the subpixel neighborhood in the old image
image_subpixel_window(old_img, &window_I, &vectors[new_p].pos, subpixel_factor);
// Iterate through pyramid levels
for (int8_t LVL = pyramid_level; LVL != -1; LVL--) {
uint16_t points_orig = *points_cnt;
*points_cnt = 0;
uint16_t new_p = 0;
// (2) get the x- and y- gradients
image_gradients(&window_I, &window_DX, &window_DY);
// Calculate the amount of points to skip - disabled until needed
float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1;
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
int32_t G[4];
image_calculate_g(&window_DX, &window_DY, G);
// Go through all points
for (uint16_t i = 0; i < max_points && i < points_orig; i++)
{
uint16_t p = i * skip_points;
// calculate G's determinant in subpixel units:
int32_t Det = (G[0] * G[3] - G[1] * G[2]) / subpixel_factor;
if (LVL == pyramid_level)
{
// Convert the point to a subpixel coordinate on the top pyramid level
vectors[new_p].pos.x = (points[p].x * subpixel_factor) >> pyramid_level;
vectors[new_p].pos.y = (points[p].y * subpixel_factor) >> pyramid_level;
vectors[new_p].flow_x = 0;
vectors[new_p].flow_y = 0;
// Check if the determinant is bigger than 1
if (Det < 1) {
continue;
}
} else {
// Convert last pyramid level flow into this pyramid level flow guess
vectors[new_p].pos.x = vectors[p].pos.x << 1;
vectors[new_p].pos.y = vectors[p].pos.y << 1;
vectors[new_p].flow_x = vectors[p].flow_x << 1;
vectors[new_p].flow_y = vectors[p].flow_y << 1;
}
// a * (Ax - Bx) + (1-a) * (Ax+1 - Bx+1)
// a * Ax - a * Bx + (1-a) * Ax+1 - (1-a) * Bx+1
// (a * Ax + (1-a) * Ax+1) - (a * Bx + (1-a) * Bx+1)
// If the pixel is outside ROI, do not track it
if ((((int32_t) vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
|| ((vectors[new_p].pos.x + vectors[new_p].flow_x) > ((pyramid_new[LVL].w - 1 - 2 * border_size)* subpixel_factor))
|| (((int32_t) vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
|| ((vectors[new_p].pos.y + vectors[new_p].flow_y) > ((pyramid_new[LVL].h - 1 - 2 * border_size)* subpixel_factor)))
{
continue;
}
// (4) iterate over taking steps in the image to minimize the error:
bool_t tracked = TRUE;
for (uint8_t it = 0; it < max_iterations; it++) {
struct point_t new_point = {
vectors[new_p].pos.x + vectors[new_p].flow_x,
vectors[new_p].pos.y + vectors[new_p].flow_y
};
// If the pixel is outside ROI, do not track it
if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) < half_window_size
|| new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) < half_window_size) {
tracked = FALSE;
break;
}
// [a] get the subpixel neighborhood in the new image
image_subpixel_window(new_img, &window_J, &new_point, subpixel_factor);
// (1) determine the subpixel neighborhood in the old image
image_subpixel_window(&pyramid_old[LVL], &window_I, &vectors[new_p].pos, subpixel_factor, border_size);
// [b] determine the image difference between the two neighborhoods
uint32_t error = image_difference(&window_I, &window_J, &window_diff);
if (error > error_threshold && it > max_iterations / 2) {
tracked = FALSE;
break;
}
// (2) get the x- and y- gradients
image_gradients(&window_I, &window_DX, &window_DY);
int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
int32_t G[4];
image_calculate_g(&window_DX, &window_DY, G);
// [d] calculate the additional flow step and possibly terminate the iteration
int16_t step_x = (G[3] * b_x - G[1] * b_y) / Det;
int16_t step_y = (G[0] * b_y - G[2] * b_x) / Det;
vectors[new_p].flow_x += step_x;
vectors[new_p].flow_y += step_y;
// calculate G's determinant in subpixel units:
int32_t Det = ( G[0] * G[3] - G[1] * G[2]);
// Check if we exceeded the treshold
if ((abs(step_x) + abs(step_y)) < step_threshold) {
break;
}
}
// Check if the determinant is bigger than 1
if (Det < 1) {
continue;
}
// If we tracked the point we update the index and the count
if (tracked) {
new_p++;
(*points_cnt)++;
}
}
// (4) iterate over taking steps in the image to minimize the error:
bool_t tracked = TRUE;
// Free the images
image_free(&window_I);
image_free(&window_J);
image_free(&window_DX);
image_free(&window_DY);
image_free(&window_diff);
for (uint8_t it = max_iterations; it--; ) {
struct point_t new_point = { vectors[new_p].pos.x + vectors[new_p].flow_x,
vectors[new_p].pos.y + vectors[new_p].flow_y };
// Return the vectors
return vectors;
// If the pixel is outside ROI, do not track it
if ( (( (int32_t)vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
|| ( new_point.x > ((pyramid_new[LVL].w - 1 - 2*border_size)*subpixel_factor))
|| (((int32_t)vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
|| ( new_point.y > ((pyramid_new[LVL].h - 1 - 2*border_size)*subpixel_factor)) )
{
tracked = FALSE;
break;
}
// [a] get the subpixel neighborhood in the new image
image_subpixel_window(&pyramid_new[LVL], &window_J, &new_point, subpixel_factor, border_size);
// [b] determine the image difference between the two neighborhoods
uint32_t error = image_difference(&window_I, &window_J, &window_diff);
if (error > error_threshold && it < max_iterations / 2) {
tracked = FALSE;
break;
}
int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
// [d] calculate the additional flow step and possibly terminate the iteration
int16_t step_x = (( (int64_t) G[3] * b_x - G[1] * b_y) * subpixel_factor) / Det;
int16_t step_y = (( (int64_t) G[0] * b_y - G[2] * b_x) * subpixel_factor) / Det;
vectors[new_p].flow_x = vectors[new_p].flow_x + step_x;
vectors[new_p].flow_y = vectors[new_p].flow_y + step_y;
// Check if we exceeded the treshold CHANGED made this better for 0.03
if ((abs(step_x) + abs(step_y)) < step_threshold) {
break;
}
} // lucas kanade step iteration
// If we tracked the point we update the index and the count
if (tracked) {
new_p++;
(*points_cnt)++;
}
} // go through all points
} // LVL of pyramid
// Free the images
image_free(&window_I);
image_free(&window_J);
image_free(&window_DX);
image_free(&window_DY);
image_free(&window_diff);
for (int8_t i = pyramid_level; i!= -1; i--){
image_free(&pyramid_old[i]);
image_free(&pyramid_new[i]);
}
pyramid_old = NULL;
pyramid_new = NULL;
// Return the vectors
return vectors;
}
sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h
+2 -2
View File
@@ -34,7 +34,7 @@
#include "std.h"
#include "image.h"
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);
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);
#endif /* OPTIC_FLOW_INT_H */
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+8 -3
View File
@@ -110,7 +110,7 @@ PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC)
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE)
#ifndef OPTICFLOW_FAST9_THRESHOLD
#define OPTICFLOW_FAST9_THRESHOLD 20
#define OPTICFLOW_FAST9_THRESHOLD 10
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
@@ -206,7 +206,7 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
// FAST corner detection (TODO: non fixed threshold)
struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
20, 20, &result->corner_cnt);
0, 0, &result->corner_cnt);
// Adaptive threshold
if (opticflow->fast9_adaptive) {
@@ -238,7 +238,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
result->tracked_cnt = result->corner_cnt;
struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
opticflow->threshold_vec, opticflow->max_track_corners);
opticflow->threshold_vec, opticflow->max_track_corners, 3);
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
@@ -299,6 +300,10 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
float diff_flow_x = 0;
float diff_flow_y = 0;
// Flow Derotation TODO:
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
if (opticflow->derotation) {
diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+3 -3
View File
@@ -48,7 +48,7 @@ struct opticflow_t {
uint16_t search_distance; ///< Search distance for blockmatching alg.
uint8_t derotation; ///< Derotation switched on or off (depended on the quality of the gyroscope measurement)
uint8_t subpixel_factor; ///< The amount of subpixels per pixel
uint16_t subpixel_factor; ///< The amount of subpixels per pixel
uint8_t max_iterations; ///< The maximum amount of iterations the Lucas Kanade algorithm should do
uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop
@@ -61,8 +61,8 @@ struct opticflow_t {
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h);
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result);
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result);
void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result);
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,