Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * radial.cpp - Implementation of the radial scanline model 00004 * 00005 * Created: Tue Jul 19 12:46:52 2005 00006 * Copyright 2005 Tim Niemueller [www.niemueller.de] 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/radial.h> 00025 00026 #include <utils/system/console_colors.h> 00027 00028 #include <cmath> 00029 #include <cstring> 00030 00031 using fawkes::point_t; 00032 00033 namespace firevision { 00034 #if 0 /* just to make Emacs auto-indent happy */ 00035 } 00036 #endif 00037 00038 /** @class ScanlineRadial <models/scanlines/radial.h> 00039 * Radial scanlines. 00040 * Uses circles to generate scanline points. A dead radius is ignored in the 00041 * center of the image (for example for the camera itself in an omni-vision system). 00042 * From there circles are used in radius_increment distances. On each circle points are 00043 * generated in a distance of about step pixels. This is done up to a given maximum 00044 * radius. If no maximum radius is supplied (max_radius=0) it is automatically 00045 * calculated depending on the image size. 00046 */ 00047 00048 /** Constructor. 00049 * @param width image width 00050 * @param height image height 00051 * @param center_x radial center center x 00052 * @param center_y radial center center y 00053 * @param radius_increment radius increment 00054 * @param step step 00055 * @param max_radius maximum radius, if set to 0 will be calculated 00056 * automatically depending on the image dimensions. 00057 * @param dead_radius inner radius to ignore 00058 */ 00059 ScanlineRadial::ScanlineRadial(unsigned int width, unsigned int height, 00060 unsigned int center_x, unsigned int center_y, 00061 unsigned int radius_increment, 00062 unsigned int step, 00063 unsigned int max_radius, unsigned int dead_radius 00064 ) 00065 { 00066 this->width = width; 00067 this->height = height; 00068 this->center_x = center_x; 00069 this->center_y = center_y; 00070 this->radius_increment = radius_increment; 00071 this->step = step; 00072 this->dead_radius = dead_radius; 00073 this->max_radius = max_radius; 00074 this->auto_max_radius = (max_radius == 0); 00075 00076 reset(); 00077 } 00078 00079 point_t 00080 ScanlineRadial::operator*() 00081 { 00082 return coord; 00083 } 00084 00085 point_t* 00086 ScanlineRadial::operator->() 00087 { 00088 return &coord; 00089 } 00090 00091 point_t * 00092 ScanlineRadial::operator++() 00093 { 00094 00095 if ( done ) return &coord; 00096 00097 bool ok = false; 00098 00099 do { 00100 00101 tmp_x = 0; 00102 tmp_y = 0; 00103 00104 if ( current_radius == 0 ) { 00105 // Special case, after first reset 00106 current_radius += radius_increment; 00107 x = 0; 00108 y = current_radius; 00109 ok = true; 00110 } else { 00111 00112 if ( x < y ) { 00113 00114 switch (sector) { 00115 case 0: 00116 tmp_x = x; 00117 tmp_y = -y; 00118 break; 00119 00120 case 1: 00121 tmp_x = y; 00122 tmp_y = -x; 00123 break; 00124 00125 case 2: 00126 tmp_x = y; 00127 tmp_y = x; 00128 break; 00129 00130 case 3: 00131 tmp_x = x; 00132 tmp_y = y; 00133 break; 00134 00135 case 4: 00136 tmp_x = -x; 00137 tmp_y = y; 00138 break; 00139 00140 case 5: 00141 tmp_x = -y; 00142 tmp_y = x; 00143 break; 00144 00145 case 6: 00146 tmp_x = -y; 00147 tmp_y = -x; 00148 break; 00149 00150 case 7: 00151 tmp_x = -x; 00152 tmp_y = -y; 00153 break; 00154 00155 default: 00156 tmp_x = 0; 00157 tmp_y = 0; 00158 break; 00159 00160 } 00161 00162 x += step; 00163 y = (int)(sqrt( (float(current_radius * current_radius) - float(x * x)) ) + 0.5); 00164 00165 ok = true; 00166 00167 } else { 00168 // cout << "x !< y" << endl; 00169 if (sector == 7) { 00170 // Need to go to next circle 00171 current_radius += radius_increment; 00172 x = 0; 00173 y = current_radius; 00174 sector = 0; 00175 if (current_radius >= max_radius) { done = true; ok = true; } 00176 } else { 00177 sector += 1; 00178 x = 0; 00179 y = current_radius; 00180 } 00181 } 00182 00183 } 00184 00185 if ( (tmp_x < -(int)center_x) || 00186 (tmp_x > (int)(width - center_x)) || 00187 (tmp_y < -(int)center_y) || 00188 (tmp_y > (int)(height - center_y)) 00189 ) { 00190 coord.x = 0; 00191 coord.y = 0; 00192 // out of image, not ok 00193 ok = false; 00194 //done = true; 00195 } else { 00196 coord.x = center_x + tmp_x; 00197 coord.y = center_y + tmp_y; 00198 } 00199 00200 } while (! ok); 00201 00202 return &coord; 00203 } 00204 00205 point_t * 00206 ScanlineRadial::operator++(int) 00207 { 00208 memcpy(&tmp_coord, &coord, sizeof(point_t)); 00209 return &tmp_coord; 00210 } 00211 00212 bool 00213 ScanlineRadial::finished() 00214 { 00215 return done; 00216 } 00217 00218 00219 /** Do a simple sort of the given array, sorted descending, biggest first 00220 * this sort is stable 00221 */ 00222 void 00223 ScanlineRadial::simpleBubbleSort(unsigned int array[], unsigned int num_elements) 00224 { 00225 bool modified = false; 00226 unsigned int end = num_elements; 00227 unsigned int tmp; 00228 do { 00229 modified = false; 00230 00231 for (unsigned int i = 0; i < end-1; ++i) { 00232 if ( array[i] < array[i+1] ) { 00233 tmp = array[i]; 00234 array[i] = array[i+1]; 00235 array[i+1] = tmp; 00236 end -= 1; 00237 modified = true; 00238 } 00239 } 00240 00241 } while ( modified ); 00242 } 00243 00244 void 00245 ScanlineRadial::reset() 00246 { 00247 current_radius = radius_increment; 00248 while (current_radius < dead_radius) { 00249 current_radius += radius_increment; 00250 } 00251 x = 0; 00252 y = current_radius; 00253 sector = 0; 00254 00255 coord.x = center_x; 00256 coord.y = center_y; 00257 00258 if ( auto_max_radius ) { 00259 // Calculate distances to corners of image 00260 unsigned int dists[4]; 00261 dists[0] = (unsigned int)sqrt( float(center_x * center_x) + float(center_y * center_y) ); 00262 dists[1] = (unsigned int)sqrt( float((width - center_x) * (width - center_x)) + float(center_y * center_y) ); 00263 dists[2] = (unsigned int)sqrt( float((width - center_x) * (width - center_x)) + float((height - center_y) * (height - center_y)) ); 00264 dists[3] = (unsigned int)sqrt( float(center_x * center_x) + float((height - center_y) * (height - center_y)) ); 00265 00266 // now the maximum corner distance is the maximum radius 00267 simpleBubbleSort(dists, 4); 00268 max_radius = dists[0] - 1; 00269 } 00270 00271 done = false; 00272 00273 if (radius_increment > max_radius) { 00274 // cout << msg_prefix << cred << "radius_increment > max_radius, resetting radius_increment to one!" << cnormal << endl; 00275 radius_increment = 1; 00276 } 00277 00278 if (dead_radius > max_radius) { 00279 // cout << msg_prefix << cred << "dead_radius > max_radius, resetting dead_radius to zero!" << cnormal << endl; 00280 dead_radius = 0; 00281 current_radius = radius_increment; 00282 } 00283 00284 } 00285 00286 const char * 00287 ScanlineRadial::get_name() 00288 { 00289 return "ScanlineModel::Radial"; 00290 } 00291 00292 00293 unsigned int 00294 ScanlineRadial::get_margin() 00295 { 00296 return radius_increment; 00297 } 00298 00299 00300 /** Set new center point. 00301 * Sets new center point to move around the scanlines in the image. 00302 * Does an implicit reset(). 00303 * @param center_x x coordinate of the new center 00304 * @param center_y y coordinate of the new center 00305 */ 00306 void 00307 ScanlineRadial::set_center(unsigned int center_x, unsigned int center_y) 00308 { 00309 this->center_x = center_x; 00310 this->center_y = center_y; 00311 reset(); 00312 } 00313 00314 00315 /** Set new radius. 00316 * Sets the new maximum and dead radius. Does an implicit reset(). 00317 * @param dead_radius new dead radius 00318 * @param max_radius new maximum radius, if set to 0 this is automatically 00319 * calculated depending on the image size. 00320 */ 00321 void 00322 ScanlineRadial::set_radius(unsigned int dead_radius, unsigned int max_radius) 00323 { 00324 this->max_radius = 0; 00325 this->dead_radius = dead_radius; 00326 this->auto_max_radius = (max_radius == 0); 00327 00328 reset(); 00329 } 00330 00331 } // end namespace firevision