cv_blob_locator.c

Go to the documentation of this file.

/*
 * Copyright (C) 2015
 *
 * 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/>.
 */
#include "modules/computer_vision/cv_blob_locator.h"
#include "modules/computer_vision/cv.h"
#include "modules/computer_vision/blob/blob_finder.h"
#include "modules/computer_vision/blob/imavmarker.h"
#include "modules/computer_vision/detect_window.h"


uint8_t color_lum_min;
uint8_t color_lum_max;

uint8_t color_cb_min;
uint8_t color_cb_max;

uint8_t color_cr_min;
uint8_t color_cr_max;

uint8_t cv_blob_locator_reset;
uint8_t cv_blob_locator_type;

int geofilter_length = 5;
int marker_size = 18;
int record_video = 0;

volatile uint32_t blob_locator = 0;

volatile bool blob_enabled = false;
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) {

  if (!marker_enabled)
    return NULL;

  struct marker_deviation_t m = marker(img, marker_size);

  uint32_t temp = m.x;
  temp = temp << 16;
  temp += m.y;
  blob_locator = temp;

  return NULL;
}

#define Img(X,Y)(((uint8_t*)img->buf)[(Y)*img->w*2+(X)*2])


// Computer vision thread
struct image_t* cv_window_func(struct image_t *img);
struct image_t* cv_window_func(struct image_t *img) {

  if (!window_enabled)
    return NULL;


  uint16_t coordinate[2] = {0,0};
  uint16_t response = 0;
  uint32_t integral_image[img->w * img->h];

  struct image_t gray;
  image_create(&gray, img->w, img->h, IMAGE_GRAYSCALE);
  image_to_grayscale(img, &gray);

  response = detect_window_sizes( (uint8_t*)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT);

  image_free(&gray);

  // Display the marker location and center-lines.
  int px = coordinate[0] & 0xFFFe;
  int py = coordinate[1] & 0xFFFe;

  if (response < 92) {

    for (int y = 0; y < img->h-1; y++) {
      Img(px, y)   = 65;
      Img(px+1, y) = 255;
    }
    for (int x = 0; x < img->w-1; x+=2) {
      Img(x, py)   = 65;
      Img(x+1, py) = 255;
    }

    uint32_t temp = coordinate[0];
    temp = temp << 16;
    temp += coordinate[1];
    blob_locator = temp;

  }

  return NULL;
}


struct image_t* cv_blob_locator_func(struct image_t *img);
struct image_t* cv_blob_locator_func(struct image_t *img) {

  if (!blob_enabled)
    return NULL;


  // Color Filter
  struct image_filter_t filter[2];
  filter[0].y_min = color_lum_min;
  filter[0].y_max = color_lum_max;
  filter[0].u_min = color_cb_min;
  filter[0].u_max = color_cb_max;
  filter[0].v_min = color_cr_min;
  filter[0].v_max = color_cr_max;

  // Output image
  struct image_t dst;
  image_create(&dst,
                 img->w,
                 img->h,
                 IMAGE_GRADIENT);

  // Labels
  uint16_t labels_count = 512;
  struct image_label_t labels[512];

  // Blob finder
  image_labeling(img, &dst, filter, 1, labels, &labels_count);

  int largest_id = -1;
  int largest_size = 0;

  // Find largest
  for (int i=0; i<labels_count; i++) {
    // Only consider large blobs
    if (labels[i].pixel_cnt > 50) {
      if (labels[i].pixel_cnt > largest_size) {
        largest_size = labels[i].pixel_cnt;
        largest_id = i;
      }
    }
  }

  if (largest_id >= 0)
  {
    uint8_t *p = (uint8_t*) img->buf;
    uint16_t* l = (uint16_t*) dst.buf;
    for (int y=0;y<dst.h;y++) {
      for (int x=0;x<dst.w/2;x++) {
        if (l[y*dst.w+x] != 0xffff) {
          uint8_t c=0xff;
          if (l[y*dst.w+x] == largest_id) {
            c = 0;
          }
          p[y*dst.w*2+x*4]=c;
          p[y*dst.w*2+x*4+1]=0x80;
          p[y*dst.w*2+x*4+2]=c;
          p[y*dst.w*2+x*4+3]=0x80;
        }
      }
    }


    uint16_t cgx = labels[largest_id].x_sum / labels[largest_id].pixel_cnt * 2;
    uint16_t cgy = labels[largest_id].y_sum / labels[largest_id].pixel_cnt;

    if ((cgx > 1) && (cgx < (dst.w-2)) &&
        (cgy > 1) && (cgy < (dst.h-2))
        ) {
      p[cgy*dst.w*2+cgx*2-4] = 0xff;
      p[cgy*dst.w*2+cgx*2-2] = 0x00;
      p[cgy*dst.w*2+cgx*2] = 0xff;
      p[cgy*dst.w*2+cgx*2+2] = 0x00;
      p[cgy*dst.w*2+cgx*2+4] = 0xff;
      p[cgy*dst.w*2+cgx*2+6] = 0x00;
      p[(cgy-1)*dst.w*2+cgx*2] = 0xff;
      p[(cgy-1)*dst.w*2+cgx*2+2] = 0x00;
      p[(cgy+1)*dst.w*2+cgx*2] = 0xff;
      p[(cgy+1)*dst.w*2+cgx*2+2] = 0x00;
    }


    uint32_t temp = cgx;
    temp = temp << 16;
    temp += cgy;
    blob_locator = temp;
  }

  image_free(&dst);

  return NULL; // No new image is available for follow up modules
}

#include "modules/computer_vision/cv_georeference.h"
#include "generated/flight_plan.h"
#include <stdio.h>


void cv_blob_locator_init(void) {
  // Red board in sunlight
  color_lum_min = 100;
  color_lum_max = 200;
  color_cb_min = 140;
  color_cb_max = 255;
  color_cr_min = 140;
  color_cr_max = 255;

  // Lamp during night
  color_lum_min = 180;
  color_lum_max = 255;
  color_cb_min = 100;
  color_cb_max = 150;
  color_cr_min = 100;
  color_cr_max = 150;

  cv_blob_locator_reset = 0;

  georeference_init();

  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func);
  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func);
  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func);
}

void cv_blob_locator_periodic(void) {

}



void cv_blob_locator_event(void) {
  switch (cv_blob_locator_type)
  {
  case 1:
    blob_enabled = true;
    marker_enabled = false;
    window_enabled = false;
    break;
  case 2:
    blob_enabled = false;
    marker_enabled = true;
    window_enabled = false;
    break;
  case 3:
    blob_enabled = false;
    marker_enabled = false;
    window_enabled = true;
    break;
  default:
    blob_enabled = false;
    marker_enabled = false;
    window_enabled = false;
    break;
  }
  if (blob_locator != 0) {
    // CV thread has results: import
    uint32_t temp = blob_locator;
    blob_locator = 0;

    // Process
    uint16_t y = temp & 0x0000ffff;
    temp = temp >> 16;
    uint16_t x = temp & 0x0000ffff;
    printf("Found %d %d \n",x,y);

    struct camera_frame_t cam;
    cam.px = x/2;
    cam.py = y/2;
    cam.f = 400;
    cam.h = 240;
    cam.w = 320;

#ifdef WP_p1
    georeference_project(&cam, WP_p1);
#endif
#ifdef WP_CAM
    georeference_filter(FALSE,WP_CAM, geofilter_length);
#endif

  }
}

extern void cv_blob_locator_start(void) {
  georeference_init();
}

extern void cv_blob_locator_stop(void) {

}

void start_vision(void) {
  georeference_init();
  record_video = 1;
  cv_blob_locator_type = 3;
}
void start_vision_land(void) {
  georeference_init();
  record_video = 1;
  cv_blob_locator_type = 2;
}
void stop_vision(void) {
  georeference_init();
  record_video = 0;
  cv_blob_locator_type = 0;
}