Point Cloud Library (PCL)  1.11.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 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 
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  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 
265 template <typename PointSource, typename PointTarget, typename Scalar>
266 void
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 
307 template <typename PointSource, typename PointTarget, typename Scalar>
308 void
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_ */
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:56
pcl
Definition: convolution.h:46
pcl::registration::DefaultConvergenceCriteria
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
Definition: default_convergence_criteria.h:65
pcl::PCLPointCloud2::Ptr
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
Definition: PCLPointCloud2.h:35
pcl::IterativeClosestPoint< PointSource, PointTarget >::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:101
pcl::IterativeClosestPointWithNormals::transformCloud
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:309
pcl::IterativeClosestPoint::determineRequiredBlobData
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: icp.hpp:267
pcl::IterativeClosestPoint< PointSource, PointTarget >::PointCloudSource
typename Registration< PointSource, PointTarget, float >::PointCloudSource PointCloudSource
Definition: icp.h:100
pcl::transformPointCloudWithNormals
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
pcl::IterativeClosestPoint::transformCloud
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:50
pcl::PCLPointCloud2
Definition: PCLPointCloud2.h:16
pcl::IterativeClosestPoint::computeTransformation
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: icp.hpp:114
pcl::IterativeClosestPointWithNormals::PointCloudSource
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:343
pcl::IterativeClosestPointWithNormals::Matrix4
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:348
pcl::toPCLPointCloud2
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
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:89
pcl::CorrespondencesPtr
shared_ptr< Correspondences > CorrespondencesPtr
Definition: correspondence.h:90
pcl::IterativeClosestPoint< PointSource, PointTarget >::Matrix4
typename Registration< PointSource, PointTarget, float >::Matrix4 Matrix4
Definition: icp.h:142