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) {
106 return m_current_point;
109 upoint_t *ScanlineStar::operator->()
111 return &m_current_point;
115 ScanlineStar::operator++()
118 return &m_current_point;
122 ScanlineStar::operator++(
int)
124 memcpy(&m_tmp_point, &m_current_point,
sizeof(
upoint_t));
132 ScanlineStar::advance()
139 m_first_on_ray =
false;
141 if ((*m_ray_iter).second->end() == m_point_iter) {
144 if (m_rays.end() == m_ray_iter) {
150 m_point_iter = (*m_ray_iter).second->begin();
151 m_first_on_ray =
true;
154 m_current_point = (*m_point_iter).second;
158 ScanlineStar::finished()
164 ScanlineStar::reset()
167 m_first_on_ray =
true;
170 m_ray_iter = m_rays.begin();
171 m_point_iter = (*m_ray_iter).second->begin();
172 m_current_point = (*m_point_iter).second;
176 ScanlineStar::get_name()
178 return "ScanlineModel::Star";
182 ScanlineStar::get_margin()
188 ScanlineStar::set_robot_pose(
float x,
float y,
float ori)
194 ScanlineStar::set_pan_tilt(
float pan,
float tilt)
202 ScanlineStar::skip_current_ray()
210 if (m_rays.end() == m_ray_iter) {
216 m_first_on_ray =
true;
217 m_point_iter = m_ray_iter->second->begin();
218 m_current_point = (*m_point_iter).second;
225 ScanlineStar::num_rays()
const
234 ScanlineStar::ray_index()
const
243 ScanlineStar::current_radius()
const
245 return m_point_iter->first;
252 ScanlineStar::current_angle()
const
254 return m_ray_iter->first;
262 ScanlineStar::first_on_ray()
const
264 return m_first_on_ray;
268 ScanlineStar::generate_scan_points()
276 while (angle <
deg2rad(359.9)) {
278 radius = m_dead_radius;
279 current_ray =
new Ray();
284 tmp.
x = m_center.x + (
unsigned int)round(sin(angle) * radius);
285 tmp.
y = m_center.y + (
unsigned int)round(cos(angle) * radius);
288 if (tmp.
x >= m_image_width || tmp.
y >= m_image_height)
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);
301 if (ignore.Y != current.
Y && ignore.U != current.
U && ignore.V != current.
V)
304 if (0 == m_previous_ray)
307 (*current_ray)[radius] = tmp;
308 m_first_ray = current_ray;
311 float dist_first = 3 * m_margin;
312 float dist_last = 3 * m_margin;
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);
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);
327 if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
331 (*current_ray)[radius] = tmp;
336 radius += m_radius_incr;
338 if (radius > m_max_radius) {
343 if (!current_ray->empty())
346 m_rays[angle] = current_ray;
347 m_previous_ray = current_ray;
352 angle += m_angle_incr;
355 m_num_rays = m_rays.size();