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