39 #ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
40 #define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
42 #include <pcl/registration/boost.h>
43 #include <pcl/correspondence.h>
44 #include <pcl/console/print.h>
50 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
55 if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ())
57 PCL_ERROR (
"[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n",
58 getClassName ().c_str ());
61 bool manual_correspondence_estimations_set =
true;
62 if (correspondence_estimations_.empty ())
64 manual_correspondence_estimations_set =
false;
65 correspondence_estimations_.resize (sources_.size ());
66 for (std::size_t i = 0; i < sources_.size (); i++)
68 correspondence_estimations_[i] = correspondence_estimation_->clone ();
71 correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree);
72 correspondence_estimations_[i]->setSearchMethodSource (src_tree);
75 if (correspondence_estimations_.size () != sources_.size ())
77 PCL_ERROR (
"[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n",
78 getClassName ().c_str ());
81 std::vector<PointCloudSourcePtr> inputs_transformed (sources_.size ());
82 for (std::size_t i = 0; i < sources_.size (); i++)
91 final_transformation_ = guess;
94 std::vector<std::size_t> input_offsets (sources_.size ());
95 std::vector<std::size_t> target_offsets (targets_.size ());
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++)
104 if (guess != Matrix4::Identity ())
107 this->transformCloud (*sources_[i], *inputs_transformed[i], guess);
111 *inputs_transformed[i] = *sources_[i];
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 ();
122 transformation_ = Matrix4::Identity ();
124 determineRequiredBlobData ();
126 for (std::size_t i = 0; i < sources_.size (); i++)
128 correspondence_estimations_[i]->setInputTarget (targets_[i]);
129 if (correspondence_estimations_[i]->requiresTargetNormals ())
133 correspondence_estimations_[i]->setTargetNormals (target_blob);
138 if (!correspondence_rejectors_.empty () && need_target_blob_)
141 for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
144 if (rej->requiresTargetPoints ())
145 rej->setTargetPoints (targets_combined_blob);
146 if (rej->requiresTargetNormals () && target_has_normals_)
147 rej->setTargetNormals (targets_combined_blob);
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_);
156 std::vector<CorrespondencesPtr> partial_correspondences_ (sources_.size ());
157 for (std::size_t i = 0; i < sources_.size (); i++)
165 previous_transformation_ = transformation_;
168 correspondences_->clear ();
169 for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
171 correspondence_estimations_[i]->setInputSource (inputs_transformed[i]);
173 if (correspondence_estimations_[i]->requiresSourceNormals ())
177 correspondence_estimations_[i]->setSourceNormals (input_transformed_blob);
181 if (use_reciprocal_correspondence_)
183 correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
187 correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
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++)
198 correspondences_->push_back (corr);
201 PCL_DEBUG (
"[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ());
204 if (need_source_blob_)
207 toPCLPointCloud2 (*inputs_transformed_combined, *inputs_transformed_combined_blob);
210 for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
212 PCL_DEBUG (
"Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ());
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_);
222 if (i < correspondence_rejectors_.size () - 1)
223 *temp_correspondences = *correspondences_;
226 int cnt = correspondences_->size ();
228 if (cnt < min_number_correspondences_)
230 PCL_ERROR (
"[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
237 transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_);
240 this->transformCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_);
242 for (std::size_t i = 0; i < sources_.size (); i++)
244 this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_);
248 final_transformation_ = transformation_ * final_transformation_;
256 converged_ =
static_cast<bool> ((*convergence_criteria_));
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));
271 if (!manual_correspondence_estimations_set)
273 correspondence_estimations_.clear ();
283 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
286 need_source_blob_ =
false;
287 need_target_blob_ =
false;
289 for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
293 need_source_blob_ |= ce->requiresSourceNormals ();
294 need_target_blob_ |= ce->requiresTargetNormals ();
296 if (ce->requiresSourceNormals () && !source_has_normals_)
298 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
300 if (ce->requiresTargetNormals () && !target_has_normals_)
302 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
306 for (std::size_t i = 0; i < correspondence_rejectors_.size (); 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_)
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 ());
317 if (rej->requiresTargetNormals () && !target_has_normals_)
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 ());