Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
cv_detect_color_object.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Kirk Scheper <kirkscheper@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 // Own header
31 #include "subsystems/abi.h"
32 #include "std.h"
33 
34 #include <stdio.h>
35 #include <stdbool.h>
36 #include <math.h>
37 #include "pthread.h"
38 
39 #define PRINT(string,...) fprintf(stderr, "[object_detector->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
40 #if OBJECT_DETECTOR_VERBOSE
41 #define VERBOSE_PRINT PRINT
42 #else
43 #define VERBOSE_PRINT(...)
44 #endif
45 
46 static pthread_mutex_t mutex;
47 
48 #ifndef COLOR_OBJECT_DETECTOR_FPS1
49 #define COLOR_OBJECT_DETECTOR_FPS1 0
50 #endif
51 #ifndef COLOR_OBJECT_DETECTOR_FPS2
52 #define COLOR_OBJECT_DETECTOR_FPS2 0
53 #endif
54 
55 // Filter Settings
62 
69 
70 bool cod_draw1 = false;
71 bool cod_draw2 = false;
72 
73 // define global variables
78  bool updated;
79 };
81 
82 // Function
83 uint32_t find_object_centroid(struct image_t *img, int32_t* p_xc, int32_t* p_yc, bool draw,
84  uint8_t lum_min, uint8_t lum_max,
85  uint8_t cb_min, uint8_t cb_max,
86  uint8_t cr_min, uint8_t cr_max);
87 
88 /*
89  * object_detector
90  * @param img - input image to process
91  * @param filter - which detection filter to process
92  * @return img
93  */
94 static struct image_t *object_detector(struct image_t *img, uint8_t filter)
95 {
96  uint8_t lum_min, lum_max;
97  uint8_t cb_min, cb_max;
98  uint8_t cr_min, cr_max;
99  bool draw;
100 
101  switch (filter){
102  case 1:
103  lum_min = cod_lum_min1;
104  lum_max = cod_lum_max1;
105  cb_min = cod_cb_min1;
106  cb_max = cod_cb_max1;
107  cr_min = cod_cr_min1;
108  cr_max = cod_cr_max1;
109  draw = cod_draw1;
110  break;
111  case 2:
112  lum_min = cod_lum_min2;
113  lum_max = cod_lum_max2;
114  cb_min = cod_cb_min2;
115  cb_max = cod_cb_max2;
116  cr_min = cod_cr_min2;
117  cr_max = cod_cr_max2;
118  draw = cod_draw2;
119  break;
120  default:
121  return img;
122  };
123 
124  int32_t x_c, y_c;
125 
126  // Filter and find centroid
127  uint32_t count = find_object_centroid(img, &x_c, &y_c, draw, lum_min, lum_max, cb_min, cb_max, cr_min, cr_max);
128  VERBOSE_PRINT("Color count %d: %u, threshold %u, x_c %d, y_c %d\n", camera, object_count, count_threshold, x_c, y_c);
129  VERBOSE_PRINT("centroid %d: (%d, %d) r: %4.2f a: %4.2f\n", camera, x_c, y_c,
130  hypotf(x_c, y_c) / hypotf(img->w * 0.5, img->h * 0.5), RadOfDeg(atan2f(y_c, x_c)));
131 
132  pthread_mutex_lock(&mutex);
133  global_filters[filter-1].color_count = count;
134  global_filters[filter-1].x_c = x_c;
135  global_filters[filter-1].y_c = y_c;
136  global_filters[filter-1].updated = true;
137  pthread_mutex_unlock(&mutex);
138 
139  return img;
140 }
141 
142 struct image_t *object_detector1(struct image_t *img, uint8_t camera_id);
143 struct image_t *object_detector1(struct image_t *img, uint8_t camera_id __attribute__((unused)))
144 {
145  return object_detector(img, 1);
146 }
147 
148 struct image_t *object_detector2(struct image_t *img, uint8_t camera_id);
149 struct image_t *object_detector2(struct image_t *img, uint8_t camera_id __attribute__((unused)))
150 {
151  return object_detector(img, 2);
152 }
153 
155 {
156  memset(global_filters, 0, 2*sizeof(struct color_object_t));
157  pthread_mutex_init(&mutex, NULL);
158 #ifdef COLOR_OBJECT_DETECTOR_CAMERA1
159 #ifdef COLOR_OBJECT_DETECTOR_LUM_MIN1
160  cod_lum_min1 = COLOR_OBJECT_DETECTOR_LUM_MIN1;
161  cod_lum_max1 = COLOR_OBJECT_DETECTOR_LUM_MAX1;
162  cod_cb_min1 = COLOR_OBJECT_DETECTOR_CB_MIN1;
163  cod_cb_max1 = COLOR_OBJECT_DETECTOR_CB_MAX1;
164  cod_cr_min1 = COLOR_OBJECT_DETECTOR_CR_MIN1;
165  cod_cr_max1 = COLOR_OBJECT_DETECTOR_CR_MAX1;
166 #endif
167 #ifdef COLOR_OBJECT_DETECTOR_DRAW1
168  cod_draw1 = COLOR_OBJECT_DETECTOR_DRAW1;
169 #endif
170 
171  cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1, 0);
172 #endif
173 
174 #ifdef COLOR_OBJECT_DETECTOR_CAMERA2
175 #ifdef COLOR_OBJECT_DETECTOR_LUM_MIN2
176  cod_lum_min2 = COLOR_OBJECT_DETECTOR_LUM_MIN2;
177  cod_lum_max2 = COLOR_OBJECT_DETECTOR_LUM_MAX2;
178  cod_cb_min2 = COLOR_OBJECT_DETECTOR_CB_MIN2;
179  cod_cb_max2 = COLOR_OBJECT_DETECTOR_CB_MAX2;
180  cod_cr_min2 = COLOR_OBJECT_DETECTOR_CR_MIN2;
181  cod_cr_max2 = COLOR_OBJECT_DETECTOR_CR_MAX2;
182 #endif
183 #ifdef COLOR_OBJECT_DETECTOR_DRAW2
184  cod_draw2 = COLOR_OBJECT_DETECTOR_DRAW2;
185 #endif
186 
187  cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2, 1);
188 #endif
189 }
190 
191 /*
192  * find_object_centroid
193  *
194  * Finds the centroid of pixels in an image within filter bounds.
195  * Also returns the amount of pixels that satisfy these filter bounds.
196  *
197  * @param img - input image to process formatted as YUV422.
198  * @param p_xc - x coordinate of the centroid of color object
199  * @param p_yc - y coordinate of the centroid of color object
200  * @param lum_min - minimum y value for the filter in YCbCr colorspace
201  * @param lum_max - maximum y value for the filter in YCbCr colorspace
202  * @param cb_min - minimum cb value for the filter in YCbCr colorspace
203  * @param cb_max - maximum cb value for the filter in YCbCr colorspace
204  * @param cr_min - minimum cr value for the filter in YCbCr colorspace
205  * @param cr_max - maximum cr value for the filter in YCbCr colorspace
206  * @param draw - whether or not to draw on image
207  * @return number of pixels of image within the filter bounds.
208  */
209 uint32_t find_object_centroid(struct image_t *img, int32_t* p_xc, int32_t* p_yc, bool draw,
210  uint8_t lum_min, uint8_t lum_max,
211  uint8_t cb_min, uint8_t cb_max,
212  uint8_t cr_min, uint8_t cr_max)
213 {
214  uint32_t cnt = 0;
215  uint32_t tot_x = 0;
216  uint32_t tot_y = 0;
217  uint8_t *buffer = img->buf;
218 
219  // Go through all the pixels
220  for (uint16_t y = 0; y < img->h; y++) {
221  for (uint16_t x = 0; x < img->w; x ++) {
222  // Check if the color is inside the specified values
223  uint8_t *yp, *up, *vp;
224  if (x % 2 == 0) {
225  // Even x
226  up = &buffer[y * 2 * img->w + 2 * x]; // U
227  yp = &buffer[y * 2 * img->w + 2 * x + 1]; // Y1
228  vp = &buffer[y * 2 * img->w + 2 * x + 2]; // V
229  //yp = &buffer[y * 2 * img->w + 2 * x + 3]; // Y2
230  } else {
231  // Uneven x
232  up = &buffer[y * 2 * img->w + 2 * x - 2]; // U
233  //yp = &buffer[y * 2 * img->w + 2 * x - 1]; // Y1
234  vp = &buffer[y * 2 * img->w + 2 * x]; // V
235  yp = &buffer[y * 2 * img->w + 2 * x + 1]; // Y2
236  }
237  if ( (*yp >= lum_min) && (*yp <= lum_max) &&
238  (*up >= cb_min ) && (*up <= cb_max ) &&
239  (*vp >= cr_min ) && (*vp <= cr_max )) {
240  cnt ++;
241  tot_x += x;
242  tot_y += y;
243  if (draw){
244  *yp = 255; // make pixel brighter in image
245  }
246  }
247  }
248  }
249  if (cnt > 0) {
250  *p_xc = (int32_t)roundf(tot_x / ((float) cnt) - img->w * 0.5f);
251  *p_yc = (int32_t)roundf(img->h * 0.5f - tot_y / ((float) cnt));
252  } else {
253  *p_xc = 0;
254  *p_yc = 0;
255  }
256  return cnt;
257 }
258 
260 {
261  static struct color_object_t local_filters[2];
262  pthread_mutex_lock(&mutex);
263  memcpy(local_filters, global_filters, 2*sizeof(struct color_object_t));
264  pthread_mutex_unlock(&mutex);
265 
266  if(local_filters[0].updated){
267  AbiSendMsgVISUAL_DETECTION(COLOR_OBJECT_DETECTION1_ID, local_filters[0].x_c, local_filters[0].y_c,
268  0, 0, local_filters[0].color_count, 0);
269  local_filters[0].updated = false;
270  }
271  if(local_filters[1].updated){
272  AbiSendMsgVISUAL_DETECTION(COLOR_OBJECT_DETECTION2_ID, local_filters[1].x_c, local_filters[1].y_c,
273  0, 0, local_filters[1].color_count, 1);
274  local_filters[1].updated = false;
275  }
276 }
VERBOSE_PRINT
#define VERBOSE_PRINT(...)
Definition: cv_detect_color_object.c:43
uint16_t
unsigned short uint16_t
Definition: types.h:16
global_filters
struct color_object_t global_filters[2]
Definition: cv_detect_color_object.c:80
color_object_detector_init
void color_object_detector_init(void)
Definition: cv_detect_color_object.c:154
cod_cr_min1
uint8_t cod_cr_min1
Definition: cv_detect_color_object.c:60
color_count
volatile int color_count
Definition: colorfilter.c:54
cod_cb_max1
uint8_t cod_cb_max1
Definition: cv_detect_color_object.c:59
cod_cr_max2
uint8_t cod_cr_max2
Definition: cv_detect_color_object.c:68
color_object_t::updated
bool updated
Definition: cv_detect_color_object.c:78
abi.h
color_object_t::color_count
uint32_t color_count
Definition: cv_detect_color_object.c:77
object_detector
static struct image_t * object_detector(struct image_t *img, uint8_t filter)
Definition: cv_detect_color_object.c:94
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
COLOR_OBJECT_DETECTION2_ID
#define COLOR_OBJECT_DETECTION2_ID
Definition: abi_sender_ids.h:504
color_object_t::y_c
int32_t y_c
Definition: cv_detect_color_object.c:76
cod_cr_max1
uint8_t cod_cr_max1
Definition: cv_detect_color_object.c:61
cod_cb_max2
uint8_t cod_cb_max2
Definition: cv_detect_color_object.c:66
mutex
static pthread_mutex_t mutex
Definition: cv_detect_color_object.c:46
uint32_t
unsigned long uint32_t
Definition: types.h:18
image_t::w
uint16_t w
Image width.
Definition: image.h:46
color_object_t::x_c
int32_t x_c
Definition: cv_detect_color_object.c:75
up
uint16_t up[LIGHT_NB]
Definition: light_solar.c:48
color_object_detector_periodic
void color_object_detector_periodic(void)
Definition: cv_detect_color_object.c:259
image_t::h
uint16_t h
Image height.
Definition: image.h:47
cod_lum_min2
uint8_t cod_lum_min2
Definition: cv_detect_color_object.c:63
object_detector1
struct image_t * object_detector1(struct image_t *img, uint8_t camera_id)
Definition: cv_detect_color_object.c:143
std.h
cv.h
COLOR_OBJECT_DETECTOR_FPS2
#define COLOR_OBJECT_DETECTOR_FPS2
Default FPS (zero means run at camera fps)
Definition: cv_detect_color_object.c:52
uint8_t
unsigned char uint8_t
Definition: types.h:14
cod_cr_min2
uint8_t cod_cr_min2
Definition: cv_detect_color_object.c:67
cod_lum_max2
uint8_t cod_lum_max2
Definition: cv_detect_color_object.c:64
cod_lum_min1
uint8_t cod_lum_min1
Definition: cv_detect_color_object.c:56
COLOR_OBJECT_DETECTION1_ID
#define COLOR_OBJECT_DETECTION1_ID
Definition: abi_sender_ids.h:500
cv_detect_color_object.h
find_object_centroid
uint32_t find_object_centroid(struct image_t *img, int32_t *p_xc, int32_t *p_yc, bool draw, uint8_t lum_min, uint8_t lum_max, uint8_t cb_min, uint8_t cb_max, uint8_t cr_min, uint8_t cr_max)
Definition: cv_detect_color_object.c:209
cod_cb_min1
uint8_t cod_cb_min1
Definition: cv_detect_color_object.c:58
COLOR_OBJECT_DETECTOR_FPS1
#define COLOR_OBJECT_DETECTOR_FPS1
Default FPS (zero means run at camera fps)
Definition: cv_detect_color_object.c:49
int32_t
signed long int32_t
Definition: types.h:19
cod_draw1
bool cod_draw1
Definition: cv_detect_color_object.c:70
cod_lum_max1
uint8_t cod_lum_max1
Definition: cv_detect_color_object.c:57
object_detector2
struct image_t * object_detector2(struct image_t *img, uint8_t camera_id)
Definition: cv_detect_color_object.c:149
image_t::buf
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
cod_cb_min2
uint8_t cod_cb_min2
Definition: cv_detect_color_object.c:65
image_t
Definition: image.h:44
color_object_t
Definition: cv_detect_color_object.c:74
cod_draw2
bool cod_draw2
Definition: cv_detect_color_object.c:71