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"
 
#ifndef BLOB_LOCATOR_FPS
#define BLOB_LOCATOR_FPS 0       
#endif
PRINT_CONFIG_VAR(BLOB_LOCATOR_FPS)
 
 
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, uint8_t camera_id);
struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id)
{
 
  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, uint8_t camera_id);
struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id)
{
 
  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, uint8_t camera_id);
struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id)
{
 
  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, BLOB_LOCATOR_FPS, 0);
  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS, 0);
  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS, 0);
}
 
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;
}