41 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
42 #define PCL_REGISTRATION_IMPL_GICP_HPP_
44 #include <pcl/registration/exceptions.h>
48 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
51 unsigned int nr_threads)
55 threads_ = omp_get_num_procs();
57 threads_ = nr_threads;
58 PCL_DEBUG(
"[pcl::GeneralizedIterativeClosestPoint::setNumberOfThreads] Setting "
59 "number of threads to %u.\n",
64 PCL_WARN(
"[pcl::GeneralizedIterativeClosestPoint::setNumberOfThreads] "
65 "Parallelization is requested, but OpenMP is not available! Continuing "
66 "without parallelization.\n");
70 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
71 template <
typename Po
intT>
78 if (k_correspondences_ >
static_cast<int>(cloud->
size())) {
79 PCL_ERROR(
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
80 "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
89 std::vector<float> nn_dist_sq(k_correspondences_);
92 if (cloud_covariances.size() < cloud->
size())
93 cloud_covariances.resize(cloud->
size());
95 #pragma omp parallel for default(none) num_threads(threads_) schedule(dynamic, 32) \
96 shared(cloud, cloud_covariances, kdtree) \
97 firstprivate(mean, cov, nn_indices, nn_dist_sq)
98 for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(cloud->
size()); ++i) {
99 const PointT& query_point = (*cloud)[i];
105 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indices, nn_dist_sq);
108 for (
int j = 0; j < k_correspondences_; j++) {
110 const double ptx = (*cloud)[nn_indices[j]].x - query_point.x,
111 pty = (*cloud)[nn_indices[j]].y - query_point.y,
112 ptz = (*cloud)[nn_indices[j]].z - query_point.z;
118 cov(0, 0) += ptx * ptx;
120 cov(1, 0) += pty * ptx;
121 cov(1, 1) += pty * pty;
123 cov(2, 0) += ptz * ptx;
124 cov(2, 1) += ptz * pty;
125 cov(2, 2) += ptz * ptz;
128 mean /=
static_cast<double>(k_correspondences_);
130 for (
int k = 0; k < 3; k++)
131 for (
int l = 0; l <= k; l++) {
132 cov(k, l) /=
static_cast<double>(k_correspondences_);
133 cov(k, l) -= mean[k] * mean[l];
134 cov(l, k) = cov(k, l);
138 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
140 Eigen::Matrix3d U = svd.matrixU();
143 for (
int k = 0; k < 3; k++) {
144 Eigen::Vector3d col = U.col(k);
148 cov += v * col * col.transpose();
150 cloud_covariances[i] = cov;
154 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
160 Eigen::Matrix3d& dR_dPhi,
161 Eigen::Matrix3d& dR_dTheta,
162 Eigen::Matrix3d& dR_dPsi)
const
164 const double cphi = std::cos(phi), sphi = std::sin(phi);
165 const double ctheta = std::cos(theta), stheta = std::sin(theta);
166 const double cpsi = std::cos(psi), spsi = std::sin(psi);
171 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
172 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
173 dR_dPhi(2, 1) = cphi * ctheta;
175 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
176 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
177 dR_dPhi(2, 2) = -ctheta * sphi;
179 dR_dTheta(0, 0) = -cpsi * stheta;
180 dR_dTheta(1, 0) = -spsi * stheta;
181 dR_dTheta(2, 0) = -ctheta;
183 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
184 dR_dTheta(1, 1) = ctheta * sphi * spsi;
185 dR_dTheta(2, 1) = -sphi * stheta;
187 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
188 dR_dTheta(1, 2) = cphi * ctheta * spsi;
189 dR_dTheta(2, 2) = -cphi * stheta;
191 dR_dPsi(0, 0) = -ctheta * spsi;
192 dR_dPsi(1, 0) = cpsi * ctheta;
195 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
196 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
199 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
200 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
204 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
209 Eigen::Matrix3d dR_dPhi;
210 Eigen::Matrix3d dR_dTheta;
211 Eigen::Matrix3d dR_dPsi;
212 getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
214 g[3] = (dR_dPhi * dCost_dR_T).trace();
215 g[4] = (dR_dTheta * dCost_dR_T).trace();
216 g[5] = (dR_dPsi * dCost_dR_T).trace();
219 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
225 Eigen::Matrix3d& ddR_dPhi_dPhi,
226 Eigen::Matrix3d& ddR_dPhi_dTheta,
227 Eigen::Matrix3d& ddR_dPhi_dPsi,
228 Eigen::Matrix3d& ddR_dTheta_dTheta,
229 Eigen::Matrix3d& ddR_dTheta_dPsi,
230 Eigen::Matrix3d& ddR_dPsi_dPsi)
const
232 const double sphi = std::sin(phi), stheta = std::sin(theta), spsi = std::sin(psi);
233 const double cphi = std::cos(phi), ctheta = std::cos(theta), cpsi = std::cos(psi);
234 ddR_dPhi_dPhi(0, 0) = 0.0;
235 ddR_dPhi_dPhi(1, 0) = 0.0;
236 ddR_dPhi_dPhi(2, 0) = 0.0;
237 ddR_dPhi_dPhi(0, 1) = -cpsi * stheta * sphi + spsi * cphi;
238 ddR_dPhi_dPhi(1, 1) = -cpsi * cphi - spsi * stheta * sphi;
239 ddR_dPhi_dPhi(2, 1) = -ctheta * sphi;
240 ddR_dPhi_dPhi(0, 2) = -spsi * sphi - cpsi * stheta * cphi;
241 ddR_dPhi_dPhi(1, 2) = -spsi * stheta * cphi + cpsi * sphi;
242 ddR_dPhi_dPhi(2, 2) = -ctheta * cphi;
244 ddR_dPhi_dTheta(0, 0) = 0.0;
245 ddR_dPhi_dTheta(1, 0) = 0.0;
246 ddR_dPhi_dTheta(2, 0) = 0.0;
247 ddR_dPhi_dTheta(0, 1) = cpsi * ctheta * cphi;
248 ddR_dPhi_dTheta(1, 1) = spsi * ctheta * cphi;
249 ddR_dPhi_dTheta(2, 1) = -stheta * cphi;
250 ddR_dPhi_dTheta(0, 2) = -cpsi * ctheta * sphi;
251 ddR_dPhi_dTheta(1, 2) = -spsi * ctheta * sphi;
252 ddR_dPhi_dTheta(2, 2) = stheta * sphi;
254 ddR_dPhi_dPsi(0, 0) = 0.0;
255 ddR_dPhi_dPsi(1, 0) = 0.0;
256 ddR_dPhi_dPsi(2, 0) = 0.0;
257 ddR_dPhi_dPsi(0, 1) = -spsi * stheta * cphi + cpsi * sphi;
258 ddR_dPhi_dPsi(1, 1) = spsi * sphi + cpsi * stheta * cphi;
259 ddR_dPhi_dPsi(2, 1) = 0.0;
260 ddR_dPhi_dPsi(0, 2) = cpsi * cphi + spsi * stheta * sphi;
261 ddR_dPhi_dPsi(1, 2) = -cpsi * stheta * sphi + spsi * cphi;
262 ddR_dPhi_dPsi(2, 2) = 0.0;
264 ddR_dTheta_dTheta(0, 0) = -cpsi * ctheta;
265 ddR_dTheta_dTheta(1, 0) = -spsi * ctheta;
266 ddR_dTheta_dTheta(2, 0) = stheta;
267 ddR_dTheta_dTheta(0, 1) = -cpsi * stheta * sphi;
268 ddR_dTheta_dTheta(1, 1) = -spsi * stheta * sphi;
269 ddR_dTheta_dTheta(2, 1) = -ctheta * sphi;
270 ddR_dTheta_dTheta(0, 2) = -cpsi * stheta * cphi;
271 ddR_dTheta_dTheta(1, 2) = -spsi * stheta * cphi;
272 ddR_dTheta_dTheta(2, 2) = -ctheta * cphi;
274 ddR_dTheta_dPsi(0, 0) = spsi * stheta;
275 ddR_dTheta_dPsi(1, 0) = -cpsi * stheta;
276 ddR_dTheta_dPsi(2, 0) = 0.0;
277 ddR_dTheta_dPsi(0, 1) = -spsi * ctheta * sphi;
278 ddR_dTheta_dPsi(1, 1) = cpsi * ctheta * sphi;
279 ddR_dTheta_dPsi(2, 1) = 0.0;
280 ddR_dTheta_dPsi(0, 2) = -spsi * ctheta * cphi;
281 ddR_dTheta_dPsi(1, 2) = cpsi * ctheta * cphi;
282 ddR_dTheta_dPsi(2, 2) = 0.0;
284 ddR_dPsi_dPsi(0, 0) = -cpsi * ctheta;
285 ddR_dPsi_dPsi(1, 0) = -spsi * ctheta;
286 ddR_dPsi_dPsi(2, 0) = 0.0;
287 ddR_dPsi_dPsi(0, 1) = -cpsi * stheta * sphi + spsi * cphi;
288 ddR_dPsi_dPsi(1, 1) = -cpsi * cphi - spsi * stheta * sphi;
289 ddR_dPsi_dPsi(2, 1) = 0.0;
290 ddR_dPsi_dPsi(0, 2) = -spsi * sphi - cpsi * stheta * cphi;
291 ddR_dPsi_dPsi(1, 2) = -spsi * stheta * cphi + cpsi * sphi;
292 ddR_dPsi_dPsi(2, 2) = 0.0;
295 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
302 Matrix4& transformation_matrix)
305 if (indices_src.size() < min_number_correspondences_) {
308 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
310 << min_number_correspondences_
311 <<
" points to estimate a transform! "
312 "Source and target have "
313 << indices_src.size() <<
" points!");
319 x[0] = transformation_matrix(0, 3);
320 x[1] = transformation_matrix(1, 3);
321 x[2] = transformation_matrix(2, 3);
324 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
325 x[4] = asin(-transformation_matrix(2, 0));
326 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
329 tmp_src_ = &cloud_src;
330 tmp_tgt_ = &cloud_tgt;
331 tmp_idx_src_ = &indices_src;
332 tmp_idx_tgt_ = &indices_tgt;
344 int inner_iterations_ = 0;
356 inner_iterations_ == max_inner_iterations_) {
357 PCL_DEBUG(
"[pcl::registration::TransformationEstimationBFGS::"
358 "estimateRigidTransformation]");
359 PCL_DEBUG(
"BFGS solver finished with exit code %i \n", result);
360 transformation_matrix.setIdentity();
361 applyState(transformation_matrix, x);
366 "[pcl::" << getClassName()
367 <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
368 "solver didn't converge!");
371 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
378 Matrix4& transformation_matrix)
381 if (indices_src.size() < min_number_correspondences_) {
383 "[pcl::GeneralizedIterativeClosestPoint::"
384 "estimateRigidTransformationNewton] Need "
386 << min_number_correspondences_
387 <<
" points to estimate a transform! "
388 "Source and target have "
389 << indices_src.size() <<
" points!");
393 Vector6d x = Vector6d::Zero();
395 x[0] = transformation_matrix(0, 3);
396 x[1] = transformation_matrix(1, 3);
397 x[2] = transformation_matrix(2, 3);
400 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
402 std::min<double>(1.0, std::max<double>(-1.0, -transformation_matrix(2, 0))));
403 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
406 tmp_src_ = &cloud_src;
407 tmp_tgt_ = &cloud_tgt;
408 tmp_idx_src_ = &indices_src;
409 tmp_idx_tgt_ = &indices_tgt;
412 OptimizationFunctorWithIndices functor(
this);
413 Eigen::Matrix<double, 6, 6> hessian;
414 Eigen::Matrix<double, 6, 1> gradient;
415 double current_x_value = functor(x);
416 functor.dfddf(x, gradient, hessian);
417 Eigen::Matrix<double, 6, 1> delta;
418 int inner_iterations_ = 0;
423 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> eigensolver(hessian);
424 Eigen::Matrix<double, 6, 6> inverted_eigenvalues =
425 Eigen::Matrix<double, 6, 6>::Zero();
426 for (
int i = 0; i < 6; ++i) {
427 const double ev = eigensolver.eigenvalues()[i];
429 inverted_eigenvalues(i, i) = 1.0 / eigensolver.eigenvalues()[5];
431 inverted_eigenvalues(i, i) = 1.0 / ev;
433 delta = eigensolver.eigenvectors() * inverted_eigenvalues *
434 eigensolver.eigenvectors().transpose() * gradient;
438 double candidate_x_value;
439 bool improvement_found =
false;
440 for (
int i = 0; i < 10; ++i, alpha /= 2) {
442 candidate_x_value = functor(candidate_x);
443 if (candidate_x_value < current_x_value) {
444 PCL_DEBUG(
"[estimateRigidTransformationNewton] Using stepsize=%g, function "
445 "value previously: %g, now: %g, "
450 current_x_value - candidate_x_value);
452 current_x_value = candidate_x_value;
453 improvement_found =
true;
457 if (!improvement_found) {
458 PCL_DEBUG(
"[estimateRigidTransformationNewton] finishing because no progress\n");
461 functor.dfddf(x, gradient, hessian);
462 if (gradient.head<3>().norm() < translation_gradient_tolerance_ &&
463 gradient.tail<3>().norm() < rotation_gradient_tolerance_) {
464 PCL_DEBUG(
"[estimateRigidTransformationNewton] finishing because gradient below "
465 "threshold: translation: %g<%g, rotation: %g<%g\n",
466 gradient.head<3>().norm(),
467 translation_gradient_tolerance_,
468 gradient.tail<3>().norm(),
469 rotation_gradient_tolerance_);
472 }
while (inner_iterations_ < max_inner_iterations_);
473 PCL_DEBUG(
"[estimateRigidTransformationNewton] solver finished after %i iterations "
476 max_inner_iterations_);
477 transformation_matrix.setIdentity();
478 applyState(transformation_matrix, x);
481 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
486 Matrix4 transformation_matrix = gicp_->base_transformation_;
487 gicp_->applyState(transformation_matrix, x);
489 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
490 for (
int i = 0; i < m; ++i) {
493 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
496 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
497 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
501 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
502 p_trans_src[1] - p_tgt[1],
503 p_trans_src[2] - p_tgt[2]);
504 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
507 f +=
static_cast<double>(d.transpose() * Md);
512 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
517 Matrix4 transformation_matrix = gicp_->base_transformation_;
518 gicp_->applyState(transformation_matrix, x);
523 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
524 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
525 for (
int i = 0; i < m; ++i) {
528 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
531 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
533 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
536 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
537 p_trans_src[1] - p_tgt[1],
538 p_trans_src[2] - p_tgt[2]);
540 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
546 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
547 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
548 dCost_dR_T += p_base_src * Md.transpose();
550 g.head<3>() *= 2.0 / m;
551 dCost_dR_T *= 2.0 / m;
552 gicp_->computeRDerivative(x, dCost_dR_T, g);
555 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
560 Matrix4 transformation_matrix = gicp_->base_transformation_;
561 gicp_->applyState(transformation_matrix, x);
565 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
566 const int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
567 for (
int i = 0; i < m; ++i) {
570 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
573 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
574 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
577 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
578 p_trans_src[1] - p_tgt[1],
579 p_trans_src[2] - p_tgt[2]);
581 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
583 f +=
static_cast<double>(d.transpose() * Md);
588 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
589 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
591 dCost_dR_T += p_base_src * Md.transpose();
593 f /=
static_cast<double>(m);
594 g.head<3>() *= (2.0 / m);
595 dCost_dR_T *= 2.0 / m;
596 gicp_->computeRDerivative(x, dCost_dR_T, g);
599 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
606 Matrix4 transformation_matrix = gicp_->base_transformation_;
607 gicp_->applyState(transformation_matrix, x);
608 const Eigen::Matrix4f transformation_matrix_float =
609 transformation_matrix.template cast<float>();
610 const Eigen::Matrix4f base_transformation_float =
611 gicp_->base_transformation_.template cast<float>();
616 Eigen::Matrix3d dR_dPhi;
617 Eigen::Matrix3d dR_dTheta;
618 Eigen::Matrix3d dR_dPsi;
619 gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
620 Eigen::Matrix3d ddR_dPhi_dPhi;
621 Eigen::Matrix3d ddR_dPhi_dTheta;
622 Eigen::Matrix3d ddR_dPhi_dPsi;
623 Eigen::Matrix3d ddR_dTheta_dTheta;
624 Eigen::Matrix3d ddR_dTheta_dPsi;
625 Eigen::Matrix3d ddR_dPsi_dPsi;
626 gicp_->getR2ndDerivatives(x[3],
635 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
636 Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero();
637 Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero();
638 Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero();
639 Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero();
640 Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero();
641 Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero();
642 Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero();
643 Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero();
644 Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero();
645 Eigen::Matrix<double, 9, 6> hessian_rot_tmp = Eigen::Matrix<double, 9, 6>::Zero();
647 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
648 for (
int i = 0; i < m; ++i) {
650 const auto& src_idx = (*gicp_->tmp_idx_src_)[i];
654 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
655 Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src);
658 const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
659 p_trans_src[1] - p_tgt[1],
660 p_trans_src[2] - p_tgt[2]);
661 const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx);
662 const Eigen::Vector3d Md(M * d);
663 gradient.head<3>() += Md;
664 hessian.topLeftCorner<3, 3>() += M;
665 p_trans_src = base_transformation_float * p_src;
666 const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
667 dCost_dR_T.noalias() += p_base_src * Md.transpose();
668 dCost_dR_T1b += p_base_src[0] * M;
669 dCost_dR_T2b += p_base_src[1] * M;
670 dCost_dR_T3b += p_base_src[2] * M;
671 hessian_rot_tmp.noalias() +=
672 Eigen::Map<const Eigen::Matrix<double, 9, 1>>{M.data()} *
673 (Eigen::Matrix<double, 1, 6>() << p_base_src[0] * p_base_src[0],
674 p_base_src[0] * p_base_src[1],
675 p_base_src[0] * p_base_src[2],
676 p_base_src[1] * p_base_src[1],
677 p_base_src[1] * p_base_src[2],
678 p_base_src[2] * p_base_src[2])
681 gradient.head<3>() *= 2.0 / m;
682 dCost_dR_T *= 2.0 / m;
683 gicp_->computeRDerivative(x, dCost_dR_T, gradient);
684 hessian.topLeftCorner<3, 3>() *= 2.0 / m;
686 dCost_dR_T1.row(0) = dCost_dR_T1b.col(0);
687 dCost_dR_T1.row(1) = dCost_dR_T2b.col(0);
688 dCost_dR_T1.row(2) = dCost_dR_T3b.col(0);
689 dCost_dR_T2.row(0) = dCost_dR_T1b.col(1);
690 dCost_dR_T2.row(1) = dCost_dR_T2b.col(1);
691 dCost_dR_T2.row(2) = dCost_dR_T3b.col(1);
692 dCost_dR_T3.row(0) = dCost_dR_T1b.col(2);
693 dCost_dR_T3.row(1) = dCost_dR_T2b.col(2);
694 dCost_dR_T3.row(2) = dCost_dR_T3b.col(2);
695 dCost_dR_T1 *= 2.0 / m;
696 dCost_dR_T2 *= 2.0 / m;
697 dCost_dR_T3 *= 2.0 / m;
698 hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace();
699 hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace();
700 hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace();
701 hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace();
702 hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace();
703 hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace();
704 hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace();
705 hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace();
706 hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace();
707 hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose();
709 int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}};
710 for (
int l = 0; l < 3; ++l) {
711 for (
int i = 0; i < 3; ++i) {
712 double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0;
713 for (
int j = 0; j < 3; ++j) {
714 for (
int k = 0; k < 3; ++k) {
715 phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k);
716 theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k);
717 psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k);
720 hessian_rot_phi(i, l) = phi_tmp;
721 hessian_rot_theta(i, l) = theta_tmp;
722 hessian_rot_psi(i, l) = psi_tmp;
725 hessian_rot_phi *= 2.0 / m;
726 hessian_rot_theta *= 2.0 / m;
727 hessian_rot_psi *= 2.0 / m;
728 hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() +
729 (ddR_dPhi_dPhi * dCost_dR_T).trace();
730 hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() +
731 (ddR_dPhi_dTheta * dCost_dR_T).trace();
732 hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() +
733 (ddR_dPhi_dPsi * dCost_dR_T).trace();
734 hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() +
735 (ddR_dTheta_dTheta * dCost_dR_T).trace();
736 hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() +
737 (ddR_dTheta_dPsi * dCost_dR_T).trace();
738 hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() +
739 (ddR_dPsi_dPsi * dCost_dR_T).trace();
740 hessian(4, 3) = hessian(3, 4);
741 hessian(5, 3) = hessian(3, 5);
742 hessian(5, 4) = hessian(4, 5);
745 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
750 auto translation_epsilon = gicp_->translation_gradient_tolerance_;
751 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
753 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
757 auto translation_grad = g.head<3>().norm();
760 auto rotation_grad = g.tail<3>().norm();
762 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
768 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
777 const std::size_t N = indices_->size();
779 mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
781 if ((!target_covariances_) || (target_covariances_->empty())) {
783 computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
786 if ((!input_covariances_) || (input_covariances_->empty())) {
788 computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
791 base_transformation_ = Matrix4::Identity();
794 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
796 std::vector<float> nn_dists(1);
800 while (!converged_) {
806 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
807 for (std::size_t i = 0; i < 4; i++)
808 for (std::size_t j = 0; j < 4; j++)
809 for (std::size_t k = 0; k < 4; k++)
810 transform_R(i, j) +=
static_cast<double>(transformation_(i, k)) *
811 static_cast<double>(guess(k, j));
813 Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
815 for (std::size_t i = 0; i < N; i++) {
816 PointSource query = output[i];
817 query.getVector4fMap() =
818 transformation_.template cast<float>() * query.getVector4fMap();
820 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
821 PCL_ERROR(
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
822 "in the target dataset for point %d in the source!\n",
823 getClassName().c_str(),
830 if (nn_dists[0] < dist_threshold) {
831 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
832 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
833 Eigen::Matrix3d& M = mahalanobis_[i];
837 Eigen::Matrix3d temp = M * R.transpose();
841 source_indices[cnt] =
static_cast<int>(i);
842 target_indices[cnt] = nn_indices[0];
847 source_indices.resize(cnt);
848 target_indices.resize(cnt);
850 previous_transformation_ = transformation_;
853 rigid_transformation_estimation_(
854 output, source_indices, *target_, target_indices, transformation_);
857 for (
int k = 0; k < 4; k++) {
858 for (
int l = 0; l < 4; l++) {
861 ratio = 1. / rotation_epsilon_;
863 ratio = 1. / transformation_epsilon_;
865 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
871 PCL_DEBUG(
"[pcl::%s::computeTransformation] Optimization issue %s\n",
872 getClassName().c_str(),
878 if (update_visualizer_ !=
nullptr) {
881 update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
885 if (nr_iterations_ >= max_iterations_ || delta < 1) {
887 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence reached. Number of "
888 "iterations: %d out of %d. Transformation difference: %f\n",
889 getClassName().c_str(),
892 (transformation_ - previous_transformation_).array().abs().sum());
893 previous_transformation_ = transformation_;
896 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence failed\n",
897 getClassName().c_str());
899 final_transformation_ = previous_transformation_ * guess;
901 PCL_DEBUG(
"Transformation "
902 "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%"
903 "5f\t%5f\t%5f\t%5f\n",
904 final_transformation_(0, 0),
905 final_transformation_(0, 1),
906 final_transformation_(0, 2),
907 final_transformation_(0, 3),
908 final_transformation_(1, 0),
909 final_transformation_(1, 1),
910 final_transformation_(1, 2),
911 final_transformation_(1, 3),
912 final_transformation_(2, 0),
913 final_transformation_(2, 1),
914 final_transformation_(2, 2),
915 final_transformation_(2, 3),
916 final_transformation_(3, 0),
917 final_transformation_(3, 1),
918 final_transformation_(3, 2),
919 final_transformation_(3, 3));
925 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
932 AngleAxis(
static_cast<Scalar
>(x[4]), Vector3::UnitY()) *
933 AngleAxis(
static_cast<Scalar
>(x[3]), Vector3::UnitX()))
935 Matrix4 T = Matrix4::Identity();
936 T.template block<3, 3>(0, 0) = R;
937 T.template block<3, 1>(0, 3) =
Vector3(
938 static_cast<Scalar
>(x[0]),
static_cast<Scalar
>(x[1]),
static_cast<Scalar
>(x[2]));
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
BFGSSpace::Status testGradient()
BFGSSpace::Status minimizeInit(FVectorType &x)
BFGSSpace::Status minimizeOneStep(FVectorType &x)
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Matrix4 &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
void applyState(Matrix4 &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Eigen::Matrix< double, 6, 6 > Matrix6d
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
void estimateRigidTransformationNewton(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Matrix4 &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
typename PointCloudSource::Ptr PointCloudSourcePtr
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &dCost_dR_T, Vector6d &g) const
Computes the derivative of the cost function w.r.t rotation angles.
typename Eigen::AngleAxis< Scalar > AngleAxis
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
typename Eigen::Matrix< Scalar, 3, 3 > Matrix3
Eigen::Matrix< double, 6, 1 > Vector6d
An exception that is thrown when the number of correspondents is not equal to the minimum required.
A base class for all pcl exceptions which inherits from std::runtime_error.
shared_ptr< const PointCloud< PointT > > ConstPtr
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
An exception that is thrown when the non linear solver didn't converge.
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
shared_ptr< KdTree< PointT, Tree > > Ptr
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
@ NegativeGradientEpsilon
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
optimization functor structure
void dfddf(const Vector6d &x, Vector6d &df, Matrix6d &ddf)
double operator()(const Vector6d &x) override
void df(const Vector6d &x, Vector6d &df) override
BFGSSpace::Status checkGradient(const Vector6d &g) override
void fdf(const Vector6d &x, double &f, Vector6d &df) override
A point structure representing Euclidean xyz coordinates, and the RGB color.