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 
47 namespace pcl
48 {
49 
50 template <typename PointSource, typename PointTarget, typename Scalar> void
52  const PointCloudSource &input,
53  PointCloudSource &output,
54  const Matrix4 &transform)
55 {
56  Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
57  Eigen::Matrix4f tr = transform.template cast<float> ();
58 
59  // XYZ is ALWAYS present due to the templatization, so we only have to check for normals
60  if (source_has_normals_)
61  {
62  Eigen::Vector3f nt, nt_t;
63  Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);
64 
65  for (std::size_t i = 0; i < input.size (); ++i)
66  {
67  const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*> (&input[i]);
68  std::uint8_t* data_out = reinterpret_cast<std::uint8_t*> (&output[i]);
69  memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
70  memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
71  memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
72 
73  if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
74  continue;
75 
76  pt_t = tr * pt;
77 
78  memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
79  memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
80  memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
81 
82  memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float));
83  memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float));
84  memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float));
85 
86  if (!std::isfinite (nt[0]) || !std::isfinite (nt[1]) || !std::isfinite (nt[2]))
87  continue;
88 
89  nt_t = rot * nt;
90 
91  memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float));
92  memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float));
93  memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float));
94  }
95  }
96  else
97  {
98  for (std::size_t i = 0; i < input.size (); ++i)
99  {
100  const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*> (&input[i]);
101  std::uint8_t* data_out = reinterpret_cast<std::uint8_t*> (&output[i]);
102  memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
103  memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
104  memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
105 
106  if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
107  continue;
108 
109  pt_t = tr * pt;
110 
111  memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
112  memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
113  memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
114  }
115  }
116 }
117 
118 
119 template <typename PointSource, typename PointTarget, typename Scalar> void
121  PointCloudSource &output, const Matrix4 &guess)
122 {
123  // Point cloud containing the correspondences of each point in <input, indices>
124  PointCloudSourcePtr input_transformed (new PointCloudSource);
125 
126  nr_iterations_ = 0;
127  converged_ = false;
128 
129  // Initialise final transformation to the guessed one
130  final_transformation_ = guess;
131 
132  // If the guessed transformation is non identity
133  if (guess != Matrix4::Identity ())
134  {
135  input_transformed->resize (input_->size ());
136  // Apply guessed transformation prior to search for neighbours
137  transformCloud (*input_, *input_transformed, guess);
138  }
139  else
140  *input_transformed = *input_;
141 
142  transformation_ = Matrix4::Identity ();
143 
144  // Make blobs if necessary
145  determineRequiredBlobData ();
146  PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
147  if (need_target_blob_)
148  pcl::toPCLPointCloud2 (*target_, *target_blob);
149 
150  // Pass in the default target for the Correspondence Estimation/Rejection code
151  correspondence_estimation_->setInputTarget (target_);
152  if (correspondence_estimation_->requiresTargetNormals ())
153  correspondence_estimation_->setTargetNormals (target_blob);
154  // Correspondence Rejectors need a binary blob
155  for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
156  {
157  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
158  if (rej->requiresTargetPoints ())
159  rej->setTargetPoints (target_blob);
160  if (rej->requiresTargetNormals () && target_has_normals_)
161  rej->setTargetNormals (target_blob);
162  }
163 
164  convergence_criteria_->setMaximumIterations (max_iterations_);
165  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
166  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
167  if (transformation_rotation_epsilon_ > 0)
168  convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
169  else
170  convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
171 
172  // Repeat until convergence
173  do
174  {
175  // Get blob data if needed
176  PCLPointCloud2::Ptr input_transformed_blob;
177  if (need_source_blob_)
178  {
179  input_transformed_blob.reset (new PCLPointCloud2);
180  toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
181  }
182  // Save the previously estimated transformation
183  previous_transformation_ = transformation_;
184 
185  // Set the source each iteration, to ensure the dirty flag is updated
186  correspondence_estimation_->setInputSource (input_transformed);
187  if (correspondence_estimation_->requiresSourceNormals ())
188  correspondence_estimation_->setSourceNormals (input_transformed_blob);
189  // Estimate correspondences
190  if (use_reciprocal_correspondence_)
191  correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
192  else
193  correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
194 
195  //if (correspondence_rejectors_.empty ())
196  CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
197  for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
198  {
199  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
200  PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
201  if (rej->requiresSourcePoints ())
202  rej->setSourcePoints (input_transformed_blob);
203  if (rej->requiresSourceNormals () && source_has_normals_)
204  rej->setSourceNormals (input_transformed_blob);
205  rej->setInputCorrespondences (temp_correspondences);
206  rej->getCorrespondences (*correspondences_);
207  // Modify input for the next iteration
208  if (i < correspondence_rejectors_.size () - 1)
209  *temp_correspondences = *correspondences_;
210  }
211 
212  std::size_t cnt = correspondences_->size ();
213  // Check whether we have enough correspondences
214  if (static_cast<int> (cnt) < min_number_correspondences_)
215  {
216  PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
218  converged_ = false;
219  break;
220  }
221 
222  // Estimate the transform
223  transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
224 
225  // Transform the data
226  transformCloud (*input_transformed, *input_transformed, transformation_);
227 
228  // Obtain the final transformation
229  final_transformation_ = transformation_ * final_transformation_;
230 
231  ++nr_iterations_;
232 
233  // Update the vizualization of icp convergence
234  //if (update_visualizer_ != 0)
235  // update_visualizer_(output, source_indices_good, *target_, target_indices_good );
236 
237  converged_ = static_cast<bool> ((*convergence_criteria_));
238  }
239  while (convergence_criteria_->getConvergenceState() == pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
240 
241  // Transform the input cloud using the final transformation
242  PCL_DEBUG ("Transformation 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%5f\t%5f\t%5f\t%5f\n",
243  final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
244  final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
245  final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
246  final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
247 
248  // Copy all the values
249  output = *input_;
250  // Transform the XYZ + normals
251  transformCloud (*input_, output, final_transformation_);
252 }
253 
254 
255 template <typename PointSource, typename PointTarget, typename Scalar> void
257 {
258  need_source_blob_ = false;
259  need_target_blob_ = false;
260  // Check estimator
261  need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
262  need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
263  // Add warnings if necessary
264  if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
265  {
266  PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
267  }
268  if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
269  {
270  PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
271  }
272  // Check rejectors
273  for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++)
274  {
275  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
276  need_source_blob_ |= rej->requiresSourcePoints ();
277  need_source_blob_ |= rej->requiresSourceNormals ();
278  need_target_blob_ |= rej->requiresTargetPoints ();
279  need_target_blob_ |= rej->requiresTargetNormals ();
280  if (rej->requiresSourceNormals () && !source_has_normals_)
281  {
282  PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
283  }
284  if (rej->requiresTargetNormals () && !target_has_normals_)
285  {
286  PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
287  }
288  }
289 }
290 
291 
292 template <typename PointSource, typename PointTarget, typename Scalar> void
294  const PointCloudSource &input,
295  PointCloudSource &output,
296  const Matrix4 &transform)
297 {
298  pcl::transformPointCloudWithNormals (input, output, transform);
299 }
300 
301 } // namespace pcl
302 
303 #endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */
304 
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:60
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:98
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:293
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:256
pcl::IterativeClosestPoint< PointSource, PointTarget >::PointCloudSource
typename Registration< PointSource, PointTarget, float >::PointCloudSource PointCloudSource
Definition: icp.h:97
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:312
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:51
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:120
pcl::IterativeClosestPointWithNormals::PointCloudSource
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:325
pcl::IterativeClosestPointWithNormals::Matrix4
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:327
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:135