Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
cv_blob_locator.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
31 
32 
35 
38 
41 
44 
46 int marker_size = 18;
47 int record_video = 0;
48 
49 volatile uint32_t blob_locator = 0;
50 
51 volatile bool blob_enabled = false;
52 volatile bool marker_enabled = false;
53 volatile bool window_enabled = false;
54 
55 // Computer vision thread
56 struct image_t* cv_marker_func(struct image_t *img);
57 struct image_t* cv_marker_func(struct image_t *img) {
58 
59  if (!marker_enabled)
60  return NULL;
61 
62  struct marker_deviation_t m = marker(img, marker_size);
63 
64  uint32_t temp = m.x;
65  temp = temp << 16;
66  temp += m.y;
67  blob_locator = temp;
68 
69  return NULL;
70 }
71 
72 #define Img(X,Y)(((uint8_t*)img->buf)[(Y)*img->w*2+(X)*2])
73 
74 
75 // Computer vision thread
76 struct image_t* cv_window_func(struct image_t *img);
77 struct image_t* cv_window_func(struct image_t *img) {
78 
79  if (!window_enabled)
80  return NULL;
81 
82 
83  uint16_t coordinate[2] = {0,0};
84  uint16_t response = 0;
85  uint32_t integral_image[img->w * img->h];
86 
87  struct image_t gray;
88  image_create(&gray, img->w, img->h, IMAGE_GRAYSCALE);
89  image_to_grayscale(img, &gray);
90 
91  response = detect_window_sizes( (uint8_t*)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT);
92 
93  image_free(&gray);
94 
95  // Display the marker location and center-lines.
96  int px = coordinate[0] & 0xFFFe;
97  int py = coordinate[1] & 0xFFFe;
98 
99  if (response < 92) {
100 
101  for (int y = 0; y < img->h-1; y++) {
102  Img(px, y) = 65;
103  Img(px+1, y) = 255;
104  }
105  for (int x = 0; x < img->w-1; x+=2) {
106  Img(x, py) = 65;
107  Img(x+1, py) = 255;
108  }
109 
110  uint32_t temp = coordinate[0];
111  temp = temp << 16;
112  temp += coordinate[1];
113  blob_locator = temp;
114 
115  }
116 
117  return NULL;
118 }
119 
120 
121 struct image_t* cv_blob_locator_func(struct image_t *img);
122 struct image_t* cv_blob_locator_func(struct image_t *img) {
123 
124  if (!blob_enabled)
125  return NULL;
126 
127 
128  // Color Filter
129  struct image_filter_t filter[2];
130  filter[0].y_min = color_lum_min;
131  filter[0].y_max = color_lum_max;
132  filter[0].u_min = color_cb_min;
133  filter[0].u_max = color_cb_max;
134  filter[0].v_min = color_cr_min;
135  filter[0].v_max = color_cr_max;
136 
137  // Output image
138  struct image_t dst;
139  image_create(&dst,
140  img->w,
141  img->h,
143 
144  // Labels
145  uint16_t labels_count = 512;
146  struct image_label_t labels[512];
147 
148  // Blob finder
149  image_labeling(img, &dst, filter, 1, labels, &labels_count);
150 
151  int largest_id = -1;
152  int largest_size = 0;
153 
154  // Find largest
155  for (int i=0; i<labels_count; i++) {
156  // Only consider large blobs
157  if (labels[i].pixel_cnt > 50) {
158  if (labels[i].pixel_cnt > largest_size) {
159  largest_size = labels[i].pixel_cnt;
160  largest_id = i;
161  }
162  }
163  }
164 
165  if (largest_id >= 0)
166  {
167  uint8_t *p = (uint8_t*) img->buf;
168  uint16_t* l = (uint16_t*) dst.buf;
169  for (int y=0;y<dst.h;y++) {
170  for (int x=0;x<dst.w/2;x++) {
171  if (l[y*dst.w+x] != 0xffff) {
172  uint8_t c=0xff;
173  if (l[y*dst.w+x] == largest_id) {
174  c = 0;
175  }
176  p[y*dst.w*2+x*4]=c;
177  p[y*dst.w*2+x*4+1]=0x80;
178  p[y*dst.w*2+x*4+2]=c;
179  p[y*dst.w*2+x*4+3]=0x80;
180  }
181  }
182  }
183 
184 
185  uint16_t cgx = labels[largest_id].x_sum / labels[largest_id].pixel_cnt * 2;
186  uint16_t cgy = labels[largest_id].y_sum / labels[largest_id].pixel_cnt;
187 
188  if ((cgx > 1) && (cgx < (dst.w-2)) &&
189  (cgy > 1) && (cgy < (dst.h-2))
190  ) {
191  p[cgy*dst.w*2+cgx*2-4] = 0xff;
192  p[cgy*dst.w*2+cgx*2-2] = 0x00;
193  p[cgy*dst.w*2+cgx*2] = 0xff;
194  p[cgy*dst.w*2+cgx*2+2] = 0x00;
195  p[cgy*dst.w*2+cgx*2+4] = 0xff;
196  p[cgy*dst.w*2+cgx*2+6] = 0x00;
197  p[(cgy-1)*dst.w*2+cgx*2] = 0xff;
198  p[(cgy-1)*dst.w*2+cgx*2+2] = 0x00;
199  p[(cgy+1)*dst.w*2+cgx*2] = 0xff;
200  p[(cgy+1)*dst.w*2+cgx*2+2] = 0x00;
201  }
202 
203 
204  uint32_t temp = cgx;
205  temp = temp << 16;
206  temp += cgy;
207  blob_locator = temp;
208  }
209 
210  image_free(&dst);
211 
212  return NULL; // No new image is available for follow up modules
213 }
214 
216 #include "generated/flight_plan.h"
217 #include <stdio.h>
218 
219 
221  // Red board in sunlight
222  color_lum_min = 100;
223  color_lum_max = 200;
224  color_cb_min = 140;
225  color_cb_max = 255;
226  color_cr_min = 140;
227  color_cr_max = 255;
228 
229  // Lamp during night
230  color_lum_min = 180;
231  color_lum_max = 255;
232  color_cb_min = 100;
233  color_cb_max = 150;
234  color_cr_min = 100;
235  color_cr_max = 150;
236 
238 
240 
241  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func);
242  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func);
243  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func);
244 }
245 
247 
248 }
249 
250 
251 
253  switch (cv_blob_locator_type)
254  {
255  case 1:
256  blob_enabled = true;
257  marker_enabled = false;
258  window_enabled = false;
259  break;
260  case 2:
261  blob_enabled = false;
262  marker_enabled = true;
263  window_enabled = false;
264  break;
265  case 3:
266  blob_enabled = false;
267  marker_enabled = false;
268  window_enabled = true;
269  break;
270  default:
271  blob_enabled = false;
272  marker_enabled = false;
273  window_enabled = false;
274  break;
275  }
276  if (blob_locator != 0) {
277  // CV thread has results: import
278  uint32_t temp = blob_locator;
279  blob_locator = 0;
280 
281  // Process
282  uint16_t y = temp & 0x0000ffff;
283  temp = temp >> 16;
284  uint16_t x = temp & 0x0000ffff;
285  printf("Found %d %d \n",x,y);
286 
287  struct camera_frame_t cam;
288  cam.px = x/2;
289  cam.py = y/2;
290  cam.f = 400;
291  cam.h = 240;
292  cam.w = 320;
293 
294 #ifdef WP_p1
295  georeference_project(&cam, WP_p1);
296 #endif
297 #ifdef WP_CAM
299 #endif
300 
301  }
302 }
303 
304 extern void cv_blob_locator_start(void) {
306 }
307 
308 extern void cv_blob_locator_stop(void) {
309 
310 }
311 
312 void start_vision(void) {
314  record_video = 1;
316 }
317 void start_vision_land(void) {
319  record_video = 1;
321 }
322 void stop_vision(void) {
324  record_video = 0;
326 }
uint8_t y_max
Definition: blob_finder.h:35
uint32_t x_sum
Sum of all x coordinates (used to find center of gravity)
Definition: blob_finder.h:50
uint8_t u_max
Definition: blob_finder.h:37
unsigned short uint16_t
Definition: types.h:16
uint8_t color_cr_max
uint8_t v_max
Definition: blob_finder.h:39
int32_t px
Target pixel coordinate (left = 0)
void cv_blob_locator_init(void)
uint8_t cv_blob_locator_reset
uint8_t y_min
YUV color filter.
Definition: blob_finder.h:34
#define MODE_BRIGHT
Definition: detect_window.h:33
void stop_vision(void)
volatile bool window_enabled
void cv_blob_locator_stop(void)
uint8_t color_cr_min
uint16_t detect_window_sizes(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate, uint32_t *integral_image, uint8_t MODE)
Definition: detect_window.c:60
void image_free(struct image_t *img)
Free the image.
Definition: image.c:63
struct image_t * cv_marker_func(struct image_t *img)
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func)
Definition: cv.c:45
uint8_t u_min
Definition: blob_finder.h:36
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:38
Definition: image.h:42
struct image_t * cv_blob_locator_func(struct image_t *img)
uint8_t color_cb_max
void georeference_init(void)
#define FALSE
Definition: std.h:5
int geofilter_length
uint16_t pixel_cnt
Number of pixels in the blob.
Definition: blob_finder.h:47
uint8_t color_lum_max
void georeference_project(struct camera_frame_t *tar, int wp)
void cv_blob_locator_periodic(void)
void georeference_filter(bool kalman, int wp, int length)
void start_vision(void)
int32_t py
Target pixel coordinate (top = 0)
struct image_t * cv_window_func(struct image_t *img)
uint16_t w
Image width.
Definition: image.h:44
unsigned long uint32_t
Definition: types.h:18
Computer vision framework for onboard processing.
uint16_t h
Image height.
Definition: image.h:45
uint8_t cv_blob_locator_type
void * buf
Image buffer (depending on the image_type)
Definition: image.h:51
volatile uint32_t blob_locator
Parse UYVY images and make a list of blobs of connected pixels.
int record_video
uint8_t v_min
Definition: blob_finder.h:38
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition: image.c:116
void start_vision_land(void)
int32_t h
Frame height [px].
unsigned char uint8_t
Definition: types.h:14
void cv_blob_locator_event(void)
int32_t f
Camera Focal length in [px].
uint8_t color_lum_min
uint32_t y_sum
Definition: blob_finder.h:51
int32_t w
Frame width [px].
static float p[2][2]
#define Img(X, Y)
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:36
int marker_size
uint8_t color_cb_min
void cv_blob_locator_start(void)
Find a IMAV pattern.
An image gradient (int16 per pixel)
Definition: image.h:38
void image_labeling(struct image_t *input, struct image_t *output, struct image_filter_t *filters, uint8_t filters_cnt, struct image_label_t *labels, uint16_t *labels_count)
Definition: blob_finder.c:33
struct marker_deviation_t marker(struct image_t *input, uint8_t M)
Definition: imavmarker.c:51
Detect a bright region surrounded by dark or viceversa - sometimes this corresponds to a window...
volatile bool marker_enabled
volatile bool blob_enabled