Fawkes API  Fawkes Development Version
star.cpp
1 
2 /***************************************************************************
3  * star.cpp - Starlike scanline model
4  *
5  * Created: Mon Nov 05 10:06:46 2007
6  * Copyright 2007 Daniel Beck
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <fvmodels/scanlines/star.h>
25 #include <fvutils/color/yuv.h>
26 #include <utils/math/angle.h>
27 
28 #include <cstring>
29 
30 using namespace fawkes;
31 
32 namespace firevision {
33 
34 /** @class ScanlineStar <fvmodels/scanlines/star.h>
35  * Star-like arranged scanline points.
36  *
37  * @author Daniel Beck
38  */
39 
40 /** Constructor.
41  * @param image_width width of the image
42  * @param image_height height of the image
43  * @param center_x x-coordinate of the center point
44  * @param center_y y-coordinate of the center point
45  * @param num_rays number of rays
46  * @param radius_incr number of pixels by which the radius is increased
47  * @param yuv_mask a mask allows to exclude certain regions of the image from
48  * inspection. More precisely, no scanline points are generated in those
49  * areas. The ignored regions have to be black, i.e. Y=0, U=127, V=127.
50  * @param dead_radius number of pixels around the center that are disregarded
51  * @param max_radius maximal radius in number of pixels
52  * @param margin margin around every scanline point that does not contain any
53  * other scanline point (in pixels)
54  */
55 ScanlineStar::ScanlineStar(unsigned int image_width,
56  unsigned int image_height,
57  unsigned int center_x,
58  unsigned int center_y,
59  unsigned int num_rays,
60  unsigned int radius_incr,
61  unsigned char *yuv_mask,
62  unsigned int dead_radius,
63  unsigned int max_radius,
64  unsigned int margin)
65 {
66  m_image_width = image_width;
67  m_image_height = image_height;
68  m_center.x = center_x;
69  m_center.y = center_y;
70  m_num_rays = num_rays;
71  m_radius_incr = radius_incr;
72  m_mask = yuv_mask;
73  m_dead_radius = dead_radius;
74  m_max_radius = max_radius;
75  m_margin = margin;
76 
77  m_angle_incr = deg2rad(360.0 / m_num_rays);
78 
79  m_first_ray = 0;
80  m_previous_ray = 0;
81 
82  m_first_on_ray = true;
83 
84  // -- sanity checks --
85  // margin
86  if (m_margin > m_radius_incr / 2) {
87  m_margin = m_radius_incr / 2;
88  }
89 
90  generate_scan_points();
91 
92  reset();
93 }
94 
95 /** Destructor. */
96 ScanlineStar::~ScanlineStar()
97 {
98  std::map<float, Ray *>::iterator rit;
99  for (rit = m_rays.begin(); rit != m_rays.end(); ++rit) {
100  delete rit->second;
101  }
102 }
103 
104 upoint_t
105 ScanlineStar::operator*()
106 {
107  return m_current_point;
108 }
109 
110 upoint_t *
111 ScanlineStar::operator->()
112 {
113  return &m_current_point;
114 }
115 
116 upoint_t *
117 ScanlineStar::operator++()
118 {
119  advance();
120  return &m_current_point;
121 }
122 
123 upoint_t *
124 ScanlineStar::operator++(int)
125 {
126  memcpy(&m_tmp_point, &m_current_point, sizeof(upoint_t));
127  advance();
128 
129  return &m_tmp_point;
130 }
131 
132 /** Calculates the next scanline point. */
133 void
134 ScanlineStar::advance()
135 {
136  if (m_done) {
137  return;
138  }
139 
140  ++m_point_iter;
141  m_first_on_ray = false;
142 
143  if ((*m_ray_iter).second->end() == m_point_iter) {
144  ++m_ray_iter;
145 
146  if (m_rays.end() == m_ray_iter) {
147  m_done = true;
148  return;
149  }
150 
151  ++m_ray_index;
152  m_point_iter = (*m_ray_iter).second->begin();
153  m_first_on_ray = true;
154  }
155 
156  m_current_point = (*m_point_iter).second;
157 }
158 
159 bool
160 ScanlineStar::finished()
161 {
162  return m_done;
163 }
164 
165 void
166 ScanlineStar::reset()
167 {
168  m_done = false;
169  m_first_on_ray = true;
170 
171  m_ray_index = 0;
172  m_ray_iter = m_rays.begin();
173  m_point_iter = (*m_ray_iter).second->begin();
174  m_current_point = (*m_point_iter).second;
175 }
176 
177 const char *
178 ScanlineStar::get_name()
179 {
180  return "ScanlineModel::Star";
181 }
182 
183 unsigned int
184 ScanlineStar::get_margin()
185 {
186  return m_margin;
187 }
188 
189 void
190 ScanlineStar::set_robot_pose(float x, float y, float ori)
191 {
192  // ignored
193 }
194 
195 void
196 ScanlineStar::set_pan_tilt(float pan, float tilt)
197 {
198  // ignored
199 }
200 
201 /** Skips the current ray and continues with the first valid scanline point of
202  * the next ray. */
203 void
204 ScanlineStar::skip_current_ray()
205 {
206  if (m_done) {
207  return;
208  }
209 
210  ++m_ray_iter;
211 
212  if (m_rays.end() == m_ray_iter) {
213  m_done = true;
214  return;
215  }
216 
217  ++m_ray_index;
218  m_first_on_ray = true;
219  m_point_iter = m_ray_iter->second->begin();
220  m_current_point = (*m_point_iter).second;
221 }
222 
223 /** Returns the number of segments in the model.
224  * @return the number of segments
225  */
226 unsigned int
227 ScanlineStar::num_rays() const
228 {
229  return m_num_rays;
230 }
231 
232 /** Return the index of the current ray.
233  * @return the index of the current ray
234  */
235 unsigned int
236 ScanlineStar::ray_index() const
237 {
238  return m_ray_index;
239 }
240 
241 /** Returns the radius of the current scanline point.
242  * @return the radius of the current scanline point
243  */
244 unsigned int
245 ScanlineStar::current_radius() const
246 {
247  return m_point_iter->first;
248 }
249 
250 /** Returns the angle of the current scanline point
251  * @return the angle of the current scanline point
252  */
253 float
254 ScanlineStar::current_angle() const
255 {
256  return m_ray_iter->first;
257 }
258 
259 /** Checks whether the current scanpoint is the first scanpoint on the
260  * current ray.
261  * @return true, if the it is the first scanpoint on the current ray
262  */
263 bool
264 ScanlineStar::first_on_ray() const
265 {
266  return m_first_on_ray;
267 }
268 
269 void
270 ScanlineStar::generate_scan_points()
271 {
272  float angle = 0.0;
273  unsigned int radius;
274  Ray * current_ray;
275  bool abort_ray;
276  YUV_t ignore(0);
277 
278  while (angle < deg2rad(359.9)) {
279  abort_ray = false;
280  radius = m_dead_radius;
281  current_ray = new Ray();
282 
283  while (!abort_ray) {
284  // calculate new (potential) scan point
285  upoint_t tmp;
286  tmp.x = m_center.x + (unsigned int)round(sin(angle) * radius);
287  tmp.y = m_center.y + (unsigned int)round(cos(angle) * radius);
288 
289  YUV_t current;
290  if (tmp.x >= m_image_width || tmp.y >= m_image_height)
291  // outside of the image
292  {
293  current = ignore;
294  abort_ray = true;
295  } else
296  // get mask value
297  {
298  current.Y = YUV422_PLANAR_Y_AT(m_mask, m_image_width, tmp.x, tmp.y);
299  current.U = YUV422_PLANAR_U_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
300  current.V = YUV422_PLANAR_V_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
301  }
302 
303  if (ignore.Y != current.Y && ignore.U != current.U && ignore.V != current.V)
304  // not masked
305  {
306  if (0 == m_previous_ray)
307  // no previous values, yet.
308  {
309  (*current_ray)[radius] = tmp;
310  m_first_ray = current_ray;
311  } else {
312  // calculate distance to last approved point on that radius
313  float dist_first = 3 * m_margin;
314  float dist_last = 3 * m_margin;
315  int diff_x;
316  int diff_y;
317 
318  if (m_first_ray->find(radius) != m_first_ray->end()) {
319  diff_x = tmp.x - (*m_first_ray)[radius].x;
320  diff_y = tmp.y - (*m_first_ray)[radius].y;
321  dist_first = sqrt(diff_x * diff_x + diff_y * diff_y);
322  }
323  if (m_previous_ray->find(radius) != m_previous_ray->end()) {
324  diff_x = tmp.x - (*m_previous_ray)[radius].x;
325  diff_y = tmp.y - (*m_previous_ray)[radius].y;
326  dist_last = sqrt(diff_x * diff_x + diff_y * diff_y);
327  }
328 
329  if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
330  // approve point (and add it to previous) if dist to last approved point
331  // on the current radius is larger than twice the margin
332  {
333  (*current_ray)[radius] = tmp;
334  }
335  }
336  }
337 
338  radius += m_radius_incr;
339 
340  if (radius > m_max_radius) {
341  abort_ray = true;
342  }
343  }
344 
345  if (!current_ray->empty())
346  // there are scanpoints on this ray
347  {
348  m_rays[angle] = current_ray;
349  m_previous_ray = current_ray;
350  } else {
351  delete current_ray;
352  }
353 
354  angle += m_angle_incr;
355  }
356 
357  m_num_rays = m_rays.size();
358 
359  /*
360  unsigned int num_rays = m_rays.size();
361  unsigned int num_points = 0;
362 
363  std::map<float, Ray*>::iterator rit;
364  for (rit = m_rays.begin(); rit != m_rays.end(); ++rit)
365  {
366  num_points += (*rit).second->size();
367  }
368  printf("Generated %d points in %d rays\n", num_points, num_rays);
369  */
370 }
371 
372 } // end namespace firevision
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
unsigned int x
x coordinate
Definition: types.h:36
unsigned int y
y coordinate
Definition: types.h:37
YUV pixel.
Definition: yuv.h:58
unsigned char V
V component.
Definition: yuv.h:61
unsigned char U
U component.
Definition: yuv.h:60
unsigned char Y
Y component.
Definition: yuv.h:59