[modules] added settings to switch between edgeflow and LK, and some extra variables

conf/modules/cv_opticflow.xml

@@ -24,10 +24,14 @@
       <define name="FX" value="343.1211" description="Field in the x direction of the camera (Defaults are from an ARDrone 2)"/>
       <define name="FY" value="348.5053" description="Field in the y direction of the camera (Defaults are from an ARDrone 2)"/>
 
+      <!-- General optical flow calculation parameters -->
+      <define name="METHOD" value="0" description="Method used to calculate optical flow"/>
+      <define name="WINDOW_SIZE" value="10" description="Window size used for block matching (pixels)"/>
+      <define name="SEARCH_DISTANCE" value="10" description="Maximum search distance for blockmatching (pixels)"/>
+      <define name="SUBPIXEL_FACTOR" value="100" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
+
       <!-- 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"/>
-      <define name="WINDOW_SIZE" value="10" description="Window size used in Lucas Kanade algorithm"/>
-      <define name="SUBPIXEL_FACTOR" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
       <define name="MAX_ITERATIONS" value="10" description="Maximum number of iterations the Lucas Kanade algorithm should take"/>
       <define name="THRESHOLD_VEC" value="2" description="TThreshold in subpixels when the iterations of Lucas Kanade should stop"/>
 
@@ -35,23 +39,30 @@
       <define name="FAST9_ADAPTIVE" value="TRUE" description="Whether we should use and adapative FAST9 crner detection threshold"/>
       <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"/>
+
+
     </section>
   </doc>
 
   <settings>
 	<dl_settings NAME="Vision stabilization">
 
-      <!-- Optical flow calculations parameters for Lucas Kanade and FAST9 -->
+      	<!-- Optical flow calculations parameters -->
       <dl_settings name="vision_calc">
-        <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"/>
+      <dl_setting var="opticflow.method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="method" values="LK_Fast9|EdgeFlow" param="METHOD"/>
-        <dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
+        <dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="20" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
+        <dl_setting var="opticflow.search_distance" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="search_distance" param="SEARCH_DISTANCE"/>
         <dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
+
+	<!-- 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"/>
         <dl_setting var="opticflow.max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS"/>
         <dl_setting var="opticflow.threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC"/>
 
         <dl_setting var="opticflow.fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="TRUE|FALSE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE"/>
         <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_settings>
     </dl_settings>
   </settings>

sw/airborne/modules/computer_vision/opticflow/edge_flow.c

@@ -21,7 +21,7 @@ static uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finisht
 static uint32_t getMinimum(uint32_t *a, uint32_t n);
 void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
               uint16_t RES);
-
+uint32_t getAmountPeaks(int32_t *edgehist, uint32_t median, int32_t size);
 /**
  * Run the optical flow with EDGEFLOW on a new image frame
  * @param[in] *opticflow The opticalflow structure that keeps track of previous images
@@ -29,6 +29,8 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_
  * @param[in] *img The image frame to calculate the optical flow from
  * @param[out] *result The optical flow result
  */
+
+
 void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                          struct opticflow_result_t *result)
 {
@@ -40,8 +42,24 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
 
   // Define Normal variables
   struct edgeflow_displacement_t displacement;
-  uint8_t disp_range = DISP_RANGE_MAX;
+  uint16_t disp_range;
-  uint16_t RES = 100;
+  if (opticflow->search_distance < DISP_RANGE_MAX) {
+    disp_range = opticflow->search_distance;
+  } else {
+    disp_range = DISP_RANGE_MAX;
+  }
+
+  uint16_t window_size;
+
+  if (opticflow->window_size < MAX_WINDOW_SIZE) {
+    window_size = opticflow->window_size;
+  } else {
+    window_size = MAX_WINDOW_SIZE;
+  }
+
+  uint16_t RES = opticflow->subpixel_factor;
+
+  //......................Calculating EdgeFlow..................... //
 
   // Calculate current frame's edge histogram
   int32_t *edge_hist_x = edge_hist[current_frame_nr].x;
@@ -105,45 +123,71 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
   // Estimate pixel wise displacement of the edge histograms for x and y direction
   calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
                               displacement.x, img->w,
-                              opticflow->window_size, disp_range,  der_shift_x);
+                              window_size, disp_range,  der_shift_x);
   calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
                               displacement.y, img->h,
-                              opticflow->window_size, disp_range, der_shift_y);
+                              window_size, disp_range, der_shift_y);
 
   // Fit a line on the pixel displacement to estimate
   // the global pixel flow and divergence (RES is resolution)
   line_fit(displacement.x, &edgeflow.div_x,
            &edgeflow.flow_x, img->w,
-           opticflow->window_size + disp_range, RES);
+           window_size + disp_range, RES);
   line_fit(displacement.y, &edgeflow.div_y,
            &edgeflow.flow_y, img->h,
-           opticflow->window_size + disp_range, RES);
+           window_size + disp_range, RES);
 
-  // Save Resulting flow in results
+  /* Save Resulting flow in results
-  result->flow_x = (int16_t)edgeflow.flow_x / (previous_frame_offset[0] * RES);
+   * Warning: The flow detected here is different in sign
-  result->flow_y = (int16_t)edgeflow.flow_y / (previous_frame_offset[1] * RES);
+   * and size, therefore this will be multiplied with
+   * the same subpixel factor and -1 to make it on par with
+   * the LK algorithm of t opticalflow_calculator.c
+   * */
+  edgeflow.flow_x = -1 * edgeflow.flow_x;
+  edgeflow.flow_y = -1 * edgeflow.flow_y;
 
-  // VELOCITY //
+  result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
+  result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
 
-  //Estimate fps per direction
+  //Fill up the results optic flow to be on par with LK_fast9
+  result->flow_der_x =  result->flow_x;
+  result->flow_der_y =  result->flow_y;
+  result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
+  result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
+  result->divergence = (float)edgeflow.flow_x / RES;
+  result->div_size = 0.0f;
+  result->noise_measurement = 0.0f;
+  result->surface_roughness = 0.0f;
+
+  //......................Calculating VELOCITY ..................... //
+
+  /*Estimate fps per direction
+   * This is the fps with adaptive horizon for subpixel flow, which is not similar
+   * to the loop speed of the algorithm. The faster the quadcopter flies
+   * the higher it becomes
+  */
   float fps_x = 0;
   float fps_y = 0;
   float time_diff_x = (float)(timeval_diff2(&edge_hist[previous_frame_x].frame_time, &img->ts)) / 1000.;
   float time_diff_y = (float)(timeval_diff2(&edge_hist[previous_frame_y].frame_time, &img->ts)) / 1000.;
   fps_x = 1 / (time_diff_x);
   fps_y = 1 / (time_diff_y);
+
   result->fps = fps_x;
 
-  //Calculate velocity
+  // Calculate velocity
   float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
   float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
   result->vel_x = vel_x;
   result->vel_y = vel_y;
 
-  // Rotate velocities from camera frame coordinates to body coordinates.
+  /* Rotate velocities from camera frame coordinates to body coordinates.
-  // IMPORTANT since these values are used for control! This the case on the ARDrone and bebop, but on other systems this might be different!
+  * IMPORTANT This frame to body orientation should bethe case for the parrot
-  result->vel_body_x = vel_y;
+  * ARdrone and Bebop, however this can be different for other quadcopters
-  result->vel_body_y = - vel_x;
+  * ALWAYS double check!
+  */
+  result->vel_body_x = - vel_y;
+  result->vel_body_y = vel_x;
 
 #if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
   draw_edgeflow_img(img, edgeflow, displacement, *edge_hist_x)
@@ -270,8 +314,6 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
   if (border[0] >= border[1] || abs(der_shift) >= 10) {
     SHIFT_TOO_FAR = 1;
   }
-
-
   {
     // TODO: replace with arm offset subtract
     for (x = border[0]; x < border[1]; x++) {
@@ -420,5 +462,22 @@ void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct
   image_draw_line(img, &point1_extra, &point2_extra);
 }
 
+/**
+ * getAmountPeaks, calculates the amount of peaks in a edge histogram
+ * @param[in] *edgehist Horizontal edge_histogram
+ * @param[in] thres The threshold from which a peak is considered significant peak or not
+ * @param[in] size  Size of the array
+ * @param[return] amount of peaks
+ */
+uint32_t getAmountPeaks(int32_t *edgehist, uint32_t thres, int32_t size)
+{
+  uint32_t  amountPeaks = 0;
+  uint32_t i = 0;
 
-
+  for (i = 1; i < size + 1;  i ++) {
+    if (edgehist[i - 1] < edgehist[i] && edgehist[i] > edgehist[i + 1] && edgehist[i] > thres) {
+      amountPeaks ++;
+    }
+  }
+  return amountPeaks;
+}

sw/airborne/modules/computer_vision/opticflow/edge_flow.h

@@ -23,7 +23,10 @@
 #define MAX_HORIZON 10
 #endif
 #ifndef DISP_RANGE_MAX
-#define DISP_RANGE_MAX 20
+#define DISP_RANGE_MAX 50
+#endif
+#ifndef MAX_WINDOW_SIZE
+#define MAX_WINDOW_SIZE 20
 #endif
 #ifndef IMAGE_HEIGHT
 #define IMAGE_HEIGHT 240

sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c

@@ -82,8 +82,13 @@ PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS)
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE)
 
+#ifndef OPTICFLOW_MAX_SEARCH_DISTANCE
+#define OPTICFLOW_MAX_SEARCH_DISTANCE 20
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_MAX_SEARCH_DISTANCE)
+
 #ifndef OPTICFLOW_SUBPIXEL_FACTOR
-#define OPTICFLOW_SUBPIXEL_FACTOR 10
+#define OPTICFLOW_SUBPIXEL_FACTOR 100
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR)
 
@@ -112,6 +117,13 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE)
 
+#ifndef OPTICFLOW_METHOD
+#define OPTICFLOW_METHOD 0
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_METHOD)
+
+
+
 /* 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);
@@ -134,8 +146,11 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
   opticflow->prev_theta = 0.0;
 
   /* Set the default values */
-  opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
+  opticflow->method = 0; //0 = LK_fast9, 1 = Edgeflow
   opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
+  opticflow->search_distance = OPTICFLOW_MAX_SEARCH_DISTANCE;
+
+  opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
   opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
   opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
   opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
@@ -143,6 +158,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
   opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
   opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
   opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
+
 }
 
 /**

sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h

@@ -42,20 +42,25 @@ struct opticflow_t {
   struct image_t prev_img_gray;     ///< Previous gray image frame
   struct timeval prev_timestamp;    ///< Timestamp of the previous frame, used for FPS calculation
 
-  uint8_t max_track_corners;        ///< Maximum amount of corners Lucas Kanade should track
+  uint8_t method;           ///< Method to use to calculate the optical flow
-  uint16_t window_size;             ///< Window size of the Lucas Kanade calculation (needs to be even)
+  uint16_t window_size;             ///< Window size for the blockmatching algorithm (general value for all methods)
+  uint16_t search_distance;         ///< Search distance for blockmatchin alg. (
+
   uint8_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
 
+  uint8_t max_track_corners;        ///< Maximum amount of corners Lucas Kanade should track
   bool_t fast9_adaptive;            ///< Whether the FAST9 threshold should be adaptive
   uint8_t fast9_threshold;          ///< FAST9 corner detection threshold
   uint16_t fast9_min_distance;      ///< Minimum distance in pixels between corners
+
 };
 
 
 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);
 
 #endif /* OPTICFLOW_CALCULATOR_H */
 

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -266,16 +266,13 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
     // Do the optical flow calculation
     struct opticflow_result_t temp_result;
 
-#if USE_LK
+    if (opticflow.method == 0) {
-    opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
+      opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
-#else
+    } else {
-#if USE_EDGEFLOW
+      if (opticflow.method == 1) {
-    edgeflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
+        edgeflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
-#else
+      } else { PRINT_CONFIG_MSG("Both edgeflow and Lukas kanade is not turned on. Define either USE_LK or use_EDGEFLOW on TRUE!"); }
-    PRINT_CONFIG_MSG("Both edgeflow and Lukas kanade is not turned on. Define either USE_LK or use_EDGEFLOW on TRUE!");
+    }
-#endif
-#endif
-
     // Copy the result if finished
     pthread_mutex_lock(&opticflow_mutex);
     memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));