Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
blob_finder.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 
30 #include "blob_finder.h"
31 #include <stdio.h>
32 
33 void image_labeling(struct image_t *input, struct image_t *output, struct image_filter_t *filters, uint8_t filters_cnt,
34  struct image_label_t *labels, uint16_t *labels_count)
35 {
36  uint8_t *input_buf = (uint8_t *)input->buf;
37  uint16_t *output_buf = (uint16_t *)output->buf;
38 
39  // Initialize labels
40  uint16_t labels_size = *labels_count;
41  uint16_t labels_cnt = 0;
42  uint16_t i, x, y;
43 
44  // Initialize first line with empty groups
45  uint16_t *p = output_buf;
46  for (i = 0; i < output->w; i++) {
47  *p++ = 0xffff;
48  }
49 
50  // Do steps of 2 for YUV image
51  // Skip first line as we need previous groups for connectivity
52  for (y = 1; y < input->h; y++) {
53  for (x = 0; x < input->w / 2; x++) {
54  uint16_t lid = 0;
55  uint8_t p_y = (input_buf[y * input->w * 2 + x * 4 + 1] + input_buf[y * input->w * 2 + x * 4 + 3]) / 2;
56  uint8_t p_u = input_buf[y * input->w * 2 + x * 4];
57  uint8_t p_v = input_buf[y * input->w * 2 + x * 4 + 2];
58 
59  // Go trough the filters
60  uint8_t f = 0;
61  for (; f < filters_cnt; f++) {
62  if (p_y > filters[f].y_min && p_y < filters[f].y_max &&
63  p_u > filters[f].u_min && p_u < filters[f].u_max &&
64  p_v > filters[f].v_min && p_v < filters[f].v_max) {
65  break;
66  }
67  }
68 
69  // Check if this pixel belongs to a filter else goto next
70  if (f >= filters_cnt) {
71  output_buf[y * output->w + x] = 0xFFFF;
72  continue;
73  }
74 
75  // Check pixel above (if the same filter then take same group)
76  lid = output_buf[(y - 1) * output->w + x];
77  if (y > 0 && lid < labels_size && labels[lid].filter == f) {
78  output_buf[y * output->w + x] = lid;
79  labels[lid].pixel_cnt++;
80  labels[lid].x_sum += x;
81  labels[lid].y_sum += y;
82  continue;
83  }
84 
85  // Check pixel top right (check for merging)
86  lid = output_buf[(y - 1) * output->w + x + 1];
87  if (y > 0 && x < output->w - 1 && lid < labels_size && labels[lid].filter == f) {
88 
89  // Merging labels if needed
90  uint16_t lid_tl = output_buf[(y - 1) * output->w + x - 1]; // Top left
91  uint16_t lid_l = output_buf[y * output->w + x - 1]; // Left
92  uint16_t m = labels[lid].id, n = labels[lid].id;
93  if (x > 0 && lid_tl < labels_size && labels[lid_tl].filter == f) {
94  // Merge with top left
95  m = labels[lid].id;
96  n = labels[lid_tl].id;
97  } else if (x > 0 && lid_l < labels_size && labels[lid_l].filter == f) {
98  // Merge with left
99  m = labels[lid].id;
100  n = labels[lid_l].id;
101  }
102 
103  // Change the id of the highest id label
104  if (m != n) {
105  if (m > n) {
106  m = n;
107  n = labels[lid].id;
108  }
109 
110  for (i = 0; i < labels_cnt; i++) {
111  if (labels[i].id == n) {
112  labels[i].id = m;
113  }
114  }
115  }
116 
117  // Update the label
118  output_buf[y * output->w + x] = lid;
119  labels[lid].pixel_cnt++;
120  labels[lid].x_sum += x;
121  labels[lid].y_sum += y;
122  continue;
123  }
124 
125  // Take top left
126  lid = output_buf[(y - 1) * output->w + x - 1];
127  if (y > 0 && x > 0 && lid < labels_size && labels[lid].filter == f) {
128  output_buf[y * output->w + x] = lid;
129  labels[lid].pixel_cnt++;
130  labels[lid].x_sum += x;
131  labels[lid].y_sum += y;
132  continue;
133  }
134 
135  // Take left
136  lid = output_buf[y * output->w + x - 1];
137  if (x > 0 && lid < labels_size && labels[lid].filter == f) {
138  output_buf[y * output->w + x] = lid;
139  labels[lid].pixel_cnt++;
140  labels[lid].x_sum += x;
141  labels[lid].y_sum += y;
142  continue;
143  }
144 
145  // Check if there is enough space
146  if (labels_cnt >= labels_size - 1) {
147  break;
148  }
149 
150  // Create new group
151  lid = labels_cnt;
152  output_buf[y * output->w + x] = lid;
153  labels[lid].id = lid;
154  labels[lid].filter = f;
155  labels[lid].pixel_cnt = 1;
156  labels[lid].x_min = x;
157  labels[lid].y_min = y;
158  labels[lid].x_sum = x;
159  labels[lid].y_sum = y;
160  labels_cnt++;
161  }
162  }
163 
164  if (labels_cnt >= labels_size - 1) {
165  printf("Break did not work: we have %d labels\n", labels_cnt);
166  }
167 
168  // Merge connected labels
169  for (i = 0; i < labels_cnt; i++) {
170  if (labels[i].id != i) {
171  uint16_t new_id = labels[i].id;
172  labels[new_id].pixel_cnt += labels[i].pixel_cnt;
173  labels[new_id].x_sum += labels[i].x_sum;
174  labels[new_id].y_sum += labels[i].y_sum;
175 
176  //printf("%d == %d, ",new_id, i);
177 
178  if (labels[i].x_min < labels[new_id].x_min) { labels[new_id].x_min = labels[i].x_min; }
179  if (labels[i].y_min < labels[new_id].y_min) { labels[new_id].y_min = labels[i].y_min; }
180  }
181  }
182 
183  *labels_count = labels_cnt;
184 
185  // Replace ID's
186  for (y = 0; y < input->h; y++) {
187  for (x = 0; x < input->w / 2; x++) {
188  uint16_t lid = output_buf[y * output->w + x];
189  if (lid < labels_cnt) {
190  output_buf[y * output->w + x] = labels[lid].id;
191  }
192  }
193  }
194 }
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
Parse UYVY images and make a list of blobs of connected pixels.
uint16_t x_min
Top left corner.
Definition: blob_finder.h:51
uint32_t pixel_cnt
Number of pixels in the blob.
Definition: blob_finder.h:50
uint8_t filter
Which filter triggered this blob.
Definition: blob_finder.h:48
uint16_t id
Blob number.
Definition: blob_finder.h:47
uint32_t x_sum
Sum of all x coordinates (used to find center of gravity)
Definition: blob_finder.h:53
uint16_t y_min
Definition: blob_finder.h:52
uint32_t y_sum
Definition: blob_finder.h:54
if(GpsFixValid() &&e_identification_started)
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
uint16_t h
Image height.
Definition: image.h:47
uint16_t w
Image width.
Definition: image.h:46
Definition: image.h:44
static float p[2][2]
float v_min
float v_max
static struct FirstOrderLowPass filters[SYS_ID_CHIRP_NB_AXES]
Definition: sys_id_chirp.c:82
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204