Point Cloud Library (PCL) 1.12.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
44namespace pcl {
45
46template <typename PointSource, typename PointTarget, typename FeatureT>
47void
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
60template <typename PointSource, typename PointTarget, typename FeatureT>
61void
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
75template <typename PointSource, typename PointTarget, typename FeatureT>
76void
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 trough 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
118template <typename PointSource, typename PointTarget, typename FeatureT>
119void
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
152template <typename PointSource, typename PointTarget, typename FeatureT>
153void
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
305template <typename PointSource, typename PointTarget, typename FeatureT>
306void
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