Point Cloud Library (PCL)  1.7.0
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  int num_rejections = 0; // For debugging
188 
189  // Initialize results
190  final_transformation_ = guess;
191  inliers_.clear ();
192  float highest_inlier_fraction = inlier_fraction_;
193  converged_ = false;
194 
195  // Temporaries
196  std::vector<int> inliers;
197  float inlier_fraction;
198  float error;
199 
200  // If guess is not the Identity matrix we check it
201  if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
202  {
203  getFitness (inliers, error);
204  inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
205 
206  if (inlier_fraction > highest_inlier_fraction)
207  {
208  inliers_ = inliers;
209  highest_inlier_fraction = inlier_fraction;
210  converged_ = true;
211  }
212  }
213 
214  // Start
215  for (int i = 0; i < max_iterations_; ++i)
216  {
217  // Temporary containers
218  std::vector<int> sample_indices (nr_samples_);
219  std::vector<int> corresponding_indices (nr_samples_);
220 
221  // Draw nr_samples_ random samples
222  selectSamples (*input_, nr_samples_, sample_indices);
223 
224  // Find corresponding features in the target cloud
225  findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
226 
227  // Apply prerejection
228  if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)){
229  ++num_rejections;
230  continue;
231  }
232 
233  // Estimate the transform from the correspondences, write to transformation_
234  transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
235 
236  // Take a backup of previous result
237  const Matrix4 final_transformation_prev = final_transformation_;
238 
239  // Set final result to current transformation
240  final_transformation_ = transformation_;
241 
242  // Transform the input and compute the error (uses input_ and final_transformation_)
243  getFitness (inliers, error);
244 
245  // If the new fit is better, update results
246  const float inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
247  if (inlier_fraction > highest_inlier_fraction)
248  {
249  inliers_ = inliers;
250  highest_inlier_fraction = inlier_fraction;
251  converged_ = true;
252  }
253  else
254  {
255  // Restore previous result
256  final_transformation_ = final_transformation_prev;
257  }
258  }
259 
260  // Apply the final transformation
261  if (converged_)
262  transformPointCloud (*input_, output, final_transformation_);
263 
264  // Debug output
265  PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
266  getClassName ().c_str (), num_rejections, max_iterations_);
267 }
268 
269 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
270 template <typename PointSource, typename PointTarget, typename FeatureT> void
272 {
273  // Initialize variables
274  inliers.clear ();
275  inliers.reserve (input_->size ());
276  fitness_score = 0.0f;
277 
278  // Use squared distance for comparison with NN search results
279  const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
280 
281  // Transform the input dataset using the final transformation
282  PointCloudSource input_transformed;
283  input_transformed.resize (input_->size ());
284  transformPointCloud (*input_, input_transformed, final_transformation_);
285 
286  // For each point in the source dataset
287  for (size_t i = 0; i < input_transformed.points.size (); ++i)
288  {
289  // Find its nearest neighbor in the target
290  std::vector<int> nn_indices (1);
291  std::vector<float> nn_dists (1);
292  tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
293 
294  // Check if point is an inlier
295  if (nn_dists[0] < max_range)
296  {
297  // Errors
298  const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x;
299  const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y;
300  const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z;
301 
302  // Update inliers
303  inliers.push_back (static_cast<int> (i));
304 
305  // Update fitness score
306  fitness_score += dx*dx + dy*dy + dz*dz;
307  }
308  }
309 
310  // Calculate MSE
311  if (inliers.size () > 0)
312  fitness_score /= static_cast<float> (inliers.size ());
313  else
314  fitness_score = std::numeric_limits<float>::max ();
315 }
316 
317 #endif
318 
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&#39;s feature descriptors.
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 findSimilarFeatures(const FeatureCloud &input_features, const std::vector< int > &sample_indices, 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&#39;s feature descriptors.