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>
48 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
53 if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ())
55 PCL_ERROR (
"[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n",
56 getClassName ().c_str ());
59 bool manual_correspondence_estimations_set =
true;
60 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++)
66 correspondence_estimations_[i] = correspondence_estimation_->clone ();
69 correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree);
70 correspondence_estimations_[i]->setSearchMethodSource (src_tree);
73 if (correspondence_estimations_.size () != sources_.size ())
75 PCL_ERROR (
"[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n",
76 getClassName ().c_str ());
79 std::vector<PointCloudSourcePtr> inputs_transformed (sources_.size ());
80 for (std::size_t i = 0; i < sources_.size (); i++)
89 final_transformation_ = guess;
92 std::vector<std::size_t> input_offsets (sources_.size ());
93 std::vector<std::size_t> target_offsets (targets_.size ());
97 std::size_t input_offset = 0;
98 std::size_t target_offset = 0;
99 for (std::size_t i = 0; i < sources_.size (); i++)
102 if (guess != Matrix4::Identity ())
105 this->transformCloud (*sources_[i], *inputs_transformed[i], guess);
109 *inputs_transformed[i] = *sources_[i];
111 *sources_combined += *sources_[i];
112 *inputs_transformed_combined += *inputs_transformed[i];
113 *targets_combined += *targets_[i];
114 input_offsets[i] = input_offset;
115 target_offsets[i] = target_offset;
116 input_offset += inputs_transformed[i]->size ();
117 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 ();
282 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
285 need_source_blob_ =
false;
286 need_target_blob_ =
false;
288 for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
292 need_source_blob_ |= ce->requiresSourceNormals ();
293 need_target_blob_ |= ce->requiresTargetNormals ();
295 if (ce->requiresSourceNormals () && !source_has_normals_)
297 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
299 if (ce->requiresTargetNormals () && !target_has_normals_)
301 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
305 for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++)
308 need_source_blob_ |= rej->requiresSourcePoints ();
309 need_source_blob_ |= rej->requiresSourceNormals ();
310 need_target_blob_ |= rej->requiresTargetPoints ();
311 need_target_blob_ |= rej->requiresTargetNormals ();
312 if (rej->requiresSourceNormals () && !source_has_normals_)
314 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
316 if (rej->requiresTargetNormals () && !target_has_normals_)
318 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());