star.cpp

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

Generated on Tue Feb 22 13:31:17 2011 for Fawkes API by  doxygen 1.4.7