Point Cloud Library (PCL)  1.13.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  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 
272 template <typename PointSource, typename PointTarget, typename Scalar>
273 void
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());
291  }
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 
314 template <typename PointSource, typename PointTarget, typename Scalar>
315 void
317  const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
318 {
319  pcl::transformPointCloudWithNormals(input, output, transform);
320 }
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.
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:264
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