24#include <fvmodels/scanlines/star.h>
25#include <fvutils/color/yuv.h>
26#include <utils/math/angle.h>
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;
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();
98 std::map<float, Ray *>::iterator rit;
99 for (rit = m_rays.begin(); rit != m_rays.end(); ++rit) {
107 return m_current_point;
113 return &m_current_point;
120 return &m_current_point;
126 memcpy(&m_tmp_point, &m_current_point,
sizeof(
upoint_t));
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;
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;
180 return "ScanlineModel::Star";
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;
247 return m_point_iter->first;
256 return m_ray_iter->first;
266 return m_first_on_ray;
270ScanlineStar::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();
void skip_current_ray()
Skips the current ray and continues with the first valid scanline point of the next ray.
virtual ~ScanlineStar()
Destructor.
fawkes::upoint_t operator*()
Get the current coordinate.
void set_pan_tilt(float pan, float tilt)
Set camera's pan/tilt values.
unsigned int current_radius() const
Returns the radius of the current scanline point.
void advance()
Calculates the next scanline point.
fawkes::upoint_t * operator->()
Get pointer to current point.
unsigned int ray_index() const
Return the index of the current ray.
fawkes::upoint_t * operator++()
Postfix ++ operator.
const char * get_name()
Get name of scanline model.
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.
bool first_on_ray() const
Checks whether the current scanpoint is the first scanpoint on the current ray.
void set_robot_pose(float x, float y, float ori)
Set the robot's pose.
unsigned int num_rays() const
Returns the number of segments in the model.
bool finished()
Check if all desired points have been processed.
unsigned int get_margin()
Get margin around points.
float current_angle() const
Returns the angle of the current scanline point.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Point with cartesian coordinates as unsigned integers.
unsigned int x
x coordinate
unsigned int y
y coordinate
unsigned char V
V component.
unsigned char U
U component.
unsigned char Y
Y component.