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
computeTransformation(PointCloudSource & output,const Matrix4 & guess)49 JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
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();
65       KdTreeReciprocalPtr src_tree(new KdTreeReciprocal);
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(
221           pcl::registration::DefaultConvergenceCriteria<
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)
278   IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource(
279       sources_combined);
280   IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget(
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
295 JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::
determineRequiredBlobData()296     determineRequiredBlobData()
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_ */
343