diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
index b3b7e07c22..8b61de897d 100644
--- a/conf/modules/cv_opticflow.xml
+++ b/conf/modules/cv_opticflow.xml
@@ -36,6 +36,7 @@
+
@@ -47,6 +48,8 @@
+
+
@@ -64,6 +67,7 @@
+
@@ -74,6 +78,8 @@
+
+
diff --git a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c
index e37ca94e34..6eb4065d6b 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c
+++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c
@@ -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;
diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h
index 52a95bf0cd..1a3e830caa 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/image.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/image.h
@@ -29,6 +29,7 @@
#include "std.h"
#include
+#include
/* 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 */
diff --git a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
index 25659cb0a1..02d5b5b77f 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
+++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
@@ -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
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index aafb78baf3..9cc5c75ed1 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -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];
+}
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
index 1fa444c642..c9b353ad57 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -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
};
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 4bb6b6c90b..90a4e46c72 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -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