Changes to the Lucas-Kanade optical flow

- 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)

conf/modules/cv_opticflow.xml

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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
+    // Create the image buffers
-	image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE);
+    image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE);
-	image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE);
+    image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE);
 
-	// Set the previous values
+    // Set the previous values
-	opticflow->got_first_img = false;
+    opticflow->got_first_img = false;
-	FLOAT_RATES_ZERO(opticflow->prev_rates);
+    FLOAT_RATES_ZERO(opticflow->prev_rates);
 
-	// Init median filters with zeros
+    // Init median filters with zeros
-	init_median_filter(&vel_x_filt);
+    init_median_filter(&vel_x_filt);
-	init_median_filter(&vel_y_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
+  // if feature_management is selected and tracked corners drop below a threshold, redetect
-  // TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance
+  if ((opticflow->feature_management) && (result->corner_cnt < opticflow->max_track_corners / 2)) {
-  // to 0 (see defines), however a more permanent solution should be considered
+    // no need for "per region" re-detection when there are no previous corners
-  // last parameter (for ROI detection) set to NULL because feature management is not implemented yet.
+    if ((!opticflow->fast9_region_detect) || (result->corner_cnt == 0)) {
-  fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
+      fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance,
-               opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
+                   opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
-               &opticflow->fast9_rsize,
+                   &opticflow->fast9_rsize,
-               &opticflow->fast9_ret_corners,
+                   &opticflow->fast9_ret_corners,
-			   NULL);
+                   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
+      //sorting region_count array according to first column (number of corners).
-  if (opticflow->fast9_adaptive) {
+      qsort(region_count, opticflow->fast9_num_regions, sizeof(region_count[0]), cmp_array);
-    // Decrease and increase the threshold based on previous values
+
-    if (result->corner_cnt < 40
+      // Detecting corners from the region with the less to the one with the most, until a desired total is reached.
-        && opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2
+      for (uint16_t i = 0; i < opticflow->fast9_num_regions && result->corner_cnt < 2 * opticflow->max_track_corners ; i++) {
-      opticflow->fast9_threshold--;
+
-    } else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) {
+        // Find the boundaries of the region of interest
-      opticflow->fast9_threshold++;
+        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

@@ -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

@@ -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