00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <models/scanlines/star.h>
00025 #include <fvutils/color/yuv.h>
00026 #include <utils/math/angle.h>
00027
00028 #include <cstring>
00029
00030 using namespace fawkes;
00031
00032 namespace firevision {
00033 #if 0
00034 }
00035 #endif
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 ScanlineStar::ScanlineStar( unsigned int image_width, unsigned int image_height,
00059 unsigned int center_x, unsigned int center_y,
00060 unsigned int num_rays, unsigned int radius_incr,
00061 unsigned char* yuv_mask,
00062 unsigned int dead_radius, unsigned int max_radius,
00063 unsigned int margin)
00064 {
00065 m_image_width = image_width;
00066 m_image_height = image_height;
00067 m_center.x = center_x;
00068 m_center.y = center_y;
00069 m_num_rays = num_rays;
00070 m_radius_incr = radius_incr;
00071 m_mask = yuv_mask;
00072 m_dead_radius = dead_radius;
00073 m_max_radius = max_radius;
00074 m_margin = margin;
00075
00076 m_angle_incr = deg2rad( 360.0/m_num_rays );
00077
00078 m_first_ray = 0;
00079 m_previous_ray = 0;
00080
00081 m_first_on_ray = true;
00082
00083
00084
00085 if (m_margin > m_radius_incr / 2)
00086 {
00087 m_margin = m_radius_incr / 2;
00088 }
00089
00090 generate_scan_points();
00091
00092 reset();
00093 }
00094
00095
00096
00097 ScanlineStar::~ScanlineStar()
00098 {
00099 std::map<float, Ray*>::iterator rit;
00100 for (rit = m_rays.begin(); rit != m_rays.end(); ++rit)
00101 {
00102 delete rit->second;
00103 }
00104 }
00105
00106 point_t
00107 ScanlineStar::operator*()
00108 {
00109 return m_current_point;
00110 }
00111
00112
00113 point_t*
00114 ScanlineStar::operator->()
00115 {
00116 return &m_current_point;
00117 }
00118
00119
00120 point_t*
00121 ScanlineStar::operator++()
00122 {
00123 advance();
00124 return &m_current_point;
00125 }
00126
00127
00128 point_t*
00129 ScanlineStar::operator++(int)
00130 {
00131 memcpy(&m_tmp_point, &m_current_point, sizeof(point_t));
00132 advance();
00133
00134 return &m_tmp_point;
00135 }
00136
00137
00138
00139 void
00140 ScanlineStar::advance()
00141 {
00142 if (m_done) { return; }
00143
00144 ++m_point_iter;
00145 m_first_on_ray = false;
00146
00147 if ( (*m_ray_iter).second->end() == m_point_iter )
00148 {
00149 ++m_ray_iter;
00150
00151 if ( m_rays.end() == m_ray_iter )
00152 {
00153 m_done = true;
00154 return;
00155 }
00156
00157 ++m_ray_index;
00158 m_point_iter = (*m_ray_iter).second->begin();
00159 m_first_on_ray = true;
00160 }
00161
00162 m_current_point = (*m_point_iter).second;
00163 }
00164
00165
00166 bool
00167 ScanlineStar::finished()
00168 {
00169 return m_done;
00170 }
00171
00172
00173 void
00174 ScanlineStar::reset()
00175 {
00176 m_done = false;
00177 m_first_on_ray = true;
00178
00179 m_ray_index = 0;
00180 m_ray_iter = m_rays.begin();
00181 m_point_iter = (*m_ray_iter).second->begin();
00182 m_current_point = (*m_point_iter).second;
00183 }
00184
00185
00186 const char*
00187 ScanlineStar::get_name()
00188 {
00189 return "ScanlineModel::Star";
00190 }
00191
00192
00193 unsigned int
00194 ScanlineStar::get_margin()
00195 {
00196 return m_margin;
00197 }
00198
00199
00200 void
00201 ScanlineStar::set_robot_pose(float x, float y, float ori)
00202 {
00203
00204 }
00205
00206
00207 void
00208 ScanlineStar::set_pan_tilt(float pan, float tilt)
00209 {
00210
00211 }
00212
00213
00214
00215
00216 void
00217 ScanlineStar::skip_current_ray()
00218 {
00219 if (m_done) { return; }
00220
00221 ++m_ray_iter;
00222
00223 if ( m_rays.end() == m_ray_iter )
00224 {
00225 m_done = true;
00226 return;
00227 }
00228
00229 ++m_ray_index;
00230 m_first_on_ray = true;
00231 m_point_iter = m_ray_iter->second->begin();
00232 m_current_point = (*m_point_iter).second;
00233 }
00234
00235
00236
00237
00238
00239 unsigned int
00240 ScanlineStar::num_rays() const
00241 {
00242 return m_num_rays;
00243 }
00244
00245
00246
00247
00248
00249 unsigned int
00250 ScanlineStar::ray_index() const
00251 {
00252 return m_ray_index;
00253 }
00254
00255
00256
00257
00258
00259 unsigned int
00260 ScanlineStar::current_radius() const
00261 {
00262 return m_point_iter->first;
00263 }
00264
00265
00266
00267
00268
00269 float
00270 ScanlineStar::current_angle() const
00271 {
00272 return m_ray_iter->first;
00273 }
00274
00275
00276
00277
00278
00279 bool
00280 ScanlineStar::first_on_ray() const
00281 {
00282 return m_first_on_ray;
00283 }
00284
00285 void
00286 ScanlineStar::generate_scan_points()
00287 {
00288 float angle = 0.0;
00289 unsigned int radius;
00290 Ray* current_ray;
00291 bool abort_ray;
00292 YUV_t ignore(0);
00293
00294 while (angle < deg2rad(359.9) )
00295 {
00296 abort_ray = false;
00297 radius = m_dead_radius;
00298 current_ray = new Ray();
00299
00300 while ( !abort_ray )
00301 {
00302
00303 point_t tmp;
00304 tmp.x = m_center.x + (unsigned int) round( sin(angle) * radius );
00305 tmp.y = m_center.y + (unsigned int) round( cos(angle) * radius );
00306
00307 YUV_t current;
00308 if ( tmp.x >= m_image_width || tmp.y >= m_image_height )
00309
00310 {
00311 current = ignore;
00312 abort_ray = true;
00313 }
00314 else
00315
00316 {
00317 current.Y = YUV422_PLANAR_Y_AT(m_mask, m_image_width, tmp.x, tmp.y);
00318 current.U = YUV422_PLANAR_U_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
00319 current.V = YUV422_PLANAR_V_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
00320 }
00321
00322 if ( ignore.Y != current.Y &&
00323 ignore.U != current.U &&
00324 ignore.V != current.V )
00325
00326 {
00327 if (0 == m_previous_ray)
00328
00329 {
00330 (*current_ray)[radius] = tmp;
00331 m_first_ray = current_ray;
00332 }
00333 else
00334 {
00335
00336 float dist_first = 3 * m_margin;
00337 float dist_last = 3 * m_margin;
00338 int diff_x;
00339 int diff_y;
00340
00341 if ( m_first_ray->find(radius) != m_first_ray->end() )
00342 {
00343 diff_x = tmp.x - (*m_first_ray)[radius].x;
00344 diff_y = tmp.y - (*m_first_ray)[radius].y;
00345 dist_first = sqrt(diff_x * diff_x + diff_y * diff_y);
00346 }
00347 if ( m_previous_ray->find(radius) != m_previous_ray->end() )
00348 {
00349 diff_x = tmp.x - (*m_previous_ray)[radius].x;
00350 diff_y = tmp.y - (*m_previous_ray)[radius].y;
00351 dist_last = sqrt(diff_x * diff_x + diff_y * diff_y);
00352 }
00353
00354 if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
00355
00356
00357 {
00358 (*current_ray)[radius] = tmp;
00359 }
00360 }
00361 }
00362
00363 radius += m_radius_incr;
00364
00365 if (radius > m_max_radius) { abort_ray = true; }
00366 }
00367
00368 if ( !current_ray->empty() )
00369
00370 {
00371 m_rays[angle] = current_ray;
00372 m_previous_ray = current_ray;
00373 }
00374 else
00375 {
00376 delete current_ray;
00377 }
00378
00379 angle += m_angle_incr;
00380 }
00381
00382 m_num_rays = m_rays.size();
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395 }
00396
00397 }