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 ScanlineStar::operator*()
105 {
106  return m_current_point;
107 }
108 
109 upoint_t *ScanlineStar::operator->()
110 {
111  return &m_current_point;
112 }
113 
114 upoint_t *
115 ScanlineStar::operator++()
116 {
117  advance();
118  return &m_current_point;
119 }
120 
121 upoint_t *
122 ScanlineStar::operator++(int)
123 {
124  memcpy(&m_tmp_point, &m_current_point, sizeof(upoint_t));
125  advance();
126 
127  return &m_tmp_point;
128 }
129 
130 /** Calculates the next scanline point. */
131 void
132 ScanlineStar::advance()
133 {
134  if (m_done) {
135  return;
136  }
137 
138  ++m_point_iter;
139  m_first_on_ray = false;
140 
141  if ((*m_ray_iter).second->end() == m_point_iter) {
142  ++m_ray_iter;
143 
144  if (m_rays.end() == m_ray_iter) {
145  m_done = true;
146  return;
147  }
148 
149  ++m_ray_index;
150  m_point_iter = (*m_ray_iter).second->begin();
151  m_first_on_ray = true;
152  }
153 
154  m_current_point = (*m_point_iter).second;
155 }
156 
157 bool
158 ScanlineStar::finished()
159 {
160  return m_done;
161 }
162 
163 void
164 ScanlineStar::reset()
165 {
166  m_done = false;
167  m_first_on_ray = true;
168 
169  m_ray_index = 0;
170  m_ray_iter = m_rays.begin();
171  m_point_iter = (*m_ray_iter).second->begin();
172  m_current_point = (*m_point_iter).second;
173 }
174 
175 const char *
176 ScanlineStar::get_name()
177 {
178  return "ScanlineModel::Star";
179 }
180 
181 unsigned int
182 ScanlineStar::get_margin()
183 {
184  return m_margin;
185 }
186 
187 void
188 ScanlineStar::set_robot_pose(float x, float y, float ori)
189 {
190  // ignored
191 }
192 
193 void
194 ScanlineStar::set_pan_tilt(float pan, float tilt)
195 {
196  // ignored
197 }
198 
199 /** Skips the current ray and continues with the first valid scanline point of
200  * the next ray. */
201 void
202 ScanlineStar::skip_current_ray()
203 {
204  if (m_done) {
205  return;
206  }
207 
208  ++m_ray_iter;
209 
210  if (m_rays.end() == m_ray_iter) {
211  m_done = true;
212  return;
213  }
214 
215  ++m_ray_index;
216  m_first_on_ray = true;
217  m_point_iter = m_ray_iter->second->begin();
218  m_current_point = (*m_point_iter).second;
219 }
220 
221 /** Returns the number of segments in the model.
222  * @return the number of segments
223  */
224 unsigned int
225 ScanlineStar::num_rays() const
226 {
227  return m_num_rays;
228 }
229 
230 /** Return the index of the current ray.
231  * @return the index of the current ray
232  */
233 unsigned int
234 ScanlineStar::ray_index() const
235 {
236  return m_ray_index;
237 }
238 
239 /** Returns the radius of the current scanline point.
240  * @return the radius of the current scanline point
241  */
242 unsigned int
243 ScanlineStar::current_radius() const
244 {
245  return m_point_iter->first;
246 }
247 
248 /** Returns the angle of the current scanline point
249  * @return the angle of the current scanline point
250  */
251 float
252 ScanlineStar::current_angle() const
253 {
254  return m_ray_iter->first;
255 }
256 
257 /** Checks whether the current scanpoint is the first scanpoint on the
258  * current ray.
259  * @return true, if the it is the first scanpoint on the current ray
260  */
261 bool
262 ScanlineStar::first_on_ray() const
263 {
264  return m_first_on_ray;
265 }
266 
267 void
268 ScanlineStar::generate_scan_points()
269 {
270  float angle = 0.0;
271  unsigned int radius;
272  Ray * current_ray;
273  bool abort_ray;
274  YUV_t ignore(0);
275 
276  while (angle < deg2rad(359.9)) {
277  abort_ray = false;
278  radius = m_dead_radius;
279  current_ray = new Ray();
280 
281  while (!abort_ray) {
282  // calculate new (potential) scan point
283  upoint_t tmp;
284  tmp.x = m_center.x + (unsigned int)round(sin(angle) * radius);
285  tmp.y = m_center.y + (unsigned int)round(cos(angle) * radius);
286 
287  YUV_t current;
288  if (tmp.x >= m_image_width || tmp.y >= m_image_height)
289  // outside of the image
290  {
291  current = ignore;
292  abort_ray = true;
293  } else
294  // get mask value
295  {
296  current.Y = YUV422_PLANAR_Y_AT(m_mask, m_image_width, tmp.x, tmp.y);
297  current.U = YUV422_PLANAR_U_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
298  current.V = YUV422_PLANAR_V_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
299  }
300 
301  if (ignore.Y != current.Y && ignore.U != current.U && ignore.V != current.V)
302  // not masked
303  {
304  if (0 == m_previous_ray)
305  // no previous values, yet.
306  {
307  (*current_ray)[radius] = tmp;
308  m_first_ray = current_ray;
309  } else {
310  // calculate distance to last approved point on that radius
311  float dist_first = 3 * m_margin;
312  float dist_last = 3 * m_margin;
313  int diff_x;
314  int diff_y;
315 
316  if (m_first_ray->find(radius) != m_first_ray->end()) {
317  diff_x = tmp.x - (*m_first_ray)[radius].x;
318  diff_y = tmp.y - (*m_first_ray)[radius].y;
319  dist_first = sqrt(diff_x * diff_x + diff_y * diff_y);
320  }
321  if (m_previous_ray->find(radius) != m_previous_ray->end()) {
322  diff_x = tmp.x - (*m_previous_ray)[radius].x;
323  diff_y = tmp.y - (*m_previous_ray)[radius].y;
324  dist_last = sqrt(diff_x * diff_x + diff_y * diff_y);
325  }
326 
327  if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
328  // approve point (and add it to previous) if dist to last approved point
329  // on the current radius is larger than twice the margin
330  {
331  (*current_ray)[radius] = tmp;
332  }
333  }
334  }
335 
336  radius += m_radius_incr;
337 
338  if (radius > m_max_radius) {
339  abort_ray = true;
340  }
341  }
342 
343  if (!current_ray->empty())
344  // there are scanpoints on this ray
345  {
346  m_rays[angle] = current_ray;
347  m_previous_ray = current_ray;
348  } else {
349  delete current_ray;
350  }
351 
352  angle += m_angle_incr;
353  }
354 
355  m_num_rays = m_rays.size();
356 
357  /*
358  unsigned int num_rays = m_rays.size();
359  unsigned int num_points = 0;
360 
361  std::map<float, Ray*>::iterator rit;
362  for (rit = m_rays.begin(); rit != m_rays.end(); ++rit)
363  {
364  num_points += (*rit).second->size();
365  }
366  printf("Generated %d points in %d rays\n", num_points, num_rays);
367  */
368 }
369 
370 } // end namespace firevision
fawkes::upoint_t
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
firevision::YUV_t_struct
YUV pixel.
Definition: yuv.h:64
firevision::YUV_t_struct::Y
unsigned char Y
Y component.
Definition: yuv.h:66
fawkes
fawkes::upoint_t::y
unsigned int y
y coordinate
Definition: types.h:37
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
fawkes::upoint_t::x
unsigned int x
x coordinate
Definition: types.h:36
firevision::YUV_t_struct::U
unsigned char U
U component.
Definition: yuv.h:67
firevision::YUV_t_struct::V
unsigned char V
V component.
Definition: yuv.h:68