Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_COLOR_GRADIENT_DOT_MODALITY 00039 #define PCL_FEATURES_COLOR_GRADIENT_DOT_MODALITY 00040 00041 #include <pcl/pcl_base.h> 00042 #include <pcl/point_cloud.h> 00043 #include <pcl/point_types.h> 00044 00045 #include <pcl/recognition/dot_modality.h> 00046 #include <pcl/recognition/quantized_map.h> 00047 00048 00049 namespace pcl 00050 { 00051 00052 /** \brief A point structure for representing RGB color 00053 * \ingroup common 00054 */ 00055 struct EIGEN_ALIGN16 PointRGB 00056 { 00057 union 00058 { 00059 union 00060 { 00061 struct 00062 { 00063 uint8_t b; 00064 uint8_t g; 00065 uint8_t r; 00066 uint8_t _unused; 00067 }; 00068 float rgb; 00069 }; 00070 uint32_t rgba; 00071 }; 00072 00073 inline PointRGB () 00074 {} 00075 00076 inline PointRGB (const uint8_t b, const uint8_t g, const uint8_t r) 00077 : b (b), g (g), r (r), _unused (0) 00078 {} 00079 00080 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00081 }; 00082 00083 00084 /** \brief A point structure representing Euclidean xyz coordinates, and the intensity value. 00085 * \ingroup common 00086 */ 00087 struct EIGEN_ALIGN16 GradientXY 00088 { 00089 union 00090 { 00091 struct 00092 { 00093 float x; 00094 float y; 00095 float angle; 00096 float magnitude; 00097 }; 00098 float data[4]; 00099 }; 00100 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00101 00102 inline bool operator< (const GradientXY & rhs) 00103 { 00104 return (magnitude > rhs.magnitude); 00105 } 00106 }; 00107 inline std::ostream & operator << (std::ostream & os, const GradientXY & p) 00108 { 00109 os << "(" << p.x << "," << p.y << " - " << p.magnitude << ")"; 00110 return (os); 00111 } 00112 00113 // -------------------------------------------------------------------------- 00114 00115 template <typename PointInT> 00116 class ColorGradientDOTModality 00117 : public DOTModality, public PCLBase<PointInT> 00118 { 00119 protected: 00120 using PCLBase<PointInT>::input_; 00121 00122 struct Candidate 00123 { 00124 GradientXY gradient; 00125 00126 int x; 00127 int y; 00128 00129 bool operator< (const Candidate & rhs) 00130 { 00131 return (gradient.magnitude > rhs.gradient.magnitude); 00132 } 00133 }; 00134 00135 public: 00136 typedef typename pcl::PointCloud<PointInT> PointCloudIn; 00137 00138 ColorGradientDOTModality (size_t bin_size); 00139 00140 virtual ~ColorGradientDOTModality (); 00141 00142 inline void 00143 setGradientMagnitudeThreshold (const float threshold) 00144 { 00145 gradient_magnitude_threshold_ = threshold; 00146 } 00147 00148 //inline QuantizedMap & 00149 //getDominantQuantizedMap () 00150 //{ 00151 // return (dominant_quantized_color_gradients_); 00152 //} 00153 00154 inline QuantizedMap & 00155 getDominantQuantizedMap () 00156 { 00157 return (dominant_quantized_color_gradients_); 00158 } 00159 00160 QuantizedMap 00161 computeInvariantQuantizedMap (const MaskMap & mask, 00162 const RegionXY & region); 00163 00164 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) 00165 * \param cloud the const boost shared pointer to a PointCloud message 00166 */ 00167 virtual void 00168 setInputCloud (const typename PointCloudIn::ConstPtr & cloud) 00169 { 00170 input_ = cloud; 00171 //processInputData (); 00172 } 00173 00174 virtual void 00175 processInputData (); 00176 00177 protected: 00178 00179 void 00180 computeMaxColorGradients (); 00181 00182 void 00183 computeDominantQuantizedGradients (); 00184 00185 //void 00186 //computeInvariantQuantizedGradients (); 00187 00188 private: 00189 size_t bin_size_; 00190 00191 float gradient_magnitude_threshold_; 00192 pcl::PointCloud<pcl::GradientXY> color_gradients_; 00193 00194 pcl::QuantizedMap dominant_quantized_color_gradients_; 00195 //pcl::QuantizedMap invariant_quantized_color_gradients_; 00196 00197 }; 00198 00199 } 00200 00201 ////////////////////////////////////////////////////////////////////////////////////////////// 00202 template <typename PointInT> 00203 pcl::ColorGradientDOTModality<PointInT>:: 00204 ColorGradientDOTModality (const size_t bin_size) 00205 : bin_size_ (bin_size), gradient_magnitude_threshold_ (80.0f), color_gradients_ (), dominant_quantized_color_gradients_ () 00206 { 00207 } 00208 00209 ////////////////////////////////////////////////////////////////////////////////////////////// 00210 template <typename PointInT> 00211 pcl::ColorGradientDOTModality<PointInT>:: 00212 ~ColorGradientDOTModality () 00213 { 00214 } 00215 00216 ////////////////////////////////////////////////////////////////////////////////////////////// 00217 template <typename PointInT> 00218 void 00219 pcl::ColorGradientDOTModality<PointInT>:: 00220 processInputData () 00221 { 00222 // extract color gradients 00223 computeMaxColorGradients (); 00224 00225 // compute dominant quantized gradient map 00226 computeDominantQuantizedGradients (); 00227 00228 // compute invariant quantized gradient map 00229 //computeInvariantQuantizedGradients (); 00230 } 00231 00232 ////////////////////////////////////////////////////////////////////////////////////////////// 00233 template <typename PointInT> 00234 void 00235 pcl::ColorGradientDOTModality<PointInT>:: 00236 computeMaxColorGradients () 00237 { 00238 const int width = input_->width; 00239 const int height = input_->height; 00240 00241 color_gradients_.points.resize (width*height); 00242 color_gradients_.width = width; 00243 color_gradients_.height = height; 00244 00245 const float pi = tan(1.0f)*4; 00246 for (int row_index = 0; row_index < height-2; ++row_index) 00247 { 00248 for (int col_index = 0; col_index < width-2; ++col_index) 00249 { 00250 const int index0 = row_index*width+col_index; 00251 const int index_c = row_index*width+col_index+2; 00252 const int index_r = (row_index+2)*width+col_index; 00253 00254 //const int index_d = (row_index+1)*width+col_index+1; 00255 00256 const unsigned char r0 = input_->points[index0].r; 00257 const unsigned char g0 = input_->points[index0].g; 00258 const unsigned char b0 = input_->points[index0].b; 00259 00260 const unsigned char r_c = input_->points[index_c].r; 00261 const unsigned char g_c = input_->points[index_c].g; 00262 const unsigned char b_c = input_->points[index_c].b; 00263 00264 const unsigned char r_r = input_->points[index_r].r; 00265 const unsigned char g_r = input_->points[index_r].g; 00266 const unsigned char b_r = input_->points[index_r].b; 00267 00268 const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0); 00269 const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0); 00270 const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0); 00271 00272 const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0); 00273 const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0); 00274 const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0); 00275 00276 const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy; 00277 const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy; 00278 const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy; 00279 00280 GradientXY gradient; 00281 gradient.x = col_index; 00282 gradient.y = row_index; 00283 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b) 00284 { 00285 gradient.magnitude = sqrt (sqr_mag_r); 00286 gradient.angle = atan2 (r_dy, r_dx) * 180.0f / pi; 00287 } 00288 else if (sqr_mag_g > sqr_mag_b) 00289 { 00290 //GradientXY gradient; 00291 gradient.magnitude = sqrt (sqr_mag_g); 00292 gradient.angle = atan2 (g_dy, g_dx) * 180.0f / pi; 00293 //gradient.x = col_index; 00294 //gradient.y = row_index; 00295 00296 //color_gradients_ (col_index+1, row_index+1) = gradient; 00297 } 00298 else 00299 { 00300 //GradientXY gradient; 00301 gradient.magnitude = sqrt (sqr_mag_b); 00302 gradient.angle = atan2 (b_dy, b_dx) * 180.0f / pi; 00303 //gradient.x = col_index; 00304 //gradient.y = row_index; 00305 00306 //color_gradients_ (col_index+1, row_index+1) = gradient; 00307 } 00308 00309 assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 && 00310 color_gradients_ (col_index+1, row_index+1).angle <= 180); 00311 00312 color_gradients_ (col_index+1, row_index+1) = gradient; 00313 } 00314 } 00315 00316 return; 00317 } 00318 00319 ////////////////////////////////////////////////////////////////////////////////////////////// 00320 //template <typename PointInT> 00321 //void 00322 //pcl::ColorGradientDOTModality<PointInT>:: 00323 //computeInvariantQuantizedGradients () 00324 //{ 00325 // const size_t input_width = input_->width; 00326 // const size_t input_height = input_->height; 00327 // 00328 // const size_t output_width = input_width / bin_size; 00329 // const size_t output_height = input_height / bin_size; 00330 // 00331 // invariant_quantized_color_gradients_.resize (output_width, output_height); 00332 // 00333 // size_t offset_x = 0; 00334 // size_t offset_y = 0; 00335 // 00336 // const size_t num_gradient_bins = 7; 00337 // const size_t max_num_of_gradients = 7; 00338 // 00339 // const float divisor = 180.0f / (num_gradient_bins - 1.0f); 00340 // 00341 // float global_max_gradient = 0.0f; 00342 // float local_max_gradient = 0.0f; 00343 // 00344 // unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData (); 00345 // 00346 // //int tmpCounter = 0; 00347 // for (size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index) 00348 // { 00349 // for (size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index) 00350 // { 00351 // std::vector<int> x_coordinates; 00352 // std::vector<int> y_coordinates; 00353 // std::vector<float> values; 00354 // 00355 // for (int row_pixel_index = -static_cast<int> (bin_size)/2; 00356 // row_pixel_index <= static_cast<int> (bin_size)/2; 00357 // row_pixel_index += static_cast<int> (bin_size)/2) 00358 // { 00359 // const size_t y_position = offset_y + row_pixel_index; 00360 // 00361 // if (y_position < 0 || y_position >= input_height) continue; 00362 // 00363 // for (int col_pixel_index = -static_cast<int> (bin_size)/2; 00364 // col_pixel_index <= static_cast<int> (bin_size)/2; 00365 // col_pixel_index += static_cast<int> (bin_size)/2) 00366 // { 00367 // const size_t x_position = offset_x + col_pixel_index; 00368 // size_t counter = 0; 00369 // 00370 // if (x_position < 0 || x_position >= input_width) continue; 00371 // 00372 // // find maximum gradient magnitude in current bin 00373 // { 00374 // local_max_gradient = 0.0f; 00375 // for (size_t row_sub_index = 0; row_sub_index < bin_size; ++row_sub_index) 00376 // { 00377 // for (size_t col_sub_index = 0; col_sub_index < bin_size; ++col_sub_index) 00378 // { 00379 // const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; 00380 // 00381 // if (magnitude > local_max_gradient) 00382 // local_max_gradient = magnitude; 00383 // } 00384 // } 00385 // } 00386 // 00387 // //*stringPointer += localMaxGradient; 00388 // 00389 // if (local_max_gradient > global_max_gradient) 00390 // { 00391 // global_max_gradient = local_max_gradient; 00392 // } 00393 // 00394 // // iteratively search for the largest gradients, set it to -1, search the next largest ... etc. 00395 // while (true) 00396 // { 00397 // float max_gradient; 00398 // size_t max_gradient_pos_x; 00399 // size_t max_gradient_pos_y; 00400 // 00401 // // find next location and value of maximum gradient magnitude in current region 00402 // { 00403 // max_gradient = 0.0f; 00404 // for (size_t row_sub_index = 0; row_sub_index < bin_size; ++row_sub_index) 00405 // { 00406 // for (size_t col_sub_index = 0; col_sub_index < bin_size; ++col_sub_index) 00407 // { 00408 // const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; 00409 // 00410 // if (magnitude > max_gradient) 00411 // { 00412 // max_gradient = magnitude; 00413 // max_gradient_pos_x = col_sub_index; 00414 // max_gradient_pos_y = row_sub_index; 00415 // } 00416 // } 00417 // } 00418 // } 00419 // 00420 // // TODO: really localMaxGradient and not maxGradient??? 00421 // if (local_max_gradient < gradient_magnitude_threshold_) 00422 // { 00423 // //*peakPointer |= 1 << (numOfGradientBins-1); 00424 // break; 00425 // } 00426 // 00427 // // TODO: replace gradient_magnitude_threshold_ here by a fixed ratio? 00428 // if (max_gradient < (local_max_gradient * gradient_magnitude_threshold_) || 00429 // counter >= max_num_of_gradients) 00430 // { 00431 // break; 00432 // } 00433 // 00434 // ++counter; 00435 // 00436 // const size_t angle = static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f); 00437 // const size_t bin_index = static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor); 00438 // 00439 // *peak_pointer |= 1 << bin_index; 00440 // 00441 // x_coordinates.push_back (max_gradient_pos_x + x_position); 00442 // y_coordinates.push_back (max_gradient_pos_y + y_position); 00443 // values.push_back (max_gradient); 00444 // 00445 // color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f; 00446 // } 00447 // 00448 // // reset values which have been set to -1 00449 // for (size_t value_index = 0; value_index < values.size (); ++value_index) 00450 // { 00451 // color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index]; 00452 // } 00453 // 00454 // x_coordinates.clear (); 00455 // y_coordinates.clear (); 00456 // values.clear (); 00457 // } 00458 // } 00459 // 00460 // if (*peak_pointer == 0) 00461 // { 00462 // *peak_pointer |= 1 << 7; 00463 // } 00464 // 00465 // //if (*peakPointer != 0) 00466 // //{ 00467 // // ++tmpCounter; 00468 // //} 00469 // 00470 // //++stringPointer; 00471 // ++peak_pointer; 00472 // 00473 // offset_x += bin_size; 00474 // } 00475 // 00476 // offset_y += bin_size; 00477 // offset_x = bin_size/2+1; 00478 // } 00479 //} 00480 00481 ////////////////////////////////////////////////////////////////////////////////////////////// 00482 template <typename PointInT> 00483 void 00484 pcl::ColorGradientDOTModality<PointInT>:: 00485 computeDominantQuantizedGradients () 00486 { 00487 const size_t input_width = input_->width; 00488 const size_t input_height = input_->height; 00489 00490 const size_t output_width = input_width / bin_size_; 00491 const size_t output_height = input_height / bin_size_; 00492 00493 dominant_quantized_color_gradients_.resize (output_width, output_height); 00494 00495 //size_t offset_x = 0; 00496 //size_t offset_y = 0; 00497 00498 const size_t num_gradient_bins = 7; 00499 const size_t max_num_of_gradients = 1; 00500 00501 const float divisor = 180.0f / (num_gradient_bins - 1.0f); 00502 00503 float global_max_gradient = 0.0f; 00504 float local_max_gradient = 0.0f; 00505 00506 unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData (); 00507 memset (peak_pointer, 0, output_width*output_height); 00508 00509 //int tmpCounter = 0; 00510 for (size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index) 00511 { 00512 for (size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index) 00513 { 00514 const size_t x_position = col_bin_index * bin_size_; 00515 const size_t y_position = row_bin_index * bin_size_; 00516 00517 //std::vector<int> x_coordinates; 00518 //std::vector<int> y_coordinates; 00519 //std::vector<float> values; 00520 00521 // iteratively search for the largest gradients, set it to -1, search the next largest ... etc. 00522 //while (counter < max_num_of_gradients) 00523 { 00524 float max_gradient; 00525 size_t max_gradient_pos_x; 00526 size_t max_gradient_pos_y; 00527 00528 // find next location and value of maximum gradient magnitude in current region 00529 { 00530 max_gradient = 0.0f; 00531 for (size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index) 00532 { 00533 for (size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index) 00534 { 00535 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; 00536 00537 if (magnitude > max_gradient) 00538 { 00539 max_gradient = magnitude; 00540 max_gradient_pos_x = col_sub_index; 00541 max_gradient_pos_y = row_sub_index; 00542 } 00543 } 00544 } 00545 } 00546 00547 if (max_gradient >= gradient_magnitude_threshold_) 00548 { 00549 const size_t angle = static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f); 00550 const size_t bin_index = static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor); 00551 00552 *peak_pointer |= 1 << bin_index; 00553 } 00554 00555 //++counter; 00556 00557 //x_coordinates.push_back (max_gradient_pos_x + x_position); 00558 //y_coordinates.push_back (max_gradient_pos_y + y_position); 00559 //values.push_back (max_gradient); 00560 00561 //color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f; 00562 } 00563 00564 //// reset values which have been set to -1 00565 //for (size_t value_index = 0; value_index < values.size (); ++value_index) 00566 //{ 00567 // color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index]; 00568 //} 00569 00570 00571 if (*peak_pointer == 0) 00572 { 00573 *peak_pointer |= 1 << 7; 00574 } 00575 00576 //if (*peakPointer != 0) 00577 //{ 00578 // ++tmpCounter; 00579 //} 00580 00581 //++stringPointer; 00582 ++peak_pointer; 00583 00584 //offset_x += bin_size; 00585 } 00586 00587 //offset_y += bin_size; 00588 //offset_x = bin_size/2+1; 00589 } 00590 } 00591 00592 ////////////////////////////////////////////////////////////////////////////////////////////// 00593 template <typename PointInT> 00594 pcl::QuantizedMap 00595 pcl::ColorGradientDOTModality<PointInT>:: 00596 computeInvariantQuantizedMap (const MaskMap & mask, 00597 const RegionXY & region) 00598 { 00599 const size_t input_width = input_->width; 00600 const size_t input_height = input_->height; 00601 00602 const size_t output_width = input_width / bin_size_; 00603 const size_t output_height = input_height / bin_size_; 00604 00605 const size_t sub_start_x = region.x / bin_size_; 00606 const size_t sub_start_y = region.y / bin_size_; 00607 const size_t sub_width = region.width / bin_size_; 00608 const size_t sub_height = region.height / bin_size_; 00609 00610 QuantizedMap map; 00611 map.resize (sub_width, sub_height); 00612 00613 //size_t offset_x = 0; 00614 //size_t offset_y = 0; 00615 00616 const size_t num_gradient_bins = 7; 00617 const size_t max_num_of_gradients = 7; 00618 00619 const float divisor = 180.0f / (num_gradient_bins - 1.0f); 00620 00621 float global_max_gradient = 0.0f; 00622 float local_max_gradient = 0.0f; 00623 00624 unsigned char * peak_pointer = map.getData (); 00625 00626 //int tmpCounter = 0; 00627 for (size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index) 00628 { 00629 for (size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index) 00630 { 00631 std::vector<size_t> x_coordinates; 00632 std::vector<size_t> y_coordinates; 00633 std::vector<float> values; 00634 00635 for (int row_pixel_index = -static_cast<int> (bin_size_)/2; 00636 row_pixel_index <= static_cast<int> (bin_size_)/2; 00637 row_pixel_index += static_cast<int> (bin_size_)/2) 00638 { 00639 const size_t y_position = /*offset_y +*/ row_pixel_index + (sub_start_y + row_bin_index)*bin_size_; 00640 00641 if (y_position < 0 || y_position >= input_height) 00642 continue; 00643 00644 for (int col_pixel_index = -static_cast<int> (bin_size_)/2; 00645 col_pixel_index <= static_cast<int> (bin_size_)/2; 00646 col_pixel_index += static_cast<int> (bin_size_)/2) 00647 { 00648 const size_t x_position = /*offset_x +*/ col_pixel_index + (sub_start_x + col_bin_index)*bin_size_; 00649 size_t counter = 0; 00650 00651 if (x_position < 0 || x_position >= input_width) 00652 continue; 00653 00654 // find maximum gradient magnitude in current bin 00655 { 00656 local_max_gradient = 0.0f; 00657 for (size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index) 00658 { 00659 for (size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index) 00660 { 00661 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; 00662 00663 if (magnitude > local_max_gradient) 00664 local_max_gradient = magnitude; 00665 } 00666 } 00667 } 00668 00669 //*stringPointer += localMaxGradient; 00670 00671 if (local_max_gradient > global_max_gradient) 00672 { 00673 global_max_gradient = local_max_gradient; 00674 } 00675 00676 // iteratively search for the largest gradients, set it to -1, search the next largest ... etc. 00677 while (true) 00678 { 00679 float max_gradient; 00680 size_t max_gradient_pos_x; 00681 size_t max_gradient_pos_y; 00682 00683 // find next location and value of maximum gradient magnitude in current region 00684 { 00685 max_gradient = 0.0f; 00686 for (size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index) 00687 { 00688 for (size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index) 00689 { 00690 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; 00691 00692 if (magnitude > max_gradient) 00693 { 00694 max_gradient = magnitude; 00695 max_gradient_pos_x = col_sub_index; 00696 max_gradient_pos_y = row_sub_index; 00697 } 00698 } 00699 } 00700 } 00701 00702 // TODO: really localMaxGradient and not maxGradient??? 00703 if (local_max_gradient < gradient_magnitude_threshold_) 00704 { 00705 //*peakPointer |= 1 << (numOfGradientBins-1); 00706 break; 00707 } 00708 00709 // TODO: replace gradient_magnitude_threshold_ here by a fixed ratio? 00710 if (/*max_gradient < (local_max_gradient * gradient_magnitude_threshold_) ||*/ 00711 counter >= max_num_of_gradients) 00712 { 00713 break; 00714 } 00715 00716 ++counter; 00717 00718 const size_t angle = static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f); 00719 const size_t bin_index = static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor); 00720 00721 *peak_pointer |= 1 << bin_index; 00722 00723 x_coordinates.push_back (max_gradient_pos_x + x_position); 00724 y_coordinates.push_back (max_gradient_pos_y + y_position); 00725 values.push_back (max_gradient); 00726 00727 color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f; 00728 } 00729 00730 // reset values which have been set to -1 00731 for (size_t value_index = 0; value_index < values.size (); ++value_index) 00732 { 00733 color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index]; 00734 } 00735 00736 x_coordinates.clear (); 00737 y_coordinates.clear (); 00738 values.clear (); 00739 } 00740 } 00741 00742 if (*peak_pointer == 0) 00743 { 00744 *peak_pointer |= 1 << 7; 00745 } 00746 00747 //if (*peakPointer != 0) 00748 //{ 00749 // ++tmpCounter; 00750 //} 00751 00752 //++stringPointer; 00753 ++peak_pointer; 00754 00755 //offset_x += bin_size; 00756 } 00757 00758 //offset_y += bin_size; 00759 //offset_x = bin_size/2+1; 00760 } 00761 00762 return map; 00763 } 00764 00765 #endif