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
30using namespace fawkes;
31
32namespace 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 */
55ScanlineStar::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. */
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
106{
107 return m_current_point;
108}
109
110upoint_t *
112{
113 return &m_current_point;
114}
115
116upoint_t *
118{
119 advance();
120 return &m_current_point;
121}
122
123upoint_t *
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. */
133void
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
159bool
161{
162 return m_done;
163}
164
165void
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
177const char *
179{
180 return "ScanlineModel::Star";
181}
182
183unsigned int
185{
186 return m_margin;
187}
188
189void
190ScanlineStar::set_robot_pose(float x, float y, float ori)
191{
192 // ignored
193}
194
195void
196ScanlineStar::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. */
203void
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 */
226unsigned int
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 */
235unsigned int
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 */
244unsigned int
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 */
253float
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 */
263bool
265{
266 return m_first_on_ray;
267}
268
269void
270ScanlineStar::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
void skip_current_ray()
Skips the current ray and continues with the first valid scanline point of the next ray.
Definition: star.cpp:204
virtual ~ScanlineStar()
Destructor.
Definition: star.cpp:96
fawkes::upoint_t operator*()
Get the current coordinate.
Definition: star.cpp:105
void set_pan_tilt(float pan, float tilt)
Set camera's pan/tilt values.
Definition: star.cpp:196
unsigned int current_radius() const
Returns the radius of the current scanline point.
Definition: star.cpp:245
void advance()
Calculates the next scanline point.
Definition: star.cpp:134
fawkes::upoint_t * operator->()
Get pointer to current point.
Definition: star.cpp:111
unsigned int ray_index() const
Return the index of the current ray.
Definition: star.cpp:236
fawkes::upoint_t * operator++()
Postfix ++ operator.
Definition: star.cpp:117
void reset()
Reset model.
Definition: star.cpp:166
const char * get_name()
Get name of scanline model.
Definition: star.cpp:178
ScanlineStar(unsigned int image_width, unsigned int image_height, unsigned int center_x, unsigned int center_y, unsigned int num_rays, unsigned int radius_incr, unsigned char *yuv_mask, unsigned int dead_radius=0, unsigned int max_radius=0, unsigned int margin=0)
Constructor.
Definition: star.cpp:55
bool first_on_ray() const
Checks whether the current scanpoint is the first scanpoint on the current ray.
Definition: star.cpp:264
void set_robot_pose(float x, float y, float ori)
Set the robot's pose.
Definition: star.cpp:190
unsigned int num_rays() const
Returns the number of segments in the model.
Definition: star.cpp:227
bool finished()
Check if all desired points have been processed.
Definition: star.cpp:160
unsigned int get_margin()
Get margin around points.
Definition: star.cpp:184
float current_angle() const
Returns the angle of the current scanline point.
Definition: star.cpp:254
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