Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 #ifndef BLOB_LOCATOR_FPS
33 #define BLOB_LOCATOR_FPS 0
34 #endif
35 PRINT_CONFIG_VAR(BLOB_LOCATOR_FPS)
36 
37 
40 
43 
46 
49 
51 int marker_size = 18;
52 int record_video = 0;
53 
54 volatile uint32_t blob_locator = 0;
55 
56 volatile bool blob_enabled = false;
57 volatile bool marker_enabled = false;
58 volatile bool window_enabled = false;
59 
60 // Computer vision thread
61 struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id);
62 struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id)
63 {
64 
65  if (!marker_enabled) {
66  return NULL;
67  }
68 
69  struct marker_deviation_t m = marker(img, marker_size);
70 
71  uint32_t temp = m.x;
72  temp = temp << 16;
73  temp += m.y;
74  blob_locator = temp;
75 
76  return NULL;
77 }
78 
79 #define Img(X,Y)(((uint8_t*)img->buf)[(Y)*img->w*2+(X)*2])
80 
81 
82 // Computer vision thread
83 struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id);
84 struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id)
85 {
86 
87  if (!window_enabled) {
88  return NULL;
89  }
90 
91 
92  uint16_t coordinate[2] = {0, 0};
93  uint16_t response = 0;
94  uint32_t integral_image[img->w * img->h];
95 
96  struct image_t gray;
97  image_create(&gray, img->w, img->h, IMAGE_GRAYSCALE);
98  image_to_grayscale(img, &gray);
99 
100  response = detect_window_sizes((uint8_t *)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT);
101 
102  image_free(&gray);
103 
104  // Display the marker location and center-lines.
105  int px = coordinate[0] & 0xFFFe;
106  int py = coordinate[1] & 0xFFFe;
107 
108  if (response < 92) {
109 
110  for (int y = 0; y < img->h - 1; y++) {
111  Img(px, y) = 65;
112  Img(px + 1, y) = 255;
113  }
114  for (int x = 0; x < img->w - 1; x += 2) {
115  Img(x, py) = 65;
116  Img(x + 1, py) = 255;
117  }
118 
119  uint32_t temp = coordinate[0];
120  temp = temp << 16;
121  temp += coordinate[1];
122  blob_locator = temp;
123 
124  }
125 
126  return NULL;
127 }
128 
129 
130 struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id);
131 struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id)
132 {
133 
134  if (!blob_enabled) {
135  return NULL;
136  }
137 
138 
139  // Color Filter
140  struct image_filter_t filter[2];
141  filter[0].y_min = color_lum_min;
142  filter[0].y_max = color_lum_max;
143  filter[0].u_min = color_cb_min;
144  filter[0].u_max = color_cb_max;
145  filter[0].v_min = color_cr_min;
146  filter[0].v_max = color_cr_max;
147 
148  // Output image
149  struct image_t dst;
150  image_create(&dst,
151  img->w,
152  img->h,
154 
155  // Labels
156  uint16_t labels_count = 512;
157  struct image_label_t labels[512];
158 
159  // Blob finder
160  image_labeling(img, &dst, filter, 1, labels, &labels_count);
161 
162  int largest_id = -1;
163  int largest_size = 0;
164 
165  // Find largest
166  for (int i = 0; i < labels_count; i++) {
167  // Only consider large blobs
168  if (labels[i].pixel_cnt > 50) {
169  if (labels[i].pixel_cnt > largest_size) {
170  largest_size = labels[i].pixel_cnt;
171  largest_id = i;
172  }
173  }
174  }
175 
176  if (largest_id >= 0) {
177  uint8_t *p = (uint8_t *) img->buf;
178  uint16_t *l = (uint16_t *) dst.buf;
179  for (int y = 0; y < dst.h; y++) {
180  for (int x = 0; x < dst.w / 2; x++) {
181  if (l[y * dst.w + x] != 0xffff) {
182  uint8_t c = 0xff;
183  if (l[y * dst.w + x] == largest_id) {
184  c = 0;
185  }
186  p[y * dst.w * 2 + x * 4] = c;
187  p[y * dst.w * 2 + x * 4 + 1] = 0x80;
188  p[y * dst.w * 2 + x * 4 + 2] = c;
189  p[y * dst.w * 2 + x * 4 + 3] = 0x80;
190  }
191  }
192  }
193 
194 
195  uint16_t cgx = labels[largest_id].x_sum / labels[largest_id].pixel_cnt * 2;
196  uint16_t cgy = labels[largest_id].y_sum / labels[largest_id].pixel_cnt;
197 
198  if ((cgx > 1) && (cgx < (dst.w - 2)) &&
199  (cgy > 1) && (cgy < (dst.h - 2))
200  ) {
201  p[cgy * dst.w * 2 + cgx * 2 - 4] = 0xff;
202  p[cgy * dst.w * 2 + cgx * 2 - 2] = 0x00;
203  p[cgy * dst.w * 2 + cgx * 2] = 0xff;
204  p[cgy * dst.w * 2 + cgx * 2 + 2] = 0x00;
205  p[cgy * dst.w * 2 + cgx * 2 + 4] = 0xff;
206  p[cgy * dst.w * 2 + cgx * 2 + 6] = 0x00;
207  p[(cgy - 1)*dst.w * 2 + cgx * 2] = 0xff;
208  p[(cgy - 1)*dst.w * 2 + cgx * 2 + 2] = 0x00;
209  p[(cgy + 1)*dst.w * 2 + cgx * 2] = 0xff;
210  p[(cgy + 1)*dst.w * 2 + cgx * 2 + 2] = 0x00;
211  }
212 
213 
214  uint32_t temp = cgx;
215  temp = temp << 16;
216  temp += cgy;
217  blob_locator = temp;
218  }
219 
220  image_free(&dst);
221 
222  return NULL; // No new image is available for follow up modules
223 }
224 
226 #include "generated/flight_plan.h"
227 #include <stdio.h>
228 
229 
231 {
232  // Red board in sunlight
233  color_lum_min = 100;
234  color_lum_max = 200;
235  color_cb_min = 140;
236  color_cb_max = 255;
237  color_cr_min = 140;
238  color_cr_max = 255;
239 
240  // Lamp during night
241  color_lum_min = 180;
242  color_lum_max = 255;
243  color_cb_min = 100;
244  color_cb_max = 150;
245  color_cr_min = 100;
246  color_cr_max = 150;
247 
249 
251 
252  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS, 0);
253  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS, 0);
254  cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS, 0);
255 }
256 
258 {
259 
260 }
261 
262 
263 
265 {
266  switch (cv_blob_locator_type) {
267  case 1:
268  blob_enabled = true;
269  marker_enabled = false;
270  window_enabled = false;
271  break;
272  case 2:
273  blob_enabled = false;
274  marker_enabled = true;
275  window_enabled = false;
276  break;
277  case 3:
278  blob_enabled = false;
279  marker_enabled = false;
280  window_enabled = true;
281  break;
282  default:
283  blob_enabled = false;
284  marker_enabled = false;
285  window_enabled = false;
286  break;
287  }
288  if (blob_locator != 0) {
289  // CV thread has results: import
290  uint32_t temp = blob_locator;
291  blob_locator = 0;
292 
293  // Process
294  uint16_t y = temp & 0x0000ffff;
295  temp = temp >> 16;
296  uint16_t x = temp & 0x0000ffff;
297  printf("Found %d %d \n", x, y);
298 
299  struct camera_frame_t cam;
300  cam.px = x / 2;
301  cam.py = y / 2;
302  cam.f = 400;
303  cam.h = 240;
304  cam.w = 320;
305 
306 #ifdef WP_p1
307  georeference_project(&cam, WP_p1);
308 #endif
309 #ifdef WP_CAM
311 #endif
312 
313  }
314 }
315 
316 extern void cv_blob_locator_start(void)
317 {
319 }
320 
321 extern void cv_blob_locator_stop(void)
322 {
323 
324 }
325 
326 void start_vision(void)
327 {
329  record_video = 1;
331 }
333 {
335  record_video = 1;
337 }
338 void stop_vision(void)
339 {
341  record_video = 0;
343 }
image_label_t::filter
uint8_t filter
Which filter triggered this blob.
Definition: blob_finder.h:48
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
image_label_t::y_sum
uint32_t y_sum
Definition: blob_finder.h:54
blob_finder.h
uint16_t
unsigned short uint16_t
Definition: types.h:16
image_filter_t::u_min
uint8_t u_min
Definition: blob_finder.h:39
image_create
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
cv_blob_locator_type
uint8_t cv_blob_locator_type
Definition: cv_blob_locator.c:48
camera_frame_t
Definition: cv_georeference.h:37
blob_enabled
volatile bool blob_enabled
Definition: cv_blob_locator.c:56
cv_blob_locator_init
void cv_blob_locator_init(void)
Definition: cv_blob_locator.c:230
cv_blob_locator.h
color_lum_max
uint8_t color_lum_max
Definition: cv_blob_locator.c:39
image_labeling
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
cv_georeference.h
color_cr_max
uint8_t color_cr_max
Definition: cv_blob_locator.c:45
cv_marker_func
struct image_t * cv_marker_func(struct image_t *img, uint8_t camera_id)
Definition: cv_blob_locator.c:62
marker_size
int marker_size
Definition: cv_blob_locator.c:51
marker_deviation_t::x
int32_t x
Definition: imavmarker.h:33
cv_add_to_device
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Definition: cv.c:46
detect_window.h
georeference_filter
void georeference_filter(bool kalman, int wp, int length)
Definition: cv_georeference.c:119
color_cb_max
uint8_t color_cb_max
Definition: cv_blob_locator.c:42
georeference_init
void georeference_init(void)
Definition: cv_georeference.c:180
uint32_t
unsigned long uint32_t
Definition: types.h:18
image_free
void image_free(struct image_t *img)
Free the image.
Definition: image.c:75
window_enabled
volatile bool window_enabled
Definition: cv_blob_locator.c:58
image_t::w
uint16_t w
Image width.
Definition: image.h:46
image_label_t::pixel_cnt
uint32_t pixel_cnt
Number of pixels in the blob.
Definition: blob_finder.h:50
image_filter_t::v_min
uint8_t v_min
Definition: blob_finder.h:41
geofilter_length
int geofilter_length
Definition: cv_blob_locator.c:50
image_filter_t::u_max
uint8_t u_max
Definition: blob_finder.h:40
camera_frame_t::px
int32_t px
Target pixel coordinate (left = 0)
Definition: cv_georeference.h:41
IMAGE_GRAYSCALE
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:37
image_filter_t
Definition: blob_finder.h:36
detect_window_sizes
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:69
image_filter_t::y_max
uint8_t y_max
Definition: blob_finder.h:38
cv_blob_locator_reset
uint8_t cv_blob_locator_reset
Definition: cv_blob_locator.c:47
marker
struct marker_deviation_t marker(struct image_t *input, uint8_t M)
Definition: imavmarker.c:51
image_to_grayscale
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition: image.c:131
image_t::h
uint16_t h
Image height.
Definition: image.h:47
color_cb_min
uint8_t color_cb_min
Definition: cv_blob_locator.c:41
camera_frame_t::py
int32_t py
Target pixel coordinate (top = 0)
Definition: cv_georeference.h:42
cv.h
blob_locator
volatile uint32_t blob_locator
Definition: cv_blob_locator.c:54
cv_blob_locator_func
struct image_t * cv_blob_locator_func(struct image_t *img, uint8_t camera_id)
Definition: cv_blob_locator.c:131
uint8_t
unsigned char uint8_t
Definition: types.h:14
cv_blob_locator_start
void cv_blob_locator_start(void)
Definition: cv_blob_locator.c:316
start_vision_land
void start_vision_land(void)
Definition: cv_blob_locator.c:332
start_vision
void start_vision(void)
Definition: cv_blob_locator.c:326
cv_blob_locator_event
void cv_blob_locator_event(void)
Definition: cv_blob_locator.c:264
marker_enabled
volatile bool marker_enabled
Definition: cv_blob_locator.c:57
stop_vision
void stop_vision(void)
Definition: cv_blob_locator.c:338
camera_frame_t::w
int32_t w
Frame width [px].
Definition: cv_georeference.h:38
georeference_project
void georeference_project(struct camera_frame_t *tar, int wp)
Definition: cv_georeference.c:53
cv_blob_locator_stop
void cv_blob_locator_stop(void)
Definition: cv_blob_locator.c:321
color_cr_min
uint8_t color_cr_min
Definition: cv_blob_locator.c:44
MODE_BRIGHT
#define MODE_BRIGHT
Definition: detect_window.h:33
false
#define false
Definition: microrl.h:7
camera_frame_t::f
int32_t f
Camera Focal length in [px].
Definition: cv_georeference.h:40
cv_blob_locator_periodic
void cv_blob_locator_periodic(void)
Definition: cv_blob_locator.c:257
image_filter_t::v_max
uint8_t v_max
Definition: blob_finder.h:42
image_filter_t::y_min
uint8_t y_min
YUV color filter.
Definition: blob_finder.h:37
marker_deviation_t::y
int32_t y
Definition: imavmarker.h:34
image_t::buf
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
camera_frame_t::h
int32_t h
Frame height [px].
Definition: cv_georeference.h:39
cv_window_func
struct image_t * cv_window_func(struct image_t *img, uint8_t camera_id)
Definition: cv_blob_locator.c:84
IMAGE_GRADIENT
@ IMAGE_GRADIENT
An image gradient (int16 per pixel)
Definition: image.h:39
color_lum_min
uint8_t color_lum_min
Definition: cv_blob_locator.c:38
FALSE
#define FALSE
Definition: std.h:5
BLOB_LOCATOR_FPS
#define BLOB_LOCATOR_FPS
Default FPS (zero means run at camera fps)
Definition: cv_blob_locator.c:33
record_video
int record_video
Definition: cv_blob_locator.c:52
Img
#define Img(X, Y)
Definition: cv_blob_locator.c:79
marker_deviation_t
Definition: imavmarker.h:32
p
static float p[2][2]
Definition: ins_alt_float.c:268
image_t
Definition: image.h:44
image_label_t::x_sum
uint32_t x_sum
Sum of all x coordinates (used to find center of gravity)
Definition: blob_finder.h:53
imavmarker.h
image_label_t
Definition: blob_finder.h:46