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