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());
60 if (correspondence_estimations_.empty()) {
62 correspondence_estimations_.resize(sources_.size());
63 for (std::size_t i = 0; i < sources_.size(); i++) {
64 correspondence_estimations_[i] = correspondence_estimation_->clone();
67 correspondence_estimations_[i]->setSearchMethodTarget(
tgt_tree);
68 correspondence_estimations_[i]->setSearchMethodSource(
src_tree);
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());
78 for (std::size_t i = 0; i < sources_.size(); i++) {
86 final_transformation_ =
guess;
96 for (std::size_t i = 0; i < sources_.size(); i++) {
98 if (
guess != Matrix4::Identity()) {
114 transformation_ = Matrix4::Identity();
116 determineRequiredBlobData();
118 for (std::size_t i = 0; i < sources_.size(); i++) {
119 correspondence_estimations_[i]->setInputTarget(targets_[i]);
120 if (correspondence_estimations_[i]->requiresTargetNormals()) {
123 correspondence_estimations_[i]->setTargetNormals(
target_blob);
128 if (!correspondence_rejectors_.empty() && need_target_blob_)
131 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
133 if (
rej->requiresTargetPoints())
135 if (
rej->requiresTargetNormals() && target_has_normals_)
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_);
146 for (std::size_t i = 0; i < sources_.size(); i++) {
152 previous_transformation_ = transformation_;
155 correspondences_->clear();
156 for (std::size_t i = 0; i < correspondence_estimations_.size(); i++) {
159 if (correspondence_estimations_[i]->requiresSourceNormals()) {
166 if (use_reciprocal_correspondence_) {
167 correspondence_estimations_[i]->determineReciprocalCorrespondences(
171 correspondence_estimations_[i]->determineCorrespondences(
174 PCL_DEBUG(
"[pcl::%s::computeTransformation] Found %d partial correspondences for "
176 getClassName().
c_str(),
184 correspondences_->push_back(
corr);
187 PCL_DEBUG(
"[pcl::%s::computeTransformation] Total correspondences: %d\n",
188 getClassName().
c_str(),
189 correspondences_->size());
192 if (need_source_blob_) {
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());
201 PCL_DEBUG(
"Applying a correspondence rejector method: %s.\n",
202 rej->getClassName().c_str());
203 if (
rej->requiresSourcePoints())
205 if (
rej->requiresSourceNormals() && source_has_normals_)
208 rej->getCorrespondences(*correspondences_);
210 if (i < correspondence_rejectors_.size() - 1)
214 int cnt = correspondences_->
size();
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(
222 Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
228 transformation_estimation_->estimateRigidTransformation(
235 this->transformCloud(
238 for (std::size_t i = 0; i < sources_.size(); i++) {
239 this->transformCloud(
244 final_transformation_ = transformation_ * final_transformation_;
252 converged_ =
static_cast<bool>((*convergence_criteria_));
253 }
while (!converged_);
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));
285 correspondence_estimations_.clear();
298 need_source_blob_ =
false;
299 need_target_blob_ =
false;
301 for (std::size_t i = 0; i < correspondence_estimations_.size(); i++) {
304 need_source_blob_ |=
ce->requiresSourceNormals();
305 need_target_blob_ |=
ce->requiresTargetNormals();
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());
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());
319 for (std::size_t i = 0; i < correspondence_rejectors_.size(); 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());
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());