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