added a proper resolution parameter in EF rather than using the subpi…

…xel factor from LK
andizza · Mar 30, 2017 · ab8cf6f · ab8cf6f
1 parent d3d805d
commit ab8cf6f
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 2 deletions.
diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
@@ -31,6 +31,7 @@
       <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="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
+      <define name="RESOLUTION_FACTOR" value="1000" description="The resolution factor needed to calculate the divergence without floats"/>
       <define name="DEROTATION" value="1" description="Derotation either turned on or off (depended on gyroscope measurements)"/>
       <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)"/>
@@ -58,6 +59,7 @@
         <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="10" max="1000" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
+        <dl_setting var="opticflow.resolution_factor" module="computer_vision/opticflow_module" min="10" step="10" max="10000" shortname="resolution_factor" param="OPTICFLOW_RESOLUTION_FACTOR"/>
         <dl_setting var="opticflow.derotation" min="0" step="1" max="1" module="computer_vision/opticflow_module" values="OFF|ON" shortname="derotation" param="OPTICFLOW_DEROTATION"/>
         <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"/>

diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -102,6 +102,11 @@ PRINT_CONFIG_VAR(OPTICFLOW_SEARCH_DISTANCE)
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR)
 
+#ifndef OPTICFLOW_RESOLUTION_FACTOR
+#define OPTICFLOW_RESOLUTION_FACTOR 100
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_RESOLUTION_FACTOR)
+
 #ifndef OPTICFLOW_MAX_ITERATIONS
 #define OPTICFLOW_MAX_ITERATIONS 10
 #endif
@@ -207,6 +212,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow)
 
   opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
   opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
+  opticflow->resolution_factor = OPTICFLOW_RESOLUTION_FACTOR;
   opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
   opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
   opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL;
@@ -466,7 +472,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
     window_size = MAX_WINDOW_SIZE;
   }
 
-  uint16_t RES = opticflow->subpixel_factor;
+  uint16_t RES = opticflow->resolution_factor;
 
   //......................Calculating EdgeFlow..................... //
 
@@ -521,7 +527,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
 
   /* Save Resulting flow in results
    * Warning: The flow detected here is different in sign
-   * and size, therefore this will be multiplied with
+   * and size, therefore this will be divided with
    * the same subpixel factor and -1 to make it on par with
    * the LK algorithm of t opticalflow_calculator.c
    * */
@@ -531,6 +537,9 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
   result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
   result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
 
+  result->flow_x = (int16_t)edgeflow.flow_x / RES;
+  result->flow_y = (int16_t)edgeflow.flow_y / RES;
+
   //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;

diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -57,6 +57,7 @@ struct opticflow_t {
   float derotation_correction_factor_y;     ///< Correction factor for derotation in Y axis, determined from a fit from the gyros and flow rotation. (wrong FOV, camera not in center)
 
   uint16_t subpixel_factor;                 ///< The amount of subpixels per pixel
+  uint16_t resolution_factor;                 ///< The resolution in EdgeFlow to determine the Divergence
   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 pyramid_level;              ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used)