color_gradient_dot_modality.h
Go to the documentation of this file.
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 
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 
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   
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 
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 
00210 template <typename PointInT>
00211 pcl::ColorGradientDOTModality<PointInT>::
00212 ~ColorGradientDOTModality ()
00213 {
00214 }
00215 
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 
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 
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 
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 
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 
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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:22:48