Point Cloud Library (PCL)  1.14.1-dev
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 
46 namespace pcl {
47 // NOLINTBEGIN(readability-container-data-pointer)
48 
49 template <typename PointSource, typename PointTarget, typename Scalar>
50 void
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 
113 template <typename PointSource, typename PointTarget, typename Scalar>
114 void
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 
270 template <typename PointSource, typename PointTarget, typename Scalar>
271 void
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 
312 template <typename PointSource, typename PointTarget, typename Scalar>
313 void
315  const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
316 {
317  pcl::transformPointCloudWithNormals(input, output, transform);
318 }
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.
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, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:372
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