Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 namespace pcl
46 {
47 
48 template <typename PointSource, typename PointTarget, typename FeatureT> void
50 {
51  if (features == nullptr || features->empty ())
52  {
53  PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
54  return;
55  }
56  input_features_ = features;
57 }
58 
59 
60 template <typename PointSource, typename PointTarget, typename FeatureT> void
62 {
63  if (features == nullptr || features->empty ())
64  {
65  PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
66  return;
67  }
68  target_features_ = features;
69  feature_tree_->setInputCloud (target_features_);
70 }
71 
72 
73 template <typename PointSource, typename PointTarget, typename FeatureT> void
75  const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices)
76 {
77  if (nr_samples > static_cast<int> (cloud.points.size ()))
78  {
79  PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
80  PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n",
81  nr_samples, cloud.points.size ());
82  return;
83  }
84 
85  sample_indices.resize (nr_samples);
86  int temp_sample;
87 
88  // Draw random samples until n samples is reached
89  for (int i = 0; i < nr_samples; i++)
90  {
91  // Select a random number
92  sample_indices[i] = getRandomIndex (static_cast<int> (cloud.points.size ()) - i);
93 
94  // Run trough list of numbers, starting at the lowest, to avoid duplicates
95  for (int j = 0; j < i; j++)
96  {
97  // Move value up if it is higher than previous selections to ensure true randomness
98  if (sample_indices[i] >= sample_indices[j])
99  {
100  sample_indices[i]++;
101  }
102  else
103  {
104  // The new number is lower, place it at the correct point and break for a sorted list
105  temp_sample = sample_indices[i];
106  for (int k = i; k > j; k--)
107  sample_indices[k] = sample_indices[k - 1];
108 
109  sample_indices[j] = temp_sample;
110  break;
111  }
112  }
113  }
114 }
115 
116 
117 template <typename PointSource, typename PointTarget, typename FeatureT> void
119  const std::vector<int> &sample_indices,
120  std::vector<std::vector<int> >& similar_features,
121  std::vector<int> &corresponding_indices)
122 {
123  // Allocate results
124  corresponding_indices.resize (sample_indices.size ());
125  std::vector<float> nn_distances (k_correspondences_);
126 
127  // Loop over the sampled features
128  for (std::size_t i = 0; i < sample_indices.size (); ++i)
129  {
130  // Current feature index
131  const int idx = sample_indices[i];
132 
133  // Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already
134  if (similar_features[idx].empty ())
135  feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
136 
137  // Select one at random and add it to corresponding_indices
138  if (k_correspondences_ == 1)
139  corresponding_indices[i] = similar_features[idx][0];
140  else
141  corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
142  }
143 }
144 
145 
146 template <typename PointSource, typename PointTarget, typename FeatureT> void
148 {
149  // Some sanity checks first
150  if (!input_features_)
151  {
152  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
153  PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
154  return;
155  }
156  if (!target_features_)
157  {
158  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
159  PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
160  return;
161  }
162 
163  if (input_->size () != input_features_->size ())
164  {
165  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
166  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",
167  input_->size (), input_features_->size ());
168  return;
169  }
170 
171  if (target_->size () != target_features_->size ())
172  {
173  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
174  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",
175  target_->size (), target_features_->size ());
176  return;
177  }
178 
179  if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
180  {
181  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
182  PCL_ERROR ("Illegal inlier fraction %f, must be in [0,1]!\n",
183  inlier_fraction_);
184  return;
185  }
186 
187  const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
188  if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
189  {
190  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
191  PCL_ERROR ("Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
192  similarity_threshold);
193  return;
194  }
195 
196  if (k_correspondences_ <= 0)
197  {
198  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
199  PCL_ERROR ("Illegal correspondence randomness %d, must be > 0!\n",
200  k_correspondences_);
201  return;
202  }
203 
204  // Initialize prerejector (similarity threshold already set to default value in constructor)
205  correspondence_rejector_poly_->setInputSource (input_);
206  correspondence_rejector_poly_->setInputTarget (target_);
207  correspondence_rejector_poly_->setCardinality (nr_samples_);
208  int num_rejections = 0; // For debugging
209 
210  // Initialize results
211  final_transformation_ = guess;
212  inliers_.clear ();
213  float lowest_error = std::numeric_limits<float>::max ();
214  converged_ = false;
215 
216  // Temporaries
217  std::vector<int> inliers;
218  float inlier_fraction;
219  float error;
220 
221  // If guess is not the Identity matrix we check it
222  if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
223  {
224  getFitness (inliers, error);
225  inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
226 
227  if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
228  {
229  inliers_ = inliers;
230  lowest_error = error;
231  converged_ = true;
232  }
233  }
234 
235  // Feature correspondence cache
236  std::vector<std::vector<int> > similar_features (input_->size ());
237 
238  // Start
239  for (int i = 0; i < max_iterations_; ++i)
240  {
241  // Temporary containers
242  std::vector<int> sample_indices;
243  std::vector<int> corresponding_indices;
244 
245  // Draw nr_samples_ random samples
246  selectSamples (*input_, nr_samples_, sample_indices);
247 
248  // Find corresponding features in the target cloud
249  findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
250 
251  // Apply prerejection
252  if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
253  {
254  ++num_rejections;
255  continue;
256  }
257 
258  // Estimate the transform from the correspondences, write to transformation_
259  transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
260 
261  // Take a backup of previous result
262  const Matrix4 final_transformation_prev = final_transformation_;
263 
264  // Set final result to current transformation
265  final_transformation_ = transformation_;
266 
267  // Transform the input and compute the error (uses input_ and final_transformation_)
268  getFitness (inliers, error);
269 
270  // Restore previous result
271  final_transformation_ = final_transformation_prev;
272 
273  // If the new fit is better, update results
274  inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
275 
276  // Update result if pose hypothesis is better
277  if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
278  {
279  inliers_ = inliers;
280  lowest_error = error;
281  converged_ = true;
282  final_transformation_ = transformation_;
283  }
284  }
285 
286  // Apply the final transformation
287  if (converged_)
288  transformPointCloud (*input_, output, final_transformation_);
289 
290  // Debug output
291  PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
292  getClassName ().c_str (), num_rejections, max_iterations_);
293 }
294 
295 
296 template <typename PointSource, typename PointTarget, typename FeatureT> void
298 {
299  // Initialize variables
300  inliers.clear ();
301  inliers.reserve (input_->size ());
302  fitness_score = 0.0f;
303 
304  // Use squared distance for comparison with NN search results
305  const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
306 
307  // Transform the input dataset using the final transformation
308  PointCloudSource input_transformed;
309  input_transformed.resize (input_->size ());
310  transformPointCloud (*input_, input_transformed, final_transformation_);
311 
312  // For each point in the source dataset
313  for (std::size_t i = 0; i < input_transformed.points.size (); ++i)
314  {
315  // Find its nearest neighbor in the target
316  std::vector<int> nn_indices (1);
317  std::vector<float> nn_dists (1);
318  tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
319 
320  // Check if point is an inlier
321  if (nn_dists[0] < max_range)
322  {
323  // Update inliers
324  inliers.push_back (static_cast<int> (i));
325 
326  // Update fitness score
327  fitness_score += nn_dists[0];
328  }
329  }
330 
331  // Calculate MSE
332  if (!inliers.empty ())
333  fitness_score /= static_cast<float> (inliers.size ());
334  else
335  fitness_score = std::numeric_limits<float>::max ();
336 }
337 
338 } // namespace pcl
339 
340 #endif
341 
void getFitness(std::vector< int > &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
typename Registration< PointSource, PointTarget >::Matrix4 Matrix4
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
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)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:221
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...
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource