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