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