Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * star.cpp - Starlike scanline model 00004 * 00005 * Created: Mon Nov 05 10:06:46 2007 00006 * Copyright 2007 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 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 /* just to make Emacs auto-indent happy */ 00034 } 00035 #endif 00036 00037 /** @class ScanlineStar <models/scanlines/star.h> 00038 * Star-like arranged scanline points. 00039 * 00040 * @author Daniel Beck 00041 */ 00042 00043 /** Constructor. 00044 * @param image_width width of the image 00045 * @param image_height height of the image 00046 * @param center_x x-coordinate of the center point 00047 * @param center_y y-coordinate of the center point 00048 * @param num_rays number of rays 00049 * @param radius_incr number of pixels by which the radius is increased 00050 * @param yuv_mask a mask allows to exclude certain regions of the image from 00051 * inspection. More precisely, no scanline points are generated in those 00052 * areas. The ignored regions have to be black, i.e. Y=0, U=127, V=127. 00053 * @param dead_radius number of pixels around the center that are disregarded 00054 * @param max_radius maximal radius in number of pixels 00055 * @param margin margin around every scanline point that does not contain any 00056 * other scanline point (in pixels) 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 // -- sanity checks -- 00084 // margin 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 /** Destructor. */ 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 /** Calculates the next scanline point. */ 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 // ignored 00204 } 00205 00206 00207 void 00208 ScanlineStar::set_pan_tilt(float pan, float tilt) 00209 { 00210 // ignored 00211 } 00212 00213 00214 /** Skips the current ray and continues with the first valid scanline point of 00215 * the next ray. */ 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 /** Returns the number of segments in the model. 00237 * @return the number of segments 00238 */ 00239 unsigned int 00240 ScanlineStar::num_rays() const 00241 { 00242 return m_num_rays; 00243 } 00244 00245 00246 /** Return the index of the current ray. 00247 * @return the index of the current ray 00248 */ 00249 unsigned int 00250 ScanlineStar::ray_index() const 00251 { 00252 return m_ray_index; 00253 } 00254 00255 00256 /** Returns the radius of the current scanline point. 00257 * @return the radius of the current scanline point 00258 */ 00259 unsigned int 00260 ScanlineStar::current_radius() const 00261 { 00262 return m_point_iter->first; 00263 } 00264 00265 00266 /** Returns the angle of the current scanline point 00267 * @return the angle of the current scanline point 00268 */ 00269 float 00270 ScanlineStar::current_angle() const 00271 { 00272 return m_ray_iter->first; 00273 } 00274 00275 /** Checks whether the current scanpoint is the first scanpoint on the 00276 * current ray. 00277 * @return true, if the it is the first scanpoint on the current ray 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 // calculate new (potential) scan point 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 // outside of the image 00310 { 00311 current = ignore; 00312 abort_ray = true; 00313 } 00314 else 00315 // get mask value 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 // not masked 00326 { 00327 if (0 == m_previous_ray) 00328 // no previous values, yet. 00329 { 00330 (*current_ray)[radius] = tmp; 00331 m_first_ray = current_ray; 00332 } 00333 else 00334 { 00335 // calculate distance to last approved point on that radius 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 // approve point (and add it to previous) if dist to last approved point 00356 // on the current radius is larger than twice the margin 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 // there are scanpoints on this ray 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 unsigned int num_rays = m_rays.size(); 00386 unsigned int num_points = 0; 00387 00388 std::map<float, Ray*>::iterator rit; 00389 for (rit = m_rays.begin(); rit != m_rays.end(); ++rit) 00390 { 00391 num_points += (*rit).second->size(); 00392 } 00393 printf("Generated %d points in %d rays\n", num_points, num_rays); 00394 */ 00395 } 00396 00397 } // end namespace firevision