[opticflow_module] Tested on both bebop and ARdrone 2.0's cameras and some minor fixes

conf/airframes/TUDELFT/tudelft_KM_conf.xml

@@ -7,7 +7,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml modules/opticflow_hover.xml"
+   settings_modules="modules/gps_ubx_ucenter.xml modules/video_thread.xml modules/cv_opticflow.xml"
    gui_color="red"
   />
   <aircraft

conf/airframes/TUDELFT/tudelft_ardrone2_opticflow.xml

@@ -28,8 +28,17 @@
     <module name="gps" type="ubx_ucenter"/>
     <module name="send_imu_mag_current"/>
     <module name="logger_file"/>
-    <module name="cv_opticflow"/>
-    <module name="opticflow_hover"/>
+    <load name="video_thread.xml">
+	<define name="VIDEO_THREAD_FPS" value="30"/>
+	<define name="VIDEO_THREAD_CAMERA" value="bottom_camera"/>
+	<define name="VIDEO_THREAD_SHOT_PATH" value="/data/ftp/internal_000/images"/>
+    </load>
+    <load name="cv_opticflow.xml">
+	<define name = "OPTICFLOW_DEBUG" value="1"/> 
+	<define name = "OPTICFLOW_SHOW_CORNERS" value="1"/> 
+	<define name = "OPTICFLOW_SHOW_FLOW" value="1"/> 
+    </load>
+    <!--module name="opticflow_hover"/-->
   </modules>
 
   <commands>

conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml

@@ -40,14 +40,10 @@
 	<define name="VIDEO_THREAD_SHOT_PATH" value="/data/ftp/internal_000/images"/>
     </load>
     <load name="cv_opticflow.xml">
-	<define name="OPTICFLOW_METHOD" value="1"/>
-	<define name="OPTICFLOW_SHOW_FLOW" value="1"/>
 	<define name="MAX_HORIZON" value="2"/>
+        <define name = "CAMERA_ROTATED_180" value="1"/> 
     </load>
-    <!--load name="video_rtp_stream.xml">
-	<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
-	<define name="VIEWVIDEO_QUALITY_FACTOR" value="70"/>
-    </load-->
+    <load name="sonar_bebop.xml"/> <!-- Needed for opticflow velocity measurement-->
   </modules>
 
   <commands>

sw/airborne/modules/computer_vision/lib/vision/edge_flow.h

@@ -42,7 +42,7 @@
 
 
 #ifndef MAX_HORIZON
-#define MAX_HORIZON 10
+#define MAX_HORIZON 2
 #else
 #if MAX_HORIZON < 2
 #define MAX_HORIZON 2

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

@@ -120,7 +120,7 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE)
 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
 
 #ifndef OPTICFLOW_FAST9_MIN_DISTANCE
-#define OPTICFLOW_FAST9_MIN_DISTANCE 10
+#define OPTICFLOW_FAST9_MIN_DISTANCE 0
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE)
 
@@ -138,6 +138,10 @@ PRINT_CONFIG_VAR(OPTICFLOW_METHOD)
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION)
 
+#ifndef CAMERA_ROTATED_180
+#define CAMERA_ROTATED 0
+#endif
+PRINT_CONFIG_VAR(CAMERA_ROTATED_180)
 
 /* Functions only used here */
 static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime);
@@ -187,7 +191,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
 void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                              struct opticflow_result_t *result)
 {
-  if (opticflow->just_switched_method == 1) {
+  if (opticflow->just_switched_method) {
+    printf("OPTICALFLOW: switched to fast9 Lukas Kanade\n");
     opticflow_calc_init(opticflow, img->w, img->h);
   }
 
@@ -214,7 +219,10 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
   // Corner detection
   // *************************************************************************************
 
-  // FAST corner detection (TODO: non fixed threshold)
+  // FAST corner detection
+  // TODO: non fixed threshold
+  // 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
   struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
                                          0, 0, &result->corner_cnt);
 
@@ -335,10 +343,6 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
   //  result->vel_x =  - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
   //  result->vel_y =  result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
 
-  // Rotate velocities from camera frame coordinates to body coordinates.
-  // IMPORTANT for control! This the case on the ARDrone and bebop, but on other systems this might be different!
-  result->vel_body_x = vel_y;
-  result->vel_body_y = - vel_x;
 
   // Determine quality of noise measurement for state filter
   //TODO Experiment with multiple noise measurement models
@@ -380,6 +384,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
   // If the methods just switched to this one, reintialize the
   // array of edge_hist structure.
   if (opticflow->just_switched_method == 1) {
+    printf("OPTICALFLOW: switched to EdgeFlow\n");
     int i;
     for (i = 0; i < MAX_HORIZON; i++) {
       edge_hist[i].x = malloc(sizeof(int32_t) * img->w);
@@ -500,14 +505,6 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
   result->vel_x = vel_x;
   result->vel_y = vel_y;
 
-  /* Rotate velocities from camera frame coordinates to body coordinates.
-  * IMPORTANT This frame to body orientation should be the case for the Parrot
-  * ARdrone and Bebop, however this can be different for other quadcopters
-  * ALWAYS double check!
-  */
-  result->vel_body_x = - vel_y;
-  result->vel_body_y = vel_x;
-
 #if OPTICFLOW_SHOW_FLOW
   draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x);
 #endif
@@ -526,14 +523,18 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
 void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                           struct opticflow_result_t *result)
 {
-  static int8_t switch_counter = OPTICFLOW_METHOD;
-  if (switch_counter != opticflow->method || opticflow->got_first_img) {
-    opticflow->just_switched_method = 1;
+
+  // A switch counter that checks in the loop if the current method is similar,
+  // to the previous (for reinitializing structs)
+  static int8_t switch_counter = -1;
+  if (switch_counter != opticflow->method) {
+    opticflow->just_switched_method = true;
     switch_counter = opticflow->method;
   } else {
-    opticflow->just_switched_method = 0;
+    opticflow->just_switched_method = false;
   }
 
+  // Switch between methods (0 = fast9/lukas-kanade, 1 = EdgeFlow)
   if (opticflow->method == 0) {
     calc_fast9_lukas_kanade(opticflow, state, img, result);
   } else {
@@ -542,10 +543,6 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
     } else {}
   }
 
-  if (opticflow->got_first_img == 1) {
-    opticflow->got_first_img = 0;
-  }
-
 }
 
 /**

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -95,9 +95,10 @@ void opticflow_module_init(void)
 
   // Initialize the opticflow calculation
   opticflow_got_result = false;
-  opticflow.got_first_img = 1;
   cv_add(opticflow_module_calc);
 
+
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
 #endif
@@ -122,7 +123,7 @@ void opticflow_module_run(void)
                            opticflow_result.flow_x,
                            opticflow_result.flow_y,
                            opticflow_result.flow_der_x,
-                           opticflow_result.flow_der_x,
+                           opticflow_result.flow_der_y,
                            quality,
                            opticflow_result.div_size,
                            opticflow_state.agl);
@@ -142,7 +143,9 @@ void opticflow_module_run(void)
 /**
  * The main optical flow calculation thread
  * This thread passes the images trough the optical flow
- * calculator based on Lucas Kanade
+ * calculator
+ * @param[in] *img The image_t structure of the captured image
+ * @return *img The processed image structure
  */
 struct image_t *opticflow_module_calc(struct image_t *img)
 {
@@ -154,9 +157,21 @@ struct image_t *opticflow_module_calc(struct image_t *img)
   opticflow_calc_frame(&opticflow, &opticflow_state, img, &opticflow_result);
 
   // Copy the result if finished
-// memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
   opticflow_got_result = true;
 
+  /* Rotate velocities from camera frame coordinates to body coordinates for control
+  * IMPORTANT!!! This frame to body orientation should be the case for the Parrot
+  * ARdrone and Bebop, however this can be different for other quadcopters
+  * ALWAYS double check!
+  */
+#if CAMERA_ROTATED_180 == 0 //Case for ARDrone 2.0
+  result->vel_body_x = result->vel_y;
+  result->vel_body_y = - result ->vel_x;
+#else   // Case for Bebop 2
+  result->vel_body_x = - result->vel_y;
+  result->vel_body_y = result ->vel_x;
+#endif
+
   return img;
 }
 
@@ -170,7 +185,5 @@ static void opticflow_agl_cb(uint8_t sender_id __attribute__((unused)), float di
   // Update the distance if we got a valid measurement
   if (distance > 0) {
     opticflow_state.agl = distance;
-
-
   }
 }