Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Changes to the Lucas-Kanade optical flow

Browse Source 
- Corners are now detected in the previous frame(instead of the new one) and tracked to the new one

- A feature management method is available where corners are kept in memory for as long as they are tracked

- Corners can be weighted against the number of times they have been tracked

- Fast9 detection will be performed again after the number of tracked corners drops below a threshold is met and
  only in the regions of the image that have the less corners (optional)
This commit is contained in:
Manos Kyriakakis
2017-06-16 04:49:46 +02:00
parent b2e22be800
commit 3ddd5d737b
7 changed files with 166 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/cv_opticflow.xml
+6
View File
@@ -36,6 +36,7 @@
<define name="MEDIAN_FILTER" value="0" description="A median filter on the resulting velocities to be turned on or off (last 5 measurements)"/>
<define name="KALMAN_FILTER" value="1" description="A kalman filter on the resulting velocities to be turned on or off (fused with accelerometers)"/>
<define name="KALMAN_FILTER_PROCESS_NOISE" value="0.01" description="The expected variance of the error of the model's prediction in the kalman filter"/>
<define name="FEATURE_MANAGEMENT" value="1" description="Whether to keep already tracked corners in memory for the next frame or re-detect new ones every time"/>
<!-- Lucas Kanade optical flow calculation parameters -->
<define name="MAX_TRACK_CORNERS" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
@@ -47,6 +48,8 @@
<define name="FAST9_THRESHOLD" value="20" description="FAST9 default threshold"/>
<define name="FAST9_MIN_DISTANCE" value="10" description="The amount of pixels between corners that should be detected"/>
<define name="FAST9_PADDING" value="20" description="The outer border in which no corners will be searched"/>
<define name="FAST9_REGION_DETECT" value="1" description="Whether to detect fast9 corners in regions of interest or the whole image (only works with feature management)"/>
<define name="FAST9_NUM_REGIONS" value="9" description="The number of regions of interest to split the image into"/>
</section>
</doc>
@@ -64,6 +67,7 @@
<dl_setting var="opticflow.median_filter" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="median_filter" param="OPTICFLOW_MEDIAN_FILTER"/>
<dl_setting var="opticflow.kalman_filter" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="kalman_filter" param="OPTICFLOW_KALMAN_FILTER"/>
<dl_setting var="opticflow.kalman_filter_process_noise" module="computer_vision/opticflow_module" min="0.0001" step="0.0001" max="0.1" shortname="KF_process_noise" param="OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE"/>
<dl_setting var="opticflow.feature_management" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="feature_management" param="OPTICFLOW_FEATURE_MANAGEMENT"/>
<!-- Specifically for Lucas Kanade and FAST9 -->
<dl_setting var="opticflow.max_track_corners" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="max_trck_corners" param="OPTICFLOW_MAX_TRACK_CORNERS"/>
@@ -74,6 +78,8 @@
<dl_setting var="opticflow.fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD"/>
<dl_setting var="opticflow.fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE"/>
<dl_setting var="opticflow.fast9_padding" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="fast9_padding" param="OPTICFLOW_FAST9_PADDING"/>
<dl_setting var="opticflow.fast9_region_detect" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="fast9_region_detect" param="OPTICFLOW_FAST9_REGION_DETECT"/>
<dl_setting var="opticflow.fast9_num_regions" module="computer_vision/opticflow_module" min="1" step="1" max="25" shortname="fast9_num_regions" param="OPTICFLOW_FAST9_NUM_REGIONS"/>
<!-- Changes pyramid level of lucas kanade optical flow. -->
sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c
+21 -2
View File
@@ -52,7 +52,7 @@ static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel
void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint16_t x_padding, uint16_t y_padding, uint16_t *num_corners, uint16_t *ret_corners_length, struct point_t **ret_corners, uint16_t *roi)
{
uint16_t corner_cnt = 0;
uint16_t corner_cnt = *num_corners;
int pixel[16];
int16_t i;
uint16_t x, y, x_min, x_max, y_min, x_start, x_end, y_start, y_end;
@@ -63,6 +63,14 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin
pixel_size = 2;
}
// Padding less than min_dist could cause overflow on some comparisons below.
if (x_padding < min_dist) {
x_padding = min_dist;
}
if (y_padding < min_dist) {
y_padding = min_dist;
}
if (!roi) {
x_start = 3 + x_padding;
y_start = 3 + y_padding;
@@ -82,7 +90,9 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin
// Go trough all the pixels (minus the borders and inside the requested roi)
for (y = y_start; y < y_end; y++) {
if (min_dist > 0) { y_min = y - min_dist; }
if (min_dist > 0) {
y_min = y - min_dist;
}
for (x = x_start; x < x_end; x++) {
// First check if we aren't in range vertical (TODO: fix less intensive way)
@@ -103,6 +113,15 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin
if ((*ret_corners)[i].y < y_min) {
break;
}
/*
// If detecting with already existing corners gives too much overlap uncomment this comparison instead of the one above.
// But, it will make the detection more time consuming
// TODO: maybe sort the corners before calling...
if(ret_corners[i].y < y_min || ret_corners[i].y > y_max){
i--;
continue;
}
*/
if (x_min < (*ret_corners)[i].x && (*ret_corners)[i].x < x_max) {
need_skip = 1;
sw/airborne/modules/computer_vision/lib/vision/image.h
+5
View File
@@ -29,6 +29,7 @@
#include "std.h"
#include <sys/time.h>
#include <state.h>
/* The different type of images we currently support */
enum image_type {
@@ -44,6 +45,7 @@ struct image_t {
uint16_t w; ///< Image width
uint16_t h; ///< Image height
struct timeval ts; ///< The timestamp of creation
struct FloatEulers *eulerAngles; ///< Pointer to the Euler Angles
uint32_t pprz_ts; ///< The timestamp in us since system startup
uint8_t buf_idx; ///< Buffer index for V4L2 freeing
@@ -55,6 +57,9 @@ struct image_t {
struct point_t {
uint32_t x; ///< The x coordinate of the point
uint32_t y; ///< The y coordinate of the point
uint16_t count; ///< Number of times the point has been tracked successfully
uint16_t x_sub; ///< The x subpixel coordinate of the point
uint16_t y_sub; ///< The y subpixel coordinate of the point
};
/* Vector structure for point differences */
sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
+5 -4
View File
@@ -76,8 +76,7 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str
{
// if no pyramids, use the old code:
if(pyramid_level == 0)
{
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);
}
@@ -87,6 +86,7 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str
// 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;
uint8_t border_size = padded_patch_size / 2 + 2; // amount of padding added to images
@@ -251,7 +251,8 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str
* @return The vectors from the original *points in subpixels
*/
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) {
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
@@ -271,7 +272,7 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img
// determine patch sizes and initialize neighborhoods
uint16_t patch_size = 2 * half_window_size;
uint32_t error_threshold = (25 * 25) *(patch_size *patch_size);
uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
uint16_t padded_patch_size = patch_size + 2;
// Create the window images
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+125 -28
View File
@@ -118,7 +118,7 @@ PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS)
PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC)
#ifndef OPTICFLOW_PYRAMID_LEVEL
#define OPTICFLOW_PYRAMID_LEVEL 0
#define OPTICFLOW_PYRAMID_LEVEL 2
#endif
PRINT_CONFIG_VAR(OPTICFLOW_PYRAMID_LEVEL)
@@ -185,6 +185,21 @@ PRINT_CONFIG_VAR(OPTICFLOW_KALMAN_FILTER)
#endif
PRINT_CONFIG_VAR(OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE)
#ifndef OPTICFLOW_FEATURE_MANAGEMENT
#define OPTICFLOW_FEATURE_MANAGEMENT 1
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FEATURE_MANAGEMENT)
#ifndef OPTICFLOW_FAST9_REGION_DETECT
#define OPTICFLOW_FAST9_REGION_DETECT 1
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_REGION_DETECT)
#ifndef OPTICFLOW_FAST9_NUM_REGIONS
#define OPTICFLOW_FAST9_NUM_REGIONS 9
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_NUM_REGIONS)
//Include median filter
#include "filters/median_filter.h"
struct MedianFilterInt vel_x_filt, vel_y_filt;
@@ -193,6 +208,7 @@ struct MedianFilterInt vel_x_filt, vel_y_filt;
/* Functions only used here */
static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime);
static int cmp_flow(const void *a, const void *b);
static int cmp_array(const void *a, const void *b);
@@ -219,6 +235,9 @@ void opticflow_calc_init(struct opticflow_t *opticflow)
opticflow->median_filter = OPTICFLOW_MEDIAN_FILTER;
opticflow->kalman_filter = OPTICFLOW_KALMAN_FILTER;
opticflow->kalman_filter_process_noise = OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE;
opticflow->feature_management = OPTICFLOW_FEATURE_MANAGEMENT;
opticflow->fast9_region_detect = OPTICFLOW_FAST9_REGION_DETECT;
opticflow->fast9_num_regions = OPTICFLOW_FAST9_NUM_REGIONS;
opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
@@ -238,17 +257,17 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
struct opticflow_result_t *result)
{
if (opticflow->just_switched_method) {
// Create the image buffers
image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE);
image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE);
// Create the image buffers
image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE);
image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE);
// Set the previous values
opticflow->got_first_img = false;
FLOAT_RATES_ZERO(opticflow->prev_rates);
// Set the previous values
opticflow->got_first_img = false;
FLOAT_RATES_ZERO(opticflow->prev_rates);
// Init median filters with zeros
init_median_filter(&vel_x_filt);
init_median_filter(&vel_y_filt);
// Init median filters with zeros
init_median_filter(&vel_x_filt);
init_median_filter(&vel_y_filt);
}
// variables for size_divergence:
@@ -276,24 +295,74 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
// Corner detection
// *************************************************************************************
// FAST corner detection
// TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance
// to 0 (see defines), however a more permanent solution should be considered
// last parameter (for ROI detection) set to NULL because feature management is not implemented yet.
fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
&opticflow->fast9_ret_corners,
NULL);
// if feature_management is selected and tracked corners drop below a threshold, redetect
if ((opticflow->feature_management) && (result->corner_cnt < opticflow->max_track_corners / 2)) {
// no need for "per region" re-detection when there are no previous corners
if ((!opticflow->fast9_region_detect) || (result->corner_cnt == 0)) {
fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
&opticflow->fast9_ret_corners,
NULL);
} else {
// allocating memory and initializing the 2d array that holds the number of corners per region and its index (for the sorting)
uint16_t **region_count = malloc(opticflow->fast9_num_regions * sizeof(uint16_t *));
for (uint16_t i = 0; i < opticflow->fast9_num_regions ; i++) {
region_count[i] = malloc(sizeof(uint16_t) * 2);
region_count[i][0] = 0;
region_count[i][1] = i;
}
for (uint16_t i = 0; i < result->corner_cnt; i++) {
region_count[(opticflow->fast9_ret_corners[i].x / (img->w / (uint8_t)sqrt(opticflow->fast9_num_regions))
+ opticflow->fast9_ret_corners[i].y / (img->h / (uint8_t)sqrt(opticflow->fast9_num_regions)) * (uint8_t)sqrt(opticflow->fast9_num_regions))][0]++;
}
// Adaptive threshold
if (opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40
&& opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2
opticflow->fast9_threshold--;
} else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) {
opticflow->fast9_threshold++;
//sorting region_count array according to first column (number of corners).
qsort(region_count, opticflow->fast9_num_regions, sizeof(region_count[0]), cmp_array);
// Detecting corners from the region with the less to the one with the most, until a desired total is reached.
for (uint16_t i = 0; i < opticflow->fast9_num_regions && result->corner_cnt < 2 * opticflow->max_track_corners ; i++) {
// Find the boundaries of the region of interest
uint16_t *roi = malloc(4 * sizeof(uint16_t));
roi[0] = (region_count[i][1] % (uint8_t)sqrt(opticflow->fast9_num_regions)) * (img->w / (uint8_t)sqrt(opticflow->fast9_num_regions));
roi[1] = (region_count[i][1] / (uint8_t)sqrt(opticflow->fast9_num_regions)) * (img->h / (uint8_t)sqrt(opticflow->fast9_num_regions));
roi[2] = roi[0] + (img->w / (uint8_t)sqrt(opticflow->fast9_num_regions));
roi[3] = roi[1] + (img->h / (uint8_t)sqrt(opticflow->fast9_num_regions));
fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
&opticflow->fast9_ret_corners,
roi);
free(roi);
}
for (uint16_t i = 0; i < opticflow->fast9_num_regions; i++) {
free(region_count[i]);
}
free(region_count);
}
} else if (!opticflow->feature_management) {
// needs to be set to 0 because result is now static
result->corner_cnt = 0;
// FAST corner detection
// TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance
// to 0 (see defines), however a more permanent solution should be considered
fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
&opticflow->fast9_ret_corners,
NULL);
// Adaptive threshold
if (opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40
&& opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2
opticflow->fast9_threshold--;
} else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) {
opticflow->fast9_threshold++;
}
}
}
@@ -303,6 +372,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
// Check if we found some corners to track
if (result->corner_cnt < 1) {
// Clear the result otherwise the previous values will be returned for this frame too
memset(result, 0, sizeof(struct opticflow_result_t));
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
return;
}
@@ -319,7 +390,6 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
opticflow->threshold_vec, opticflow->max_track_corners, opticflow->pyramid_level);
#if OPTICFLOW_SHOW_FLOW
printf("show: n tracked = %d\n", result->tracked_cnt);
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
#endif
@@ -423,6 +493,18 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
if (opticflow->feature_management) {
result->corner_cnt = result->tracked_cnt;
//get the new positions of the corners and the "residual" subpixel positions
for (uint16_t i = 0; i < result->tracked_cnt; i++) {
opticflow->fast9_ret_corners[i].x = (uint32_t)((vectors[i].pos.x + (float)vectors[i].flow_x) / opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].y = (uint32_t)((vectors[i].pos.y + (float)vectors[i].flow_y) / opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].x_sub = (uint16_t)((vectors[i].pos.x + vectors[i].flow_x) % opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].y_sub = (uint16_t)((vectors[i].pos.y + vectors[i].flow_y) % opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].count = vectors[i].pos.count;
}
}
free(vectors);
image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
}
@@ -614,6 +696,8 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
if (switch_counter != opticflow->method) {
opticflow->just_switched_method = true;
switch_counter = opticflow->method;
// Clear the static result
memset(result, 0, sizeof(struct opticflow_result_t));
} else {
opticflow->just_switched_method = false;
}
@@ -762,4 +846,17 @@ static int cmp_flow(const void *a, const void *b)
b_p->flow_y);
}
/**
* Compare the rows of an integer (uint16_t) 2D array based on the first column.
* Used for sorting.
* @param[in] *a The first row (should be *uint16_t)
* @param[in] *b The second flow vector (should be *uint16_t)
* @return Negative if a[0] < b[0],0 if a[0] == b[0] and positive if a[0] > b[0]
*/
static int cmp_array(const void *a, const void *b)
{
const uint16_t *pa = *(const uint16_t **)a;
const uint16_t *pb = *(const uint16_t **)b;
return pa[0] - pb[0];
}
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+3
View File
@@ -70,6 +70,9 @@ struct opticflow_t {
uint16_t fast9_rsize; ///< Amount of corners allocated
struct point_t *fast9_ret_corners; ///< Corners
bool feature_management; ///< Decides whether to keep track corners in memory for the next frame instead of re-detecting every time
bool fast9_region_detect; ///< Decides whether to detect fast9 corners in specific regions of interest or the whole image (only for feature management)
uint8_t fast9_num_regions; ///< The number of regions of interest the image is split into
};
sw/airborne/modules/computer_vision/opticflow_module.c
+1 -1
View File
@@ -183,7 +183,7 @@ struct image_t *opticflow_module_calc(struct image_t *img)
temp_state.rates = pose.rates;
// Do the optical flow calculation
struct opticflow_result_t temp_result = {}; // new initialization
static struct opticflow_result_t temp_result = {}; // static so that the number of corners is kept between frames
opticflow_calc_frame(&opticflow, &temp_state, img, &temp_result);
// Copy the result if finished