Point Cloud Library (PCL) 1.12.1
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 std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
64 std::uint8_t* 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 std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
95 std::uint8_t* 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 std::size_t cnt = correspondences_->size();
205 // Check whether we have enough correspondences
206 if (static_cast<int>(cnt) < 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 vizualization of icp convergence
230 // if (update_visualizer_ != 0)
231 // update_visualizer_(output, source_indices_good, *target_, target_indices_good );
232
233 converged_ = static_cast<bool>((*convergence_criteria_));
234 } while (convergence_criteria_->getConvergenceState() ==
236 Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
237
238 // Transform the input cloud using the final transformation
239 PCL_DEBUG("Transformation "
240 "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%"
241 "5f\t%5f\t%5f\t%5f\n",
242 final_transformation_(0, 0),
243 final_transformation_(0, 1),
244 final_transformation_(0, 2),
245 final_transformation_(0, 3),
246 final_transformation_(1, 0),
247 final_transformation_(1, 1),
248 final_transformation_(1, 2),
249 final_transformation_(1, 3),
250 final_transformation_(2, 0),
251 final_transformation_(2, 1),
252 final_transformation_(2, 2),
253 final_transformation_(2, 3),
254 final_transformation_(3, 0),
255 final_transformation_(3, 1),
256 final_transformation_(3, 2),
257 final_transformation_(3, 3));
258
259 // Copy all the values
260 output = *input_;
261 // Transform the XYZ + normals
262 transformCloud(*input_, output, final_transformation_);
263}
264
265template <typename PointSource, typename PointTarget, typename Scalar>
266void
268{
269 need_source_blob_ = false;
270 need_target_blob_ = false;
271 // Check estimator
272 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals();
273 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals();
274 // Add warnings if necessary
275 if (correspondence_estimation_->requiresSourceNormals() && !source_has_normals_) {
276 PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
277 "but we can't provide them.\n",
278 getClassName().c_str());
279 }
280 if (correspondence_estimation_->requiresTargetNormals() && !target_has_normals_) {
281 PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
282 "but we can't provide them.\n",
283 getClassName().c_str());
284 }
285 // Check rejectors
286 for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
287 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
288 need_source_blob_ |= rej->requiresSourcePoints();
289 need_source_blob_ |= rej->requiresSourceNormals();
290 need_target_blob_ |= rej->requiresTargetPoints();
291 need_target_blob_ |= rej->requiresTargetNormals();
292 if (rej->requiresSourceNormals() && !source_has_normals_) {
293 PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
294 "normals, but we can't provide them.\n",
295 getClassName().c_str(),
296 rej->getClassName().c_str());
297 }
298 if (rej->requiresTargetNormals() && !target_has_normals_) {
299 PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
300 "normals, but we can't provide them.\n",
301 getClassName().c_str(),
302 rej->getClassName().c_str());
303 }
304 }
305}
306
307template <typename PointSource, typename PointTarget, typename Scalar>
308void
310 const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
311{
312 pcl::transformPointCloudWithNormals(input, output, transform);
313}
314
315} // namespace pcl
316
317#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:267
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
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:309
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:310
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:238
shared_ptr< ::pcl::PCLPointCloud2 > Ptr