Point Cloud Library (PCL)  1.7.2
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  output.height = 1;
146  output.width = static_cast<uint32_t> (output.points.size ());
147 }
148 
149 
150 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
151 template <typename PointInT, typename PointOutT> void
153  const PointCloudIn &input, KdTree &tree, float base_scale, int nr_scales_per_octave,
154  PointCloudOut &output)
155 {
156  // Compute the difference of Gaussians (DoG) scale space
157  std::vector<float> scales (nr_scales_per_octave + 3);
158  for (int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale)
159  {
160  scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast<float> (i_scale) - 1.0f) / static_cast<float> (nr_scales_per_octave));
161  }
162  Eigen::MatrixXf diff_of_gauss;
163  computeScaleSpace (input, tree, scales, diff_of_gauss);
164 
165  // Find extrema in the DoG scale space
166  std::vector<int> extrema_indices, extrema_scales;
167  findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales);
168 
169  output.points.reserve (output.points.size () + extrema_indices.size ());
170  // Save scale?
171  if (scale_idx_ != -1)
172  {
173  // Add keypoints to output
174  for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
175  {
176  PointOutT keypoint;
177  const int &keypoint_index = extrema_indices[i_keypoint];
178 
179  keypoint.x = input.points[keypoint_index].x;
180  keypoint.y = input.points[keypoint_index].y;
181  keypoint.z = input.points[keypoint_index].z;
182  memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset,
183  &scales[extrema_scales[i_keypoint]], sizeof (float));
184  output.points.push_back (keypoint);
185  }
186  }
187  else
188  {
189  // Add keypoints to output
190  for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
191  {
192  PointOutT keypoint;
193  const int &keypoint_index = extrema_indices[i_keypoint];
194 
195  keypoint.x = input.points[keypoint_index].x;
196  keypoint.y = input.points[keypoint_index].y;
197  keypoint.z = input.points[keypoint_index].z;
198 
199  output.points.push_back (keypoint);
200  }
201  }
202 }
203 
204 
205 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
206 template <typename PointInT, typename PointOutT>
208  const PointCloudIn &input, KdTree &tree, const std::vector<float> &scales,
209  Eigen::MatrixXf &diff_of_gauss)
210 {
211  diff_of_gauss.resize (input.size (), scales.size () - 1);
212 
213  // For efficiency, we will only filter over points within 3 standard deviations
214  const float max_radius = 3.0f * scales.back ();
215 
216  for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
217  {
218  std::vector<int> nn_indices;
219  std::vector<float> nn_dist;
220  tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // *
221  // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale,
222  // regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch
223  // here instead of using searchForNeighbors.
224 
225  // For each scale, compute the Gaussian "filter response" at the current point
226  float filter_response = 0.0f;
227  float previous_filter_response;
228  for (size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
229  {
230  float sigma_sqr = powf (scales[i_scale], 2.0f);
231 
232  float numerator = 0.0f;
233  float denominator = 0.0f;
234  for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
235  {
236  const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]);
237  const float &dist_sqr = nn_dist[i_neighbor];
238  if (dist_sqr <= 9*sigma_sqr)
239  {
240  float w = expf (-0.5f * dist_sqr / sigma_sqr);
241  numerator += value * w;
242  denominator += w;
243  }
244  else break; // i.e. if dist > 3 standard deviations, then terminate early
245  }
246  previous_filter_response = filter_response;
247  filter_response = numerator / denominator;
248 
249  // Compute the difference between adjacent scales
250  if (i_scale > 0)
251  diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response;
252  }
253  }
254 }
255 
256 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
257 template <typename PointInT, typename PointOutT> void
259  const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss,
260  std::vector<int> &extrema_indices, std::vector<int> &extrema_scales)
261 {
262  const int k = 25;
263  std::vector<int> nn_indices (k);
264  std::vector<float> nn_dist (k);
265 
266  const int nr_scales = static_cast<int> (diff_of_gauss.cols ());
267  std::vector<float> min_val (nr_scales), max_val (nr_scales);
268 
269  for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
270  {
271  // Define the local neighborhood around the current point
272  const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist); //*
273  // * note: the neighborhood for finding local extrema is best defined as a small fixed-k neighborhood, regardless of
274  // the configurable search method specified by the user, so we directly employ tree.nearestKSearch here instead
275  // of using searchForNeighbors
276 
277  // At each scale, find the extreme values of the DoG within the current neighborhood
278  for (int i_scale = 0; i_scale < nr_scales; ++i_scale)
279  {
280  min_val[i_scale] = std::numeric_limits<float>::max ();
281  max_val[i_scale] = -std::numeric_limits<float>::max ();
282 
283  for (size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor)
284  {
285  const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale);
286 
287  min_val[i_scale] = (std::min) (min_val[i_scale], d);
288  max_val[i_scale] = (std::max) (max_val[i_scale], d);
289  }
290  }
291 
292  // If the current point is an extreme value with high enough contrast, add it as a keypoint
293  for (int i_scale = 1; i_scale < nr_scales - 1; ++i_scale)
294  {
295  const float &val = diff_of_gauss (i_point, i_scale);
296 
297  // Does the point have sufficient contrast?
298  if (fabs (val) >= min_contrast_)
299  {
300  // Is it a local minimum?
301  if ((val == min_val[i_scale]) &&
302  (val < min_val[i_scale - 1]) &&
303  (val < min_val[i_scale + 1]))
304  {
305  extrema_indices.push_back (i_point);
306  extrema_scales.push_back (i_scale);
307  }
308  // Is it a local maximum?
309  else if ((val == max_val[i_scale]) &&
310  (val > max_val[i_scale - 1]) &&
311  (val > max_val[i_scale + 1]))
312  {
313  extrema_indices.push_back (i_point);
314  extrema_scales.push_back (i_scale);
315  }
316  }
317  }
318  }
319 }
320 
321 #define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint<T,U>;
322 
323 #endif // #ifndef PCL_SIFT_KEYPOINT_IMPL_H_
324 
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:131
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:223