[vision] Better FAST9 corner detection

conf/modules/cv_opticflow.xml

@@ -64,6 +64,25 @@
     <flag name="LDFLAGS" value="lrt"/>
     <flag name="LDFLAGS" value="static-libgcc"/>
     <define name="OPTICFLOW_DEBUG"/>
+
+    <raw>
+      VIEWVIDEO_DEV         ?= UDP1
+      VIEWVIDEO_HOST        ?= $(MODEM_HOST)
+      VIEWVIDEO_PORT_OUT    ?= 5000
+      VIEWVIDEO_PORT_IN     ?= 4999
+      VIEWVIDEO_BROADCAST   ?= $(MODEM_BROADCAST)
+      VIEWVIDEO_DEV_LOWER   = $(shell echo $(VIEWVIDEO_DEV) | tr A-Z a-z)
+
+      VIEWVID_G_CFLAGS  = -DVIEWVIDEO_HOST=\"$(VIEWVIDEO_HOST)\" -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT)
+      VIEWVID_CFLAGS  = -DUSE_$(VIEWVIDEO_DEV) -DVIEWVIDEO_DEV=$(VIEWVIDEO_DEV_LOWER)
+      VIEWVID_CFLAGS += -D$(VIEWVIDEO_DEV)_PORT_OUT=$(VIEWVIDEO_PORT_OUT) -D$(VIEWVIDEO_DEV)_PORT_IN=$(VIEWVIDEO_PORT_IN)
+      VIEWVID_CFLAGS += -D$(VIEWVIDEO_DEV)_BROADCAST=$(VIEWVIDEO_BROADCAST) -D$(VIEWVIDEO_DEV)_HOST=\"$(VIEWVIDEO_HOST)\"
+      ifeq ($(VIEWVIDEO_USE_NC),)
+        ap.CFLAGS += $(VIEWVID_G_CFLAGS) $(VIEWVID_CFLAGS)
+      else
+        ap.CFLAGS += $(VIEWVID_G_CFLAGS) -DVIEWVIDEO_USE_NC
+      endif
+    </raw>
   </makefile>
 
   <makefile target="nps">

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

@@ -40,15 +40,16 @@ static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel
  * Do a FAST9 corner detection
  * @param[in] *img The image to do the corner detection on
  * @param[in] threshold The threshold which we use for FAST9
+ * @param[in] min_dist The minimum distance in pixels between detections
  * @param[out] *num_corner The amount of corners found
  * @return The corners found
  */
-struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *num_corners)
+struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint32_t *num_corners)
 {
-  int corner_cnt = 0;
-  int rsize = 512;
+  uint32_t corner_cnt = 0;
+  uint16_t rsize = 512;
   int pixel[16];
-  int x, y;
+  uint16_t x, y, i;
   struct point_t *ret_corners = malloc(sizeof(struct point_t) * rsize);
 
   // Set the pixel size
@@ -62,6 +63,21 @@ struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *n
   // Go trough all the pixels (minus the borders)
   for (y = 3; y < img->h - 3; y++)
     for (x = 3; x < img->w - 3; x++) {
+      // First check if we aren't in range vertical (TODO: fix less intensive way)
+      bool_t need_skip = FALSE;
+      for(i = 0; i < corner_cnt; i++) {
+        if(x-min_dist < ret_corners[i].x && ret_corners[i].x < x+min_dist
+          && y-min_dist < ret_corners[i].y && ret_corners[i].y < y+min_dist) {
+          need_skip = TRUE;
+          break;
+        }
+      }
+
+      if(need_skip) {
+        x += min_dist;
+        continue;
+      }
+
       const uint8_t *p = ((uint8_t *)img->buf) + y * img->w * pixel_size + x * pixel_size + pixel_size/2;
 
       // Calculate the threshold values
@@ -3607,6 +3623,8 @@ struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *n
       ret_corners[corner_cnt].y = y;
       corner_cnt++;
 
+      // Skip some in the width direction
+      x += min_dist;
     }
 
   *num_corners = corner_cnt;

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

@@ -37,6 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "std.h"
 #include "lib/vision/image.h"
 
-struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *num_corners);
+struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint32_t *num_corners);
 
 #endif

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

@@ -112,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(img, 20, &result->corner_cnt);
+  struct point_t *pnts_fast = fast9_detect(img, 20, 10, &result->corner_cnt);
 
   /*// Copy the points and remove neighboring corners
   const float min_distance2 = 10 * 10;

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -35,6 +35,7 @@
 
 #include "lib/v4l/v4l2.h"
 #include "lib/encoding/jpeg.h"
+#include "lib/encoding/rtp.h"
 
 /* default sonar/agl to use in opticflow visual_estimator */
 #ifndef OPTICFLOW_AGL_ID
@@ -170,31 +171,15 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
     pthread_mutex_unlock(&opticflow_mutex);
 
 #ifdef OPTICFLOW_DEBUG
-    jpeg_encode_image(&img, &img_jpeg, 99, TRUE);
-
-    // Open process to send using netcat (in a fork because sometimes kills itself???)
-    pid_t pid = fork();
-
-    if(pid < 0) {
-      printf("[viewvideo] Could not create netcat fork.\n");
-    }
-    else if(pid ==0) {
-      // We are the child and want to send the image
-      FILE *netcat = popen("nc 192.168.1.2 5000 2>/dev/null", "w");
-      if (netcat != NULL) {
-        fwrite(img_jpeg.buf, sizeof(uint8_t), img_jpeg.buf_size, netcat);
-        pclose(netcat); // Ignore output, because it is too much when not connected
-      } else {
-        printf("[viewvideo] Failed to open netcat process.\n");
-      }
-
-      // Exit the program since we don't want to continue after transmitting
-      exit(0);
-    }
-    else {
-      // We want to wait until the child is finished
-      wait(NULL);
-    }
+    jpeg_encode_image(&img, &img_jpeg, 50, FALSE);
+    rtp_frame_send(
+      &VIEWVIDEO_DEV,           // UDP device
+      &img_jpeg,
+      0,                        // Format 422
+      50, // Jpeg-Quality
+      0,                        // DRI Header
+      0                         // 90kHz time increment
+    );
 #endif
 
     // Free the image