Point Cloud Library (PCL)  1.11.1
joint_icp.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, 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  */
38 
39 #ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
40 #define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
41 
42 #include <pcl/registration/boost.h>
43 #include <pcl/correspondence.h>
44 #include <pcl/console/print.h>
45 
46 
47 namespace pcl
48 {
49 
50 template <typename PointSource, typename PointTarget, typename Scalar> void
52  PointCloudSource &output, const Matrix4 &guess)
53 {
54  // Point clouds containing the correspondences of each point in <input, indices>
55  if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ())
56  {
57  PCL_ERROR ("[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n",
58  getClassName ().c_str ());
59  return;
60  }
61  bool manual_correspondence_estimations_set = true;
62  if (correspondence_estimations_.empty ())
63  {
64  manual_correspondence_estimations_set = false;
65  correspondence_estimations_.resize (sources_.size ());
66  for (std::size_t i = 0; i < sources_.size (); i++)
67  {
68  correspondence_estimations_[i] = correspondence_estimation_->clone ();
70  KdTreePtr tgt_tree (new KdTree);
71  correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree);
72  correspondence_estimations_[i]->setSearchMethodSource (src_tree);
73  }
74  }
75  if (correspondence_estimations_.size () != sources_.size ())
76  {
77  PCL_ERROR ("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n",
78  getClassName ().c_str ());
79  return;
80  }
81  std::vector<PointCloudSourcePtr> inputs_transformed (sources_.size ());
82  for (std::size_t i = 0; i < sources_.size (); i++)
83  {
84  inputs_transformed[i].reset (new PointCloudSource);
85  }
86 
87  nr_iterations_ = 0;
88  converged_ = false;
89 
90  // Initialise final transformation to the guessed one
91  final_transformation_ = guess;
92 
93  // Make a combined transformed input and output
94  std::vector<std::size_t> input_offsets (sources_.size ());
95  std::vector<std::size_t> target_offsets (targets_.size ());
96  PointCloudSourcePtr sources_combined (new PointCloudSource);
97  PointCloudSourcePtr inputs_transformed_combined (new PointCloudSource);
98  PointCloudTargetPtr targets_combined (new PointCloudTarget);
99  std::size_t input_offset = 0;
100  std::size_t target_offset = 0;
101  for (std::size_t i = 0; i < sources_.size (); i++)
102  {
103  // If the guessed transformation is non identity
104  if (guess != Matrix4::Identity ())
105  {
106  // Apply guessed transformation prior to search for neighbours
107  this->transformCloud (*sources_[i], *inputs_transformed[i], guess);
108  }
109  else
110  {
111  *inputs_transformed[i] = *sources_[i];
112  }
113  *sources_combined += *sources_[i];
114  *inputs_transformed_combined += *inputs_transformed[i];
115  *targets_combined += *targets_[i];
116  input_offsets[i] = input_offset;
117  target_offsets[i] = target_offset;
118  input_offset += inputs_transformed[i]->size ();
119  target_offset += targets_[i]->size ();
120  }
121 
122  transformation_ = Matrix4::Identity ();
123  // Make blobs if necessary
124  determineRequiredBlobData ();
125  // Pass in the default target for the Correspondence Estimation/Rejection code
126  for (std::size_t i = 0; i < sources_.size (); i++)
127  {
128  correspondence_estimations_[i]->setInputTarget (targets_[i]);
129  if (correspondence_estimations_[i]->requiresTargetNormals ())
130  {
131  PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
132  pcl::toPCLPointCloud2 (*targets_[i], *target_blob);
133  correspondence_estimations_[i]->setTargetNormals (target_blob);
134  }
135  }
136 
137  PCLPointCloud2::Ptr targets_combined_blob (new PCLPointCloud2);
138  if (!correspondence_rejectors_.empty () && need_target_blob_)
139  pcl::toPCLPointCloud2 (*targets_combined, *targets_combined_blob);
140 
141  for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
142  {
143  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
144  if (rej->requiresTargetPoints ())
145  rej->setTargetPoints (targets_combined_blob);
146  if (rej->requiresTargetNormals () && target_has_normals_)
147  rej->setTargetNormals (targets_combined_blob);
148  }
149 
150  convergence_criteria_->setMaximumIterations (max_iterations_);
151  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
152  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
153  convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
154 
155  // Repeat until convergence
156  std::vector<CorrespondencesPtr> partial_correspondences_ (sources_.size ());
157  for (std::size_t i = 0; i < sources_.size (); i++)
158  {
159  partial_correspondences_[i].reset (new pcl::Correspondences);
160  }
161 
162  do
163  {
164  // Save the previously estimated transformation
165  previous_transformation_ = transformation_;
166 
167  // Set the source each iteration, to ensure the dirty flag is updated
168  correspondences_->clear ();
169  for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
170  {
171  correspondence_estimations_[i]->setInputSource (inputs_transformed[i]);
172  // Get blob data if needed
173  if (correspondence_estimations_[i]->requiresSourceNormals ())
174  {
175  PCLPointCloud2::Ptr input_transformed_blob (new PCLPointCloud2);
176  toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob);
177  correspondence_estimations_[i]->setSourceNormals (input_transformed_blob);
178  }
179 
180  // Estimate correspondences on each cloud pair separately
181  if (use_reciprocal_correspondence_)
182  {
183  correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
184  }
185  else
186  {
187  correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
188  }
189  PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n",
190  getClassName ().c_str (),
191  partial_correspondences_[i]->size (), i);
192  for (std::size_t j = 0; j < partial_correspondences_[i]->size (); j++)
193  {
194  pcl::Correspondence corr = partial_correspondences_[i]->at (j);
195  // Update the offsets to be for the combined clouds
196  corr.index_query += input_offsets[i];
197  corr.index_match += target_offsets[i];
198  correspondences_->push_back (corr);
199  }
200  }
201  PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ());
202 
203  PCLPointCloud2::Ptr inputs_transformed_combined_blob;
204  if (need_source_blob_)
205  {
206  inputs_transformed_combined_blob.reset (new PCLPointCloud2);
207  toPCLPointCloud2 (*inputs_transformed_combined, *inputs_transformed_combined_blob);
208  }
209  CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
210  for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
211  {
212  PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ());
213  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
214  PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
215  if (rej->requiresSourcePoints ())
216  rej->setSourcePoints (inputs_transformed_combined_blob);
217  if (rej->requiresSourceNormals () && source_has_normals_)
218  rej->setSourceNormals (inputs_transformed_combined_blob);
219  rej->setInputCorrespondences (temp_correspondences);
220  rej->getCorrespondences (*correspondences_);
221  // Modify input for the next iteration
222  if (i < correspondence_rejectors_.size () - 1)
223  *temp_correspondences = *correspondences_;
224  }
225 
226  int cnt = correspondences_->size ();
227  // Check whether we have enough correspondences
228  if (cnt < min_number_correspondences_)
229  {
230  PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
232  converged_ = false;
233  break;
234  }
235 
236  // Estimate the transform jointly, on a combined correspondence set
237  transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_);
238 
239  // Transform the combined data
240  this->transformCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_);
241  // And all its components
242  for (std::size_t i = 0; i < sources_.size (); i++)
243  {
244  this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_);
245  }
246 
247  // Obtain the final transformation
248  final_transformation_ = transformation_ * final_transformation_;
249 
250  ++nr_iterations_;
251 
252  // Update the vizualization of icp convergence
253  //if (update_visualizer_ != 0)
254  // update_visualizer_(output, source_indices_good, *target_, target_indices_good );
255 
256  converged_ = static_cast<bool> ((*convergence_criteria_));
257  }
258  while (!converged_);
259 
260  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",
261  final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
262  final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
263  final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
264  final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
265 
266  // For fitness checks, etc, we'll use an aggregated cloud for now (should be evaluating independently for correctness, but this requires propagating a few virtual methods from Registration)
269 
270  // If we automatically set the correspondence estimators, we should clear them now
271  if (!manual_correspondence_estimations_set)
272  {
273  correspondence_estimations_.clear ();
274  }
275 
276 
277  // By definition, this method will return an empty cloud (for compliance with the ICP API).
278  // We can figure out a better solution, if necessary.
279  output = PointCloudSource ();
280 }
281 
282 
283 template <typename PointSource, typename PointTarget, typename Scalar> void
285 {
286  need_source_blob_ = false;
287  need_target_blob_ = false;
288  // Check estimators
289  for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
290  {
291  CorrespondenceEstimationPtr& ce = correspondence_estimations_[i];
292 
293  need_source_blob_ |= ce->requiresSourceNormals ();
294  need_target_blob_ |= ce->requiresTargetNormals ();
295  // Add warnings if necessary
296  if (ce->requiresSourceNormals () && !source_has_normals_)
297  {
298  PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
299  }
300  if (ce->requiresTargetNormals () && !target_has_normals_)
301  {
302  PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
303  }
304  }
305  // Check rejectors
306  for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++)
307  {
308  registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
309  need_source_blob_ |= rej->requiresSourcePoints ();
310  need_source_blob_ |= rej->requiresSourceNormals ();
311  need_target_blob_ |= rej->requiresTargetPoints ();
312  need_target_blob_ |= rej->requiresTargetNormals ();
313  if (rej->requiresSourceNormals () && !source_has_normals_)
314  {
315  PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
316  }
317  if (rej->requiresTargetNormals () && !target_has_normals_)
318  {
319  PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
320  }
321  }
322 }
323 
324 } // namespace pcl
325 
326 #endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */
327 
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: icp.h:212
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: icp.h:178
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: joint_icp.h:58
void determineRequiredBlobData() override
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: joint_icp.hpp:284
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: joint_icp.h:61
typename KdTree::Ptr KdTreeReciprocalPtr
Definition: joint_icp.h:69
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: joint_icp.hpp:51
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: joint_icp.h:62
typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
Definition: joint_icp.h:79
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: joint_icp.h:57
typename KdTree::Ptr KdTreePtr
Definition: joint_icp.h:66
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: joint_icp.h:115
shared_ptr< CorrespondenceRejector > Ptr
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
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:241
Correspondence represents a match between two entities (e.g., points, descriptors,...
index_t index_query
Index of the query (source) point.
index_t index_match
Index of the matching (target) point.
shared_ptr< ::pcl::PCLPointCloud2 > Ptr