Point Cloud Library (PCL)  1.12.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 
48 template <typename PointSource, typename PointTarget, typename Scalar>
49 void
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 auto* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
64  auto* 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 auto* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
95  auto* 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 
112 template <typename PointSource, typename PointTarget, typename Scalar>
113 void
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  // Check whether we have enough correspondences
205  if (correspondences_->size() < min_number_correspondences_) {
206  PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
207  "Relax your threshold parameters.\n",
208  getClassName().c_str());
209  convergence_criteria_->setConvergenceState(
211  Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
212  converged_ = false;
213  break;
214  }
215 
216  // Estimate the transform
217  transformation_estimation_->estimateRigidTransformation(
218  *input_transformed, *target_, *correspondences_, transformation_);
219 
220  // Transform the data
221  transformCloud(*input_transformed, *input_transformed, transformation_);
222 
223  // Obtain the final transformation
224  final_transformation_ = transformation_ * final_transformation_;
225 
226  ++nr_iterations_;
227 
228  // Update the vizualization of icp convergence
229  if (update_visualizer_ != nullptr) {
230  pcl::Indices source_indices_good, target_indices_good;
231  for (const Correspondence& corr : *correspondences_) {
232  source_indices_good.emplace_back(corr.index_query);
233  target_indices_good.emplace_back(corr.index_match);
234  }
235  update_visualizer_(
236  *input_transformed, source_indices_good, *target_, target_indices_good);
237  }
238 
239  converged_ = static_cast<bool>((*convergence_criteria_));
240  } while (convergence_criteria_->getConvergenceState() ==
242  Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
243 
244  // Transform the input cloud using the final transformation
245  PCL_DEBUG("Transformation "
246  "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%"
247  "5f\t%5f\t%5f\t%5f\n",
248  final_transformation_(0, 0),
249  final_transformation_(0, 1),
250  final_transformation_(0, 2),
251  final_transformation_(0, 3),
252  final_transformation_(1, 0),
253  final_transformation_(1, 1),
254  final_transformation_(1, 2),
255  final_transformation_(1, 3),
256  final_transformation_(2, 0),
257  final_transformation_(2, 1),
258  final_transformation_(2, 2),
259  final_transformation_(2, 3),
260  final_transformation_(3, 0),
261  final_transformation_(3, 1),
262  final_transformation_(3, 2),
263  final_transformation_(3, 3));
264 
265  // Copy all the values
266  output = *input_;
267  // Transform the XYZ + normals
268  transformCloud(*input_, output, final_transformation_);
269 }
270 
271 template <typename PointSource, typename PointTarget, typename Scalar>
272 void
274 {
275  need_source_blob_ = false;
276  need_target_blob_ = false;
277  // Check estimator
278  need_source_blob_ |= correspondence_estimation_->requiresSourceNormals();
279  need_target_blob_ |= correspondence_estimation_->requiresTargetNormals();
280  // Add warnings if necessary
281  if (correspondence_estimation_->requiresSourceNormals() && !source_has_normals_) {
282  PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
283  "but we can't provide them.\n",
284  getClassName().c_str());
285  }
286  if (correspondence_estimation_->requiresTargetNormals() && !target_has_normals_) {
287  PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
288  "but we can't provide them.\n",
289  getClassName().c_str());
290  }
291  // Check rejectors
292  for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
293  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
294  need_source_blob_ |= rej->requiresSourcePoints();
295  need_source_blob_ |= rej->requiresSourceNormals();
296  need_target_blob_ |= rej->requiresTargetPoints();
297  need_target_blob_ |= rej->requiresTargetNormals();
298  if (rej->requiresSourceNormals() && !source_has_normals_) {
299  PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
300  "normals, but we can't provide them.\n",
301  getClassName().c_str(),
302  rej->getClassName().c_str());
303  }
304  if (rej->requiresTargetNormals() && !target_has_normals_) {
305  PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
306  "normals, but we can't provide them.\n",
307  getClassName().c_str(),
308  rej->getClassName().c_str());
309  }
310  }
311 }
312 
313 template <typename PointSource, typename PointTarget, typename Scalar>
314 void
316  const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
317 {
318  pcl::transformPointCloudWithNormals(input, output, transform);
319 }
320 
321 } // namespace pcl
322 
323 #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:273
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
void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform) override
Apply a rigid transform to a given dataset.
Definition: icp.hpp:315
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:240
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