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