24 #include <fvmodels/scanlines/star.h>
25 #include <fvutils/color/yuv.h>
26 #include <utils/math/angle.h>
32 namespace firevision {
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,
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;
73 m_dead_radius = dead_radius;
74 m_max_radius = max_radius;
77 m_angle_incr =
deg2rad(360.0 / m_num_rays);
82 m_first_on_ray =
true;
86 if (m_margin > m_radius_incr / 2) {
87 m_margin = m_radius_incr / 2;
90 generate_scan_points();
96 ScanlineStar::~ScanlineStar()
98 std::map<float, Ray *>::iterator rit;
99 for (rit = m_rays.begin(); rit != m_rays.end(); ++rit) {
105 ScanlineStar::operator*()
107 return m_current_point;
111 ScanlineStar::operator->()
113 return &m_current_point;
117 ScanlineStar::operator++()
120 return &m_current_point;
124 ScanlineStar::operator++(
int)
126 memcpy(&m_tmp_point, &m_current_point,
sizeof(
upoint_t));
134 ScanlineStar::advance()
141 m_first_on_ray =
false;
143 if ((*m_ray_iter).second->end() == m_point_iter) {
146 if (m_rays.end() == m_ray_iter) {
152 m_point_iter = (*m_ray_iter).second->begin();
153 m_first_on_ray =
true;
156 m_current_point = (*m_point_iter).second;
160 ScanlineStar::finished()
166 ScanlineStar::reset()
169 m_first_on_ray =
true;
172 m_ray_iter = m_rays.begin();
173 m_point_iter = (*m_ray_iter).second->begin();
174 m_current_point = (*m_point_iter).second;
178 ScanlineStar::get_name()
180 return "ScanlineModel::Star";
184 ScanlineStar::get_margin()
190 ScanlineStar::set_robot_pose(
float x,
float y,
float ori)
196 ScanlineStar::set_pan_tilt(
float pan,
float tilt)
204 ScanlineStar::skip_current_ray()
212 if (m_rays.end() == m_ray_iter) {
218 m_first_on_ray =
true;
219 m_point_iter = m_ray_iter->second->begin();
220 m_current_point = (*m_point_iter).second;
227 ScanlineStar::num_rays()
const
236 ScanlineStar::ray_index()
const
245 ScanlineStar::current_radius()
const
247 return m_point_iter->first;
254 ScanlineStar::current_angle()
const
256 return m_ray_iter->first;
264 ScanlineStar::first_on_ray()
const
266 return m_first_on_ray;
270 ScanlineStar::generate_scan_points()
278 while (angle <
deg2rad(359.9)) {
280 radius = m_dead_radius;
281 current_ray =
new Ray();
286 tmp.
x = m_center.x + (
unsigned int)round(sin(angle) * radius);
287 tmp.
y = m_center.y + (
unsigned int)round(cos(angle) * radius);
290 if (tmp.
x >= m_image_width || tmp.
y >= m_image_height)
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);
303 if (ignore.Y != current.
Y && ignore.U != current.
U && ignore.V != current.
V)
306 if (0 == m_previous_ray)
309 (*current_ray)[radius] = tmp;
310 m_first_ray = current_ray;
313 float dist_first = 3 * m_margin;
314 float dist_last = 3 * m_margin;
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);
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);
329 if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
333 (*current_ray)[radius] = tmp;
338 radius += m_radius_incr;
340 if (radius > m_max_radius) {
345 if (!current_ray->empty())
348 m_rays[angle] = current_ray;
349 m_previous_ray = current_ray;
354 angle += m_angle_incr;
357 m_num_rays = m_rays.size();