/** * @file lucas_kanade.c * * Compute the optical flow of several points using the pyramidal Lucas-Kanade algorithm by Yves Bouguet * The initial fixed-point implementation is done by G. de Croon and is adapted by * Freek van Tienen for the implementation in Paparazzi. * Pyramids implementation and related development done by Hrvoje Brezak. * @param[in] *new_img The newest grayscale image (TODO: fix YUV422 support) * @param[in] *old_img The old grayscale image (TODO: fix YUV422 support) * @param[in] *points Points to start tracking from * @param[in,out] points_cnt The amount of points and it returns the amount of points tracked * @param[in] half_window_size Half the window size (in both x and y direction) to search inside * @param[in] subpixel_factor The subpixel factor which calculations should be based on * @param[in] max_iterations Maximum amount of iterations to find the new point * @param[in] step_threshold The threshold of additional subpixel flow 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 * * Pyramidal implementation of Lucas-Kanade feature tracker. * * Uses input images to build pyramid of padded images. * <p>For every pyramid level:</p> * <p> For all points:</p> * - (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 * - (5) use calculated flow as initial flow estimation for next level of pyramid */ 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) { // if no pyramids, use the old code: if (pyramid_level == 0) { // use the old code in this case: return opticFlowLK_flat(new_img, old_img, points, points_cnt, half_window_size, subpixel_factor, max_iterations, step_threshold, max_points, keep_bad_points); } // Allocate some memory for returning the vectors struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points); // Determine patch sizes and initialize neighborhoods uint16_t patch_size = 2 * half_window_size + 1; // TODO: Feature management shows that this threshold rejects corners maybe too often, maybe another formula could be chosen uint32_t error_threshold = (25 * 25) * (patch_size * patch_size); uint16_t padded_patch_size = patch_size + 2; uint16_t border_size = padded_patch_size / 2 + 2; // amount of padding added to images // 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)); // Build pyramid levels pyramid_build(old_img, pyramid_old, pyramid_level, border_size); pyramid_build(new_img, pyramid_new, pyramid_level, border_size); // 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); // 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; // Calculate the amount of points to skip float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1; // Go through all points for (uint16_t i = 0; i < max_points && i < points_orig; i++) { uint16_t p = i * skip_points; if (LVL == pyramid_level) { // Convert point position on original image 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; } else { // (5) use calculated flow as initial flow estimation for next level of pyramid 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; } // If the pixel is outside original image, 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) > (uint32_t)((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) > (uint32_t)((pyramid_new[LVL].h - 1 - 2 * border_size)* subpixel_factor))) { continue; } // (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); // (2) get the x- and y- gradients image_gradients(&window_I, &window_DX, &window_DY); // (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); // calculate G's determinant in subpixel units: int32_t Det = (G[0] * G[3] - G[1] * G[2]); // Check if the determinant is bigger than 1 if (Det < 1) { continue; } // (4) iterate over taking steps in the image to minimize the error: bool tracked = true; 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, 0, 0, 0 }; // If the pixel is outside original image, do not track it if ((((int32_t)vectors[new_p].pos.x + vectors[new_p].flow_x) < 0) || (new_point.x > (uint32_t)((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 > (uint32_t)((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; vectors[new_p].error = error; // 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)++; } else if (keep_bad_points) { vectors[new_p].pos.x = 0; vectors[new_p].pos.y = 0; vectors[new_p].flow_x = 0; vectors[new_p].flow_y = 0; vectors[new_p].error = LARGE_FLOW_ERROR; new_p++; (*points_cnt)++; } } // go through all points
/** * 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 * 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) * @param[in] *points Points to start tracking from * @param[in,out] points_cnt The amount of points and it returns the amount of points tracked * @param[in] half_window_size Half the window size (in both x and y direction) to search inside * @param[in] subpixel_factor The subpixel factor which calculations should be based on * @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. * @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 // 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; // 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); // Calculate the amount of points to skip float skip_points = (points_orig > max_points) ? points_orig / max_points : 1; // Go trough all points for (uint16_t i = 0; i < max_points && i < points_orig; i++) { uint16_t p = i * skip_points; // 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; } // 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; // (1) determine the subpixel neighborhood in the old image image_subpixel_window(old_img, &window_I, &vectors[new_p].pos, subpixel_factor); // (2) get the x- and y- gradients image_gradients(&window_I, &window_DX, &window_DY); // (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); // calculate G's determinant in subpixel units: int32_t Det = (G[0] * G[3] - G[1] * G[2]) / subpixel_factor; // Check if the determinant is bigger than 1 if (Det < 1) { continue; } // 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) // (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); // [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 = (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; // Check if we exceeded the treshold if ((abs(step_x) + abs(step_y)) < step_threshold) { break; } } // If we tracked the point we update the index and the count if (tracked) { new_p++; (*points_cnt)++; } } // Free the images image_free(&window_I); image_free(&window_J); image_free(&window_DX); image_free(&window_DY); image_free(&window_diff); // Return the vectors return vectors; }