Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
ia_ransac.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 IA_RANSAC_HPP_
42#define IA_RANSAC_HPP_
43
45
46namespace pcl {
47
48template <typename PointSource, typename PointTarget, typename FeatureT>
49void
51 const FeatureCloudConstPtr& features)
52{
53 if (features == nullptr || features->empty()) {
54 PCL_ERROR(
55 "[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n",
56 getClassName().c_str());
57 return;
58 }
59 input_features_ = features;
60}
61
62template <typename PointSource, typename PointTarget, typename FeatureT>
63void
65 const FeatureCloudConstPtr& features)
66{
67 if (features == nullptr || features->empty()) {
68 PCL_ERROR(
69 "[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n",
70 getClassName().c_str());
71 return;
72 }
73 target_features_ = features;
74 feature_tree_->setInputCloud(target_features_);
75}
76
77template <typename PointSource, typename PointTarget, typename FeatureT>
78void
80 const PointCloudSource& cloud,
81 unsigned int nr_samples,
82 float min_sample_distance,
83 pcl::Indices& sample_indices)
84{
85 if (nr_samples > cloud.size()) {
86 PCL_ERROR("[pcl::%s::selectSamples] ", getClassName().c_str());
87 PCL_ERROR("The number of samples (%u) must not be greater than the number of "
88 "points (%zu)!\n",
89 nr_samples,
90 static_cast<std::size_t>(cloud.size()));
91 return;
92 }
93
94 // Iteratively draw random samples until nr_samples is reached
95 index_t iterations_without_a_sample = 0;
96 const auto max_iterations_without_a_sample = 3 * cloud.size();
97 sample_indices.clear();
98 while (sample_indices.size() < nr_samples) {
99 // Choose a sample at random
100 const auto sample_index = getRandomIndex(cloud.size());
101
102 // Check to see if the sample is 1) unique and 2) far away from the other samples
103 bool valid_sample = true;
104 for (const auto& sample_idx : sample_indices) {
105 float distance_between_samples =
106 euclideanDistance(cloud[sample_index], cloud[sample_idx]);
107
108 if (sample_index == sample_idx ||
109 distance_between_samples < min_sample_distance) {
110 valid_sample = false;
111 break;
112 }
113 }
114
115 // If the sample is valid, add it to the output
116 if (valid_sample) {
117 sample_indices.push_back(sample_index);
118 iterations_without_a_sample = 0;
119 }
120 else
121 ++iterations_without_a_sample;
122
123 // If no valid samples can be found, relax the inter-sample distance requirements
124 if (static_cast<std::size_t>(iterations_without_a_sample) >=
125 max_iterations_without_a_sample) {
126 PCL_WARN("[pcl::%s::selectSamples] ", getClassName().c_str());
127 PCL_WARN("No valid sample found after %zu iterations. Relaxing "
128 "min_sample_distance_ to %f\n",
129 static_cast<std::size_t>(iterations_without_a_sample),
130 0.5 * min_sample_distance);
131
132 min_sample_distance_ *= 0.5f;
133 min_sample_distance = min_sample_distance_;
134 iterations_without_a_sample = 0;
135 }
136 }
137}
138
139template <typename PointSource, typename PointTarget, typename FeatureT>
140void
142 findSimilarFeatures(const FeatureCloud& input_features,
143 const pcl::Indices& sample_indices,
144 pcl::Indices& corresponding_indices)
145{
146 pcl::Indices nn_indices(k_correspondences_);
147 std::vector<float> nn_distances(k_correspondences_);
148
149 corresponding_indices.resize(sample_indices.size());
150 for (std::size_t i = 0; i < sample_indices.size(); ++i) {
151 // Find the k features nearest to input_features[sample_indices[i]]
152 feature_tree_->nearestKSearch(input_features,
153 sample_indices[i],
154 k_correspondences_,
155 nn_indices,
156 nn_distances);
157
158 // Select one at random and add it to corresponding_indices
159 const auto random_correspondence = getRandomIndex(k_correspondences_);
160 corresponding_indices[i] = nn_indices[random_correspondence];
161 }
162}
163
164template <typename PointSource, typename PointTarget, typename FeatureT>
165float
167 const PointCloudSource& cloud, float)
168{
169 pcl::Indices nn_index(1);
170 std::vector<float> nn_distance(1);
171
172 const ErrorFunctor& compute_error = *error_functor_;
173 float error = 0;
174
175 for (const auto& point : cloud) {
176 // Find the distance between point and its nearest neighbor in the target point
177 // cloud
178 tree_->nearestKSearch(point, 1, nn_index, nn_distance);
179
180 // Compute the error
181 error += compute_error(nn_distance[0]);
182 }
183 return (error);
184}
185
186template <typename PointSource, typename PointTarget, typename FeatureT>
187void
189 computeTransformation(PointCloudSource& output, const Eigen::Matrix4f& guess)
190{
191 // Some sanity checks first
192 if (!input_features_) {
193 PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
194 PCL_ERROR(
195 "No source features were given! Call setSourceFeatures before aligning.\n");
196 return;
197 }
198 if (!target_features_) {
199 PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
200 PCL_ERROR(
201 "No target features were given! Call setTargetFeatures before aligning.\n");
202 return;
203 }
204
205 if (input_->size() != input_features_->size()) {
206 PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
207 PCL_ERROR("The source points and source feature points need to be in a one-to-one "
208 "relationship! Current input cloud sizes: %ld vs %ld.\n",
209 input_->size(),
210 input_features_->size());
211 return;
212 }
213
214 if (target_->size() != target_features_->size()) {
215 PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
216 PCL_ERROR("The target points and target feature points need to be in a one-to-one "
217 "relationship! Current input cloud sizes: %ld vs %ld.\n",
218 target_->size(),
219 target_features_->size());
220 return;
221 }
222
223 if (!error_functor_)
224 error_functor_.reset(new TruncatedError(static_cast<float>(corr_dist_threshold_)));
225
226 pcl::Indices sample_indices(nr_samples_);
227 pcl::Indices corresponding_indices(nr_samples_);
228 PointCloudSource input_transformed;
229 float lowest_error(0);
230
231 final_transformation_ = guess;
232 int i_iter = 0;
233 converged_ = false;
234 if (!guess.isApprox(Eigen::Matrix4f::Identity(), 0.01f)) {
235 // If guess is not the Identity matrix we check it.
236 transformPointCloud(*input_, input_transformed, final_transformation_);
237 lowest_error =
238 computeErrorMetric(input_transformed, static_cast<float>(corr_dist_threshold_));
239 i_iter = 1;
240 }
241
242 for (; i_iter < max_iterations_; ++i_iter) {
243 // Draw nr_samples_ random samples
244 selectSamples(*input_, nr_samples_, min_sample_distance_, sample_indices);
245
246 // Find corresponding features in the target cloud
247 findSimilarFeatures(*input_features_, sample_indices, corresponding_indices);
248
249 // Estimate the transform from the samples to their corresponding points
250 transformation_estimation_->estimateRigidTransformation(
251 *input_, sample_indices, *target_, corresponding_indices, transformation_);
252
253 // Transform the data and compute the error
254 transformPointCloud(*input_, input_transformed, transformation_);
255 float error =
256 computeErrorMetric(input_transformed, static_cast<float>(corr_dist_threshold_));
257
258 // If the new error is lower, update the final transformation
259 if (i_iter == 0 || error < lowest_error) {
260 lowest_error = error;
261 final_transformation_ = transformation_;
262 converged_ = true;
263 }
264 }
265
266 // Apply the final transformation
267 transformPointCloud(*input_, output, final_transformation_);
268}
269
270} // namespace pcl
271
272#endif //#ifndef IA_RANSAC_HPP_
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
float computeErrorMetric(const PointCloudSource &cloud, float threshold)
An error metric for that computes the quality of the alignment between the given cloud and the target...
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition ia_ransac.h:71
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud's feature descriptors.
Definition ia_ransac.hpp:64
void selectSamples(const PointCloudSource &cloud, unsigned int nr_samples, float min_sample_distance, pcl::Indices &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
Definition ia_ransac.hpp:79
void findSimilarFeatures(const FeatureCloud &input_features, const pcl::Indices &sample_indices, pcl::Indices &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition ia_ransac.h:83
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud's feature descriptors.
Definition ia_ransac.hpp:50
Define standard C methods to do distance calculations.
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.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition distances.h:204
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133