Point Cloud Library (PCL) 1.13.0
icp.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, 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_IMPL_ICP_HPP_
42#define PCL_REGISTRATION_IMPL_ICP_HPP_
43
44#include <pcl/correspondence.h>
45
46namespace pcl {
47
48template <typename PointSource, typename PointTarget, typename Scalar>
49void
51 const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
52{
53 Eigen::Vector4f pt(0.0f, 0.0f, 0.0f, 1.0f), pt_t;
54 Eigen::Matrix4f tr = transform.template cast<float>();
55
56 // XYZ is ALWAYS present due to the templatization, so we only have to check for
57 // normals
58 if (source_has_normals_) {
59 Eigen::Vector3f nt, nt_t;
60 Eigen::Matrix3f rot = tr.block<3, 3>(0, 0);
61
62 for (std::size_t i = 0; i < input.size(); ++i) {
63 const auto* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
64 auto* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
65 memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
66 memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
67 memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));
68
69 if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
70 continue;
71
72 pt_t = tr * pt;
73
74 memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
75 memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
76 memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
77
78 memcpy(&nt[0], data_in + nx_idx_offset_, sizeof(float));
79 memcpy(&nt[1], data_in + ny_idx_offset_, sizeof(float));
80 memcpy(&nt[2], data_in + nz_idx_offset_, sizeof(float));
81
82 if (!std::isfinite(nt[0]) || !std::isfinite(nt[1]) || !std::isfinite(nt[2]))
83 continue;
84
85 nt_t = rot * nt;
86
87 memcpy(data_out + nx_idx_offset_, &nt_t[0], sizeof(float));
88 memcpy(data_out + ny_idx_offset_, &nt_t[1], sizeof(float));
89 memcpy(data_out + nz_idx_offset_, &nt_t[2], sizeof(float));
90 }
91 }
92 else {
93 for (std::size_t i = 0; i < input.size(); ++i) {
94 const auto* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
95 auto* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
96 memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
97 memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
98 memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));
99
100 if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
101 continue;
102
103 pt_t = tr * pt;
104
105 memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
106 memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
107 memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
108 }
109 }
110}
111
112template <typename PointSource, typename PointTarget, typename Scalar>
113void
115 PointCloudSource& output, const Matrix4& guess)
116{
117 // Point cloud containing the correspondences of each point in <input, indices>
118 PointCloudSourcePtr input_transformed(new PointCloudSource);
119
120 nr_iterations_ = 0;
121 converged_ = false;
122
123 // Initialise final transformation to the guessed one
124 final_transformation_ = guess;
125
126 // If the guessed transformation is non identity
127 if (guess != Matrix4::Identity()) {
128 input_transformed->resize(input_->size());
129 // Apply guessed transformation prior to search for neighbours
130 transformCloud(*input_, *input_transformed, guess);
131 }
132 else
133 *input_transformed = *input_;
134
135 transformation_ = Matrix4::Identity();
136
137 // Make blobs if necessary
138 determineRequiredBlobData();
139 PCLPointCloud2::Ptr target_blob(new PCLPointCloud2);
140 if (need_target_blob_)
141 pcl::toPCLPointCloud2(*target_, *target_blob);
142
143 // Pass in the default target for the Correspondence Estimation/Rejection code
144 correspondence_estimation_->setInputTarget(target_);
145 if (correspondence_estimation_->requiresTargetNormals())
146 correspondence_estimation_->setTargetNormals(target_blob);
147 // Correspondence Rejectors need a binary blob
148 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
149 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
150 if (rej->requiresTargetPoints())
151 rej->setTargetPoints(target_blob);
152 if (rej->requiresTargetNormals() && target_has_normals_)
153 rej->setTargetNormals(target_blob);
154 }
155
156 convergence_criteria_->setMaximumIterations(max_iterations_);
157 convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);
158 convergence_criteria_->setTranslationThreshold(transformation_epsilon_);
159 if (transformation_rotation_epsilon_ > 0)
160 convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
161 else
162 convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
163
164 // Repeat until convergence
165 do {
166 // Get blob data if needed
167 PCLPointCloud2::Ptr input_transformed_blob;
168 if (need_source_blob_) {
169 input_transformed_blob.reset(new PCLPointCloud2);
170 toPCLPointCloud2(*input_transformed, *input_transformed_blob);
171 }
172 // Save the previously estimated transformation
173 previous_transformation_ = transformation_;
174
175 // Set the source each iteration, to ensure the dirty flag is updated
176 correspondence_estimation_->setInputSource(input_transformed);
177 if (correspondence_estimation_->requiresSourceNormals())
178 correspondence_estimation_->setSourceNormals(input_transformed_blob);
179 // Estimate correspondences
180 if (use_reciprocal_correspondence_)
181 correspondence_estimation_->determineReciprocalCorrespondences(
182 *correspondences_, corr_dist_threshold_);
183 else
184 correspondence_estimation_->determineCorrespondences(*correspondences_,
185 corr_dist_threshold_);
186
187 // if (correspondence_rejectors_.empty ())
188 CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
189 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
190 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
191 PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
192 rej->getClassName().c_str());
193 if (rej->requiresSourcePoints())
194 rej->setSourcePoints(input_transformed_blob);
195 if (rej->requiresSourceNormals() && source_has_normals_)
196 rej->setSourceNormals(input_transformed_blob);
197 rej->setInputCorrespondences(temp_correspondences);
198 rej->getCorrespondences(*correspondences_);
199 // Modify input for the next iteration
200 if (i < correspondence_rejectors_.size() - 1)
201 *temp_correspondences = *correspondences_;
202 }
203
204 // Check whether we have enough correspondences
205 if (correspondences_->size() < min_number_correspondences_) {
206 PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
207 "Relax your threshold parameters.\n",
208 getClassName().c_str());
209 convergence_criteria_->setConvergenceState(
211 Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
212 converged_ = false;
213 break;
214 }
215
216 // Estimate the transform
217 transformation_estimation_->estimateRigidTransformation(
218 *input_transformed, *target_, *correspondences_, transformation_);
219
220 // Transform the data
221 transformCloud(*input_transformed, *input_transformed, transformation_);
222
223 // Obtain the final transformation
224 final_transformation_ = transformation_ * final_transformation_;
225
226 ++nr_iterations_;
227
228 // Update the vizualization of icp convergence
229 if (update_visualizer_ != nullptr) {
230 pcl::Indices source_indices_good, target_indices_good;
231 for (const Correspondence& corr : *correspondences_) {
232 source_indices_good.emplace_back(corr.index_query);
233 target_indices_good.emplace_back(corr.index_match);
234 }
235 update_visualizer_(
236 *input_transformed, source_indices_good, *target_, target_indices_good);
237 }
238
239 converged_ = static_cast<bool>((*convergence_criteria_));
240 } while (convergence_criteria_->getConvergenceState() ==
242 Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
243
244 // Transform the input cloud using the final transformation
245 PCL_DEBUG("Transformation "
246 "is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%"
247 "5f\t%5f\t%5f\t%5f\n",
248 final_transformation_(0, 0),
249 final_transformation_(0, 1),
250 final_transformation_(0, 2),
251 final_transformation_(0, 3),
252 final_transformation_(1, 0),
253 final_transformation_(1, 1),
254 final_transformation_(1, 2),
255 final_transformation_(1, 3),
256 final_transformation_(2, 0),
257 final_transformation_(2, 1),
258 final_transformation_(2, 2),
259 final_transformation_(2, 3),
260 final_transformation_(3, 0),
261 final_transformation_(3, 1),
262 final_transformation_(3, 2),
263 final_transformation_(3, 3));
264
265 // Copy all the values
266 output = *input_;
267 // Transform the XYZ + normals
268 transformCloud(*input_, output, final_transformation_);
269}
270
271template <typename PointSource, typename PointTarget, typename Scalar>
272void
274{
275 need_source_blob_ = false;
276 need_target_blob_ = false;
277 // Check estimator
278 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals();
279 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals();
280 // Add warnings if necessary
281 if (correspondence_estimation_->requiresSourceNormals() && !source_has_normals_) {
282 PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
283 "but we can't provide them.\n",
284 getClassName().c_str());
285 }
286 if (correspondence_estimation_->requiresTargetNormals() && !target_has_normals_) {
287 PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
288 "but we can't provide them.\n",
289 getClassName().c_str());
290 }
291 // Check rejectors
292 for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
293 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
294 need_source_blob_ |= rej->requiresSourcePoints();
295 need_source_blob_ |= rej->requiresSourceNormals();
296 need_target_blob_ |= rej->requiresTargetPoints();
297 need_target_blob_ |= rej->requiresTargetNormals();
298 if (rej->requiresSourceNormals() && !source_has_normals_) {
299 PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
300 "normals, but we can't provide them.\n",
301 getClassName().c_str(),
302 rej->getClassName().c_str());
303 }
304 if (rej->requiresTargetNormals() && !target_has_normals_) {
305 PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
306 "normals, but we can't provide them.\n",
307 getClassName().c_str(),
308 rej->getClassName().c_str());
309 }
310 }
311}
312
313template <typename PointSource, typename PointTarget, typename Scalar>
314void
316 const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
317{
318 pcl::transformPointCloudWithNormals(input, output, transform);
319}
320
321} // namespace pcl
322
323#endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:142
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:100
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:50
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: icp.hpp:273
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: icp.hpp:114
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:101
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:343
void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform) override
Apply a rigid transform to a given dataset.
Definition: icp.hpp:315
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:348
shared_ptr< CorrespondenceRejector > Ptr
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Transform a point cloud and rotate its normals using an Eigen transform.
Definition: transforms.hpp:349
shared_ptr< Correspondences > CorrespondencesPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:240
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Correspondence represents a match between two entities (e.g., points, descriptors,...
shared_ptr< ::pcl::PCLPointCloud2 > Ptr