00001
00002 /***************************************************************************
00003  *  star.h - Starlike scanline model
00004  *
00005  *  Generated: 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 /** @class ScanlineStar <models/scanlines/star.h>
00033  * Star-like arranged scanline points.
00034  *
00035  * @author Daniel Beck
00036  */
00037 
00038 /** Constructor.
00039  * @param image_width width of the image
00040  * @param image_height height of the image
00041  * @param center_x x-coordinate of the center point
00042  * @param center_y y-coordinate of the center point
00043  * @param num_rays number of rays
00044  * @param radius_incr number of pixels by which the radius is increased
00045  * @param yuv_mask a mask allows to exclude certain regions of the image from
00046  *        inspection. More precisely, no scanline points are generated in those
00047  *        areas. The ignored regions have to be black, i.e. Y=0, U=127, V=127.
00048  * @param dead_radius number of pixels around the center that are disregarded
00049  * @param max_radius maximal radius in number of pixels
00050  * @param margin margin around every scanline point that does not contain any
00051  *               other scanline point (in pixels)
00052  */
00053 ScanlineStar::ScanlineStar( unsigned int image_width, unsigned int image_height,
00054                             unsigned int center_x, unsigned int center_y,
00055                             unsigned int num_rays, unsigned int radius_incr,
00056                             unsigned char* yuv_mask,
00057                             unsigned int dead_radius, unsigned int max_radius,
00058                             unsigned int margin)
00059 {
00060   m_image_width = image_width;
00061   m_image_height = image_height;
00062   m_center.x = center_x;
00063   m_center.y = center_y;
00064   m_num_rays = num_rays;
00065   m_radius_incr = radius_incr;
00066   m_mask = yuv_mask;
00067   m_dead_radius = dead_radius;
00068   m_max_radius = max_radius;
00069   m_margin = margin;
00070
00071   m_angle_incr = deg2rad( 360.0/m_num_rays );
00072
00073   m_first_ray = 0;
00074   m_previous_ray = 0;
00075
00076   m_first_on_ray = true;
00077
00078   // -- sanity checks --
00079   // margin
00080   if (m_margin > m_radius_incr / 2)
00081     {
00082       m_margin = m_radius_incr / 2;
00083     }
00084
00085   generate_scan_points();
00086
00087   reset();
00088 }
00089
00090 
00091 /** Destructor. */
00092 ScanlineStar::~ScanlineStar()
00093 {
00094   std::map<float, Ray*>::iterator rit;
00095   for (rit = m_rays.begin(); rit != m_rays.end(); ++rit)
00096     {
00097       delete rit->second;
00098     }
00099 }
00100
00101 point_t
00102 ScanlineStar::operator*()
00103 {
00104   return m_current_point;
00105 }
00106
00107
00108 point_t*
00109 ScanlineStar::operator->()
00110 {
00111   return &m_current_point;
00112 }
00113
00114
00115 point_t*
00116 ScanlineStar::operator++()
00117 {
00118   advance();
00119   return &m_current_point;
00120 }
00121
00122
00123 point_t*
00124 ScanlineStar::operator++(int)
00125 {
00126   memcpy(&m_tmp_point, &m_current_point, sizeof(point_t));
00127   advance();
00128
00129   return &m_tmp_point;
00130 }
00131
00132 
00133 /** Calculates the next scanline point. */
00134 void
00135 ScanlineStar::advance()
00136 {
00137   if (m_done) { return; }
00138
00139   ++m_point_iter;
00140   m_first_on_ray = false;
00141
00142   if ( (*m_ray_iter).second->end() == m_point_iter )
00143     {
00144       ++m_ray_iter;
00145
00146       if ( m_rays.end() == m_ray_iter )
00147         {
00148           m_done = true;
00149           return;
00150         }
00151
00152       ++m_ray_index;
00153       m_point_iter = (*m_ray_iter).second->begin();
00154       m_first_on_ray = true;
00155     }
00156
00157   m_current_point = (*m_point_iter).second;
00158 }
00159
00160
00161 bool
00162 ScanlineStar::finished()
00163 {
00164   return m_done;
00165 }
00166
00167
00168 void
00169 ScanlineStar::reset()
00170 {
00171   m_done = false;
00172   m_first_on_ray = true;
00173
00174   m_ray_index = 0;
00175   m_ray_iter = m_rays.begin();
00176   m_point_iter = (*m_ray_iter).second->begin();
00177   m_current_point = (*m_point_iter).second;
00178 }
00179
00180
00181 const char*
00182 ScanlineStar::get_name()
00183 {
00184   return "ScanlineModel::Star";
00185 }
00186
00187
00188 unsigned int
00189 ScanlineStar::get_margin()
00190 {
00191   return m_margin;
00192 }
00193
00194
00195 void
00196 ScanlineStar::set_robot_pose(float x, float y, float ori)
00197 {
00198   // ignored
00199 }
00200
00201
00202 void
00203 ScanlineStar::set_pan_tilt(float pan, float tilt)
00204 {
00205   // ignored
00206 }
00207
00208 
00209 /** Skips the current ray and continues with the first valid scanline point of
00210  * the next ray. */
00211 void
00212 ScanlineStar::skip_current_ray()
00213 {
00214   if (m_done) { return; }
00215
00216   ++m_ray_iter;
00217
00218   if ( m_rays.end() == m_ray_iter )
00219     {
00220       m_done = true;
00221       return;
00222     }
00223
00224   ++m_ray_index;
00225   m_first_on_ray = true;
00226   m_point_iter = m_ray_iter->second->begin();
00227   m_current_point = (*m_point_iter).second;
00228 }
00229
00230 
00231 /** Returns the number of segments in the model.
00232  * @return the number of segments
00233  */
00234 unsigned int
00235 ScanlineStar::num_rays() const
00236 {
00237   return m_num_rays;
00238 }
00239
00240 
00241 /** Return the index of the current ray.
00242  * @return the index of the current ray
00243  */
00244 unsigned int
00245 ScanlineStar::ray_index() const
00246 {
00247   return m_ray_index;
00248 }
00249
00250 
00251 /** Returns the radius of the current scanline point.
00252  * @return the radius of the current scanline point
00253  */
00254 unsigned int
00255 ScanlineStar::current_radius() const
00256 {
00257   return m_point_iter->first;
00258 }
00259
00260 
00261 /** Returns the angle of the current scanline point
00262  * @return the angle of the current scanline point
00263  */
00264 float
00265 ScanlineStar::current_angle() const
00266 {
00267   return m_ray_iter->first;
00268 }
00269 
00270 /** Checks whether the current scanpoint is the first scanpoint on the
00271  * current ray.
00272  * @return true, if the it is the first scanpoint on the current ray
00273  */
00274 bool
00275 ScanlineStar::first_on_ray() const
00276 {
00277   return m_first_on_ray;
00278 }
00279
00280 void
00281 ScanlineStar::generate_scan_points()
00282 {
00283   float angle = 0.0;
00284   unsigned int radius;
00285   Ray* current_ray;
00286   bool abort_ray;
00287   YUV_t ignore(0);
00288
00289   while (angle < deg2rad(359.9) )
00290     {
00291       abort_ray = false;
00292       radius = m_dead_radius;
00293       current_ray = new Ray();
00294
00295       while ( !abort_ray )
00296         {
00297           // calculate new (potential) scan point
00298           point_t tmp;
00299           tmp.x = m_center.x + (unsigned int) round( sin(angle) * radius );
00300           tmp.y = m_center.y + (unsigned int) round( cos(angle) * radius );
00301
00302           YUV_t current;
00303           if ( tmp.x >= m_image_width || tmp.y >= m_image_height )
00304             // outside of the image
00305             {
00306               current = ignore;
00307               abort_ray = true;
00308             }
00309           else
00310             // get mask value
00311             {
00312               current.Y = YUV422_PLANAR_Y_AT(m_mask, m_image_width, tmp.x, tmp.y);
00313               current.U = YUV422_PLANAR_U_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
00314               current.V = YUV422_PLANAR_V_AT(m_mask, m_image_width, m_image_height, tmp.x, tmp.y);
00315             }
00316
00317           if ( ignore.Y != current.Y &&
00318                ignore.U != current.U &&
00319                ignore.V != current.V )
00320             // not masked
00321             {
00322               if (0 == m_previous_ray)
00323                 // no previous values, yet.
00324                 {
00325                   (*current_ray)[radius] = tmp;
00326                   m_first_ray = current_ray;
00327                 }
00328               else
00329                 {
00330                   // calculate distance to last approved point on that radius
00331                   float dist_first = 3 * m_margin;
00332                   float dist_last = 3 * m_margin;
00333                   int diff_x;
00334                   int diff_y;
00335
00336                   if ( m_first_ray->find(radius) != m_first_ray->end() )
00337                     {
00338                       diff_x = tmp.x - (*m_first_ray)[radius].x;
00339                       diff_y = tmp.y - (*m_first_ray)[radius].y;
00340                       dist_first = sqrt(diff_x * diff_x + diff_y * diff_y);
00341                     }
00342                   if ( m_previous_ray->find(radius) != m_previous_ray->end() )
00343                     {
00344                       diff_x = tmp.x - (*m_previous_ray)[radius].x;
00345                       diff_y = tmp.y - (*m_previous_ray)[radius].y;
00346                       dist_last = sqrt(diff_x * diff_x + diff_y * diff_y);
00347                     }
00348
00349                   if (dist_first > 2 * m_margin && dist_last > 2 * m_margin)
00350                     // approve point (and add it to previous) if dist to last approved point
00351                     // on the current radius is larger than twice the margin
00352                     {
00353                       (*current_ray)[radius] = tmp;
00354                     }
00355                 }
00356             }
00357
00358           radius += m_radius_incr;
00359
00360           if (radius > m_max_radius) { abort_ray = true; }
00361         }
00362
00363       if ( !current_ray->empty() )
00364         // there are scanpoints on this ray
00365         {
00366           m_rays[angle] = current_ray;
00367           m_previous_ray = current_ray;
00368         }
00369       else
00370         {
00371           delete current_ray;
00372         }
00373
00374       angle += m_angle_incr;
00375     }
00376
00377   m_num_rays = m_rays.size();
00378
00379   /*
00380   unsigned int num_rays = m_rays.size();
00381   unsigned int num_points = 0;
00382 
00383   std::map<float, Ray*>::iterator rit;
00384   for (rit = m_rays.begin(); rit != m_rays.end(); ++rit)
00385     {
00386       num_points += (*rit).second->size();
00387     }
00388   printf("Generated %d points in %d rays\n", num_points, num_rays);
00389   */
00390 }