Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sift_keypoint.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_SIFT_KEYPOINT_IMPL_H_
39 #define PCL_SIFT_KEYPOINT_IMPL_H_
40 
41 #include <pcl/keypoints/sift_keypoint.h>
42 #include <pcl/common/io.h>
43 #include <pcl/filters/voxel_grid.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointInT, typename PointOutT> void
47 pcl::SIFTKeypoint<PointInT, PointOutT>::setScales (float min_scale, int nr_octaves, int nr_scales_per_octave)
48 {
49  min_scale_ = min_scale;
50  nr_octaves_ = nr_octaves;
51  nr_scales_per_octave_ = nr_scales_per_octave;
52 }
53 
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointInT, typename PointOutT> void
58 {
59  min_contrast_ = min_contrast;
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointInT, typename PointOutT> bool
65 {
66  if (min_scale_ <= 0)
67  {
68  PCL_ERROR ("[pcl::%s::initCompute] : Minimum scale (%f) must be strict positive!\n",
69  name_.c_str (), min_scale_);
70  return (false);
71  }
72  if (nr_octaves_ < 1)
73  {
74  PCL_ERROR ("[pcl::%s::initCompute] : Number of octaves (%d) must be at least 1!\n",
75  name_.c_str (), nr_octaves_);
76  return (false);
77  }
78  if (nr_scales_per_octave_ < 1)
79  {
80  PCL_ERROR ("[pcl::%s::initCompute] : Number of scales per octave (%d) must be at least 1!\n",
81  name_.c_str (), nr_scales_per_octave_);
82  return (false);
83  }
84  if (min_contrast_ < 0)
85  {
86  PCL_ERROR ("[pcl::%s::initCompute] : Minimum contrast (%f) must be non-negative!\n",
87  name_.c_str (), min_contrast_);
88  return (false);
89  }
90 
91  this->setKSearch (1);
92  tree_.reset (new pcl::search::KdTree<PointInT> (true));
93  return (true);
94 }
95 
96 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
97 template <typename PointInT, typename PointOutT> void
99 {
100  if (surface_ && surface_ != input_)
101  {
102  PCL_WARN ("[pcl::%s::detectKeypoints] : ", name_.c_str ());
103  PCL_WARN ("A search surface has been set by setSearchSurface, but this SIFT keypoint detection algorithm does ");
104  PCL_WARN ("not support search surfaces other than the input cloud. ");
105  PCL_WARN ("The cloud provided in setInputCloud is being used instead.\n");
106  }
107 
108  // Check if the output has a "scale" field
109  scale_idx_ = pcl::getFieldIndex<PointOutT> (output, "scale", out_fields_);
110 
111  // Make sure the output cloud is empty
112  output.points.clear ();
113 
114  // Create a local copy of the input cloud that will be resized for each octave
115  boost::shared_ptr<pcl::PointCloud<PointInT> > cloud (new pcl::PointCloud<PointInT> (*input_));
116 
117  VoxelGrid<PointInT> voxel_grid;
118  // Search for keypoints at each octave
119  float scale = min_scale_;
120  for (int i_octave = 0; i_octave < nr_octaves_; ++i_octave)
121  {
122  // Downsample the point cloud
123  const float s = 1.0f * scale; // note: this can be adjusted
124  voxel_grid.setLeafSize (s, s, s);
125  voxel_grid.setInputCloud (cloud);
126  boost::shared_ptr<pcl::PointCloud<PointInT> > temp (new pcl::PointCloud<PointInT>);
127  voxel_grid.filter (*temp);
128  cloud = temp;
129 
130  // Make sure the downsampled cloud still has enough points
131  const size_t min_nr_points = 25;
132  if (cloud->points.size () < min_nr_points)
133  break;
134 
135  // Update the KdTree with the downsampled points
136  tree_->setInputCloud (cloud);
137 
138  // Detect keypoints for the current scale
139  detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output);
140 
141  // Increase the scale by another octave
142  scale *= 2;
143  }
144 
145  // Set final properties
146  output.height = 1;
147  output.width = static_cast<uint32_t> (output.points.size ());
148  output.header = input_->header;
149  output.sensor_origin_ = input_->sensor_origin_;
150  output.sensor_orientation_ = input_->sensor_orientation_;
151 }
152 
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
155 template <typename PointInT, typename PointOutT> void
157  const PointCloudIn &input, KdTree &tree, float base_scale, int nr_scales_per_octave,
158  PointCloudOut &output)
159 {
160  // Compute the difference of Gaussians (DoG) scale space
161  std::vector<float> scales (nr_scales_per_octave + 3);
162  for (int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale)
163  {
164  scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast<float> (i_scale) - 1.0f) / static_cast<float> (nr_scales_per_octave));
165  }
166  Eigen::MatrixXf diff_of_gauss;
167  computeScaleSpace (input, tree, scales, diff_of_gauss);
168 
169  // Find extrema in the DoG scale space
170  std::vector<int> extrema_indices, extrema_scales;
171  findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales);
172 
173  output.points.reserve (output.points.size () + extrema_indices.size ());
174  // Save scale?
175  if (scale_idx_ != -1)
176  {
177  // Add keypoints to output
178  for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
179  {
180  PointOutT keypoint;
181  const int &keypoint_index = extrema_indices[i_keypoint];
182 
183  keypoint.x = input.points[keypoint_index].x;
184  keypoint.y = input.points[keypoint_index].y;
185  keypoint.z = input.points[keypoint_index].z;
186  memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset,
187  &scales[extrema_scales[i_keypoint]], sizeof (float));
188  output.points.push_back (keypoint);
189  }
190  }
191  else
192  {
193  // Add keypoints to output
194  for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
195  {
196  PointOutT keypoint;
197  const int &keypoint_index = extrema_indices[i_keypoint];
198 
199  keypoint.x = input.points[keypoint_index].x;
200  keypoint.y = input.points[keypoint_index].y;
201  keypoint.z = input.points[keypoint_index].z;
202 
203  output.points.push_back (keypoint);
204  }
205  }
206 }
207 
208 
209 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
210 template <typename PointInT, typename PointOutT>
212  const PointCloudIn &input, KdTree &tree, const std::vector<float> &scales,
213  Eigen::MatrixXf &diff_of_gauss)
214 {
215  diff_of_gauss.resize (input.size (), scales.size () - 1);
216 
217  // For efficiency, we will only filter over points within 3 standard deviations
218  const float max_radius = 3.0f * scales.back ();
219 
220  for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
221  {
222  std::vector<int> nn_indices;
223  std::vector<float> nn_dist;
224  tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // *
225  // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale,
226  // regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch
227  // here instead of using searchForNeighbors.
228 
229  // For each scale, compute the Gaussian "filter response" at the current point
230  float filter_response = 0.0f;
231  float previous_filter_response;
232  for (size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
233  {
234  float sigma_sqr = powf (scales[i_scale], 2.0f);
235 
236  float numerator = 0.0f;
237  float denominator = 0.0f;
238  for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
239  {
240  const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]);
241  const float &dist_sqr = nn_dist[i_neighbor];
242  if (dist_sqr <= 9*sigma_sqr)
243  {
244  float w = expf (-0.5f * dist_sqr / sigma_sqr);
245  numerator += value * w;
246  denominator += w;
247  }
248  else break; // i.e. if dist > 3 standard deviations, then terminate early
249  }
250  previous_filter_response = filter_response;
251  filter_response = numerator / denominator;
252 
253  // Compute the difference between adjacent scales
254  if (i_scale > 0)
255  diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response;
256  }
257  }
258 }
259 
260 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
261 template <typename PointInT, typename PointOutT> void
263  const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss,
264  std::vector<int> &extrema_indices, std::vector<int> &extrema_scales)
265 {
266  const int k = 25;
267  std::vector<int> nn_indices (k);
268  std::vector<float> nn_dist (k);
269 
270  const int nr_scales = static_cast<int> (diff_of_gauss.cols ());
271  std::vector<float> min_val (nr_scales), max_val (nr_scales);
272 
273  for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
274  {
275  // Define the local neighborhood around the current point
276  const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist); //*
277  // * note: the neighborhood for finding local extrema is best defined as a small fixed-k neighborhood, regardless of
278  // the configurable search method specified by the user, so we directly employ tree.nearestKSearch here instead
279  // of using searchForNeighbors
280 
281  // At each scale, find the extreme values of the DoG within the current neighborhood
282  for (int i_scale = 0; i_scale < nr_scales; ++i_scale)
283  {
284  min_val[i_scale] = std::numeric_limits<float>::max ();
285  max_val[i_scale] = -std::numeric_limits<float>::max ();
286 
287  for (size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor)
288  {
289  const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale);
290 
291  min_val[i_scale] = (std::min) (min_val[i_scale], d);
292  max_val[i_scale] = (std::max) (max_val[i_scale], d);
293  }
294  }
295 
296  // If the current point is an extreme value with high enough contrast, add it as a keypoint
297  for (int i_scale = 1; i_scale < nr_scales - 1; ++i_scale)
298  {
299  const float &val = diff_of_gauss (i_point, i_scale);
300 
301  // Does the point have sufficient contrast?
302  if (fabs (val) >= min_contrast_)
303  {
304  // Is it a local minimum?
305  if ((val == min_val[i_scale]) &&
306  (val < min_val[i_scale - 1]) &&
307  (val < min_val[i_scale + 1]))
308  {
309  extrema_indices.push_back (i_point);
310  extrema_scales.push_back (i_scale);
311  }
312  // Is it a local maximum?
313  else if ((val == max_val[i_scale]) &&
314  (val > max_val[i_scale - 1]) &&
315  (val > max_val[i_scale + 1]))
316  {
317  extrema_indices.push_back (i_point);
318  extrema_scales.push_back (i_scale);
319  }
320  }
321  }
322  }
323 }
324 
325 #define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint<T,U>;
326 
327 #endif // #ifndef PCL_SIFT_KEYPOINT_IMPL_H_
328 
SIFTKeypoint detects the Scale Invariant Feature Transform keypoints for a given point cloud dataset ...
Definition: sift_keypoint.h:94
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:132
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
void setScales(float min_scale, int nr_octaves, int nr_scales_per_octave)
Specify the range of scales over which to search for keypoints.
void setMinimumContrast(float min_contrast)
Provide a threshold to limit detection of keypoints without sufficient contrast.
void detectKeypoints(PointCloudOut &output)
Detect the SIFT keypoints for a set of points given in setInputCloud () using the spatial locator in ...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:56
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:224