Point Cloud Library (PCL)  1.7.2
sample_consensus_prerejective.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
42 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
43 
44 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointSource, typename PointTarget, typename FeatureT> void
47 {
48  if (features == NULL || features->empty ())
49  {
50  PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
51  return;
52  }
53  input_features_ = features;
54 }
55 
56 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57 template <typename PointSource, typename PointTarget, typename FeatureT> void
59 {
60  if (features == NULL || features->empty ())
61  {
62  PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
63  return;
64  }
65  target_features_ = features;
66  feature_tree_->setInputCloud (target_features_);
67 }
68 
69 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointSource, typename PointTarget, typename FeatureT> void
72  const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices)
73 {
74  if (nr_samples > static_cast<int> (cloud.points.size ()))
75  {
76  PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
77  PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n",
78  nr_samples, cloud.points.size ());
79  return;
80  }
81 
82  sample_indices.resize (nr_samples);
83  int temp_sample;
84 
85  // Draw random samples until n samples is reached
86  for (int i = 0; i < nr_samples; i++)
87  {
88  // Select a random number
89  sample_indices[i] = getRandomIndex (static_cast<int> (cloud.points.size ()) - i);
90 
91  // Run trough list of numbers, starting at the lowest, to avoid duplicates
92  for (int j = 0; j < i; j++)
93  {
94  // Move value up if it is higher than previous selections to ensure true randomness
95  if (sample_indices[i] >= sample_indices[j])
96  {
97  sample_indices[i]++;
98  }
99  else
100  {
101  // The new number is lower, place it at the correct point and break for a sorted list
102  temp_sample = sample_indices[i];
103  for (int k = i; k > j; k--)
104  sample_indices[k] = sample_indices[k - 1];
105 
106  sample_indices[j] = temp_sample;
107  break;
108  }
109  }
110  }
111 }
112 
113 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
114 template <typename PointSource, typename PointTarget, typename FeatureT> void
116  const std::vector<int> &sample_indices,
117  std::vector<std::vector<int> >& similar_features,
118  std::vector<int> &corresponding_indices)
119 {
120  // Allocate results
121  corresponding_indices.resize (sample_indices.size ());
122  std::vector<float> nn_distances (k_correspondences_);
123 
124  // Loop over the sampled features
125  for (size_t i = 0; i < sample_indices.size (); ++i)
126  {
127  // Current feature index
128  const int idx = sample_indices[i];
129 
130  // Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already
131  if (similar_features[idx].empty ())
132  feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
133 
134  // Select one at random and add it to corresponding_indices
135  if (k_correspondences_ == 1)
136  corresponding_indices[i] = similar_features[idx][0];
137  else
138  corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
139  }
140 }
141 
142 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
143 template <typename PointSource, typename PointTarget, typename FeatureT> void
145 {
146  // Some sanity checks first
147  if (!input_features_)
148  {
149  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
150  PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
151  return;
152  }
153  if (!target_features_)
154  {
155  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
156  PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
157  return;
158  }
159 
160  if (input_->size () != input_features_->size ())
161  {
162  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
163  PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
164  input_->size (), input_features_->size ());
165  return;
166  }
167 
168  if (target_->size () != target_features_->size ())
169  {
170  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
171  PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
172  target_->size (), target_features_->size ());
173  return;
174  }
175 
176  if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
177  {
178  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
179  PCL_ERROR ("Illegal inlier fraction %f, must be in [0,1]!\n",
180  inlier_fraction_);
181  return;
182  }
183 
184  const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
185  if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
186  {
187  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
188  PCL_ERROR ("Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
189  similarity_threshold);
190  return;
191  }
192 
193  if (k_correspondences_ <= 0)
194  {
195  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
196  PCL_ERROR ("Illegal correspondence randomness %d, must be > 0!\n",
197  k_correspondences_);
198  return;
199  }
200 
201  // Initialize prerejector (similarity threshold already set to default value in constructor)
202  correspondence_rejector_poly_->setInputSource (input_);
203  correspondence_rejector_poly_->setInputTarget (target_);
204  correspondence_rejector_poly_->setCardinality (nr_samples_);
205  int num_rejections = 0; // For debugging
206 
207  // Initialize results
208  final_transformation_ = guess;
209  inliers_.clear ();
210  float lowest_error = std::numeric_limits<float>::max ();
211  converged_ = false;
212 
213  // Temporaries
214  std::vector<int> inliers;
215  float inlier_fraction;
216  float error;
217 
218  // If guess is not the Identity matrix we check it
219  if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
220  {
221  getFitness (inliers, error);
222  inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
223  error /= static_cast<float> (inliers.size ());
224 
225  if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
226  {
227  inliers_ = inliers;
228  lowest_error = error;
229  converged_ = true;
230  }
231  }
232 
233  // Feature correspondence cache
234  std::vector<std::vector<int> > similar_features (input_->size ());
235 
236  // Start
237  for (int i = 0; i < max_iterations_; ++i)
238  {
239  // Temporary containers
240  std::vector<int> sample_indices;
241  std::vector<int> corresponding_indices;
242 
243  // Draw nr_samples_ random samples
244  selectSamples (*input_, nr_samples_, sample_indices);
245 
246  // Find corresponding features in the target cloud
247  findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
248 
249  // Apply prerejection
250  if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
251  {
252  ++num_rejections;
253  continue;
254  }
255 
256  // Estimate the transform from the correspondences, write to transformation_
257  transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
258 
259  // Take a backup of previous result
260  const Matrix4 final_transformation_prev = final_transformation_;
261 
262  // Set final result to current transformation
263  final_transformation_ = transformation_;
264 
265  // Transform the input and compute the error (uses input_ and final_transformation_)
266  getFitness (inliers, error);
267 
268  // Restore previous result
269  final_transformation_ = final_transformation_prev;
270 
271  // If the new fit is better, update results
272  inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
273 
274  // Update result if pose hypothesis is better
275  if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
276  {
277  inliers_ = inliers;
278  lowest_error = error;
279  converged_ = true;
280  final_transformation_ = transformation_;
281  }
282  }
283 
284  // Apply the final transformation
285  if (converged_)
286  transformPointCloud (*input_, output, final_transformation_);
287 
288  // Debug output
289  PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
290  getClassName ().c_str (), num_rejections, max_iterations_);
291 }
292 
293 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
294 template <typename PointSource, typename PointTarget, typename FeatureT> void
296 {
297  // Initialize variables
298  inliers.clear ();
299  inliers.reserve (input_->size ());
300  fitness_score = 0.0f;
301 
302  // Use squared distance for comparison with NN search results
303  const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
304 
305  // Transform the input dataset using the final transformation
306  PointCloudSource input_transformed;
307  input_transformed.resize (input_->size ());
308  transformPointCloud (*input_, input_transformed, final_transformation_);
309 
310  // For each point in the source dataset
311  for (size_t i = 0; i < input_transformed.points.size (); ++i)
312  {
313  // Find its nearest neighbor in the target
314  std::vector<int> nn_indices (1);
315  std::vector<float> nn_dists (1);
316  tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
317 
318  // Check if point is an inlier
319  if (nn_dists[0] < max_range)
320  {
321  // Update inliers
322  inliers.push_back (static_cast<int> (i));
323 
324  // Update fitness score
325  fitness_score += nn_dists[0];
326  }
327  }
328 
329  // Calculate MSE
330  if (inliers.size () > 0)
331  fitness_score /= static_cast<float> (inliers.size ());
332  else
333  fitness_score = std::numeric_limits<float>::max ();
334 }
335 
336 #endif
337 
void getFitness(std::vector< int > &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method.
Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
Registration< PointSource, PointTarget >::Matrix4 Matrix4
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.
void findSimilarFeatures(const std::vector< int > &sample_indices, std::vector< std::vector< int > > &similar_features, std::vector< int > &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
void selectSamples(const PointCloudSource &cloud, int nr_samples, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.