Merge pull request #1558 from knmcguire/pull_request_ARdrone_edgeflow

edgeflow: improved optic flow calculation a new method to calculate the global optical flow in a efficient way. It should run twice as fast as the current implementation.
2026-06-04 22:17:01 +08:00 · 2016-03-21 15:14:18 +01:00
parent 794d786704 402034944e
commit 382873ac24
9 changed files with 745 additions and 29 deletions
@@ -1,4 +1,15 @@
 <conf>
+  <aircraft
+   name="ARDrone2_opticflow"
+   ac_id="12"
+   airframe="airframes/TUDELFT/tudelft_ardrone2_opticflow.xml"
+   radio="radios/dummy.xml"
+   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"
+   gui_color="red"
+  />
  <aircraft
   name="LadyLisaBluegiga"
   ac_id="114"
@@ -85,5 +85,12 @@
      <arg flag="4252" />
    </program>
    </session>
+   <session name="Flight UDP/WiFi">
+      <program name="Server"/>
+      <program name="GCS"/>
+      <program name="Data Link">
+        <arg flag="-udp"/>
+      </program>
+    </session>
  </section>
 </control_panel>
@@ -24,10 +24,15 @@
      <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"/>
+      <define name="DEROTATION" value="1" description="Derotation either turned on or off (depended on gyroscope measurements)"/>
+
      <!-- 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"/>

@@ -41,11 +46,16 @@
  <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.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
+      <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="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"/>
+        <dl_setting var="opticflow.derotation" min="0" step="1" max="1" module="computer_vision/opticflow_module" values="OFF|ON" shortname="derotation" param="OPTICFLOW_DEROTATION"/>
+
+	<!-- 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"/>

@@ -82,7 +92,9 @@
    <!-- Main vision calculations -->
    <file name="fast_rosten.c" dir="modules/computer_vision/lib/vision"/>
    <file name="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>
+    <file name="edge_flow.c" dir="modules/computer_vision/lib/vision"/>
  </makefile>

+
 </module>

@@ -0,0 +1,353 @@
+/*
+ * Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
+ *
+ * This file is part of Paparazzi.
+ *
+ * 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
+ */
+
+#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
+ * @param[in] *img  The image frame to calculate the edge histogram from
+ * @param[out] *edge_histogram  The edge histogram from the current frame_step
+ * @param[in] direction  Indicating if the histogram is made in either x or y direction
+ * @param[in] edge_threshold  A threshold if a gradient is considered a edge or not
+ */
+void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
+                              char direction, uint16_t edge_threshold)
+{
+  uint8_t *img_buf = (uint8_t *)img->buf;
+
+  // TODO use arm_conv_q31()
+  int32_t sobel_sum = 0;
+  int32_t Sobel[3] = { -1, 0, 1};
+
+  uint32_t y = 0, x = 0;
+  int32_t c = 0;
+
+  uint32_t idx = 0;
+
+  uint16_t image_width = img->w;
+  uint16_t image_height = img->h;
+  uint32_t interlace;
+  if (img->type == IMAGE_GRAYSCALE) {
+    interlace = 1;
+  } else {
+    if (img->type == IMAGE_YUV422) {
+      interlace = 2;
+    } else
+      while (1);   // hang to show user something isn't right
+  }
+
+
+  // compute edge histogram
+  if (direction == 'x') {
+    // set values that are not visited
+    edge_histogram[0] = edge_histogram[image_width - 1] = 0;
+    for (x = 1; x < image_width - 1; x++) {
+      edge_histogram[x] = 0;
+      for (y = 0; y < image_height; y++) {
+        sobel_sum = 0;
+
+        for (c = -1; c <= 1; c++) {
+          idx = interlace * (image_width * y + (x + c)); // 2 for interlace
+
+          sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
+        }
+        sobel_sum = abs(sobel_sum);
+        if (sobel_sum > edge_threshold) {
+          edge_histogram[x] += sobel_sum;
+        }
+      }
+    }
+  } else if (direction == 'y') {
+    // set values that are not visited
+    edge_histogram[0] = edge_histogram[image_height - 1] = 0;
+    for (y = 1; y < image_height - 1; y++) {
+      edge_histogram[y] = 0;
+      for (x = 0; x < image_width; x++) {
+        sobel_sum = 0;
+
+        for (c = -1; c <= 1; c++) {
+          idx = interlace * (image_width * (y + c) + x); // 2 for interlace
+
+          sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
+        }
+        sobel_sum = abs(sobel_sum);
+        if (sobel_sum > edge_threshold) {
+          edge_histogram[y] += sobel_sum;
+        }
+      }
+    }
+  } else
+    while (1);  // hang to show user something isn't right
+}
+
+/**
+ * Calculate_displacement calculates the displacement between two histograms
+ * @param[in] *edge_histogram  The edge histogram from the current frame_step
+ * @param[in] *edge_histogram_prev  The edge histogram from the previous frame_step
+ * @param[in] *displacement array with pixel displacement of the sequential edge histograms
+ * @param[in] size  Indicating the size of the displacement array
+ * @param[in] window Indicating the search window size
+ * @param[in] disp_range  Indicating the maximum disparity range for the block matching
+ * @param[in] der_shift  The pixel shift estimated by the angle rate of the IMU
+ */
+void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement,
+                                 uint16_t size,
+                                 uint8_t window, uint8_t disp_range, int32_t der_shift)
+{
+  int32_t c = 0, r = 0;
+  uint32_t x = 0;
+  uint32_t SAD_temp[2 * DISP_RANGE_MAX + 1]; // size must be at least 2*D + 1
+
+  int32_t W = window;
+  int32_t D = disp_range;
+
+
+  uint8_t SHIFT_TOO_FAR = 0;
+  memset(displacement, 0, size);
+
+  int32_t border[2];
+
+  if (der_shift < 0) {
+    border[0] =  W + D + der_shift;
+    border[1] = size - W - D;
+  } else if (der_shift > 0) {
+    border[0] =  W + D;
+    border[1] = size - W - D - der_shift;
+  } else {
+    border[0] =  W + D;
+    border[1] = size - W - D;
+  }
+
+  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++) {
+      displacement[x] = 0;
+      for (c = -D; c <= D; c++) {
+        SAD_temp[c + D] = 0;
+        for (r = -W; r <= W; r++) {
+          SAD_temp[c + D] += abs(edge_histogram[x + r] - edge_histogram_prev[x + r + c + der_shift]);
+        }
+      }
+      if (!SHIFT_TOO_FAR) {
+        displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
+      } else {
+        displacement[x] = 0;
+      }
+    }
+  }
+
+}
+
+/**
+ * Calculate minimum of an array
+ * @param[in] *a Array containing values
+ * @param[in] *n The size of the array
+ * @return The index of the smallest value of the array
+ */
+uint32_t getMinimum(uint32_t *a, uint32_t n)
+{
+  uint32_t i;
+  uint32_t min_ind = 0;
+  uint32_t min_err = a[min_ind];
+  uint32_t min_err_tot = 0;
+  for (i = 1; i < n; i++) {
+    if (a[i] <= min_err) {
+      min_ind = i;
+      min_err = a[i];
+      min_err_tot += min_err;
+    }
+  }
+  //*min_error = min_err_tot;
+  return min_ind;
+}
+
+
+/**
+ * Fits a linear model to an array with pixel displacements with least squares
+ * @param[in] *displacements Array with Pixel Displacements
+ * @param[out] *divergence Global divergence of pixel displacements
+ * @param[out] *flow  Global translational flow from pixel displacements
+ * @param[in] *size Size of displacement array
+ * @param[in] border  A border offset of the array that should not be considerd for the line fit
+ * @param[in] RES  Resolution to be used for the integer based linefit
+ */
+void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
+              uint16_t RES)
+{
+  int32_t x;
+
+  int32_t count = 0;
+  int32_t sumY = 0;
+  int32_t sumX = 0;
+  int32_t sumX2 = 0;
+  int32_t sumXY = 0;
+  int32_t xMean = 0;
+  int32_t yMean = 0;
+  int32_t divergence_int = 0;
+  int32_t border_int = (int32_t)border;
+  int32_t size_int = (int32_t)size;
+  uint32_t total_error = 0;
+
+  *divergence = 0;
+  *flow = 0;
+
+  // compute fixed sums
+  int32_t xend = size_int - border_int - 1;
+  sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int;
+  sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6;
+  xMean = (size_int - 1) / 2;
+  count = size_int - 2 * border_int;
+
+  for (x = border_int; x < size - border_int; x++) {
+    sumY += displacement[x];
+    sumXY += x * displacement[x];
+  }
+
+  yMean = RES * sumY / count;
+
+  divergence_int = (RES * sumXY - sumX * yMean) / (sumX2 - sumX * xMean);    // compute slope of line ax + b
+  *divergence = divergence_int;
+  *flow = yMean - *divergence * xMean;  // compute b (or y) intercept of line ax + b
+
+  for (x = border_int; x < size - border_int; x++) {
+    total_error += abs(RES * displacement[x] - divergence_int * x + yMean);
+  }
+}
+
+/**
+ * Draws edgehistogram, displacement and linefit directly on the image for debugging (only for edgeflow in horizontal direction!!)
+ * @param[out] *img The image structure where will be drawn on
+ * @param[in] edgeflow Information structure for flow information
+ * @param[in] Displacement Pixel wise Displacement array
+ * @param[in] *edge_hist_x Horizontal edge_histogram
+ */
+void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement,
+                       int32_t *edge_hist_x)
+{
+  struct point_t point1;
+  struct point_t point2;
+  struct point_t point1_prev;
+  struct point_t point2_prev;
+  struct point_t point1_extra;
+  struct point_t point2_extra;
+  uint16_t i;
+
+  for (i = 120; i < 240; i++) {
+    point1.y = -(uint16_t)edge_hist_x[i] / 100 + img->h / 3;
+    point1.x = i;
+    point2.y = -(uint16_t)edge_hist_x[i + 1] / 100 + img->h / 3;
+    point2.x = i + 1;
+
+    point1_prev.y = -(uint16_t)displacement.x[i] * 5 + img->h * 2 / 3;
+    point1_prev.x = i;
+    point2_prev.y = -(uint16_t)displacement.x[i + 1] * 5 + img->h * 2 / 3;
+    point2_prev.x = i + 1;
+
+    image_draw_line(img, &point1, &point2);
+    image_draw_line(img, &point1_prev, &point2_prev);
+  }
+
+  point1_extra.y = (edgeflow.flow_x + edgeflow.div_x * -180) / 100 + img->h / 2;
+  point1_extra.x = 0;
+  point2_extra.y = (edgeflow.flow_x + edgeflow.div_x * 180) / 100 + img->h / 2;
+  point2_extra.x = 360;
+  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;
+}
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
+ *
+ * This file is part of Paparazzi.
+ *
+ * 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_
+#define EDGE_FLOW_H_
+
+
+#include "std.h"
+#include "opticflow/inter_thread_data.h"
+#include "lib/vision/image.h"
+#include "lib/v4l/v4l2.h"
+#include "opticflow/opticflow_calculator.h"
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+
+#ifndef MAX_HORIZON
+#define MAX_HORIZON 10
+#endif
+#ifndef DISP_RANGE_MAX
+#define DISP_RANGE_MAX 50
+#endif
+#ifndef MAX_WINDOW_SIZE
+#define MAX_WINDOW_SIZE 20
+#endif
+#ifndef IMAGE_HEIGHT
+#define IMAGE_HEIGHT 240
+#endif
+#ifndef IMAGE_WIDTH
+#define IMAGE_WIDTH  360
+#endif
+#ifndef OPTICFLOW_FOV_W
+#define OPTICFLOW_FOV_W 0.89360857702
+#endif
+#ifndef OPTICFLOW_FOV_H
+#define OPTICFLOW_FOV_H 0.67020643276
+#endif
+
+struct edge_hist_t {
+  int32_t x[IMAGE_WIDTH];
+  int32_t y[IMAGE_HEIGHT];
+  struct timeval frame_time;
+  float roll;
+  float pitch;
+};
+
+struct edgeflow_displacement_t {
+  int32_t x[IMAGE_WIDTH];
+  int32_t y[IMAGE_HEIGHT];
+};
+
+struct edge_flow_t {
+  int32_t flow_x;
+  int32_t div_x;
+  int32_t flow_y;
+  int32_t div_y;
+};
+
+
+// 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,
+                                 uint16_t size,
+                                 uint8_t window, uint8_t disp_range, int32_t der_shift);
+
+// Local assisting functions (only used here)
+// TODO: find a way to incorperate/find these functions in paparazzi
+uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finishtime);
+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);
+
+
+#endif /* EDGE_FLOW_H_ */
@@ -40,8 +40,11 @@ struct opticflow_result_t {
  int16_t flow_der_x;     ///< The derotated flow calculation in the x direction (in subpixels)
  int16_t flow_der_y;     ///< The derotated flow calculation in the y direction (in subpixels)

-  float vel_x;            ///< The velocity in the x direction
-  float vel_y;            ///< The velocity in the y direction
+  float vel_x;            ///< The velocity in the x direction (image coordinates)
+  float vel_y;            ///< The velocity in the y direction (image coordinates)
+
+  float vel_body_x;     ///< The velocity in the x direction (body fixed coordinates)
+  float vel_body_y;     ///< The velocity in the y direction (body fixed coordinates)

  float div_size;         ///< Divergence as determined with the size_divergence script

@@ -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.
 *
@@ -35,11 +36,12 @@
 // Own Header
 #include "opticflow_calculator.h"

+
 // Computer Vision
 #include "lib/vision/image.h"
 #include "lib/vision/lucas_kanade.h"
 #include "lib/vision/fast_rosten.h"
-
+#include "lib/vision/edge_flow.h"
 #include "size_divergence.h"
 #include "linear_flow_fit.h"

@@ -82,8 +84,13 @@ PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS)
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE)

+#ifndef OPTICFLOW_SEARCH_DISTANCE
+#define OPTICFLOW_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 +119,21 @@ 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)
+
+#if OPTICFLOW_METHOD > 1
+#error WARNING: Both Lukas Kanade and EdgeFlow are NOT selected
+#endif
+
+#ifndef OPTICFLOW_DEROTATION
+#define OPTICFLOW_DEROTATION 1
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION)
+
+
 /* 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 +156,12 @@ 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_SEARCH_DISTANCE;
+  opticflow->derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON
+
+  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,17 +169,17 @@ 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;
-}

+}
 /**
- * Run the optical flow on a new image frame
+ * Run the optical flow with fast9 and lukaskanade on a new image frame
 * @param[in] *opticflow The opticalflow structure that keeps track of previous images
 * @param[in] *state The state of the drone
 * @param[in] *img The image frame to calculate the optical flow from
 * @param[out] *result The optical flow result
 */
-void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
-                          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)
 {
  // variables for size_divergence:
  float size_divergence; int n_samples;
@@ -270,8 +296,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
  }

  // Flow Derotation
-  float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
-  float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
+  float diff_flow_x = 0;
+  float diff_flow_y = 0;
+
+  if (opticflow->derotation) {
+    diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
+    diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
+  }
+
  result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
  result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
  opticflow->prev_phi = state->phi;
@@ -280,16 +312,19 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
  // Velocity calculation
  // Right now this formula is under assumption that the flow only exist in the center axis of the camera.
  // TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
-  float vel_hor = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor  / OPTICFLOW_FX;
-  float vel_ver = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor  / OPTICFLOW_FY;
+  float vel_x = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor  / OPTICFLOW_FX;
+  float vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor  / OPTICFLOW_FY;
+  result->vel_x = vel_x;
+  result->vel_y = vel_y;

  // Velocity calculation: uncomment if focal length of the camera is not known or incorrect.
  //  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.
-  result->vel_x = vel_ver;
-  result->vel_y = - vel_hor;
+  // 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
@@ -307,6 +342,169 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
  image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
 }

+/**
+ * Run the optical flow with EDGEFLOW on a new image frame
+ * @param[in] *opticflow The opticalflow structure that keeps track of previous images
+ * @param[in] *state The state of the drone
+ * @param[in] *img The image frame to calculate the optical flow from
+ * @param[out] *result The optical flow result
+ */
+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
+  static struct edge_hist_t edge_hist[MAX_HORIZON];
+  static uint8_t current_frame_nr = 0;
+  static struct edge_flow_t edgeflow;
+  static uint8_t previous_frame_offset[2] = {1, 1};
+
+  // Define Normal variables
+  struct edgeflow_displacement_t displacement;
+  uint16_t disp_range;
+  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;
+  int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
+  calculate_edge_histogram(img, edge_hist_x, 'x', 0);
+  calculate_edge_histogram(img, edge_hist_y, 'y', 0);
+
+  // Copy frame time and angles of image to calculated edge histogram
+  memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
+  edge_hist[current_frame_nr].pitch = state->theta;
+  edge_hist[current_frame_nr].roll = state->phi;
+
+  // Calculate which previous edge_hist to compare with the current
+  uint8_t previous_frame_nr[2];
+  calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);
+
+  //Select edge histogram from the previous frame nr
+  int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
+  int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;
+
+  //Calculate the corresponding derotation of the two frames
+  int16_t der_shift_x = 0;
+  int16_t der_shift_y = 0;
+
+  if (opticflow->derotation) {
+    der_shift_x = -(int16_t)((edge_hist[previous_frame_nr[0]].roll - edge_hist[current_frame_nr].roll) *
+                             (float)img->w / (OPTICFLOW_FOV_W));
+    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
+  calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
+                              displacement.x, img->w,
+                              window_size, disp_range,  der_shift_x);
+  calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
+                              displacement.y, img->h,
+                              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,
+           window_size + disp_range, RES);
+  line_fit(displacement.y, &edgeflow.div_y,
+           &edgeflow.flow_y, img->h,
+           window_size + disp_range, RES);
+
+  /* Save Resulting flow in results
+   * Warning: The flow detected here is different in sign
+   * 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;
+
+  result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
+  result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
+
+  //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_diff(&edge_hist[previous_frame_nr[0]].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);
+
+  result->fps = fps_x;
+
+  // 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.
+  * 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_DEBUG && OPTICFLOW_SHOW_FLOW
+  draw_edgeflow_img(img, edgeflow, displacement, *edge_hist_x)
+#endif
+  // Increment and wrap current time frame
+  current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
+}
+
+
+/**
+ * Run the optical flow on a new image frame
+ * @param[in] *opticflow The opticalflow structure that keeps track of previous images
+ * @param[in] *state The state of the drone
+ * @param[in] *img The image frame to calculate the optical flow from
+ * @param[out] *result The optical flow result
+ */
+void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+                          struct opticflow_result_t *result)
+{
+  if (opticflow->method == 0) {
+    calc_fast9_lukas_kanade(opticflow, state, img, result);
+  } else {
+    if (opticflow->method == 1) {
+      calc_edgeflow_tot(opticflow, state, img, result);
+    } else {}
+  }
+}
+
 /**
 * Calculate the difference from start till finish
 * @param[in] *starttime The start time to calculate the difference from
@@ -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.
 *
@@ -42,21 +43,30 @@ 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
-  uint16_t window_size;             ///< Window size of the Lucas Kanade calculation (needs to be even)
+  uint8_t method;               ///< Method to use to calculate the optical flow
+  uint16_t window_size;             ///< Window size for the blockmatching algorithm (general value for all methods)
+  uint16_t search_distance;         ///< Search distance for blockmatching alg.
+  uint8_t derotation;             ///< Derotation switched on or off (depended on the quality of the gyroscope measurement)
+
  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);
+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_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+                       struct opticflow_result_t *result);
 #endif /* OPTICFLOW_CALCULATOR_H */


@@ -37,6 +37,7 @@
 #include "lib/encoding/jpeg.h"
 #include "lib/encoding/rtp.h"

+
 /* Default sonar/agl to use in opticflow visual_estimator */
 #ifndef OPTICFLOW_AGL_ID
 #define OPTICFLOW_AGL_ID ABI_BROADCAST    ///< Default sonar/agl to use in opticflow visual_estimator
@@ -77,6 +78,8 @@ static pthread_t opticflow_calc_thread;            ///< The optical flow calcula
 static bool_t opticflow_got_result;                ///< When we have an optical flow calculation
 static pthread_mutex_t opticflow_mutex;            ///< Mutex lock fo thread safety

+struct UdpSocket video_sock;
+
 /* Static functions */
 static void *opticflow_module_calc(void *data);                   ///< The main optical flow calculation thread
 static void opticflow_agl_cb(uint8_t sender_id, float distance);  ///< Callback function of the ground altitude
@@ -140,6 +143,12 @@ void opticflow_module_init(void)
 #if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
 #endif
+
+#if OPTICFLOW_DEBUG
+
+  udp_socket_create(&video_sock, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST);
+
+#endif
 }

 /**
@@ -167,8 +176,8 @@ void opticflow_module_run(void)
    //TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
    if (opticflow_result.tracked_cnt > 0) {
      AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
-                                  opticflow_result.vel_x,
-                                  opticflow_result.vel_y,
+                                  opticflow_result.vel_body_x,
+                                  opticflow_result.vel_body_y,
                                  0.0f,
                                  opticflow_result.noise_measurement
                                 );
@@ -224,9 +233,14 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
 #if OPTICFLOW_DEBUG
  // Create a new JPEG image
  struct image_t img_jpeg;
+
  image_create(&img_jpeg, opticflow_dev->w, opticflow_dev->h, IMAGE_JPEG);
 #endif

+  struct image_t img_gray;
+  image_create(&img_gray, opticflow_dev->w, opticflow_dev->h, IMAGE_YUV422);
+
+
  /* Main loop of the optical flow calculation */
  while (TRUE) {
    // Try to fetch an image
@@ -250,12 +264,12 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
    pthread_mutex_unlock(&opticflow_mutex);

 #if OPTICFLOW_DEBUG
-    jpeg_encode_image(&img, &img_jpeg, 70, FALSE);
+    jpeg_encode_image(&img, &img_jpeg, 50, FALSE);
    rtp_frame_send(
-      &VIEWVIDEO_DEV,           // UDP device
+      &video_sock,           // UDP device
      &img_jpeg,
      0,                        // Format 422
-      70, // Jpeg-Quality
+      50, // Jpeg-Quality
      0,                        // DRI Header
      0                         // 90kHz time increment
    );
@@ -267,6 +281,7 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))

 #if OPTICFLOW_DEBUG
  image_free(&img_jpeg);
+  image_free(&img_gray);
 #endif
 }