Point Cloud Library (PCL) 1.15.0
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
163 // Repeat until convergence
164 do {
165 // Get blob data if needed
166 PCLPointCloud2::Ptr input_transformed_blob;
167 if (need_source_blob_) {
168 input_transformed_blob.reset(new PCLPointCloud2);
169 toPCLPointCloud2(*input_transformed, *input_transformed_blob);
170 }
171 // Save the previously estimated transformation
172 previous_transformation_ = transformation_;
173
174 // Set the source each iteration, to ensure the dirty flag is updated
175 correspondence_estimation_->setInputSource(input_transformed);
176 if (correspondence_estimation_->requiresSourceNormals())
177 correspondence_estimation_->setSourceNormals(input_transformed_blob);
178 // Estimate correspondences
179 if (use_reciprocal_correspondence_)
180 correspondence_estimation_->determineReciprocalCorrespondences(
181 *correspondences_, corr_dist_threshold_);
182 else
183 correspondence_estimation_->determineCorrespondences(*correspondences_,
184 corr_dist_threshold_);
185
186 // if (correspondence_rejectors_.empty ())
187 CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
188 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
189 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
190 PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
191 rej->getClassName().c_str());
192 if (rej->requiresSourcePoints())
193 rej->setSourcePoints(input_transformed_blob);
194 if (rej->requiresSourceNormals() && source_has_normals_)
195 rej->setSourceNormals(input_transformed_blob);
196 rej->setInputCorrespondences(temp_correspondences);
197 rej->getCorrespondences(*correspondences_);
198 // Modify input for the next iteration
199 if (i < correspondence_rejectors_.size() - 1)
200 *temp_correspondences = *correspondences_;
201 }
202
203 // Check whether we have enough correspondences
204 if (correspondences_->size() < min_number_correspondences_) {
205 PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
206 "Relax your threshold parameters.\n",
207 getClassName().c_str());
208 convergence_criteria_->setConvergenceState(
210 Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
211 converged_ = false;
212 break;
213 }
214
215 // Estimate the transform
216 transformation_estimation_->estimateRigidTransformation(
217 *input_transformed, *target_, *correspondences_, transformation_);
218
219 // Transform the data
220 transformCloud(*input_transformed, *input_transformed, transformation_);
221
222 // Obtain the final transformation
223 final_transformation_ = transformation_ * final_transformation_;
224
225 ++nr_iterations_;
226
227 // Update the visualization of icp convergence
228 if (update_visualizer_ != nullptr) {
229 pcl::Indices source_indices_good, target_indices_good;
230 for (const Correspondence& corr : *correspondences_) {
231 source_indices_good.emplace_back(corr.index_query);
232 target_indices_good.emplace_back(corr.index_match);
233 }
234 update_visualizer_(
235 *input_transformed, source_indices_good, *target_, target_indices_good);
236 }
237
238 converged_ = static_cast<bool>((*convergence_criteria_));
239 } while (convergence_criteria_->getConvergenceState() ==
241 Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
242
243 // Transform the input cloud using the final transformation
244 PCL_DEBUG("Transformation "
245 "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%"
246 "5f\t%5f\t%5f\t%5f\n",
247 final_transformation_(0, 0),
248 final_transformation_(0, 1),
249 final_transformation_(0, 2),
250 final_transformation_(0, 3),
251 final_transformation_(1, 0),
252 final_transformation_(1, 1),
253 final_transformation_(1, 2),
254 final_transformation_(1, 3),
255 final_transformation_(2, 0),
256 final_transformation_(2, 1),
257 final_transformation_(2, 2),
258 final_transformation_(2, 3),
259 final_transformation_(3, 0),
260 final_transformation_(3, 1),
261 final_transformation_(3, 2),
262 final_transformation_(3, 3));
263
264 // Copy all the values
265 output = *input_;
266 // Transform the XYZ + normals
267 transformCloud(*input_, output, final_transformation_);
268}
269
270template <typename PointSource, typename PointTarget, typename Scalar>
271void
273{
274 need_source_blob_ = false;
275 need_target_blob_ = false;
276 // Check estimator
277 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals();
278 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals();
279 // Add warnings if necessary
280 if (correspondence_estimation_->requiresSourceNormals() && !source_has_normals_) {
281 PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
282 "but we can't provide them.\n",
283 getClassName().c_str());
284 }
285 if (correspondence_estimation_->requiresTargetNormals() && !target_has_normals_) {
286 PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
287 "but we can't provide them.\n",
288 getClassName().c_str());
289 }
290 // Check rejectors
291 for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
292 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
293 need_source_blob_ |= rej->requiresSourcePoints();
294 need_source_blob_ |= rej->requiresSourceNormals();
295 need_target_blob_ |= rej->requiresTargetPoints();
296 need_target_blob_ |= rej->requiresTargetNormals();
297 if (rej->requiresSourceNormals() && !source_has_normals_) {
298 PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
299 "normals, but we can't provide them.\n",
300 getClassName().c_str(),
301 rej->getClassName().c_str());
302 }
303 if (rej->requiresTargetNormals() && !target_has_normals_) {
304 PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
305 "normals, but we can't provide them.\n",
306 getClassName().c_str(),
307 rej->getClassName().c_str());
308 }
309 }
310}
311
312template <typename PointSource, typename PointTarget, typename Scalar>
313void
319// NOLINTEND(readability-container-data-pointer)
320
321} // namespace pcl
322
323#endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition icp.h:143
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition icp.h:101
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:272
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:102
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition icp.h:345
void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform) override
Apply a rigid transform to a given dataset.
Definition icp.hpp:314
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition icp.h:350
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, bool padding)
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