narf_keypoint.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00036 #include <iostream>
00037 #include <vector>
00038 #include <pcl/keypoints/narf_keypoint.h>
00039 #include <pcl/features/range_image_border_extractor.h>
00040 #include <pcl/pcl_macros.h>
00041 #include <pcl/common/polynomial_calculations.h>
00042 #include <pcl/range_image/range_image.h>
00043 
00044 namespace pcl 
00045 {
00046 
00048 NarfKeypoint::NarfKeypoint (RangeImageBorderExtractor* range_image_border_extractor, float support_size) :
00049     BaseClass (), interest_image_ (NULL), interest_points_ (NULL)
00050 {
00051   name_ = "NarfKeypoint";
00052   clearData ();
00053   setRangeImageBorderExtractor (range_image_border_extractor);
00054   if (support_size > 0.0f)
00055     parameters_.support_size = support_size;
00056 }
00057 
00059 NarfKeypoint::~NarfKeypoint ()
00060 {
00061   //std::cerr << __PRETTY_FUNCTION__<<" called.\n";
00062   clearData ();
00063 }
00064 
00066 void
00067   NarfKeypoint::clearData ()
00068 {
00069   //std::cerr << __PRETTY_FUNCTION__<<" called.\n";
00070   
00071   for (size_t scale_space_idx = 1; scale_space_idx<border_extractor_scale_space_.size (); ++scale_space_idx)
00072     delete border_extractor_scale_space_[scale_space_idx];
00073   border_extractor_scale_space_.clear ();
00074   for (size_t scale_space_idx = 1; scale_space_idx<range_image_scale_space_.size (); ++scale_space_idx)
00075     delete range_image_scale_space_[scale_space_idx];
00076   range_image_scale_space_.clear ();
00077   for (size_t scale_space_idx = 1; scale_space_idx<interest_image_scale_space_.size (); ++scale_space_idx)
00078     delete[] interest_image_scale_space_[scale_space_idx];
00079   interest_image_scale_space_.clear ();
00080   is_interest_point_image_.clear ();
00081   delete[] interest_image_; interest_image_=NULL;
00082   delete interest_points_;  interest_points_=NULL;
00083 }
00084 
00086 void
00087 NarfKeypoint::setRangeImageBorderExtractor (RangeImageBorderExtractor* range_image_border_extractor)
00088 {
00089   clearData ();
00090   range_image_border_extractor_ = range_image_border_extractor;
00091 }
00092 
00094 void
00095 NarfKeypoint::setRangeImage (const RangeImage* range_image)
00096 {
00097   clearData ();
00098   range_image_border_extractor_->setRangeImage (range_image);
00099 }
00100 
00102 void
00103 NarfKeypoint::calculateScaleSpace ()
00104 {
00105   //MEASURE_FUNCTION_TIME;
00106   
00107   if (range_image_border_extractor_ == NULL || !range_image_border_extractor_->hasRangeImage () ||
00108       !border_extractor_scale_space_.empty ())  // Nothing to compute or already done
00109     return;
00110   border_extractor_scale_space_.push_back (range_image_border_extractor_);
00111   range_image_scale_space_.push_back (const_cast<RangeImage*> (reinterpret_cast<const RangeImage*> (&range_image_border_extractor_->getRangeImage ())));
00112   
00113   if (!parameters_.use_recursive_scale_reduction)
00114     return;
00115   
00116   while (0.5f*range_image_scale_space_.back ()->getAngularResolution () < deg2rad (2.0f))
00117   {
00118     range_image_scale_space_.push_back (getRangeImage ().getNew ());
00119     range_image_scale_space_[range_image_scale_space_.size ()-2]->getHalfImage (*range_image_scale_space_.back ());
00120     border_extractor_scale_space_.push_back (new RangeImageBorderExtractor);
00121     border_extractor_scale_space_.back ()->getParameters () = range_image_border_extractor_->getParameters ();
00122     border_extractor_scale_space_.back ()->setRangeImage (range_image_scale_space_.back ());
00123   }
00124 }
00125 
00126 #define USE_BEAM_AVERAGE 1
00127 
00128 namespace 
00129 {  // Some helper functions in an anonymous namespace - only available in this file
00130   inline void 
00131   nkdGetScores (float distance_factor, float surface_change_score, float pixelDistance,
00132                 float optimal_distance, float& negative_score, float& positive_score)
00133   {
00134     negative_score = 1.0f - 0.5f * surface_change_score * (std::max) (1.0f - distance_factor/optimal_distance, 0.0f);
00135     negative_score = powf (negative_score, 2);
00136     
00137     if (pixelDistance < 2.0)
00138       positive_score = surface_change_score;
00139     else
00140       positive_score = surface_change_score * (1.0f-distance_factor);
00141   }
00142 
00143   void 
00144   translateDirection180To360 (Eigen::Vector2f& direction_vector)
00145   {
00146     // The following code does the equivalent to this:
00147     // Get the angle of the 2D direction (with positive x) alpha, and return the direction of 2*alpha
00148     // We do this to create a normal angular wrap-around at -180,180 instead of the one at -90,90, 
00149     // enabling us to calculate the average angle as the direction of the sum of all unit vectors.
00150     // We use sin (2a)=2*sin (a)*cos (a) and cos (2a)=2cos^2 (a)-1 so that we needn't actually compute the angles,
00151     // which would be expensive
00152     float cos_a = direction_vector[0],
00153           cos_2a = 2*cos_a*cos_a - 1.0f,
00154           sin_a = direction_vector[1],
00155           sin_2a = 2.0f*sin_a*cos_a;
00156     direction_vector[0] = cos_2a;
00157     direction_vector[1] = sin_2a;
00158   }
00159 
00160   void 
00161   translateDirection360To180 (Eigen::Vector2f& direction_vector)
00162   {
00163     // Inverse of the above
00164     float cos_2a = direction_vector[0],
00165           cos_a = sqrtf (0.5f* (cos_2a+1.0f)),
00166           sin_2a = direction_vector[1],
00167           sin_a = sin_2a / (2.0f*cos_a);
00168     direction_vector[0] = cos_a;
00169     direction_vector[1] = sin_a;
00170   }
00171   
00172   inline Eigen::Vector2f 
00173   nkdGetDirectionVector (const Eigen::Vector3f& direction, const Eigen::Affine3f& rotation)
00174   {
00175     Eigen::Vector3f rotated_direction = rotation*direction;
00176     Eigen::Vector2f direction_vector (rotated_direction[0], rotated_direction[1]);
00177     direction_vector.normalize ();
00178     if (direction_vector[0]<0.0f)
00179       direction_vector *= -1.0f;
00180     
00181 
00182 #   if USE_BEAM_AVERAGE
00183       translateDirection180To360 (direction_vector);
00184 #   endif
00185     
00186     return direction_vector;
00187   }
00188   
00189   inline float 
00190   nkdGetDirectionAngle (const Eigen::Vector3f& direction, const Eigen::Affine3f& rotation)
00191   {
00192     Eigen::Vector3f rotated_direction = rotation*direction;
00193     Eigen::Vector2f direction_vector (rotated_direction[0], rotated_direction[1]);
00194     direction_vector.normalize ();
00195     float angle = 0.5f*normAngle (2.0f*acosf (direction_vector[0]));
00196     return (angle);
00197   }
00198   
00199   inline void 
00200   propagateInvalidBeams (int new_radius, std::vector<bool>& old_beams, std::vector<bool>& new_beams)
00201   {
00202     new_beams.clear ();
00203     new_beams.resize (std::max (8*new_radius,1), false);
00204     if (new_radius >= 2)
00205     {
00206       float mapping_factor = 1.0f + (1.0f / static_cast<float> (new_radius-1));
00207       for (size_t old_idx=0; old_idx<old_beams.size (); ++old_idx)
00208       {
00209         if (old_beams[old_idx])
00210         {
00211           int middle_idx = static_cast<int> (pcl_lrint (mapping_factor * static_cast<float> (old_idx)));
00212           for (int idx_offset=-1; idx_offset<=1; ++idx_offset)
00213           {
00214             if (idx_offset != 0)
00215             {
00216               int old_neighbor_idx = static_cast<int> (old_idx) + idx_offset;
00217               if (old_neighbor_idx<0)
00218                 old_neighbor_idx += static_cast<int> (old_beams.size ());
00219               if (old_neighbor_idx>= static_cast<int> (old_beams.size ()))
00220                 old_neighbor_idx -= static_cast<int> (old_beams.size ());
00221               if (!old_beams[old_neighbor_idx])
00222                 continue;
00223             }
00224             int new_idx = middle_idx+idx_offset;
00225             if (new_idx<0)
00226               new_idx += static_cast<int> (new_beams.size ());
00227             if (new_idx>= static_cast<int> (new_beams.size ()))
00228               new_idx -= static_cast<int> (new_beams.size ());
00229             new_beams[new_idx] = true;
00230           }
00231         }
00232       }
00233     }
00234   }
00235   
00236   inline bool
00237   isBetterInterestPoint (const InterestPoint& p1, const InterestPoint& p2)
00238   {
00239     return (p1.strength > p2.strength);
00240   }
00241   
00242   inline bool
00243   secondPairElementIsGreater (const std::pair<int, float>& p1, const std::pair<int, float>& p2)
00244   {
00245     return (p1.second > p2.second);
00246   }
00247 
00248 }  // end anonymous namespace
00249 
00250 void 
00251 NarfKeypoint::calculateInterestImage () 
00252 {
00253   //std::cout << __PRETTY_FUNCTION__ << " called.\n";
00254   
00255   if (interest_image_!=NULL)  // Already done
00256     return;
00257   
00258   if (parameters_.calculate_sparse_interest_image)
00259     calculateSparseInterestImage ();
00260   else
00261     calculateCompleteInterestImage ();
00262 }
00263 
00264 void 
00265 NarfKeypoint::calculateCompleteInterestImage ()
00266 {
00267   //std::cout << __PRETTY_FUNCTION__ << " called.\n";
00268   
00269   if (parameters_.support_size <= 0.0f)
00270   {
00271     std::cerr << __PRETTY_FUNCTION__<<": parameters_.support_size is not set!\n";
00272     return;
00273   }
00274   if (range_image_border_extractor_==NULL)
00275   {
00276     std::cerr << __PRETTY_FUNCTION__<<": range_image_border_extractor_ is not set!\n";
00277     return;
00278   }
00279   
00280   float search_radius = 0.5f * parameters_.support_size,
00281         radius_squared = search_radius*search_radius,
00282         radius_reciprocal = 1.0f / search_radius;
00283   
00284   calculateScaleSpace ();
00285   //std::cout << PVARN(range_image_scale_space_.size ());
00286   
00287   std::vector<float> start_usage_ranges;
00288   start_usage_ranges.resize (range_image_scale_space_.size ());
00289   start_usage_ranges[int (range_image_scale_space_.size ())-1] = 0.0f;
00290   for (int scale_idx = int (range_image_scale_space_.size ())-2;  scale_idx >= 0; --scale_idx)
00291   {
00292     start_usage_ranges[scale_idx] = parameters_.support_size / 
00293       tanf (static_cast<float> (parameters_.optimal_range_image_patch_size) * range_image_scale_space_[scale_idx+1]->getAngularResolution ());
00294     //std::cout << PVARN(start_usage_ranges[scale_idx]);
00295   }
00296   
00297   //double interest_value_calculation_start_time = getTime ();
00298   interest_image_scale_space_.clear ();
00299   interest_image_scale_space_.resize (range_image_scale_space_.size (), NULL);
00300   for (int scale_idx = int (range_image_scale_space_.size ())-1;  scale_idx >= 0; --scale_idx)
00301   {
00302     const RangeImage& range_image = *range_image_scale_space_[scale_idx];
00303     RangeImageBorderExtractor& border_extractor = *border_extractor_scale_space_[scale_idx];
00304     int original_max_no_of_threads = border_extractor.getParameters ().max_no_of_threads;
00305     border_extractor.getParameters ().max_no_of_threads = parameters_.max_no_of_threads;
00306     const ::pcl::PointCloud<BorderDescription>& border_descriptions = border_extractor.getBorderDescriptions ();
00307     const float* surface_change_scores = border_extractor.getSurfaceChangeScores ();
00308     const Eigen::Vector3f* surface_change_directions = border_extractor.getSurfaceChangeDirections ();
00309     float start_usage_range = start_usage_ranges[scale_idx];
00310     
00311     int width  = range_image.width,
00312         height = range_image.height,
00313         array_size = width*height;
00314 
00315     float* interest_image = new float[array_size];
00316     interest_image_scale_space_[scale_idx] = interest_image;
00317     //for (int i=0; i<array_size; ++i)
00318       //interest_image[i] = -1.0f;
00319     
00320     const int angle_histogram_size = 18;
00321     std::vector<float> angle_histogram;
00322     angle_histogram.resize(angle_histogram_size);
00323     
00324     std::vector<bool> was_touched;
00325     was_touched.resize (array_size, false);
00326     std::vector<int> neighbors_to_check;
00327     
00328 #   pragma omp parallel for num_threads (parameters_.max_no_of_threads) default (shared) \
00329                             firstprivate (was_touched, neighbors_to_check, angle_histogram) schedule (dynamic, 10)
00330     for (int index=0; index<array_size; ++index)
00331     {
00332       float& interest_value = interest_image[index];
00333       interest_value = 0.0f;
00334       if (!range_image.isValid (index))
00335         continue;
00336       int y = index/range_image.width,
00337           x = index - y*range_image.width;
00338       
00339       const BorderTraits& border_traits = border_descriptions.points[index].traits;
00340       if (border_traits[BORDER_TRAIT__SHADOW_BORDER] || border_traits[BORDER_TRAIT__VEIL_POINT])
00341         continue;
00342       
00343       const PointWithRange& point = range_image.getPoint (index);
00344       
00345       if (point.range < start_usage_range)  // Point is close enough that we can use the value calculated at a lower resolution
00346       {
00347         const RangeImage& half_range_image = *range_image_scale_space_[scale_idx+1];
00348         float* half_interest_image = interest_image_scale_space_[scale_idx+1];
00349         int half_x = std::min (x/2, int (half_range_image.width)-1),
00350             half_y = std::min (y/2, int (half_range_image.height)-1);
00351         interest_value = half_interest_image[half_y*half_range_image.width + half_x];
00352         continue;
00353       }
00354       
00355       Eigen::Affine3f rotation_to_viewer_coordinate_system;
00356       range_image.getRotationToViewerCoordinateFrame (point.getVector3fMap (),
00357                                                       rotation_to_viewer_coordinate_system);
00358       float negative_score = 1.0f;
00359       
00360       // -----Start region growing to cover all connected points within the support size-----
00361       neighbors_to_check.clear ();
00362       neighbors_to_check.push_back (index);
00363       was_touched[index] = true;
00364       
00365       angle_histogram.clear ();
00366       angle_histogram.resize(angle_histogram_size, 0);
00367       for (size_t neighbors_to_check_idx=0; neighbors_to_check_idx<neighbors_to_check.size (); ++neighbors_to_check_idx)
00368       {
00369         int index2 = neighbors_to_check[neighbors_to_check_idx];
00370         if (!range_image.isValid (index2))
00371           continue;
00372         const BorderTraits& border_traits2 = border_descriptions.points[index2].traits;
00373         if (border_traits2[BORDER_TRAIT__SHADOW_BORDER] || border_traits2[BORDER_TRAIT__VEIL_POINT])
00374           continue;
00375         int y2 = index2/range_image.width,
00376             x2 = index2 - y2*range_image.width;
00377         const PointWithRange& point2 = range_image.getPoint (index2);
00378         
00379         float pixelDistance = static_cast<float> (std::max (abs (x2-x), abs (y2-y)));
00380         float distance_squared = squaredEuclideanDistance (point, point2);
00381         if (pixelDistance > 2.0f)  // Always consider immediate neighbors, even if to far away
00382         {
00383           if (distance_squared>radius_squared)
00384             continue;
00385         }
00386         
00387         for (int y3=std::max (0,y2-1); y3<=std::min (y2+1,int (range_image.height)-1); ++y3)
00388         {
00389           for (int x3=std::max (0,x2-1); x3<=std::min (x2+1,int (range_image.width)-1); ++x3)
00390           {
00391             int index3 = y3*range_image.width + x3;
00392             if (!was_touched[index3])
00393             {
00394               neighbors_to_check.push_back (index3);
00395               was_touched[index3] = true;
00396             }
00397           }
00398         }
00399         
00400         float surface_change_score = surface_change_scores[index2];
00401         if (surface_change_score < parameters_.min_surface_change_score)  // Pixel not 'interesting' enough to consider?
00402           continue;
00403         const Eigen::Vector3f& surface_change_direction = surface_change_directions[index2];
00404         
00405         float distance = sqrtf (distance_squared);
00406         float distance_factor = radius_reciprocal*distance;
00407         float positive_score, current_negative_score;
00408         nkdGetScores (distance_factor, surface_change_score, pixelDistance,
00409                      parameters_.optimal_distance_to_high_surface_change,
00410                      current_negative_score, positive_score);
00411         float angle = nkdGetDirectionAngle (surface_change_direction, rotation_to_viewer_coordinate_system);
00412         int histogram_cell = (std::min) (angle_histogram_size-1,
00413                           static_cast<int> (pcl_lrint (floorf ( (angle+deg2rad (90.0f))/deg2rad (180.0f) * angle_histogram_size))));
00414         float& histogram_value = angle_histogram[histogram_cell];
00415         
00416         histogram_value = (std::max) (histogram_value, positive_score);
00417         negative_score  = (std::min) (negative_score, current_negative_score);
00418       }
00419       
00420       // Reset was_touched to false
00421       for (size_t neighbors_to_check_idx=0; neighbors_to_check_idx<neighbors_to_check.size (); ++neighbors_to_check_idx)
00422         was_touched[neighbors_to_check[neighbors_to_check_idx]] = false;
00423       
00424       float angle_change_value = 0.0f;
00425       for (int histogram_cell1=0; histogram_cell1<angle_histogram_size-1; ++histogram_cell1)
00426       {
00427         if (angle_histogram[histogram_cell1]==0.0f)
00428           continue;
00429         for (int histogram_cell2=histogram_cell1+1; histogram_cell2<angle_histogram_size; ++histogram_cell2)
00430         {
00431           if (angle_histogram[histogram_cell2]==0.0f)
00432             continue;
00433           // TODO: lookup table for the following:
00434           float normalized_angle_diff = 2.0f*float (histogram_cell2-histogram_cell1)/float (angle_histogram_size);
00435           normalized_angle_diff = (normalized_angle_diff <= 1.0f ? normalized_angle_diff : 2.0f-normalized_angle_diff);
00436           
00437           angle_change_value = std::max (angle_histogram[histogram_cell1] * angle_histogram[histogram_cell2] *
00438                                          normalized_angle_diff,   angle_change_value);
00439         }
00440       }
00441       angle_change_value = sqrtf (angle_change_value);
00442       interest_value = negative_score * angle_change_value;
00443       
00444       if (parameters_.add_points_on_straight_edges)
00445       {
00446         float max_histogram_cell_value = 0.0f;
00447         for (int histogram_cell=0; histogram_cell<angle_histogram_size; ++histogram_cell)
00448           max_histogram_cell_value = (std::max) (max_histogram_cell_value, angle_histogram[histogram_cell]);
00449         //std::cout << PVARN(max_histogram_cell_value);
00450         interest_value = 0.5f*(interest_value+max_histogram_cell_value);
00451         //std::cout << PVARN(interest_value);
00452       }
00453     }
00454     
00455     border_extractor.getParameters ().max_no_of_threads = original_max_no_of_threads;
00456   }
00457   
00458   if (interest_image_scale_space_.empty ())
00459     interest_image_ = NULL;
00460   else
00461     interest_image_ = interest_image_scale_space_[0];
00462 }
00463 
00464 void 
00465 NarfKeypoint::calculateSparseInterestImage ()
00466 {
00467   if (parameters_.support_size <= 0.0f)
00468   {
00469     std::cerr << __PRETTY_FUNCTION__<<": parameters_.support_size is not set!\n";
00470     return;
00471   }
00472   if (range_image_border_extractor_==NULL)
00473   {
00474     std::cerr << __PRETTY_FUNCTION__<<": range_image_border_extractor_ is not set!\n";
00475     return;
00476   }
00477 
00478   float search_radius = 0.5f * parameters_.support_size,
00479         radius_reciprocal = 1.0f / search_radius,
00480         increased_radius = 1.5f * search_radius,
00481         increased_radius_squared = increased_radius*increased_radius,
00482         radius_overhead = increased_radius-search_radius,
00483         radius_overhead_squared = radius_overhead*radius_overhead;
00484   
00485   const RangeImage& range_image = range_image_border_extractor_->getRangeImage ();
00486   RangeImageBorderExtractor& border_extractor = *range_image_border_extractor_;
00487   int original_max_no_of_threads = border_extractor.getParameters ().max_no_of_threads;
00488   border_extractor.getParameters ().max_no_of_threads = parameters_.max_no_of_threads;
00489   const ::pcl::PointCloud<BorderDescription>& border_descriptions = border_extractor.getBorderDescriptions ();
00490   const float* surface_change_scores = border_extractor.getSurfaceChangeScores ();
00491   const Eigen::Vector3f* surface_change_directions = border_extractor.getSurfaceChangeDirections ();
00492   
00493   int width  = range_image.width,
00494       height = range_image.height,
00495       array_size = width*height;
00496   
00497   interest_image_ = new float[array_size];
00498   
00499   for (int index=0; index<array_size; ++index)
00500   {
00501     interest_image_[index] = 0.0f;
00502     if (!range_image.isValid (index))
00503       continue;
00504     const BorderTraits& border_traits = border_descriptions.points[index].traits;
00505     if (border_traits[BORDER_TRAIT__SHADOW_BORDER] || border_traits[BORDER_TRAIT__VEIL_POINT])
00506       continue;
00507     interest_image_[index] = 2.0f;
00508   }
00509   
00510   const int angle_histogram_size = 18;
00511   std::vector<float> angle_histogram;
00512   angle_histogram.resize(angle_histogram_size);
00513   std::vector<std::vector<std::pair<int, float> > > angle_elements (angle_histogram_size);
00514   std::vector<bool> relevant_point_still_valid;
00515   
00516   std::vector<bool> was_touched;
00517   was_touched.resize (array_size, false);
00518   std::vector<int> neighbors_to_check,
00519                    neighbors_within_radius_overhead;
00520   
00521   //double interest_value_calculation_start_time = getTime ();
00522 # pragma omp parallel for default (shared) num_threads (parameters_.max_no_of_threads) schedule (guided, 10) \
00523                           firstprivate (was_touched, neighbors_to_check, angle_histogram, neighbors_within_radius_overhead, \
00524                                         angle_elements, relevant_point_still_valid) 
00525   for (int index=0; index<array_size; ++index)
00526   {
00527     if (interest_image_[index] <= 1.0f)
00528       continue;
00529     int y = index/range_image.width,
00530         x = index - y*range_image.width;
00531     
00532     const PointWithRange& point = range_image.getPoint (index);
00533     
00534     Eigen::Affine3f rotation_to_viewer_coordinate_system;
00535     range_image.getRotationToViewerCoordinateFrame (point.getVector3fMap (),
00536                                                     rotation_to_viewer_coordinate_system);
00537     
00538     // -----Start region growing to cover all connected points within the support size-----
00539     neighbors_to_check.clear ();
00540     neighbors_to_check.push_back (index);
00541     neighbors_within_radius_overhead.clear ();
00542     was_touched[index] = true;
00543     
00544     for (int angle_histogram_idx=0; angle_histogram_idx<angle_histogram_size; ++angle_histogram_idx)
00545     {
00546       angle_histogram[angle_histogram_idx] = 0;
00547       angle_elements[angle_histogram_idx].clear ();
00548     }
00549     
00550     for (size_t neighbors_to_check_idx=0; neighbors_to_check_idx<neighbors_to_check.size (); ++neighbors_to_check_idx)
00551     {
00552       int index2 = neighbors_to_check[neighbors_to_check_idx];
00553       if (!range_image.isValid (index2))
00554         continue;
00555       const BorderTraits& border_traits2 = border_descriptions.points[index2].traits;
00556       if (border_traits2[BORDER_TRAIT__SHADOW_BORDER] || border_traits2[BORDER_TRAIT__VEIL_POINT])
00557         continue;
00558       int y2 = index2/range_image.width,
00559           x2 = index2 - y2*range_image.width;
00560       const PointWithRange& point2 = range_image.getPoint (index2);
00561       
00562       float pixelDistance = static_cast<float> (std::max (abs (x2-x), abs (y2-y)));
00563 
00564       float distance_squared = squaredEuclideanDistance (point, point2);
00565       if (distance_squared <= radius_overhead_squared) 
00566         neighbors_within_radius_overhead.push_back (index2);
00567 
00568       if (pixelDistance > 2.0f)  // Always consider immediate neighbors, even if to far away
00569       {
00570         if (distance_squared>increased_radius_squared)
00571           continue;
00572       }
00573       
00574       for (int y3=std::max (0,y2-1); y3<=std::min (y2+1,int (range_image.height)-1); ++y3)
00575       {
00576         for (int x3=std::max (0,x2-1); x3<=std::min (x2+1,int (range_image.width)-1); ++x3)
00577         {
00578           int index3 = y3*range_image.width + x3;
00579           if (!was_touched[index3])
00580           {
00581             neighbors_to_check.push_back (index3);
00582             was_touched[index3] = true;
00583           }
00584         }
00585       }
00586       
00587       float surface_change_score = surface_change_scores[index2];
00588       if (surface_change_score < parameters_.min_surface_change_score)  // Pixel not 'interesting' enough to consider?
00589         continue;
00590       const Eigen::Vector3f& surface_change_direction = surface_change_directions[index2];
00591       
00592       float angle = nkdGetDirectionAngle (surface_change_direction, rotation_to_viewer_coordinate_system);
00593       int histogram_cell = (std::min) (angle_histogram_size-1,
00594                                        static_cast<int> (pcl_lrint (floorf ( (angle+deg2rad (90.0f))/deg2rad (180.0f) * angle_histogram_size))));
00595       float& histogram_value = angle_histogram[histogram_cell];
00596       histogram_value = (std::max) (histogram_value, surface_change_score);
00597       angle_elements[histogram_cell].push_back (std::make_pair(index2, surface_change_score));
00598     }
00599     
00600     // Reset was_touched to false
00601     for (size_t neighbors_to_check_idx=0; neighbors_to_check_idx<neighbors_to_check.size (); ++neighbors_to_check_idx)
00602       was_touched[neighbors_to_check[neighbors_to_check_idx]] = false;
00603     
00604     float angle_change_value = 0.0f;
00605     for (int histogram_cell1=0; histogram_cell1<angle_histogram_size-1; ++histogram_cell1)
00606     {
00607       if (angle_histogram[histogram_cell1]==0.0f)
00608         continue;
00609       for (int histogram_cell2=histogram_cell1+1; histogram_cell2<angle_histogram_size; ++histogram_cell2)
00610       {
00611         if (angle_histogram[histogram_cell2]==0.0f)
00612           continue;
00613         // TODO: lookup table for the following:
00614         float normalized_angle_diff = 2.0f*float (histogram_cell2-histogram_cell1)/float (angle_histogram_size);
00615         normalized_angle_diff = (normalized_angle_diff <= 1.0f ? normalized_angle_diff : 2.0f-normalized_angle_diff);
00616         
00617         angle_change_value = std::max (angle_histogram[histogram_cell1] * angle_histogram[histogram_cell2] *
00618                                        normalized_angle_diff,   angle_change_value);
00619       }
00620     }
00621     angle_change_value = sqrtf (angle_change_value);
00622     float maximum_interest_value = angle_change_value;
00623     
00624     if (parameters_.add_points_on_straight_edges)
00625     {
00626       float max_histogram_cell_value = 0.0f;
00627       for (int histogram_cell=0; histogram_cell<angle_histogram_size; ++histogram_cell)
00628         max_histogram_cell_value = (std::max) (max_histogram_cell_value, angle_histogram[histogram_cell]);
00629       maximum_interest_value = 0.5f * (maximum_interest_value+max_histogram_cell_value);
00630     }
00631     
00632     // Every point in distance search_radius cannot have a higher value
00633     // Therefore: if too low, set all to zero. Else calculate properly
00634     if (maximum_interest_value < parameters_.min_interest_value)
00635       for (size_t neighbors_idx=0; neighbors_idx<neighbors_within_radius_overhead.size (); ++neighbors_idx)
00636         interest_image_[neighbors_within_radius_overhead[neighbors_idx]] = 0.0f;
00637     else
00638     {
00639       // Reduce number of neighbors to go through by filtering close by points with the same angle
00640       bool do_neighbor_size_reduction = true;
00641       if (do_neighbor_size_reduction)
00642       {
00643         float min_distance_between_relevant_points = 0.25f * search_radius,
00644               min_distance_between_relevant_points_squared = powf(min_distance_between_relevant_points, 2);
00645         for (int angle_histogram_idx=0; angle_histogram_idx<angle_histogram_size; ++angle_histogram_idx)
00646         {
00647           std::vector<std::pair<int,float> >& relevent_point_indices = angle_elements[angle_histogram_idx];
00648           std::sort(relevent_point_indices.begin(), relevent_point_indices.end(), secondPairElementIsGreater);
00649           relevant_point_still_valid.clear();
00650           relevant_point_still_valid.resize(relevent_point_indices.size(), true);
00651           for (int rpi_idx1=0; rpi_idx1<int(relevent_point_indices.size ())-1; ++rpi_idx1)
00652           {
00653             if (!relevant_point_still_valid[rpi_idx1])
00654               continue;
00655             const PointWithRange& relevant_point1 = range_image.getPoint (relevent_point_indices[rpi_idx1].first);
00656             for (int rpi_idx2=rpi_idx1+1; rpi_idx2<int(relevent_point_indices.size ()); ++rpi_idx2)
00657             {
00658               if (!relevant_point_still_valid[rpi_idx2])
00659                 continue;
00660               const PointWithRange& relevant_point2 = range_image.getPoint (relevent_point_indices[rpi_idx2].first);
00661               float distance_squared = (relevant_point1.getVector3fMap ()-relevant_point2.getVector3fMap ()).norm ();
00662               if (distance_squared > min_distance_between_relevant_points_squared)
00663                 continue;
00664               relevant_point_still_valid[rpi_idx2] = false;
00665             }
00666           }
00667           int newPointIdx=0;
00668           for (int oldPointIdx=0; oldPointIdx<int(relevant_point_still_valid.size()); ++oldPointIdx) {
00669             if (relevant_point_still_valid[oldPointIdx])
00670               relevent_point_indices[newPointIdx++] = relevent_point_indices[oldPointIdx];
00671           }
00672           relevent_point_indices.resize(newPointIdx);
00673         }
00674       }
00675       
00676       // Caclulate interest values for neighbors
00677       for (size_t neighbors_idx=0; neighbors_idx<neighbors_within_radius_overhead.size (); ++neighbors_idx)
00678       {
00679         int index2 = neighbors_within_radius_overhead[neighbors_idx];
00680         int y2 = index2/range_image.width,
00681             x2 = index2 - y2*range_image.width;
00682         const PointWithRange& point2 = range_image.getPoint (index2);
00683         float& interest_value = interest_image_[index2];
00684         if (interest_value <= 1.0f)
00685           continue;
00686         float negative_score = 1.0;
00687 
00688         for (int angle_histogram_idx=0; angle_histogram_idx<angle_histogram_size; ++angle_histogram_idx)
00689         {
00690           float& histogram_value = angle_histogram[angle_histogram_idx];
00691           histogram_value = 0;
00692           const std::vector<std::pair<int,float> >& relevent_point_indices = angle_elements[angle_histogram_idx];
00693           for (size_t rpi_idx=0; rpi_idx<relevent_point_indices.size (); ++rpi_idx)
00694           {
00695             int index3 = relevent_point_indices[rpi_idx].first;
00696             int y3 = index3/range_image.width,
00697                 x3 = index3 - y3*range_image.width;
00698             const PointWithRange& point3 = range_image.getPoint (index3);
00699             float surface_change_score = relevent_point_indices[rpi_idx].second;
00700             
00701             float pixelDistance = static_cast<float> (std::max (abs (x3-x2), abs (y3-y2)));
00702             float distance = (point3.getVector3fMap ()-point2.getVector3fMap ()).norm ();
00703             float distance_factor = radius_reciprocal*distance;
00704             float positive_score, current_negative_score;
00705             nkdGetScores (distance_factor, surface_change_score, pixelDistance,
00706                          parameters_.optimal_distance_to_high_surface_change,
00707                          current_negative_score, positive_score);
00708             histogram_value = (std::max) (histogram_value, positive_score);
00709             negative_score  = (std::min) (negative_score, current_negative_score);
00710           }
00711         }
00712         float angle_change_value = 0.0f;
00713         for (int histogram_cell1=0; histogram_cell1<angle_histogram_size-1; ++histogram_cell1)
00714         {
00715           if (angle_histogram[histogram_cell1]==0.0f)
00716             continue;
00717           for (int histogram_cell2=histogram_cell1+1; histogram_cell2<angle_histogram_size; ++histogram_cell2)
00718           {
00719             if (angle_histogram[histogram_cell2]==0.0f)
00720               continue;
00721             // TODO: lookup table for the following:
00722             float normalized_angle_diff = 2.0f*float (histogram_cell2-histogram_cell1)/float (angle_histogram_size);
00723             normalized_angle_diff = (normalized_angle_diff <= 1.0f ? normalized_angle_diff : 2.0f-normalized_angle_diff);
00724             angle_change_value = std::max (angle_change_value, angle_histogram[histogram_cell1] *
00725                                                                angle_histogram[histogram_cell2] *
00726                                                                normalized_angle_diff);
00727           }
00728         }
00729         angle_change_value = sqrtf (angle_change_value);
00730         interest_value = negative_score * angle_change_value;
00731         if (parameters_.add_points_on_straight_edges)
00732         {
00733           float max_histogram_cell_value = 0.0f;
00734           for (int histogram_cell=0; histogram_cell<angle_histogram_size; ++histogram_cell)
00735             max_histogram_cell_value = (std::max) (max_histogram_cell_value, angle_histogram[histogram_cell]);
00736           //std::cout << PVARN(max_histogram_cell_value);
00737           interest_value = 0.5f*(interest_value+max_histogram_cell_value);
00738           //std::cout << PVARN(interest_value);
00739         }
00740       }
00741     }
00742   }
00743   
00744   border_extractor.getParameters ().max_no_of_threads = original_max_no_of_threads;
00745 }
00746 
00747 void 
00748 NarfKeypoint::calculateInterestPoints ()
00749 {
00750   //std::cout << __PRETTY_FUNCTION__ << " called.\n";
00751 
00752   if (interest_points_ != NULL)
00753     return;
00754 
00755   calculateInterestImage ();
00756   
00757   interest_points_ = new ::pcl::PointCloud<InterestPoint>;
00758   
00759   float max_distance_squared = powf (0.3f*parameters_.support_size, 2);
00760   
00761   const RangeImage& range_image = range_image_border_extractor_->getRangeImage ();
00762   const ::pcl::PointCloud<BorderDescription>& border_descriptions =
00763       range_image_border_extractor_->getBorderDescriptions ();
00764   int width  = range_image.width,
00765       height = range_image.height,
00766       size = width*height;
00767   is_interest_point_image_.clear ();
00768   is_interest_point_image_.resize (size, false);
00769   
00770   typedef double RealForPolynomial;
00771   PolynomialCalculationsT<RealForPolynomial> polynomial_calculations;
00772   BivariatePolynomialT<RealForPolynomial> polynomial (2);
00773   std::vector<Eigen::Matrix<RealForPolynomial, 3, 1> > sample_points;
00774   std::vector<RealForPolynomial> x_values, y_values;
00775   std::vector<int> types;
00776   std::vector<bool> invalid_beams, old_invalid_beams;
00777   
00778   pcl::PointCloud<InterestPoint>::VectorType tmp_interest_points;
00779   for (int y=0; y<height; ++y)
00780   {
00781     for (int x=0; x<width; ++x)
00782     {
00783       int index = y*width + x;
00784       float interest_value = interest_image_[index];
00785       if (!range_image.isValid (index) || interest_value < parameters_.min_interest_value)
00786         continue;
00787       const PointWithRange& point = range_image.getPoint (index);
00788       bool is_maximum = true;
00789       for (int y2=y-1; y2<=y+1&&is_maximum&&parameters_.do_non_maximum_suppression; ++y2)
00790       {
00791         for (int x2=x-1; x2<=x+1; ++x2)
00792         {
00793           if (!range_image.isInImage (x2,y2))
00794             continue;
00795           int index2 = y2*width + x2;
00796           float interest_value2 = interest_image_[index2];
00797           if (interest_value2 <= interest_value)
00798             continue;
00799           is_maximum = false;
00800           break;
00801         }
00802       }
00803       if (!is_maximum)
00804         continue;
00805       
00806       PointWithRange keypoint_3d = point;
00807       int keypoint_x_int=x, keypoint_y_int=y;
00808       
00809       int no_of_polynomial_approximations_per_point = parameters_.no_of_polynomial_approximations_per_point;
00810       if (!parameters_.do_non_maximum_suppression)
00811         no_of_polynomial_approximations_per_point = 0;
00812       
00813       for (int poly_step=0; poly_step<no_of_polynomial_approximations_per_point; ++poly_step)
00814       {
00815         sample_points.clear ();
00816         invalid_beams.clear ();
00817         old_invalid_beams.clear ();
00818         for (int radius=0, stop=false;  !stop;  ++radius) 
00819         {
00820           std::swap (invalid_beams, old_invalid_beams);
00821           propagateInvalidBeams (radius, old_invalid_beams, invalid_beams);
00822           int x2=keypoint_x_int-radius-1, y2=keypoint_y_int-radius;  // Top left - 1
00823           stop = true;
00824           for (int i=0; (radius==0&&i==0) || i<8*radius; ++i)
00825           {
00826             if (i<=2*radius) ++x2; else if (i<=4*radius) ++y2; else if (i<=6*radius) --x2; else --y2;
00827             if (invalid_beams[i] || !range_image.isValid (x2, y2))
00828               continue;
00829             int index2 = y2*width + x2;
00830             const BorderTraits& neighbor_border_traits = border_descriptions.points[index2].traits;
00831             if (neighbor_border_traits[BORDER_TRAIT__SHADOW_BORDER] || neighbor_border_traits[BORDER_TRAIT__VEIL_POINT])
00832             {
00833               invalid_beams[i] = true;
00834               continue;
00835             }
00836             const PointWithRange& neighbor = range_image.getPoint (index2);
00837             float distance_squared = squaredEuclideanDistance (point, neighbor);
00838             if (distance_squared>max_distance_squared)
00839             {
00840               invalid_beams[i] = true;
00841               continue;
00842             }
00843             stop = false; // There is a point in range -> Have to check further distances
00844             
00845             float interest_value2 = interest_image_[index2];
00846             sample_points.push_back (Eigen::Vector3d (x2-keypoint_x_int, y2-keypoint_y_int, interest_value2));
00847           }
00848         }
00849         if (!polynomial_calculations.bivariatePolynomialApproximation (sample_points, 2, polynomial))
00850           continue;
00851 
00852         polynomial.findCriticalPoints (x_values, y_values, types);
00853         
00854         if (!types.empty () && types[0]==0)
00855         {
00856           float keypoint_x = static_cast<float> (x_values[0]+keypoint_x_int),
00857                 keypoint_y = static_cast<float> (y_values[0]+keypoint_y_int);
00858           
00859           keypoint_x_int = static_cast<int> (pcl_lrint (keypoint_x));
00860           keypoint_y_int = static_cast<int> (pcl_lrint (keypoint_y));
00861           
00862           range_image.calculate3DPoint (keypoint_x, keypoint_y, keypoint_3d);
00863           if (!pcl_isfinite (keypoint_3d.range))
00864           {
00865             keypoint_3d = point;
00866             break;
00867           }
00868         }
00869         else
00870         {
00871           break;
00872         }
00873       }
00874       
00875       InterestPoint interest_point;
00876       interest_point.getVector3fMap () = keypoint_3d.getVector3fMap ();
00877       interest_point.strength = interest_value;
00878       tmp_interest_points.push_back (interest_point);
00879     }
00880   }
00881   
00882   std::sort (tmp_interest_points.begin (), tmp_interest_points.end (), isBetterInterestPoint);
00883   
00884   float min_distance_squared = powf (parameters_.min_distance_between_interest_points*parameters_.support_size, 2);
00885   for (size_t int_point_idx=0; int_point_idx<tmp_interest_points.size (); ++int_point_idx)
00886   {
00887     if (parameters_.max_no_of_interest_points > 0  &&  int (interest_points_->size ()) >= parameters_.max_no_of_interest_points)
00888       break;
00889     const InterestPoint& interest_point = tmp_interest_points[int_point_idx];
00890     
00891     bool better_point_too_close = false;
00892     for (size_t int_point_idx2=0; int_point_idx2<interest_points_->points.size (); ++int_point_idx2)
00893     {
00894       const InterestPoint& interest_point2 = interest_points_->points[int_point_idx2];
00895       float distance_squared = (interest_point.getVector3fMap ()-interest_point2.getVector3fMap ()).squaredNorm ();
00896       if (distance_squared < min_distance_squared)
00897       {
00898         better_point_too_close = true;
00899         break;
00900       }
00901     }
00902     if (better_point_too_close)
00903       continue;
00904     interest_points_->points.push_back (interest_point);
00905     int image_x, image_y;
00906     //std::cout << interest_point.x<<","<<interest_point.y<<","<<interest_point.z<<", "<<std::flush;
00907     range_image.getImagePoint (interest_point.getVector3fMap (), image_x, image_y);
00908     if (range_image.isValid (image_x, image_y))
00909       is_interest_point_image_[image_y*width + image_x] = true;
00910   }
00911   interest_points_->width = static_cast<uint32_t> (interest_points_->points.size ());
00912   interest_points_->height = 1;
00913   interest_points_->is_dense = true;
00914 }
00915 
00916 const RangeImage& 
00917 NarfKeypoint::getRangeImage ()
00918 {
00919   return (range_image_border_extractor_->getRangeImage ());
00920 }
00921 
00922 void 
00923 NarfKeypoint::detectKeypoints (NarfKeypoint::PointCloudOut& output)
00924 {
00925   output.points.clear ();
00926   
00927   if (indices_)
00928   {
00929     std::cerr << __PRETTY_FUNCTION__
00930               << ": Sorry, usage of indices for the extraction is not supported for NARF interest points (yet).\n\n";
00931     return;
00932   }
00933   
00934   if (range_image_border_extractor_ == NULL)
00935   {
00936     std::cerr << __PRETTY_FUNCTION__
00937               << ": RangeImageBorderExtractor member is not set. "
00938               <<   "Sorry, this is needed for the NARF keypoint extraction.\n\n";
00939     return;
00940   }
00941   
00942   if (!range_image_border_extractor_->hasRangeImage ())
00943   {
00944     std::cerr << __PRETTY_FUNCTION__
00945               << ": RangeImage is not set. Sorry, the NARF keypoint extraction works on range images, "
00946                    "not on normal point clouds.\n\n"
00947               << " Use setRangeImage (...).\n\n";
00948     return;
00949   }
00950   
00951   calculateInterestPoints ();
00952   
00953   int size = getRangeImage ().width * getRangeImage ().height;
00954   
00955   for (int index=0; index<size; ++index)
00956   {
00957     if (!is_interest_point_image_[index])
00958       continue;
00959     output.points.push_back (index);
00960   }
00961 }
00962 
00963 void 
00964 NarfKeypoint::compute (NarfKeypoint::PointCloudOut& output)
00965 {
00966   //std::cout << __PRETTY_FUNCTION__ << " called.\n";
00967   detectKeypoints (output);
00968 }
00969 
00970 }  // end namespace pcl


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:43