39 #ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
40 #define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
42 #include <pcl/console/print.h>
43 #include <pcl/registration/boost.h>
44 #include <pcl/correspondence.h>
48 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
54 if (sources_.size() != targets_.size() || sources_.empty() || targets_.empty()) {
55 PCL_ERROR(
"[pcl::%s::computeTransformation] Must set InputSources and InputTargets "
56 "to the same, nonzero size!\n",
57 getClassName().c_str());
60 bool manual_correspondence_estimations_set =
true;
61 if (correspondence_estimations_.empty()) {
62 manual_correspondence_estimations_set =
false;
63 correspondence_estimations_.resize(sources_.size());
64 for (std::size_t i = 0; i < sources_.size(); i++) {
65 correspondence_estimations_[i] = correspondence_estimation_->clone();
68 correspondence_estimations_[i]->setSearchMethodTarget(tgt_tree);
69 correspondence_estimations_[i]->setSearchMethodSource(src_tree);
72 if (correspondence_estimations_.size() != sources_.size()) {
73 PCL_ERROR(
"[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be "
74 "the same size as the joint\n",
75 getClassName().c_str());
78 std::vector<PointCloudSourcePtr> inputs_transformed(sources_.size());
79 for (std::size_t i = 0; i < sources_.size(); i++) {
87 final_transformation_ = guess;
90 std::vector<std::size_t> input_offsets(sources_.size());
91 std::vector<std::size_t> target_offsets(targets_.size());
95 std::size_t input_offset = 0;
96 std::size_t target_offset = 0;
97 for (std::size_t i = 0; i < sources_.size(); i++) {
99 if (guess != Matrix4::Identity()) {
101 this->transformCloud(*sources_[i], *inputs_transformed[i], guess);
104 *inputs_transformed[i] = *sources_[i];
106 *sources_combined += *sources_[i];
107 *inputs_transformed_combined += *inputs_transformed[i];
108 *targets_combined += *targets_[i];
109 input_offsets[i] = input_offset;
110 target_offsets[i] = target_offset;
111 input_offset += inputs_transformed[i]->size();
112 target_offset += targets_[i]->size();
115 transformation_ = Matrix4::Identity();
117 determineRequiredBlobData();
119 for (std::size_t i = 0; i < sources_.size(); i++) {
120 correspondence_estimations_[i]->setInputTarget(targets_[i]);
121 if (correspondence_estimations_[i]->requiresTargetNormals()) {
124 correspondence_estimations_[i]->setTargetNormals(target_blob);
129 if (!correspondence_rejectors_.empty() && need_target_blob_)
132 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
134 if (rej->requiresTargetPoints())
135 rej->setTargetPoints(targets_combined_blob);
136 if (rej->requiresTargetNormals() && target_has_normals_)
137 rej->setTargetNormals(targets_combined_blob);
140 convergence_criteria_->setMaximumIterations(max_iterations_);
141 convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);
142 convergence_criteria_->setTranslationThreshold(transformation_epsilon_);
143 convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
146 std::vector<CorrespondencesPtr> partial_correspondences_(sources_.size());
147 for (std::size_t i = 0; i < sources_.size(); i++) {
153 previous_transformation_ = transformation_;
156 correspondences_->clear();
157 for (std::size_t i = 0; i < correspondence_estimations_.size(); i++) {
158 correspondence_estimations_[i]->setInputSource(inputs_transformed[i]);
160 if (correspondence_estimations_[i]->requiresSourceNormals()) {
163 correspondence_estimations_[i]->setSourceNormals(input_transformed_blob);
167 if (use_reciprocal_correspondence_) {
168 correspondence_estimations_[i]->determineReciprocalCorrespondences(
169 *partial_correspondences_[i], corr_dist_threshold_);
172 correspondence_estimations_[i]->determineCorrespondences(
173 *partial_correspondences_[i], corr_dist_threshold_);
175 PCL_DEBUG(
"[pcl::%s::computeTransformation] Found %d partial correspondences for "
177 getClassName().c_str(),
178 partial_correspondences_[i]->size(),
180 for (std::size_t j = 0; j < partial_correspondences_[i]->size(); j++) {
185 correspondences_->push_back(corr);
188 PCL_DEBUG(
"[pcl::%s::computeTransformation] Total correspondences: %d\n",
189 getClassName().c_str(),
190 correspondences_->size());
193 if (need_source_blob_) {
195 toPCLPointCloud2(*inputs_transformed_combined, *inputs_transformed_combined_blob);
198 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
199 PCL_DEBUG(
"Applying a correspondence rejector method: %s.\n",
200 correspondence_rejectors_[i]->getClassName().c_str());
202 PCL_DEBUG(
"Applying a correspondence rejector method: %s.\n",
203 rej->getClassName().c_str());
204 if (rej->requiresSourcePoints())
205 rej->setSourcePoints(inputs_transformed_combined_blob);
206 if (rej->requiresSourceNormals() && source_has_normals_)
207 rej->setSourceNormals(inputs_transformed_combined_blob);
208 rej->setInputCorrespondences(temp_correspondences);
209 rej->getCorrespondences(*correspondences_);
211 if (i < correspondence_rejectors_.size() - 1)
212 *temp_correspondences = *correspondences_;
215 int cnt = correspondences_->size();
217 if (cnt < min_number_correspondences_) {
218 PCL_ERROR(
"[pcl::%s::computeTransformation] Not enough correspondences found. "
219 "Relax your threshold parameters.\n",
220 getClassName().c_str());
221 convergence_criteria_->setConvergenceState(
223 Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
229 transformation_estimation_->estimateRigidTransformation(
230 *inputs_transformed_combined,
236 this->transformCloud(
237 *inputs_transformed_combined, *inputs_transformed_combined, transformation_);
239 for (std::size_t i = 0; i < sources_.size(); i++) {
240 this->transformCloud(
241 *inputs_transformed[i], *inputs_transformed[i], transformation_);
245 final_transformation_ = transformation_ * final_transformation_;
253 converged_ =
static_cast<bool>((*convergence_criteria_));
254 }
while (!converged_);
256 PCL_DEBUG(
"Transformation "
257 "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%"
258 "5f\t%5f\t%5f\t%5f\n",
259 final_transformation_(0, 0),
260 final_transformation_(0, 1),
261 final_transformation_(0, 2),
262 final_transformation_(0, 3),
263 final_transformation_(1, 0),
264 final_transformation_(1, 1),
265 final_transformation_(1, 2),
266 final_transformation_(1, 3),
267 final_transformation_(2, 0),
268 final_transformation_(2, 1),
269 final_transformation_(2, 2),
270 final_transformation_(2, 3),
271 final_transformation_(3, 0),
272 final_transformation_(3, 1),
273 final_transformation_(3, 2),
274 final_transformation_(3, 3));
285 if (!manual_correspondence_estimations_set) {
286 correspondence_estimations_.clear();
294 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
299 need_source_blob_ =
false;
300 need_target_blob_ =
false;
302 for (std::size_t i = 0; i < correspondence_estimations_.size(); i++) {
305 need_source_blob_ |= ce->requiresSourceNormals();
306 need_target_blob_ |= ce->requiresTargetNormals();
308 if (ce->requiresSourceNormals() && !source_has_normals_) {
309 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
310 "but we can't provide them.\n",
311 getClassName().c_str());
313 if (ce->requiresTargetNormals() && !target_has_normals_) {
314 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
315 "but we can't provide them.\n",
316 getClassName().c_str());
320 for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
322 need_source_blob_ |= rej->requiresSourcePoints();
323 need_source_blob_ |= rej->requiresSourceNormals();
324 need_target_blob_ |= rej->requiresTargetPoints();
325 need_target_blob_ |= rej->requiresTargetNormals();
326 if (rej->requiresSourceNormals() && !source_has_normals_) {
327 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
328 "normals, but we can't provide them.\n",
329 getClassName().c_str(),
330 rej->getClassName().c_str());
332 if (rej->requiresTargetNormals() && !target_has_normals_) {
333 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
334 "normals, but we can't provide them.\n",
335 getClassName().c_str(),
336 rej->getClassName().c_str());