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
detect_window.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  *
20  */
21 
27 #define RES 100
28 #define N_WINDOW_SIZES 1
29 
30 #include "cv.h"
31 #include "detect_window.h"
32 #include <stdio.h>
33 
35 {
36  cv_add_to_device(&BLOB_LOCATOR_CAMERA, detect_window);
37 }
38 
39 struct image_t* detect_window(struct image_t *img)
40 {
41 
42  uint16_t coordinate[2];
43  coordinate[0] = 0; coordinate[1] = 0;
44  uint16_t response = 0;
45  uint32_t integral_image[img->w * img->h];
46  struct image_t gray;
47  image_create(&gray, img->w, img->h, IMAGE_GRAYSCALE);
48  image_to_grayscale(img, &gray);
49 
50  response = detect_window_sizes((uint8_t *)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image,
51  MODE_BRIGHT);
52  printf("Coordinate: %d, %d\n", coordinate[0], coordinate[1]);
53  printf("Response = %d\n", response);
54 
55  image_free(&gray);
56  return NULL; // No new image was created
57 }
58 
59 
60 uint16_t detect_window_sizes(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate,
61  uint32_t *integral_image, uint8_t MODE)
62 {
63  // whether to calculate the integral image (only do once):
64  uint8_t calculate_integral_image = 1;
65  uint16_t sizes[N_WINDOW_SIZES];
66  uint16_t best_response[N_WINDOW_SIZES];
67  uint16_t best_index = 0;
68  uint16_t best_xc = 0;
69  uint16_t best_yc = 0;
70  uint16_t s = 0;
71  sizes[0] = 100; //sizes[1] = 40; sizes[2] = 50; sizes[3] = 60;
72 
73  for (s = 0; s < N_WINDOW_SIZES; s++) {
74 
75  // coordinate will contain the coordinate, best_response will be the best match * 100
76  calculate_integral_image = (s == 0); // only calculate the integal image for the first window size
77  best_response[s] = detect_window_one_size(in, image_width, image_height, coordinate, &sizes[s],
78  calculate_integral_image, integral_image, MODE);
79  if (s == 0 || best_response[s] < best_response[best_index]) {
80  best_index = s;
81  best_xc = coordinate[0];
82  best_yc = coordinate[1];
83  }
84  }
85 
86  coordinate[0] = best_xc;
87  coordinate[1] = best_yc;
88  return best_response[best_index];
89 }
90 
91 uint16_t detect_window_one_size(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate,
92  uint16_t *size, uint8_t calculate_integral_image, uint32_t *integral_image, uint8_t MODE)
93 {
94  /*
95  * Steps:
96  * (0) filter out the bad pixels (i.e., those lower than 4) and replace them with a disparity of 6.
97  * (1) get integral image (if calculate_integral_image == 1)
98  * (2) determine responses per location while determining the best-matching location (put it in coordinate)
99  */
100 
101  // output of the function:
102  uint16_t min_response = RES;
103 
104  // parameters:
105  //uint16_t image_border = 10;
106  uint16_t window_size, border_size, feature_size, px_whole, px_inner, px_border, px_outer;
107  uint16_t relative_border = 15; // border in percentage of window size
108 
109  // declaration other vars:
110  uint16_t x, y;
111  uint32_t response;
112 
113  // (1) get integral image (if calculate_integral_image == 1)
114 
115  if (calculate_integral_image) {
116  get_integral_image(in, image_width, image_height, integral_image);
117  }
118 
119  // window size is without border, feature size is with border:
120  window_size = (*size);
121  border_size = (relative_border * window_size) / 100; // percentage
122  feature_size = window_size + 2 * border_size;
123  px_inner = feature_size - 2 * border_size;
124  px_inner = px_inner * px_inner;
125  px_whole = feature_size * feature_size;
126  px_border = px_whole - px_inner;
127  px_outer = border_size * window_size;
128 
129  // (2) determine a response map for that size
130  for (x = 0; x < image_width - feature_size; x++) {
131  for (y = 0; y < image_height - feature_size; y++) {
132  response = get_window_response(x, y, feature_size, border_size, integral_image, image_width, image_height, px_inner,
133  px_border, MODE);
134 
135  if (response < RES) {
136  if (MODE == MODE_DARK) {
137  // the inside is further away than the outside, perform the border test:
138  response = get_border_response(x, y, feature_size, window_size, border_size, integral_image, image_width, image_height,
139  px_inner, px_outer);
140  }
141 
142  if (response < min_response) {
143  coordinate[0] = x;
144  coordinate[1] = y;
145  min_response = response;
146  }
147  } else {
148  //in[x+y*image_width] = 255;
149 
150  }
151  }
152  }
153 
154  // the coordinate is at the top left corner of the feature,
155  // the center of the window is then at:
156  coordinate[0] += feature_size / 2;
157  coordinate[1] += feature_size / 2;
158 
159  return min_response;
160 }
161 
162 // this function can help if the window is not visible anymore:
163 uint16_t detect_escape(uint8_t *in __attribute__((unused)), uint32_t image_width, uint32_t image_height, uint16_t *escape_coordinate,
164  uint32_t *integral_image, uint8_t n_cells)
165 {
166  uint16_t c, r, min_c, min_r;
167  uint16_t cell_width, cell_height;
168  uint32_t min_avg = 10000;
169  uint32_t avg;
170  uint16_t border = 10;
171  cell_width = (image_width - 2 * border) / n_cells;
172  cell_height = (image_height - 2 * border) / n_cells;
173  // Get the average disparities of all cells in a grid:
174  for (c = 0; c < n_cells; c++) {
175  for (r = 0; r < n_cells; r++) {
176  avg = get_avg_disparity(c * cell_width + border, r * cell_height + border, (c + 1) * cell_width + border,
177  (r + 1) * cell_height + border, integral_image, image_width, image_height);
178  if (avg < min_avg) {
179  min_avg = avg;
180  min_c = c;
181  min_r = r;
182  }
183  }
184  }
185  // return coordinates for the best option:
186  if (min_avg == 10000) {
187  escape_coordinate[0] = image_width / 2;
188  escape_coordinate[1] = image_height / 2;
189  } else {
190  escape_coordinate[0] = min_c * cell_width + border + cell_width / 2;
191  escape_coordinate[1] = min_r * cell_height + border + cell_height / 2;
192  }
193 
194  return min_avg;
195 }
196 
197 void get_integral_image(uint8_t *in, uint32_t image_width, uint32_t image_height, uint32_t *integral_image)
198 {
199  uint16_t x, y;
200  for (x = 0; x < image_width; x++) {
201  for (y = 0; y < image_height; y++) {
202  if (x >= 1 && y >= 1) {
203  integral_image[x + y * image_width] = (uint32_t) in[x + y * image_width] + integral_image[x - 1 + y * image_width] +
204  integral_image[x + (y - 1) * image_width] - integral_image[x - 1 + (y - 1) * image_width];
205  } else if (x >= 1) {
206  integral_image[x + y * image_width] = (uint32_t) in[x + y * image_width] + integral_image[x - 1 + y * image_width];
207  } else if (y >= 1) {
208  integral_image[x + y * image_width] = (uint32_t) in[x + y * image_width] + integral_image[x + (y - 1) * image_width];
209  } else {
210  integral_image[x + y * image_width] = (uint32_t) in[x + y * image_width];
211  }
212  }
213  }
214 }
215 
216 uint32_t get_sum_disparities(uint16_t min_x, uint16_t min_y, uint16_t max_x, uint16_t max_y, uint32_t *integral_image,
217  uint32_t image_width, uint32_t image_height)
218 {
219  uint32_t sum;
220  // If variables are not unsigned, then check for negative inputs
221  // if (min_x + min_y * image_width < 0) { return 0; }
222  if (max_x + max_y * image_width >= image_width * image_height) { return 0; }
223  sum = integral_image[min_x + min_y * image_width] + integral_image[max_x + max_y * image_width] -
224  integral_image[max_x + min_y * image_width] - integral_image[min_x + max_y * image_width];
225  return sum;
226 }
227 
228 uint32_t get_avg_disparity(uint16_t min_x, uint16_t min_y, uint16_t max_x, uint16_t max_y, uint32_t *integral_image,
229  uint32_t image_width, uint32_t image_height __attribute__((unused)))
230 {
231  uint16_t w, h;
232  uint32_t sum, avg, n_pix;
233 
234  // width and height of the window
235  w = max_x - min_x + 1;
236  h = max_y - min_y + 1;
237  n_pix = w * h;
238  // sum over the area:
239  sum = integral_image[min_x + min_y * image_width] + integral_image[max_x + max_y * image_width] -
240  integral_image[max_x + min_y * image_width] - integral_image[min_x + max_y * image_width];
241  // take the average, scaled by RES:
242  avg = (sum * RES) / n_pix;
243  return avg;
244 }
245 
246 
247 uint16_t get_window_response(uint16_t x, uint16_t y, uint16_t feature_size, uint16_t border, uint32_t *integral_image,
248  uint16_t image_width, uint16_t image_height, uint16_t px_inner, uint16_t px_border, uint8_t MODE)
249 {
250  uint32_t whole_area, inner_area, resp;
251 
252  whole_area = get_sum_disparities(x, y, x + feature_size, y + feature_size, integral_image, image_width, image_height);
253 
254  inner_area = get_sum_disparities(x + border, y + border, x + feature_size - border, y + feature_size - border,
255  integral_image, image_width, image_height);
256 
257  if (MODE == MODE_DARK) {
258  if (whole_area - inner_area > 0) {
259  resp = (inner_area * RES * px_border) / ((whole_area - inner_area) * px_inner);
260  } else {
261  resp = RES;
262  }
263  } else { //if(MODE == MODE_BRIGHT)
264  if (inner_area > 0 && (inner_area / px_inner) > 0) {
265  resp = (RES * (whole_area - inner_area) / px_border) / (inner_area / px_inner);
266  //printf("%u: %u %u %u %u\n",resp,(RES*RES*(whole_area - inner_area)/px_border), (inner_area/px_inner), px_inner, px_border);
267  } else {
268  resp = RES;
269  }
270  }
271 
272 
273  return resp;
274 }
275 
276 uint16_t get_border_response(uint16_t x, uint16_t y, uint16_t feature_size, uint16_t window_size, uint16_t border,
277  uint32_t *integral_image, uint16_t image_width, uint16_t image_height, uint16_t px_inner, uint16_t px_outer)
278 {
279  uint32_t inner_area, avg_inner, left_area, right_area, up_area, down_area, darkest, avg_dark, resp;
280  // inner area
281  inner_area = get_sum_disparities(x + border, y + border, x + feature_size - border, y + feature_size - border,
282  integral_image, image_width, image_height);
283  avg_inner = (RES * inner_area) / px_inner;
284  // outer areas:
285  left_area = get_sum_disparities(x, y + border, x + border, y + border + window_size, integral_image, image_width,
286  image_height);
287  right_area = get_sum_disparities(x + border + window_size, y + border, x + 2 * border + window_size,
288  y + border + window_size, integral_image, image_width, image_height);
289  up_area = get_sum_disparities(x + border, y, x + border + window_size, y + border, integral_image, image_width,
290  image_height);
291  down_area = get_sum_disparities(x + border, y + border + window_size, x + border + window_size,
292  y + 2 * border + window_size, integral_image, image_width, image_height);
293  // darkest outer area:
294  darkest = (left_area < right_area) ? left_area : right_area;
295  darkest = (darkest < up_area) ? darkest : up_area;
296  darkest = (darkest < down_area) ? darkest : down_area;
297  avg_dark = RES * darkest / px_outer;
298  if (avg_dark < avg_inner) {
299  resp = RES;
300  } else {
301  if (avg_dark == 0) {
302  resp = RES;
303  } else {
304  resp = RES * avg_inner / avg_dark;
305  }
306  }
307 
308  return resp;
309 }
310 
311 void filter_bad_pixels(uint8_t *in, uint32_t image_width, uint32_t image_height)
312 {
313  uint16_t x, y;
314  for (x = 0; x < image_width; x++) {
315  for (y = 0; y < image_height; y++) {
316  if (in[x + y * image_width] < 4) {
317  in[x + y * image_width] = 6;
318  }
319  }
320  }
321 }
322 
323 
324 void transform_illuminance_image(uint8_t *in, uint8_t *out, uint32_t image_width, uint32_t image_height, uint8_t n_bits,
325  uint8_t bright_win)
326 {
327  uint16_t x, y;
328  for (x = 0; x < image_width; x++) {
329  for (y = 0; y < image_height; y++) {
330  // we put the right image entirely in the left image instead of in the even rows:
331  if (!bright_win) {
332  out[x + y * image_width] = in[2 * (x + y * image_width)] >> n_bits;
333  } else {
334  out[x + y * image_width] = (255 - in[2 * (x + y * image_width)]) >> n_bits;
335  }
336  }
337  }
338 }
void filter_bad_pixels(uint8_t *in, uint32_t image_width, uint32_t image_height)
unsigned short uint16_t
Definition: types.h:16
struct image_t * detect_window(struct image_t *img)
Definition: detect_window.c:39
#define N_WINDOW_SIZES
Definition: detect_window.c:28
#define MODE_BRIGHT
Definition: detect_window.h:33
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 video_listener * cv_add_to_device(struct video_config_t *device, cv_function func)
Definition: cv.c:45
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
#define RES
Definition: detect_window.c:27
#define MODE_DARK
Definition: detect_window.h:32
uint16_t detect_escape(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *escape_coordinate, uint32_t *integral_image, uint8_t n_cells)
uint16_t get_border_response(uint16_t x, uint16_t y, uint16_t feature_size, uint16_t window_size, uint16_t border, uint32_t *integral_image, uint16_t image_width, uint16_t image_height, uint16_t px_inner, uint16_t px_outer)
uint16_t get_window_response(uint16_t x, uint16_t y, uint16_t feature_size, uint16_t border, uint32_t *integral_image, uint16_t image_width, uint16_t image_height, uint16_t px_inner, uint16_t px_border, uint8_t MODE)
uint16_t w
Image width.
Definition: image.h:44
uint16_t detect_window_one_size(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate, uint16_t *size, uint8_t calculate_integral_image, uint32_t *integral_image, uint8_t MODE)
Definition: detect_window.c:91
unsigned long uint32_t
Definition: types.h:18
Computer vision framework for onboard processing.
uint16_t h
Image height.
Definition: image.h:45
void * buf
Image buffer (depending on the image_type)
Definition: image.h:51
void transform_illuminance_image(uint8_t *in, uint8_t *out, uint32_t image_width, uint32_t image_height, uint8_t n_bits, uint8_t bright_win)
void get_integral_image(uint8_t *in, uint32_t image_width, uint32_t image_height, uint32_t *integral_image)
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition: image.c:116
unsigned char uint8_t
Definition: types.h:14
uint32_t get_sum_disparities(uint16_t min_x, uint16_t min_y, uint16_t max_x, uint16_t max_y, uint32_t *integral_image, uint32_t image_width, uint32_t image_height)
void detect_window_init(void)
Definition: detect_window.c:34
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:36
Detect a bright region surrounded by dark or viceversa - sometimes this corresponds to a window...
uint32_t get_avg_disparity(uint16_t min_x, uint16_t min_y, uint16_t max_x, uint16_t max_y, uint32_t *integral_image, uint32_t image_width, uint32_t image_height)