Point Cloud Library (PCL)  1.9.1
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 
224  if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
225  {
226  inliers_ = inliers;
227  lowest_error = error;
228  converged_ = true;
229  }
230  }
231 
232  // Feature correspondence cache
233  std::vector<std::vector<int> > similar_features (input_->size ());
234 
235  // Start
236  for (int i = 0; i < max_iterations_; ++i)
237  {
238  // Temporary containers
239  std::vector<int> sample_indices;
240  std::vector<int> corresponding_indices;
241 
242  // Draw nr_samples_ random samples
243  selectSamples (*input_, nr_samples_, sample_indices);
244 
245  // Find corresponding features in the target cloud
246  findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
247 
248  // Apply prerejection
249  if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
250  {
251  ++num_rejections;
252  continue;
253  }
254 
255  // Estimate the transform from the correspondences, write to transformation_
256  transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
257 
258  // Take a backup of previous result
259  const Matrix4 final_transformation_prev = final_transformation_;
260 
261  // Set final result to current transformation
262  final_transformation_ = transformation_;
263 
264  // Transform the input and compute the error (uses input_ and final_transformation_)
265  getFitness (inliers, error);
266 
267  // Restore previous result
268  final_transformation_ = final_transformation_prev;
269 
270  // If the new fit is better, update results
271  inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
272 
273  // Update result if pose hypothesis is better
274  if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
275  {
276  inliers_ = inliers;
277  lowest_error = error;
278  converged_ = true;
279  final_transformation_ = transformation_;
280  }
281  }
282 
283  // Apply the final transformation
284  if (converged_)
285  transformPointCloud (*input_, output, final_transformation_);
286 
287  // Debug output
288  PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
289  getClassName ().c_str (), num_rejections, max_iterations_);
290 }
291 
292 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename PointSource, typename PointTarget, typename FeatureT> void
295 {
296  // Initialize variables
297  inliers.clear ();
298  inliers.reserve (input_->size ());
299  fitness_score = 0.0f;
300 
301  // Use squared distance for comparison with NN search results
302  const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
303 
304  // Transform the input dataset using the final transformation
305  PointCloudSource input_transformed;
306  input_transformed.resize (input_->size ());
307  transformPointCloud (*input_, input_transformed, final_transformation_);
308 
309  // For each point in the source dataset
310  for (size_t i = 0; i < input_transformed.points.size (); ++i)
311  {
312  // Find its nearest neighbor in the target
313  std::vector<int> nn_indices (1);
314  std::vector<float> nn_dists (1);
315  tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
316 
317  // Check if point is an inlier
318  if (nn_dists[0] < max_range)
319  {
320  // Update inliers
321  inliers.push_back (static_cast<int> (i));
322 
323  // Update fitness score
324  fitness_score += nn_dists[0];
325  }
326  }
327 
328  // Calculate MSE
329  if (inliers.size () > 0)
330  fitness_score /= static_cast<float> (inliers.size ());
331  else
332  fitness_score = std::numeric_limits<float>::max ();
333 }
334 
335 #endif
336 
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
Registration< PointSource, PointTarget >::Matrix4 Matrix4
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud&#39;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&#39;s feature descriptors.