Point Cloud Library (PCL)  1.7.1
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/registration/boost.h>
45 #include <pcl/correspondence.h>
46 
47 ///////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointSource, typename PointTarget, typename Scalar> void
50  const PointCloudSource &input,
51  PointCloudSource &output,
52  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 normals
58  if (source_has_normals_)
59  {
60  Eigen::Vector3f nt, nt_t;
61  Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);
62 
63  for (size_t i = 0; i < input.size (); ++i)
64  {
65  const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
66  uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
67  memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
68  memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
69  memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
70 
71  if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
72  continue;
73 
74  pt_t = tr * pt;
75 
76  memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
77  memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
78  memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
79 
80  memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float));
81  memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float));
82  memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float));
83 
84  if (!pcl_isfinite (nt[0]) || !pcl_isfinite (nt[1]) || !pcl_isfinite (nt[2]))
85  continue;
86 
87  nt_t = rot * nt;
88 
89  memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float));
90  memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float));
91  memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float));
92  }
93  }
94  else
95  {
96  for (size_t i = 0; i < input.size (); ++i)
97  {
98  const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
99  uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
100  memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
101  memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
102  memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
103 
104  if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
105  continue;
106 
107  pt_t = tr * pt;
108 
109  memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
110  memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
111  memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
112  }
113  }
114 
115 }
116 
117 ///////////////////////////////////////////////////////////////////////////////////////////
118 template <typename PointSource, typename PointTarget, typename Scalar> void
120  PointCloudSource &output, const Matrix4 &guess)
121 {
122  // Point cloud containing the correspondences of each point in <input, indices>
123  PointCloudSourcePtr input_transformed (new PointCloudSource);
124 
125  nr_iterations_ = 0;
126  converged_ = false;
127 
128  // Initialise final transformation to the guessed one
129  final_transformation_ = guess;
130 
131  // If the guessed transformation is non identity
132  if (guess != Matrix4::Identity ())
133  {
134  input_transformed->resize (input_->size ());
135  // Apply guessed transformation prior to search for neighbours
136  transformCloud (*input_, *input_transformed, guess);
137  }
138  else
139  *input_transformed = *input_;
140 
141  transformation_ = Matrix4::Identity ();
142 
143  // Pass in the default target for the Correspondence Estimation/Rejection code
144  correspondence_estimation_->setInputTarget (target_);
145  // We should be doing something like this
146  // for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
147  // {
148  // correspondence_rejectors_[i]->setTargetCloud (target_);
149  // if (target_has_normals_)
150  // correspondence_rejectors_[i]->setTargetNormals (target_);
151  // }
152 
153  convergence_criteria_->setMaximumIterations (max_iterations_);
154  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
155  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
156  convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
157 
158  // Repeat until convergence
159  do
160  {
161  // Save the previously estimated transformation
162  previous_transformation_ = transformation_;
163 
164  // Set the source each iteration, to ensure the dirty flag is updated
165  correspondence_estimation_->setInputSource (input_transformed);
166  // Estimate correspondences
167  if (use_reciprocal_correspondence_)
168  correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
169  else
170  correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
171 
172  //if (correspondence_rejectors_.empty ())
173  CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
174  for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
175  {
176  PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ());
177  // We should be doing something like this
178  // correspondence_rejectors_[i]->setInputSource (input_transformed);
179  // if (source_has_normals_)
180  // correspondence_rejectors_[i]->setInputNormals (input_transformed);
181  correspondence_rejectors_[i]->setInputCorrespondences (temp_correspondences);
182  correspondence_rejectors_[i]->getCorrespondences (*correspondences_);
183  // Modify input for the next iteration
184  if (i < correspondence_rejectors_.size () - 1)
185  *temp_correspondences = *correspondences_;
186  }
187 
188  size_t cnt = correspondences_->size ();
189  // Check whether we have enough correspondences
190  if (cnt < min_number_correspondences_)
191  {
192  PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
194  converged_ = false;
195  break;
196  }
197 
198  // Estimate the transform
199  transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
200 
201  // Tranform the data
202  transformCloud (*input_transformed, *input_transformed, transformation_);
203 
204  // Obtain the final transformation
205  final_transformation_ = transformation_ * final_transformation_;
206 
207  ++nr_iterations_;
208 
209  // Update the vizualization of icp convergence
210  //if (update_visualizer_ != 0)
211  // update_visualizer_(output, source_indices_good, *target_, target_indices_good );
212 
213  converged_ = static_cast<bool> ((*convergence_criteria_));
214  }
215  while (!converged_);
216 
217  // Transform the input cloud using the final transformation
218  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",
219  final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
220  final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
221  final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
222  final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
223 
224  // Copy all the values
225  output = *input_;
226  // Transform the XYZ + normals
227  transformCloud (*input_, output, final_transformation_);
228 }
229 
230 ///////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointSource, typename PointTarget, typename Scalar> void
233  const PointCloudSource &input,
234  PointCloudSource &output,
235  const Matrix4 &transform)
236 {
237  pcl::transformPointCloudWithNormals (input, output, transform);
238 }
239 
240 #endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */