24 #include <fvmodels/scanlines/star.h> 25 #include <fvutils/color/yuv.h> 26 #include <utils/math/angle.h> 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,
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;
72 m_dead_radius = dead_radius;
73 m_max_radius = max_radius;
76 m_angle_incr =
deg2rad( 360.0/m_num_rays );
81 m_first_on_ray =
true;
85 if (m_margin > m_radius_incr / 2)
87 m_margin = m_radius_incr / 2;
90 generate_scan_points();
97 ScanlineStar::~ScanlineStar()
99 std::map<float, Ray*>::iterator rit;
100 for (rit = m_rays.begin(); rit != m_rays.end(); ++rit)
107 ScanlineStar::operator*()
109 return m_current_point;
114 ScanlineStar::operator->()
116 return &m_current_point;
121 ScanlineStar::operator++()
124 return &m_current_point;
129 ScanlineStar::operator++(
int)
131 memcpy(&m_tmp_point, &m_current_point,
sizeof(
upoint_t));
140 ScanlineStar::advance()
142 if (m_done) {
return; }
145 m_first_on_ray =
false;
147 if ( (*m_ray_iter).second->end() == m_point_iter )
151 if ( m_rays.end() == m_ray_iter )
158 m_point_iter = (*m_ray_iter).second->begin();
159 m_first_on_ray =
true;
162 m_current_point = (*m_point_iter).second;
167 ScanlineStar::finished()
174 ScanlineStar::reset()
177 m_first_on_ray =
true;
180 m_ray_iter = m_rays.begin();
181 m_point_iter = (*m_ray_iter).second->begin();
182 m_current_point = (*m_point_iter).second;
187 ScanlineStar::get_name()
189 return "ScanlineModel::Star";
194 ScanlineStar::get_margin()
201 ScanlineStar::set_robot_pose(
float x,
float y,
float ori)
208 ScanlineStar::set_pan_tilt(
float pan,
float tilt)
217 ScanlineStar::skip_current_ray()
219 if (m_done) {
return; }
223 if ( m_rays.end() == m_ray_iter )
230 m_first_on_ray =
true;
231 m_point_iter = m_ray_iter->second->begin();
232 m_current_point = (*m_point_iter).second;
240 ScanlineStar::num_rays()
const 250 ScanlineStar::ray_index()
const 260 ScanlineStar::current_radius()
const 262 return m_point_iter->first;
270 ScanlineStar::current_angle()
const 272 return m_ray_iter->first;
280 ScanlineStar::first_on_ray()
const 282 return m_first_on_ray;
286 ScanlineStar::generate_scan_points()
294 while (angle <
deg2rad(359.9) )
297 radius = m_dead_radius;
298 current_ray =
new Ray();
304 tmp.
x = m_center.x + (
unsigned int) round( sin(angle) * radius );
305 tmp.
y = m_center.y + (
unsigned int) round( cos(angle) * radius );
308 if ( tmp.
x >= m_image_width || tmp.
y >= m_image_height )
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);
322 if ( ignore.
Y != current.
Y &&
323 ignore.
U != current.
U &&
324 ignore.
V != current.
V )
327 if (0 == m_previous_ray)
330 (*current_ray)[radius] = tmp;
331 m_first_ray = current_ray;
336 float dist_first = 3 * m_margin;
337 float dist_last = 3 * m_margin;
341 if ( m_first_ray->find(radius) != m_first_ray->end() )
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);
347 if ( m_previous_ray->find(radius) != m_previous_ray->end() )
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);
354 if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
358 (*current_ray)[radius] = tmp;
363 radius += m_radius_incr;
365 if (radius > m_max_radius) { abort_ray =
true; }
368 if ( !current_ray->empty() )
371 m_rays[angle] = current_ray;
372 m_previous_ray = current_ray;
379 angle += m_angle_incr;
382 m_num_rays = m_rays.size();
unsigned char V
V component.
Fawkes library namespace.
unsigned int y
y coordinate
unsigned int x
x coordinate
unsigned char Y
Y component.
Point with cartesian coordinates as unsigned integers.
unsigned char U
U component.
float deg2rad(float deg)
Convert an angle given in degrees to radians.