[modules] edgeflow added copyright

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

@@ -1,11 +1,82 @@
 /*
- * edge_flow.c
+ * Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
  *
- *  Created on: Feb 22, 2016
+ * This file is part of Paparazzi.
- *      Author: knmcguire
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
  */
-#include <lib/vision/edge_flow.h>
 
+/**
+ * @file modules/computer_vision/lib/vision/edge_flow.ch
+ * @brief calculate optical flow with EdgeFlow
+ *
+ * Edge-histogram matching, implementation by K. N. McGuire
+ * Publication: Local Histogram Matching for Efficient Optical Flow Computation Applied to Velocity Estimation on Pocket Drones
+ * by K.N. McGuire et al. (2016), ICRA 2016
+ */
+
+#include <lib/vision/edge_flow.h>
+/**
+ * Calc_previous_frame_nr; adaptive Time Horizon
+ * @param[in] *opticflow The opticalflow structure
+ * @param[in] *result The optical flow result
+ * @param[in] *current_frame_nr: The current frame number of the circular array of the edge_hist struct
+ * @param[in] *previous_frame_offset: previous frame offset of how far the method should compare the edgehistogram
+ * @param[out] *previous_frame_nr: previous frame index of the edgehist struct
+ */
+void  calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr,
+                             uint8_t *previous_frame_offset, uint8_t *previous_frame_nr)
+{
+  // Adaptive Time Horizon:
+  // if the flow measured in previous frame is small,
+  // the algorithm will choose an frame further away back from the
+  // current frame to detect subpixel flow
+  if (MAX_HORIZON > 2) {
+
+    uint32_t flow_mag_x, flow_mag_y;
+    flow_mag_x = abs(result->flow_x);
+    flow_mag_y = abs(result->flow_y);
+    uint32_t min_flow = 3;
+    uint32_t max_flow = opticflow->search_distance - 3;
+
+    uint8_t previous_frame_offset_x = previous_frame_offset[0];
+    uint8_t previous_frame_offset_y = previous_frame_offset[1];
+
+    // IF statements which will decrement the previous frame offset
+    // if the measured flow of last loop is higher than max value (higher flow measured)
+    // and visa versa
+    if (flow_mag_x > max_flow && previous_frame_offset_x > 1) {
+      previous_frame_offset[0] = previous_frame_offset_x - 1;
+    }
+    if (flow_mag_x < min_flow && previous_frame_offset_x < MAX_HORIZON - 1) {
+      previous_frame_offset[0] = previous_frame_offset_x + 1;
+    }
+    if (flow_mag_y > max_flow && previous_frame_offset_y > 1) {
+      previous_frame_offset[1] = previous_frame_offset_y - 1;
+    }
+    if (flow_mag_y < min_flow && previous_frame_offset_y < MAX_HORIZON - 1) {
+      previous_frame_offset[1] = previous_frame_offset_y + 1;
+    }
+  }
+
+  //Wrap index previous frame offset from current frame nr.
+  previous_frame_nr[0] = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
+                         MAX_HORIZON;
+  previous_frame_nr[1] = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
+                         MAX_HORIZON;
+}
 
 /**
  * Calculate a edge/gradient histogram for each dimension of the image

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

@@ -1,8 +1,30 @@
 /*
- * edge_flow.h
+ * Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
  *
- *  Created on: Feb 22, 2016
+ * This file is part of Paparazzi.
- *      Author: knmcguire
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file modules/computer_vision/lib/vision/edge_flow.ch
+ * @brief calculate optical flow with EdgeFlow
+ *
+ * Edge-histogram matching, implementation by K. N. McGuire
+ * Publication: Local Histogram Matching for Efficient Optical Flow Computation Applied to Velocity Estimation on Pocket Drones
+ * by K.N. McGuire et al. (2016), ICRA 2016
  */
 
 #ifndef EDGE_FLOW_H_
@@ -61,11 +83,12 @@ struct edge_flow_t {
   int32_t div_y;
 };
 
-void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+
-                         struct opticflow_result_t *result);
 // Local functions of the EDGEFLOW algorithm
 void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement,
                        int32_t *edge_hist_x);
+void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr,
+                            uint8_t *previous_frame_offset, uint8_t *previous_frame_nr);
 void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
                               char direction, uint16_t edge_threshold);
 void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement,

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

@@ -1,6 +1,7 @@
 /*
  * Copyright (C) 2014 Hann Woei Ho
  *               2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
  *
  * This file is part of Paparazzi.
  *
@@ -333,7 +334,7 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
  * @param[in] *img The image frame to calculate the optical flow from
  * @param[out] *result The optical flow result
  */
-void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                        struct opticflow_result_t *result)
 {
   // Define Static Variables
@@ -374,52 +375,18 @@ void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *stat
   edge_hist[current_frame_nr].pitch = state->theta;
   edge_hist[current_frame_nr].roll = state->phi;
 
-  // Adaptive Time Horizon:
+  // Calculate which previous edge_hist to compare with the current
-  // if the flow measured in previous frame is small,
+  uint8_t previous_frame_nr[2];
-  // the algorithm will choose an frame further away back from the
+  calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);
-  // current frame to detect subpixel flow
-  if (MAX_HORIZON > 2) {
-
-    uint32_t flow_mag_x, flow_mag_y;
-    flow_mag_x = abs(edgeflow.flow_x);
-    flow_mag_y = abs(edgeflow.flow_y);
-    uint32_t min_flow = 3;
-    uint32_t max_flow = disp_range * RES - 3 * RES;
-
-    uint8_t previous_frame_offset_x = previous_frame_offset[0];
-    uint8_t previous_frame_offset_y = previous_frame_offset[1];
-
-    // IF statements which will decrement the previous frame offset
-    // if the measured flow of last loop is higher than max value (higher flow measured)
-    // and visa versa
-    if (flow_mag_x > max_flow && previous_frame_offset_x > 1) {
-      previous_frame_offset[0] = previous_frame_offset_x - 1;
-    }
-    if (flow_mag_x < min_flow && previous_frame_offset_x < MAX_HORIZON - 1) {
-      previous_frame_offset[0] = previous_frame_offset_x + 1;
-    }
-    if (flow_mag_y > max_flow && previous_frame_offset_y > 1) {
-      previous_frame_offset[1] = previous_frame_offset_y - 1;
-    }
-    if (flow_mag_y < min_flow && previous_frame_offset_y < MAX_HORIZON - 1) {
-      previous_frame_offset[1] = previous_frame_offset_y + 1;
-    }
-  }
-
-  //Wrap index previous frame offset from current frame nr.
-  uint8_t previous_frame_x = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
-                             MAX_HORIZON;
-  uint8_t previous_frame_y = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
-                             MAX_HORIZON;
 
   //Select edge histogram from the previous frame nr
-  int32_t *prev_edge_histogram_x = edge_hist[previous_frame_x].x;
+  int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
-  int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].y;
+  int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;
 
   //Calculate the corrosponding derotation of the two frames
-  int16_t der_shift_x = -(int16_t)((edge_hist[previous_frame_x].roll - edge_hist[current_frame_nr].roll) *
+  int16_t der_shift_x = -(int16_t)((edge_hist[previous_frame_nr[0]].roll - edge_hist[current_frame_nr].roll) *
                                    (float)img->w / (OPTICFLOW_FOV_W));
-  int16_t der_shift_y = -(int16_t)((edge_hist[previous_frame_x].pitch - edge_hist[current_frame_nr].pitch) *
+  int16_t der_shift_y = -(int16_t)((edge_hist[previous_frame_nr[1]].pitch - edge_hist[current_frame_nr].pitch) *
                                    (float)img->h / (OPTICFLOW_FOV_H));
 
   // Estimate pixel wise displacement of the edge histograms for x and y direction
@@ -470,8 +437,8 @@ void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *stat
   */
   float fps_x = 0;
   float fps_y = 0;
-  float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_x].frame_time, &img->ts)) / 1000.;
+  float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.;
-  float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_y].frame_time, &img->ts)) / 1000.;
+  float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.;
   fps_x = 1 / (time_diff_x);
   fps_y = 1 / (time_diff_y);
 
@@ -513,7 +480,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
     calc_fast9_lukas_kanade(opticflow, state, img, result);
   } else {
     if (opticflow->method == 1) {
-      calc_edgeflow(opticflow, state, img, result);
+      calc_edgeflow_tot(opticflow, state, img, result);
     } else {
       PRINT_CONFIG_MSG("Both edgeflow and Lukas kanade is not turned on. Define either USE_LK or use_EDGEFLOW on TRUE!");
     }

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

@@ -1,6 +1,7 @@
 /*
  * Copyright (C) 2014 Hann Woei Ho
  *               2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
  *
  * This file is part of Paparazzi.
  *
@@ -63,7 +64,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
                           struct opticflow_result_t *result);
 void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                              struct opticflow_result_t *result);
-void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                        struct opticflow_result_t *result);
 #endif /* OPTICFLOW_CALCULATOR_H */