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