[computer vision] Opticflow multiple cameras (#2618)

* cv.c changes to support multiple cameras, compiles but untested
* opticflow module runs on both cameras
* modified airframe to showcase module running with two cameras
* updated OPTICFLOW ABI message to send camera id info
* now possible to subscribe to the ABI message from a specific camera
* Added camera_id field to conform with new telemetry message
* Updated pprzlink

sw/airborne/modules/computer_vision/bebop_ae_awb.c

@@ -146,7 +146,7 @@ PRINT_CONFIG_VAR(BEBOP_AWB_MIN_GREY_PIXELS)
 struct ae_setting_t ae_set;
 struct awb_setting_t awb_set;
 
-static struct image_t *update_ae_awb(struct image_t *img)
+static struct image_t *update_ae_awb(struct image_t *img, uint8_t camera_id)
 {
   static struct isp_yuv_stats_t yuv_stats;
 
@@ -352,5 +352,5 @@ void bebop_ae_awb_init(void)
   awb_set.gain_scheduling_tolerance = BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE;
   awb_set.gain_scheduling_step      = BEBOP_AWB_GAIN_SCHEDULING_STEP;
 
-  cv_add_to_device_async(&front_camera, update_ae_awb, BEBOP_AE_AWB_NICE, 0);
+  cv_add_to_device_async(&front_camera, update_ae_awb, BEBOP_AE_AWB_NICE, 0, 0);
 }

sw/airborne/modules/computer_vision/colorfilter.c

@@ -56,7 +56,7 @@ volatile int color_count = 0;
 #include "subsystems/abi.h"
 
 // Function
-static struct image_t *colorfilter_func(struct image_t *img)
+static struct image_t *colorfilter_func(struct image_t *img, uint8_t camera_id)
 {
   // Filter
   color_count = image_yuv422_colorfilt(img, img,
@@ -81,5 +81,5 @@ static struct image_t *colorfilter_func(struct image_t *img)
 
 void colorfilter_init(void)
 {
-  cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS);
+  cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS, 0);
 }

sw/airborne/modules/computer_vision/cv.c

@@ -43,7 +43,7 @@ static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B)
 }
 
 
-struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps)
+struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
 {
   // Create a new video listener
   struct video_listener *new_listener = malloc(sizeof(struct video_listener));
@@ -54,6 +54,7 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi
   new_listener->next = NULL;
   new_listener->async = NULL;
   new_listener->maximum_fps = fps;
+  new_listener->id = id;
 
   // Initialise the device that we want our function to use
   add_video_device(device);
@@ -80,10 +81,10 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi
 
 
 struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level,
-    uint16_t fps)
+    uint16_t fps, uint8_t id)
 {
   // Create a normal listener
-  struct video_listener *listener = cv_add_to_device(device, func, fps);
+  struct video_listener *listener = cv_add_to_device(device, func, fps, id);
 
   // Add asynchronous structure to override default synchronous behavior
   listener->async = malloc(sizeof(struct cv_async));
@@ -163,7 +164,7 @@ void *cv_async_thread(void *args)
     }
 
     // Execute vision function from this thread
-    listener->func(&async->img_copy);
+    listener->func(&async->img_copy, listener->id);
 
     // Mark image as processed
     async->img_processed = true;
@@ -198,7 +199,7 @@ void cv_run_device(struct video_config_t *device, struct image_t *img)
       }
     } else {
       // Execute the cvFunction and catch result
-      result = listener->func(img);
+      result = listener->func(img, listener->id);
 
       // If result gives an image pointer, use it in the next stage
       if (result != NULL) {

sw/airborne/modules/computer_vision/cv.h

@@ -36,7 +36,7 @@
 
 #include BOARD_CONFIG
 
-typedef struct image_t *(*cv_function)(struct image_t *img);
+typedef struct image_t *(*cv_function)(struct image_t *img, uint8_t camera_id);
 
 struct cv_async {
   pthread_t thread_id;
@@ -53,6 +53,7 @@ struct video_listener {
   struct cv_async *async;
   struct timeval ts;
   cv_function func;
+  uint8_t id;
 
   // Can be set by user
   uint16_t maximum_fps;
@@ -61,9 +62,9 @@ struct video_listener {
 
 extern bool add_video_device(struct video_config_t *device);
 
-extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps);
+extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id);
 extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level,
-    uint16_t fps);
+    uint16_t fps, uint8_t id);
 
 extern void cv_run_device(struct video_config_t *device, struct image_t *img);
 

sw/airborne/modules/computer_vision/cv_blob_locator.c

@@ -58,8 +58,8 @@ volatile bool marker_enabled = false;
 volatile bool window_enabled = false;
 
 // Computer vision thread
-struct image_t *cv_marker_func(struct image_t *img);
-struct image_t *cv_marker_func(struct image_t *img)
+struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id);
+struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id)
 {
 
   if (!marker_enabled) {
@@ -80,8 +80,8 @@ struct image_t *cv_marker_func(struct image_t *img)
 
 
 // Computer vision thread
-struct image_t *cv_window_func(struct image_t *img);
-struct image_t *cv_window_func(struct image_t *img)
+struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id);
+struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id)
 {
 
   if (!window_enabled) {
@@ -127,8 +127,8 @@ struct image_t *cv_window_func(struct image_t *img)
 }
 
 
-struct image_t *cv_blob_locator_func(struct image_t *img);
-struct image_t *cv_blob_locator_func(struct image_t *img)
+struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id);
+struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id)
 {
 
   if (!blob_enabled) {
@@ -249,9 +249,9 @@ void cv_blob_locator_init(void)
 
   georeference_init();
 
-  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS);
-  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS);
-  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS);
+  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS, 0);
+  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS, 0);
+  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS, 0);
 }
 
 void cv_blob_locator_periodic(void)

sw/airborne/modules/computer_vision/cv_detect_color_object.c

@@ -139,14 +139,14 @@ static struct image_t *object_detector(struct image_t *img, uint8_t filter)
   return img;
 }
 
-struct image_t *object_detector1(struct image_t *img);
-struct image_t *object_detector1(struct image_t *img)
+struct image_t *object_detector1(struct image_t *img, uint8_t camera_id);
+struct image_t *object_detector1(struct image_t *img, uint8_t camera_id)
 {
   return object_detector(img, 1);
 }
 
-struct image_t *object_detector2(struct image_t *img);
-struct image_t *object_detector2(struct image_t *img)
+struct image_t *object_detector2(struct image_t *img, uint8_t camera_id);
+struct image_t *object_detector2(struct image_t *img, uint8_t camera_id)
 {
   return object_detector(img, 2);
 }
@@ -168,7 +168,7 @@ void color_object_detector_init(void)
   cod_draw1 = COLOR_OBJECT_DETECTOR_DRAW1;
 #endif
 
-  cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1);
+  cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1, 0);
 #endif
 
 #ifdef COLOR_OBJECT_DETECTOR_CAMERA2
@@ -184,7 +184,7 @@ void color_object_detector_init(void)
   cod_draw2 = COLOR_OBJECT_DETECTOR_DRAW2;
 #endif
 
-  cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2);
+  cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2, 1);
 #endif
 }
 

sw/airborne/modules/computer_vision/cv_opencvdemo.c

@@ -33,8 +33,8 @@
 PRINT_CONFIG_VAR(OPENCVDEMO_FPS)
 
 // Function
-struct image_t *opencv_func(struct image_t *img);
-struct image_t *opencv_func(struct image_t *img)
+struct image_t *opencv_func(struct image_t *img, uint8_t camera_id);
+struct image_t *opencv_func(struct image_t *img, uint8_t camera_id)
 {
 
   if (img->type == IMAGE_YUV422) {
@@ -49,6 +49,6 @@ struct image_t *opencv_func(struct image_t *img)
 
 void opencvdemo_init(void)
 {
-  cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS);
+  cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS, 0);
 }
 

sw/airborne/modules/computer_vision/detect_contour.c

@@ -33,8 +33,8 @@
 PRINT_CONFIG_VAR(DETECT_CONTOUR_FPS)
 
 // Function
-struct image_t *contour_func(struct image_t *img);
-struct image_t *contour_func(struct image_t *img)
+struct image_t *contour_func(struct image_t *img, uint8_t camera_id);
+struct image_t *contour_func(struct image_t *img, uint8_t camera_id)
 {
 
   if (img->type == IMAGE_YUV422) {
@@ -46,7 +46,7 @@ struct image_t *contour_func(struct image_t *img)
 
 void detect_contour_init(void)
 {
-  cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS);
+  cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS, 0);
   // in the mavlab, bright
   cont_thres.lower_y = 16;  cont_thres.lower_u = 135; cont_thres.lower_v = 80;
   cont_thres.upper_y = 100; cont_thres.upper_u = 175; cont_thres.upper_v = 165;

sw/airborne/modules/computer_vision/detect_gate.c

@@ -131,7 +131,7 @@ struct vision_relative_position_struct {
 
 
 // Function
-static struct image_t *detect_gate_func(struct image_t *img)
+static struct image_t *detect_gate_func(struct image_t *img, uint8_t camera_id)
 {
   // detect the gate and draw it in the image:
   if (just_filtering) {
@@ -354,7 +354,7 @@ void detect_gate_init(void)
   detect_gate_y = 0;
   detect_gate_z = 0;
 
-  cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS);
+  cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS, 0);
 
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VISION_POSITION_ESTIMATE, send_detect_gate_visual_position);
 }

sw/airborne/modules/computer_vision/detect_window.c

@@ -39,13 +39,13 @@ PRINT_CONFIG_VAR(DETECT_WINDOW_FPS)
 void detect_window_init(void)
 {
 #ifdef DETECT_WINDOW_CAMERA
-  cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS);
+  cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS, 0);
 #else
 #warning "DETECT_WINDOW_CAMERA not defined, CV callback not added to device"
 #endif
 }
 
-struct image_t *detect_window(struct image_t *img)
+struct image_t *detect_window(struct image_t *img, uint8_t camera_id)
 {
 
   uint16_t coordinate[2];

sw/airborne/modules/computer_vision/detect_window.h

@@ -35,7 +35,7 @@
 #include "inttypes.h"
 
 extern void detect_window_init(void);
-extern struct image_t* detect_window(struct image_t *img);
+extern struct image_t* detect_window(struct image_t *img, uint8_t camera_id);
 
 uint16_t detect_window_sizes(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate,
                              uint32_t *integral_image, uint8_t MODE);

sw/airborne/modules/computer_vision/lib/vision/act_fast.c

@@ -22,16 +22,21 @@ All rights reserved.
  * de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission.
  *
  */
-
 #include "fast_rosten.h"
 #include "act_fast.h"
 #include "math.h"
 #include "image.h"
 #include "../../opticflow/opticflow_calculator.h"
 
+// ACT-FAST agents arrays
 // equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
+// TODO Currently hardcoded to two cameras
 #define MAX_AGENTS FAST9_MAX_CORNERS
-struct agent_t agents[MAX_AGENTS];
+#ifndef OPTICFLOW_CAMERA2
+  struct agent_t agents[1][MAX_AGENTS];
+#else
+  struct agent_t agents[2][MAX_AGENTS];
+#endif
 
 /**
  * Do an ACT-FAST corner detection.
@@ -46,8 +51,9 @@ struct agent_t agents[MAX_AGENTS];
  * @param[in] min_gradient The minimum gradient, in order to determine when to take a long or short step
  * @param[in] gradient_method: 0 = simple {-1, 0, 1}, 1 = Sobel {-1,0,1,-2,0,2,-1,0,1}
 */
-void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
-              uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method)
+void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, 
+              uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
+              int gradient_method, int camera_id)
 {
 
   /*
@@ -56,7 +62,7 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
    * 2) loop over the agents, moving and checking for corners
    */
 
-  // ensure that n_agents is never bigger than MAX_AGENTS
+  // ensure that n_agents is never bigger than max_agents
   n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
   // min_gradient should be bigger than 0:
   min_gradient = (min_gradient == 0) ? 1 : min_gradient;
@@ -84,7 +90,7 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
       py = ((float)(rand() % 10000) + 1) / 10000.0f;
       pnorm = sqrtf(px * px + py * py);
       struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
-      agents[a] = ag;
+      agents[camera_id][a] = ag;
       a++;
       if (a == n_agents) { break; }
     }
@@ -105,41 +111,41 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
     // loop over the agents
     for (a = 0; a < n_agents; a++) {
       // only do something if the agent is active:
-      if (agents[a].active) {
+      if (agents[camera_id][a].active) {
         // check if this position is a corner:
-        uint16_t x = (uint16_t) agents[a].x;
-        uint16_t y = (uint16_t) agents[a].y;
+        uint16_t x = (uint16_t) agents[camera_id][a].x;
+        uint16_t y = (uint16_t) agents[camera_id][a].y;
         if (fast9_detect_pixel(img, fast_threshold, x, y)) {
           // we arrived at a corner, yeah!!!
-          agents[a].active = 0;
+          agents[camera_id][a].active = 0;
           continue;
         } else {
           // make a step:
-          struct point_t loc = { .x = agents[a].x, .y = agents[a].y};
+          struct point_t loc = { .x = agents[camera_id][a].x, .y = agents[camera_id][a].y};
           image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
           int gradient = (abs(dx) + abs(dy)) / 2;
           if (abs(gradient) >= min_gradient) {
             // determine the angle and make a step in that direction:
             float norm_factor = sqrtf((float)(dx * dx + dy * dy));
-            agents[a].x += (dy / norm_factor) * short_step;
-            agents[a].y += (dx / norm_factor) * short_step;
+            agents[camera_id][a].x += (dy / norm_factor) * short_step;
+            agents[camera_id][a].y += (dx / norm_factor) * short_step;
           } else {
             // make a step in the preferred direction:
-            agents[a].x += agents[a].preferred_dir_x * long_step;
-            agents[a].y += agents[a].preferred_dir_y * long_step;
+            agents[camera_id][a].x += agents[camera_id][a].preferred_dir_x * long_step;
+            agents[camera_id][a].y += agents[camera_id][a].preferred_dir_y * long_step;
           }
         }
 
         // let the agent move over the image in a toroid world:
-        if (agents[a].x > img->w - border) {
-          agents[a].x = border;
-        } else if (agents[a].x < border) {
-          agents[a].x = img->w - border;
+        if (agents[camera_id][a].x > img->w - border) {
+          agents[camera_id][a].x = border;
+        } else if (agents[camera_id][a].x < border) {
+          agents[camera_id][a].x = img->w - border;
         }
-        if (agents[a].y > img->h - border) {
-          agents[a].y = border;
-        } else if (agents[a].y < border) {
-          agents[a].y = img->h - border;
+        if (agents[camera_id][a].y > img->h - border) {
+          agents[camera_id][a].y = border;
+        } else if (agents[camera_id][a].y < border) {
+          agents[camera_id][a].y = img->h - border;
         }
       }
     }
@@ -150,20 +156,20 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
   for (a = 0; a < n_agents; a++) {
 
     // for active agents do a last check on the new position:
-    if (agents[a].active) {
+    if (agents[camera_id][a].active) {
       // check if the last step brought the agent to a corner:
-      uint16_t x = (uint16_t) agents[a].x;
-      uint16_t y = (uint16_t) agents[a].y;
+      uint16_t x = (uint16_t) agents[camera_id][a].x;
+      uint16_t y = (uint16_t) agents[camera_id][a].y;
       if (fast9_detect_pixel(img, fast_threshold, x, y)) {
         // we arrived at a corner, yeah!!!
-        agents[a].active = 0;
+        agents[camera_id][a].active = 0;
       }
     }
 
     // if inactive, the agent is a corner:
-    if (!agents[a].active) {
-      (*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x;
-      (*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y;
+    if (!agents[camera_id][a].active) {
+      (*ret_corners)[(*num_corners)].x = (uint32_t) agents[camera_id][a].x;
+      (*ret_corners)[(*num_corners)].y = (uint32_t) agents[camera_id][a].y;
       (*num_corners)++;
     }
   }

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

@@ -37,6 +37,7 @@ struct agent_t {
 #include "lib/vision/image.h"
 
 void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
-              uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method);
+              uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
+              int gradient_method, int camera_id);
 
 #endif

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

@@ -52,6 +52,7 @@ struct opticflow_result_t {
 
   float surface_roughness; ///< Surface roughness as determined with a linear optical flow fit
   float divergence;       ///< Divergence as determined with a linear flow fit
+  uint8_t camera_id;      ///< Camera id as passed to cv_add_to_device
 
   float noise_measurement;  ///< noise of measurement, for state filter
 };

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

@@ -79,12 +79,13 @@ struct opticflow_t {
   int actfast_min_gradient;       ///< Threshold that decides when there is sufficient texture for edge following
   int actfast_gradient_method;    ///< Whether to use a simple or Sobel filter
 
-
+  const struct video_config_t *camera;
+  uint8_t id;
 };
 
 #define FAST9_MAX_CORNERS 512
 
-extern void opticflow_calc_init(struct opticflow_t *opticflow);
+extern void opticflow_calc_init(struct opticflow_t opticflow[]);
 extern bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img,
                           struct opticflow_result_t *result);
 

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -48,17 +48,28 @@ PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
 #ifndef OPTICFLOW_FPS
 #define OPTICFLOW_FPS 0       ///< Default FPS (zero means run at camera fps)
 #endif
+
+#ifndef OPTICFLOW_FPS_CAMERA2
+#define OPTICFLOW_FPS_CAMERA2 0       ///< Default FPS (zero means run at camera fps)
+#endif
 PRINT_CONFIG_VAR(OPTICFLOW_FPS)
+PRINT_CONFIG_VAR(OPTICFLOW_FPS_CAMERA2)
+
+#ifdef OPTICFLOW_CAMERA2
+#define ACTIVE_CAMERAS 2
+#else
+#define ACTIVE_CAMERAS 1
+#endif
 
 /* The main opticflow variables */
-struct opticflow_t opticflow;                      ///< Opticflow calculations
-static struct opticflow_result_t opticflow_result; ///< The opticflow result
+struct opticflow_t opticflow[ACTIVE_CAMERAS];                         ///< Opticflow calculations
+static struct opticflow_result_t opticflow_result[ACTIVE_CAMERAS];    ///< The opticflow result
 
-static bool opticflow_got_result;                ///< When we have an optical flow calculation
-static pthread_mutex_t opticflow_mutex;            ///< Mutex lock fo thread safety
+static bool opticflow_got_result[ACTIVE_CAMERAS];       ///< When we have an optical flow calculation
+static pthread_mutex_t opticflow_mutex;                  ///< Mutex lock fo thread safety
 
 /* Static functions */
-struct image_t *opticflow_module_calc(struct image_t *img);     ///< The main optical flow calculation thread
+struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id);     ///< The main optical flow calculation thread
 
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
@@ -70,15 +81,19 @@ struct image_t *opticflow_module_calc(struct image_t *img);     ///< The main op
 static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
 {
   pthread_mutex_lock(&opticflow_mutex);
-  if (opticflow_result.noise_measurement < 0.8) {
-    pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
-                                 &opticflow_result.fps, &opticflow_result.corner_cnt,
-                                 &opticflow_result.tracked_cnt, &opticflow_result.flow_x,
-                                 &opticflow_result.flow_y, &opticflow_result.flow_der_x,
-                                 &opticflow_result.flow_der_y, &opticflow_result.vel_body.x,
-                                 &opticflow_result.vel_body.y, &opticflow_result.vel_body.z,
-                                 &opticflow_result.div_size, &opticflow_result.surface_roughness,
-                                 &opticflow_result.divergence); // TODO: no noise measurement here...
+  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
+    if (opticflow_result[idx_camera].noise_measurement < 0.8) {
+      pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
+                                   &opticflow_result[idx_camera].fps, &opticflow_result[idx_camera].corner_cnt,
+                                   &opticflow_result[idx_camera].tracked_cnt, &opticflow_result[idx_camera].flow_x,
+                                   &opticflow_result[idx_camera].flow_y, &opticflow_result[idx_camera].flow_der_x,
+                                   &opticflow_result[idx_camera].flow_der_y, &opticflow_result[idx_camera].vel_body.x,
+                                   &opticflow_result[idx_camera].vel_body.y, &opticflow_result[idx_camera].vel_body.z,
+                                   &opticflow_result[idx_camera].div_size,
+                                   &opticflow_result[idx_camera].surface_roughness,
+                                   &opticflow_result[idx_camera].divergence,
+                                   &opticflow_result[idx_camera].camera_id); // TODO: no noise measurement here...
+    }
   }
   pthread_mutex_unlock(&opticflow_mutex);
 }
@@ -90,10 +105,15 @@ static void opticflow_telem_send(struct transport_tx *trans, struct link_device
 void opticflow_module_init(void)
 {
   // Initialize the opticflow calculation
-  opticflow_got_result = false;
-  opticflow_calc_init(&opticflow);
+  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
+    opticflow_got_result[idx_camera] = false;
+  }
+  opticflow_calc_init(opticflow);
 
-  cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS);
+  cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS, 0);
+#ifdef OPTICFLOW_CAMERA2
+  cv_add_to_device(&OPTICFLOW_CAMERA2, opticflow_module_calc, OPTICFLOW_FPS_CAMERA2, 1);
+#endif
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
@@ -109,27 +129,29 @@ void opticflow_module_run(void)
 {
   pthread_mutex_lock(&opticflow_mutex);
   // Update the stabilization loops on the current calculation
-  if (opticflow_got_result) {
-    uint32_t now_ts = get_sys_time_usec();
-    AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID, now_ts,
-                           opticflow_result.flow_x,
-                           opticflow_result.flow_y,
-                           opticflow_result.flow_der_x,
-                           opticflow_result.flow_der_y,
-                           opticflow_result.noise_measurement,
-                           opticflow_result.div_size);
-    //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
-    if (opticflow_result.noise_measurement < 0.8) {
-      AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID, now_ts,
-                                  opticflow_result.vel_body.x,
-                                  opticflow_result.vel_body.y,
-                                  0.0f, //opticflow_result.vel_body.z,
-                                  opticflow_result.noise_measurement,
-                                  opticflow_result.noise_measurement,
-                                  -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
-                                 );
+  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
+    if (opticflow_got_result[idx_camera]) {
+      uint32_t now_ts = get_sys_time_usec();
+      AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID + idx_camera, now_ts,
+                             opticflow_result[idx_camera].flow_x,
+                             opticflow_result[idx_camera].flow_y,
+                             opticflow_result[idx_camera].flow_der_x,
+                             opticflow_result[idx_camera].flow_der_y,
+                             opticflow_result[idx_camera].noise_measurement,
+                             opticflow_result[idx_camera].div_size);
+      //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
+      if (opticflow_result[idx_camera].noise_measurement < 0.8) {
+        AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,
+                                    opticflow_result[idx_camera].vel_body.x,
+                                    opticflow_result[idx_camera].vel_body.y,
+                                    0.0f, //opticflow_result.vel_body.z,
+                                    opticflow_result[idx_camera].noise_measurement,
+                                    opticflow_result[idx_camera].noise_measurement,
+                                    -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
+        );
+      }
+      opticflow_got_result[idx_camera] = false;
     }
-    opticflow_got_result = false;
   }
   pthread_mutex_unlock(&opticflow_mutex);
 }
@@ -139,9 +161,10 @@ void opticflow_module_run(void)
  * This thread passes the images trough the optical flow
  * calculator
  * @param[in] *img The image_t structure of the captured image
+ * @param[in] camera_id The camera index id
  * @return *img The processed image structure
  */
-struct image_t *opticflow_module_calc(struct image_t *img)
+struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id)
 {
   // Copy the state
   // TODO : put accelerometer values at pose of img timestamp
@@ -150,12 +173,12 @@ struct image_t *opticflow_module_calc(struct image_t *img)
   img->eulers = pose.eulers;
 
   // Do the optical flow calculation
-  static struct opticflow_result_t temp_result; // static so that the number of corners is kept between frames
-  if(opticflow_calc_frame(&opticflow, img, &temp_result)){
+  static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
+  if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){
     // Copy the result if finished
     pthread_mutex_lock(&opticflow_mutex);
-    opticflow_result = temp_result;
-    opticflow_got_result = true;
+    opticflow_result[camera_id] = temp_result[camera_id];
+    opticflow_got_result[camera_id] = true;
     pthread_mutex_unlock(&opticflow_mutex);
   }
   return img;

sw/airborne/modules/computer_vision/opticflow_module.h

@@ -31,7 +31,7 @@
 #include "opticflow/opticflow_calculator.h"
 
 // Needed for settings
-extern struct opticflow_t opticflow;
+extern struct opticflow_t opticflow[];
 
 // Module functions
 extern void opticflow_module_init(void);

sw/airborne/modules/computer_vision/qrcode/qr_code.c

@@ -42,7 +42,7 @@ PRINT_CONFIG_VAR(QRCODE_FPS)
 void qrcode_init(void)
 {
   // Add qrscan to the list of image processing tasks in video_thread
-  cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS);
+  cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS, 0);
 }
 
 // Telemetry
@@ -51,7 +51,7 @@ void qrcode_init(void)
 
 zbar_image_scanner_t *scanner = 0;
 
-struct image_t *qrscan(struct image_t *img)
+struct image_t *qrscan(struct image_t *img, uint8_t camera_id)
 {
   int i, j;
 

sw/airborne/modules/computer_vision/qrcode/qr_code.h

@@ -35,7 +35,7 @@
 
 extern bool drawRectangleAroundQRCode;
 extern void qrcode_init(void);
-extern struct image_t *qrscan(struct image_t *img);
+extern struct image_t *qrscan(struct image_t *img, uint8_t camera_id);
 
 
 #endif

sw/airborne/modules/computer_vision/undistort_image.c

@@ -43,7 +43,7 @@ static float K[9] = {0.0f, 0.0f, 0.0f,
                      0.0f, 0.0f, 1.0f};
 
 // Function
-static struct image_t *undistort_image_func(struct image_t *img)
+static struct image_t *undistort_image_func(struct image_t *img, uint8_t camera_id)
 {
   // TODO: These commands could actually only be run when the parameters or image size are changed
   float normalized_step = (max_x_normalized - min_x_normalized) / img->w;
@@ -118,5 +118,5 @@ void undistort_image_init(void)
   min_x_normalized = UNDISTORT_MIN_X_NORMALIZED;
   max_x_normalized = UNDISTORT_MAX_X_NORMALIZED;
   center_ratio = UNDISTORT_CENTER_RATIO;
-  listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS);
+  listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS), 0;
 }

sw/airborne/modules/computer_vision/video_capture.c

@@ -61,7 +61,7 @@ int video_capture_index = 0;
 static char save_dir[256];
 
 // Forward function declarations
-struct image_t *video_capture_func(struct image_t *img);
+struct image_t *video_capture_func(struct image_t *img, uint8_t camera_id);
 void video_capture_save(struct image_t *img);
 
 
@@ -80,11 +80,11 @@ void video_capture_init(void)
   // This prevents empty folders if nothing is actually recorded.
 
   // Add function to computer vision pipeline
-  cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS);
+  cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS, 0);
 }
 
 
-struct image_t *video_capture_func(struct image_t *img)
+struct image_t *video_capture_func(struct image_t *img, uint8_t camera_id)
 {
   // If take_shot bool is set, save the image
   if (video_capture_take_shot || video_capture_record_video) {

sw/airborne/modules/computer_vision/video_usb_logger.c

@@ -111,7 +111,7 @@ static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg)
 
 }
 
-static struct image_t *log_image(struct image_t *img)
+static struct image_t *log_image(struct image_t *img, uint8_t camera_id)
 {
   if (!created_jpeg) {
 
@@ -148,7 +148,7 @@ void video_usb_logger_start(void)
   }
 
   // Subscribe to a camera
-  cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS);
+  cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS, 0);
 }
 
 /** Stop the logger an nicely close the file */

sw/airborne/modules/computer_vision/viewvideo.c

@@ -191,7 +191,7 @@ static struct image_t *viewvideo_function(struct UdpSocket *viewvideo_socket, st
 }
 
 #ifdef VIEWVIDEO_CAMERA
-static struct image_t *viewvideo_function1(struct image_t *img)
+static struct image_t *viewvideo_function1(struct image_t *img, uint8_t camera_id)
 {
   static uint16_t rtp_packet_nr = 0;
   static uint32_t rtp_frame_time = 0;
@@ -202,7 +202,7 @@ static struct image_t *viewvideo_function1(struct image_t *img)
 #endif
 
 #ifdef VIEWVIDEO_CAMERA2
-static struct image_t *viewvideo_function2(struct image_t *img)
+static struct image_t *viewvideo_function2(struct image_t *img, uint8_t camera_id)
 {
   static uint16_t rtp_packet_nr = 0;
   static uint32_t rtp_frame_time = 0;
@@ -263,13 +263,13 @@ void viewvideo_init(void)
 
 #ifdef VIEWVIDEO_CAMERA
   cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function1,
-                         VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS);
+                         VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS, 0);
   fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA1 at %u FPS \n", VIEWVIDEO_FPS);
 #endif
 
 #ifdef VIEWVIDEO_CAMERA2
   cv_add_to_device_async(&VIEWVIDEO_CAMERA2, viewvideo_function2,
-                         VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS);
+                         VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS, 1);
   fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA2 at %u FPS \n", VIEWVIDEO_FPS);
 #endif
 }

sw/airborne/modules/pano_unwrap/pano_unwrap.c

@@ -300,7 +300,7 @@ static void unwrap_LUT(struct image_t *img_raw, struct image_t *img)
   }
 }
 
-static struct image_t *camera_cb(struct image_t *img)
+static struct image_t *camera_cb(struct image_t *img, uint8_t camera_id)
 {
   set_output_image_size();
   update_LUT(img);
@@ -314,6 +314,6 @@ void pano_unwrap_init()
 {
   image_create(&pano_unwrapped_image, 0, 0, IMAGE_YUV422);
   set_output_image_size();
-  cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS);
+  cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS, 0);
 }
 

sw/airborne/modules/sensors/opticflow_pmw3901.c

@@ -143,6 +143,7 @@ static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t
 #if SENSOR_SYNC_SEND_OPTICFLOW_PMW3901
   float dummy_f = 0.f;
   uint16_t dummy_u16 = 0;
+  uint8_t dummy_u8 = 0;
   float fps = 1.f / dt;
   DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice,
       &fps,                 /* fps */
@@ -157,7 +158,8 @@ static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t
       &dummy_f,             /* vel_z */
       &dummy_f,             /* div_size */
       &dummy_f,             /* surface_roughness */
-      &dummy_f              /* divergence */
+      &dummy_f,             /* divergence */
+      &dummy_u8             /* camera_id */
       );
 #endif
 }

sw/airborne/modules/wedgebug/wedgebug.c

@@ -299,9 +299,9 @@ uint8_t getMedian(uint8_t *a, uint32_t n);
 
 //Core
 static struct image_t *copy_left_img_func(struct image_t
-    *img); // Function 1: Copies left image into a buffer (buf_left)
+    *img, uint8_t camera_id); // Function 1: Copies left image into a buffer (buf_left)
 static struct image_t *copy_right_img_func(struct image_t
-    *img); // Function 2: Copies left image into a buffer (buf_right)
+    *img, uint8_t camera_id); // Function 2: Copies left image into a buffer (buf_right)
 void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left,
                          struct image_t *right); // Function 3: Copies gray pixel values of left and right UYVY images into merged YY image
 void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right);
@@ -540,7 +540,7 @@ uint16_t getMedian16bit(uint16_t *a, uint32_t n)
 // Core:
 
 // Function 1
-static struct image_t *copy_left_img_func(struct image_t *img)
+static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id)
 {
   image_copy(img, &img_left);
   //show_image_data(img);
@@ -550,7 +550,7 @@ static struct image_t *copy_left_img_func(struct image_t *img)
 
 
 // Function 2
-static struct image_t *copy_right_img_func(struct image_t *img)
+static struct image_t *copy_right_img_func(struct image_t *img, uint8_t camera_id)
 {
   image_copy(img, &img_right);
   //show_image_data(img);
@@ -1655,8 +1655,8 @@ void wedgebug_init()
 
 
   // Adding callback functions
-  cv_add_to_device(&WEDGEBUG_CAMERA_LEFT, copy_left_img_func, WEDGEBUG_CAMERA_LEFT_FPS);
-  cv_add_to_device(&WEDGEBUG_CAMERA_RIGHT, copy_right_img_func, WEDGEBUG_CAMERA_RIGHT_FPS);
+  cv_add_to_device(&WEDGEBUG_CAMERA_LEFT, copy_left_img_func, WEDGEBUG_CAMERA_LEFT_FPS, 0);
+  cv_add_to_device(&WEDGEBUG_CAMERA_RIGHT, copy_right_img_func, WEDGEBUG_CAMERA_RIGHT_FPS, 1);
 
 
   //Initialization of constant rotation matrices and transition vectors for frame to frame transformations

sw/airborne/subsystems/abi_sender_ids.h

@@ -355,8 +355,16 @@
 #define FLOW_OPTICFLOW_ID 1
 #endif
 
+#ifndef FLOW_OPTICFLOW_CAM1_ID
+#define FLOW_OPTICFLOW_CAM1_ID 1
+#endif
+
+#ifndef FLOW_OPTICFLOW_CAM2_ID
+#define FLOW_OPTICFLOW_CAM2_ID 2
+#endif
+
 #ifndef FLOW_OPTICFLOW_PMW3901_ID
-#define FLOW_OPTICFLOW_PMW3901_ID 2
+#define FLOW_OPTICFLOW_PMW3901_ID 3
 #endif
 
 /*
@@ -374,12 +382,20 @@
 #define VEL_OPTICFLOW_ID 3
 #endif
 
+#ifndef VEL_OPTICFLOW_CAM1_ID
+#define VEL_OPTICFLOW_CAM1_ID 3
+#endif
+
+#ifndef VEL_OPTICFLOW_CAM2_ID
+#define VEL_OPTICFLOW_CAM2_ID 4
+#endif
+
 #ifndef VEL_STEREOCAM_ID
-#define VEL_STEREOCAM_ID 4
+#define VEL_STEREOCAM_ID 5
 #endif
 
 #ifndef VEL_OPTICFLOW_PMW3901_ID
-#define VEL_OPTICFLOW_PMW3901_ID 5
+#define VEL_OPTICFLOW_PMW3901_ID 6
 #endif
 
 /*