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