[vision] Optic flow debugging and small fixes

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

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <stdlib.h>
 #include "fast_rosten.h"
 
-static void fast_make_offsets(int32_t *pixel, uint16_t row_stride);
+static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel_size);
 
 /**
  * Do a FAST9 corner detection
@@ -57,12 +57,12 @@ struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *n
     pixel_size = 2;
 
   // Calculate the pixel offsets
-  fast_make_offsets(pixel, img->w*pixel_size);
+  fast_make_offsets(pixel, img->w, pixel_size);
 
   // Go trough all the pixels (minus the borders)
   for (y = 3; y < img->h - 3; y++)
     for (x = 3; x < img->w - 3; x++) {
-      const uint8_t *p = (uint8_t *)img->buf + y * img->w * pixel_size + x + pixel_size/2;
+      const uint8_t *p = ((uint8_t *)img->buf) + y * img->w * pixel_size + x * pixel_size + pixel_size/2;
 
       // Calculate the threshold values
       int16_t cb = *p + threshold;
@@ -3618,22 +3618,22 @@ struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *n
  * @param[out] *pixel The offset array of the different pixels
  * @param[in] row_stride The row stride in the image
  */
-static void fast_make_offsets(int32_t *pixel, uint16_t row_stride)
+static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel_size)
 {
-  pixel[0] = 0 + row_stride * 3;
-  pixel[1] = 1 + row_stride * 3;
-  pixel[2] = 2 + row_stride * 2;
-  pixel[3] = 3 + row_stride * 1;
-  pixel[4] = 3 + row_stride * 0;
-  pixel[5] = 3 + row_stride * -1;
-  pixel[6] = 2 + row_stride * -2;
-  pixel[7] = 1 + row_stride * -3;
-  pixel[8] = 0 + row_stride * -3;
-  pixel[9] = -1 + row_stride * -3;
-  pixel[10] = -2 + row_stride * -2;
-  pixel[11] = -3 + row_stride * -1;
-  pixel[12] = -3 + row_stride * 0;
-  pixel[13] = -3 + row_stride * 1;
-  pixel[14] = -2 + row_stride * 2;
-  pixel[15] = -1 + row_stride * 3;
+  pixel[0]  = 0*pixel_size  + row_stride * 3*pixel_size;
+  pixel[1]  = 1*pixel_size  + row_stride * 3*pixel_size;
+  pixel[2]  = 2*pixel_size  + row_stride * 2*pixel_size;
+  pixel[3]  = 3*pixel_size  + row_stride * 1*pixel_size;
+  pixel[4]  = 3*pixel_size  + row_stride * 0*pixel_size;
+  pixel[5]  = 3*pixel_size  + row_stride * -1*pixel_size;
+  pixel[6]  = 2*pixel_size  + row_stride * -2*pixel_size;
+  pixel[7]  = 1*pixel_size  + row_stride * -3*pixel_size;
+  pixel[8]  = 0*pixel_size  + row_stride * -3*pixel_size;
+  pixel[9]  = -1*pixel_size + row_stride * -3*pixel_size;
+  pixel[10] = -2*pixel_size + row_stride * -2*pixel_size;
+  pixel[11] = -3*pixel_size + row_stride * -1*pixel_size;
+  pixel[12] = -3*pixel_size + row_stride * 0*pixel_size;
+  pixel[13] = -3*pixel_size + row_stride * 1*pixel_size;
+  pixel[14] = -2*pixel_size + row_stride * 2*pixel_size;
+  pixel[15] = -1*pixel_size + row_stride * 3*pixel_size;
 }

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

@@ -77,7 +77,7 @@ void image_copy(struct image_t *input, struct image_t *output)
   output->h = input->h;
   output->buf_size = input->buf_size;
   memcpy(&output->ts, &input->ts, sizeof(struct timeval));
-  memcpy(input->buf, output->buf, input->buf_size);
+  memcpy(output->buf, input->buf, input->buf_size);
 }
 
 /**

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

@@ -98,9 +98,12 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
   memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval));
 
-  // Conver the first image to gray
+  // Convert image to grayscale
+  image_to_grayscale(img, &opticflow->img_gray);
+
+  // Copy to previous image if not set
   if (!opticflow->got_first_img) {
-    image_to_grayscale(img, &opticflow->prev_img_gray);
+    image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
     opticflow->got_first_img = TRUE;
   }
 
@@ -109,7 +112,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   // *************************************************************************************
 
   // FAST corner detection (TODO: non fixed threashold)
-  struct point_t *pnts_fast = fast9_detect(&opticflow->prev_img_gray, 5, &result->corner_cnt);
+  struct point_t *pnts_fast = fast9_detect(img, 20, &result->corner_cnt);
 
   /*// Copy the points and remove neighboring corners
   const float min_distance2 = 10 * 10;
@@ -136,15 +139,17 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   result->count = count_fil;*/
 
 
+  image_to_grayscale(img, img);
   uint8_t *im = (uint8_t *)img->buf;
   for(int i = 0; i < result->corner_cnt; i++) {
-    uint16_t idx = 2*pnts_fast[i].y*opticflow->img_w + 2*pnts_fast[i].x;
+    uint32_t idx = 2*pnts_fast[i].y*opticflow->img_w + pnts_fast[i].x*2;
     im[idx] = 255;
-    idx = idx+1 % (opticflow->img_w*opticflow->img_h*2);
+    /*idx = idx+1 % (opticflow->img_w*opticflow->img_h*2);
     im[idx] = 255;
     idx = idx+1 % (opticflow->img_w*opticflow->img_h*2);
-    im[idx] = 255;
+    im[idx] = 255;*/
   }
+  free(pnts_fast);
 
   // *************************************************************************************
   // Corner Tracking