sift_keypoint.hpp
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_SIFT_KEYPOINT_IMPL_H_
00039 #define PCL_SIFT_KEYPOINT_IMPL_H_
00040 
00041 #include <pcl/keypoints/sift_keypoint.h>
00042 #include <pcl/common/io.h>
00043 #include <pcl/filters/voxel_grid.h>
00044 
00046 template <typename PointInT, typename PointOutT> void 
00047 pcl::SIFTKeypoint<PointInT, PointOutT>::setScales (float min_scale, int nr_octaves, int nr_scales_per_octave)
00048 {
00049   min_scale_ = min_scale;
00050   nr_octaves_ = nr_octaves;
00051   nr_scales_per_octave_ = nr_scales_per_octave;
00052 }
00053 
00054 
00056 template <typename PointInT, typename PointOutT> void 
00057 pcl::SIFTKeypoint<PointInT, PointOutT>::setMinimumContrast (float min_contrast)
00058 {
00059   min_contrast_ = min_contrast;
00060 }
00061 
00063 template <typename PointInT, typename PointOutT> bool
00064 pcl::SIFTKeypoint<PointInT, PointOutT>::initCompute ()
00065 {
00066   if (min_scale_ <= 0)
00067   {
00068     PCL_ERROR ("[pcl::%s::initCompute] : Minimum scale (%f) must be strict positive!\n", 
00069                name_.c_str (), min_scale_);
00070     return (false);
00071   }
00072   if (nr_octaves_ < 1)
00073   {
00074     PCL_ERROR ("[pcl::%s::initCompute] : Number of octaves (%d) must be at least 1!\n", 
00075                name_.c_str (), nr_octaves_);
00076     return (false);
00077   }
00078   if (nr_scales_per_octave_ < 1)
00079   {
00080     PCL_ERROR ("[pcl::%s::initCompute] : Number of scales per octave (%d) must be at least 1!\n", 
00081                name_.c_str (), nr_scales_per_octave_);
00082     return (false);
00083   }
00084   if (min_contrast_ < 0)
00085   {
00086     PCL_ERROR ("[pcl::%s::initCompute] : Minimum contrast (%f) must be non-negative!\n", 
00087                name_.c_str (), min_contrast_);
00088     return (false);
00089   }
00090   
00091   this->setKSearch (1);
00092   tree_.reset (new pcl::search::KdTree<PointInT> (true));
00093   return (true);
00094 }
00095 
00097 template <typename PointInT, typename PointOutT> void 
00098 pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
00099 {
00100   if (surface_ && surface_ != input_)
00101   {
00102     PCL_WARN ("[pcl::%s::detectKeypoints] : ", name_.c_str ());
00103     PCL_WARN ("A search surface has been set by setSearchSurface, but this SIFT keypoint detection algorithm does ");
00104     PCL_WARN ("not support search surfaces other than the input cloud.  ");
00105     PCL_WARN ("The cloud provided in setInputCloud is being used instead.\n");
00106   }
00107 
00108   // Check if the output has a "scale" field
00109   scale_idx_ = pcl::getFieldIndex<PointOutT> (output, "scale", out_fields_);
00110 
00111   // Make sure the output cloud is empty
00112   output.points.clear ();
00113 
00114   // Create a local copy of the input cloud that will be resized for each octave
00115   boost::shared_ptr<pcl::PointCloud<PointInT> > cloud (new pcl::PointCloud<PointInT> (*input_));
00116 
00117   VoxelGrid<PointInT> voxel_grid;
00118   // Search for keypoints at each octave
00119   float scale = min_scale_;
00120   for (int i_octave = 0; i_octave < nr_octaves_; ++i_octave)
00121   {
00122     // Downsample the point cloud
00123     const float s = 1.0f * scale; // note: this can be adjusted
00124     voxel_grid.setLeafSize (s, s, s);
00125     voxel_grid.setInputCloud (cloud);
00126     boost::shared_ptr<pcl::PointCloud<PointInT> > temp (new pcl::PointCloud<PointInT>);    
00127     voxel_grid.filter (*temp);
00128     cloud = temp;
00129 
00130     // Make sure the downsampled cloud still has enough points
00131     const size_t min_nr_points = 25;
00132     if (cloud->points.size () < min_nr_points)
00133       break;
00134 
00135     // Update the KdTree with the downsampled points
00136     tree_->setInputCloud (cloud);
00137 
00138     // Detect keypoints for the current scale
00139     detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output);
00140 
00141     // Increase the scale by another octave
00142     scale *= 2;
00143   }
00144 
00145   output.height = 1;
00146   output.width = static_cast<uint32_t> (output.points.size ());
00147 }
00148 
00149 
00151 template <typename PointInT, typename PointOutT> void 
00152 pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypointsForOctave (
00153     const PointCloudIn &input, KdTree &tree, float base_scale, int nr_scales_per_octave, 
00154     PointCloudOut &output)
00155 {
00156   // Compute the difference of Gaussians (DoG) scale space
00157   std::vector<float> scales (nr_scales_per_octave + 3);
00158   for (int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale)
00159   {
00160     scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast<float> (i_scale) - 1.0f) / static_cast<float> (nr_scales_per_octave));
00161   }
00162   Eigen::MatrixXf diff_of_gauss;
00163   computeScaleSpace (input, tree, scales, diff_of_gauss);
00164 
00165   // Find extrema in the DoG scale space
00166   std::vector<int> extrema_indices, extrema_scales;
00167   findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales);
00168 
00169   output.points.reserve (output.points.size () + extrema_indices.size ());
00170   // Save scale?
00171   if (scale_idx_ != -1)
00172   {
00173     // Add keypoints to output
00174     for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
00175     {
00176       PointOutT keypoint;
00177       const int &keypoint_index = extrema_indices[i_keypoint];
00178    
00179       keypoint.x = input.points[keypoint_index].x;
00180       keypoint.y = input.points[keypoint_index].y;
00181       keypoint.z = input.points[keypoint_index].z;
00182       memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset,
00183               &scales[extrema_scales[i_keypoint]], sizeof (float));
00184       output.points.push_back (keypoint); 
00185     }
00186   }
00187   else
00188   {
00189     // Add keypoints to output
00190     for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
00191     {
00192       PointOutT keypoint;
00193       const int &keypoint_index = extrema_indices[i_keypoint];
00194    
00195       keypoint.x = input.points[keypoint_index].x;
00196       keypoint.y = input.points[keypoint_index].y;
00197       keypoint.z = input.points[keypoint_index].z;
00198 
00199       output.points.push_back (keypoint); 
00200     }
00201   }
00202 }
00203 
00204 
00206 template <typename PointInT, typename PointOutT> 
00207 void pcl::SIFTKeypoint<PointInT, PointOutT>::computeScaleSpace (
00208     const PointCloudIn &input, KdTree &tree, const std::vector<float> &scales, 
00209     Eigen::MatrixXf &diff_of_gauss)
00210 {
00211   diff_of_gauss.resize (input.size (), scales.size () - 1);
00212 
00213   // For efficiency, we will only filter over points within 3 standard deviations 
00214   const float max_radius = 3.0f * scales.back ();
00215 
00216   for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
00217   {
00218     std::vector<int> nn_indices;
00219     std::vector<float> nn_dist;
00220     tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // *
00221     // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale, 
00222     //   regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch 
00223     //   here instead of using searchForNeighbors.
00224 
00225     // For each scale, compute the Gaussian "filter response" at the current point
00226     float filter_response = 0.0f;
00227     float previous_filter_response;
00228     for (size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
00229     {
00230       float sigma_sqr = powf (scales[i_scale], 2.0f);
00231 
00232       float numerator = 0.0f;
00233       float denominator = 0.0f;
00234       for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
00235       {
00236         const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]);
00237         const float &dist_sqr = nn_dist[i_neighbor];
00238         if (dist_sqr <= 9*sigma_sqr)
00239         {
00240           float w = expf (-0.5f * dist_sqr / sigma_sqr);
00241           numerator += value * w;
00242           denominator += w;
00243         }
00244         else break; // i.e. if dist > 3 standard deviations, then terminate early
00245       }
00246       previous_filter_response = filter_response;
00247       filter_response = numerator / denominator;
00248 
00249       // Compute the difference between adjacent scales
00250       if (i_scale > 0)
00251         diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response;
00252     }
00253   }
00254 }
00255 
00257 template <typename PointInT, typename PointOutT> void 
00258 pcl::SIFTKeypoint<PointInT, PointOutT>::findScaleSpaceExtrema (
00259     const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss, 
00260     std::vector<int> &extrema_indices, std::vector<int> &extrema_scales)
00261 {
00262   const int k = 25;
00263   std::vector<int> nn_indices (k);
00264   std::vector<float> nn_dist (k);
00265 
00266   const int nr_scales = static_cast<int> (diff_of_gauss.cols ());
00267   std::vector<float> min_val (nr_scales), max_val (nr_scales);
00268 
00269   for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
00270   {
00271     // Define the local neighborhood around the current point
00272     const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist); //*
00273     // * note: the neighborhood for finding local extrema is best defined as a small fixed-k neighborhood, regardless of
00274     //   the configurable search method specified by the user, so we directly employ tree.nearestKSearch here instead 
00275     //   of using searchForNeighbors
00276 
00277     // At each scale, find the extreme values of the DoG within the current neighborhood
00278     for (int i_scale = 0; i_scale < nr_scales; ++i_scale)
00279     {
00280       min_val[i_scale] = std::numeric_limits<float>::max ();
00281       max_val[i_scale] = -std::numeric_limits<float>::max ();
00282 
00283       for (size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor)
00284       {
00285         const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale);
00286 
00287         min_val[i_scale] = (std::min) (min_val[i_scale], d);
00288         max_val[i_scale] = (std::max) (max_val[i_scale], d);
00289       }
00290     }
00291 
00292     // If the current point is an extreme value with high enough contrast, add it as a keypoint 
00293     for (int i_scale = 1; i_scale < nr_scales - 1; ++i_scale)
00294     {
00295       const float &val = diff_of_gauss (i_point, i_scale);
00296 
00297       // Does the point have sufficient contrast?
00298       if (fabs (val) >= min_contrast_)
00299       {
00300         // Is it a local minimum?
00301         if ((val == min_val[i_scale]) && 
00302             (val <  min_val[i_scale - 1]) && 
00303             (val <  min_val[i_scale + 1]))
00304         {
00305           extrema_indices.push_back (i_point);
00306           extrema_scales.push_back (i_scale);
00307         }
00308         // Is it a local maximum?
00309         else if ((val == max_val[i_scale]) && 
00310                  (val >  max_val[i_scale - 1]) && 
00311                  (val >  max_val[i_scale + 1]))
00312         {
00313           extrema_indices.push_back (i_point);
00314           extrema_scales.push_back (i_scale);
00315         }
00316       }
00317     }
00318   }
00319 }
00320 
00321 #define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint<T,U>;
00322 
00323 #endif // #ifndef PCL_SIFT_KEYPOINT_IMPL_H_
00324 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:33:37