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>
49 template <
typename Po
intT>
56 if (k_correspondences_ > static_cast<int>(cloud->
size())) {
57 PCL_ERROR(
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
58 "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
66 std::vector<float> nn_dist_sq(k_correspondences_);
69 if (cloud_covariances.size() < cloud->
size())
70 cloud_covariances.resize(cloud->
size());
72 auto matrices_iterator = cloud_covariances.begin();
73 for (
auto points_iterator = cloud->
begin(); points_iterator != cloud->
end();
74 ++points_iterator, ++matrices_iterator) {
75 const PointT& query_point = *points_iterator;
76 Eigen::Matrix3d& cov = *matrices_iterator;
82 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indices, nn_dist_sq);
85 for (
int j = 0; j < k_correspondences_; j++) {
87 const double ptx = (*cloud)[nn_indices[j]].x - query_point.x,
88 pty = (*cloud)[nn_indices[j]].y - query_point.y,
89 ptz = (*cloud)[nn_indices[j]].z - query_point.z;
95 cov(0, 0) += ptx * ptx;
97 cov(1, 0) += pty * ptx;
98 cov(1, 1) += pty * pty;
100 cov(2, 0) += ptz * ptx;
101 cov(2, 1) += ptz * pty;
102 cov(2, 2) += ptz * ptz;
105 mean /=
static_cast<double>(k_correspondences_);
107 for (
int k = 0; k < 3; k++)
108 for (
int l = 0; l <= k; l++) {
109 cov(k, l) /=
static_cast<double>(k_correspondences_);
110 cov(k, l) -= mean[k] * mean[l];
111 cov(l, k) = cov(k, l);
115 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
117 Eigen::Matrix3d U = svd.matrixU();
120 for (
int k = 0; k < 3; k++) {
121 Eigen::Vector3d col = U.col(k);
125 cov += v * col * col.transpose();
130 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
136 Eigen::Matrix3d& dR_dPhi,
137 Eigen::Matrix3d& dR_dTheta,
138 Eigen::Matrix3d& dR_dPsi)
const
140 const double cphi = std::cos(phi), sphi = std::sin(phi);
141 const double ctheta = std::cos(theta), stheta = std::sin(theta);
142 const double cpsi = std::cos(psi), spsi = std::sin(psi);
147 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
148 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
149 dR_dPhi(2, 1) = cphi * ctheta;
151 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
152 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
153 dR_dPhi(2, 2) = -ctheta * sphi;
155 dR_dTheta(0, 0) = -cpsi * stheta;
156 dR_dTheta(1, 0) = -spsi * stheta;
157 dR_dTheta(2, 0) = -ctheta;
159 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
160 dR_dTheta(1, 1) = ctheta * sphi * spsi;
161 dR_dTheta(2, 1) = -sphi * stheta;
163 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
164 dR_dTheta(1, 2) = cphi * ctheta * spsi;
165 dR_dTheta(2, 2) = -cphi * stheta;
167 dR_dPsi(0, 0) = -ctheta * spsi;
168 dR_dPsi(1, 0) = cpsi * ctheta;
171 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
172 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
175 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
176 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
180 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
185 Eigen::Matrix3d dR_dPhi;
186 Eigen::Matrix3d dR_dTheta;
187 Eigen::Matrix3d dR_dPsi;
188 getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
190 g[3] = (dR_dPhi * dCost_dR_T).trace();
191 g[4] = (dR_dTheta * dCost_dR_T).trace();
192 g[5] = (dR_dPsi * dCost_dR_T).trace();
195 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
201 Eigen::Matrix3d& ddR_dPhi_dPhi,
202 Eigen::Matrix3d& ddR_dPhi_dTheta,
203 Eigen::Matrix3d& ddR_dPhi_dPsi,
204 Eigen::Matrix3d& ddR_dTheta_dTheta,
205 Eigen::Matrix3d& ddR_dTheta_dPsi,
206 Eigen::Matrix3d& ddR_dPsi_dPsi)
const
208 const double sphi = std::sin(phi), stheta = std::sin(theta), spsi = std::sin(psi);
209 const double cphi = std::cos(phi), ctheta = std::cos(theta), cpsi = std::cos(psi);
210 ddR_dPhi_dPhi(0, 0) = 0.0;
211 ddR_dPhi_dPhi(1, 0) = 0.0;
212 ddR_dPhi_dPhi(2, 0) = 0.0;
213 ddR_dPhi_dPhi(0, 1) = -cpsi * stheta * sphi + spsi * cphi;
214 ddR_dPhi_dPhi(1, 1) = -cpsi * cphi - spsi * stheta * sphi;
215 ddR_dPhi_dPhi(2, 1) = -ctheta * sphi;
216 ddR_dPhi_dPhi(0, 2) = -spsi * sphi - cpsi * stheta * cphi;
217 ddR_dPhi_dPhi(1, 2) = -spsi * stheta * cphi + cpsi * sphi;
218 ddR_dPhi_dPhi(2, 2) = -ctheta * cphi;
220 ddR_dPhi_dTheta(0, 0) = 0.0;
221 ddR_dPhi_dTheta(1, 0) = 0.0;
222 ddR_dPhi_dTheta(2, 0) = 0.0;
223 ddR_dPhi_dTheta(0, 1) = cpsi * ctheta * cphi;
224 ddR_dPhi_dTheta(1, 1) = spsi * ctheta * cphi;
225 ddR_dPhi_dTheta(2, 1) = -stheta * cphi;
226 ddR_dPhi_dTheta(0, 2) = -cpsi * ctheta * sphi;
227 ddR_dPhi_dTheta(1, 2) = -spsi * ctheta * sphi;
228 ddR_dPhi_dTheta(2, 2) = stheta * sphi;
230 ddR_dPhi_dPsi(0, 0) = 0.0;
231 ddR_dPhi_dPsi(1, 0) = 0.0;
232 ddR_dPhi_dPsi(2, 0) = 0.0;
233 ddR_dPhi_dPsi(0, 1) = -spsi * stheta * cphi + cpsi * sphi;
234 ddR_dPhi_dPsi(1, 1) = spsi * sphi + cpsi * stheta * cphi;
235 ddR_dPhi_dPsi(2, 1) = 0.0;
236 ddR_dPhi_dPsi(0, 2) = cpsi * cphi + spsi * stheta * sphi;
237 ddR_dPhi_dPsi(1, 2) = -cpsi * stheta * sphi + spsi * cphi;
238 ddR_dPhi_dPsi(2, 2) = 0.0;
240 ddR_dTheta_dTheta(0, 0) = -cpsi * ctheta;
241 ddR_dTheta_dTheta(1, 0) = -spsi * ctheta;
242 ddR_dTheta_dTheta(2, 0) = stheta;
243 ddR_dTheta_dTheta(0, 1) = -cpsi * stheta * sphi;
244 ddR_dTheta_dTheta(1, 1) = -spsi * stheta * sphi;
245 ddR_dTheta_dTheta(2, 1) = -ctheta * sphi;
246 ddR_dTheta_dTheta(0, 2) = -cpsi * stheta * cphi;
247 ddR_dTheta_dTheta(1, 2) = -spsi * stheta * cphi;
248 ddR_dTheta_dTheta(2, 2) = -ctheta * cphi;
250 ddR_dTheta_dPsi(0, 0) = spsi * stheta;
251 ddR_dTheta_dPsi(1, 0) = -cpsi * stheta;
252 ddR_dTheta_dPsi(2, 0) = 0.0;
253 ddR_dTheta_dPsi(0, 1) = -spsi * ctheta * sphi;
254 ddR_dTheta_dPsi(1, 1) = cpsi * ctheta * sphi;
255 ddR_dTheta_dPsi(2, 1) = 0.0;
256 ddR_dTheta_dPsi(0, 2) = -spsi * ctheta * cphi;
257 ddR_dTheta_dPsi(1, 2) = cpsi * ctheta * cphi;
258 ddR_dTheta_dPsi(2, 2) = 0.0;
260 ddR_dPsi_dPsi(0, 0) = -cpsi * ctheta;
261 ddR_dPsi_dPsi(1, 0) = -spsi * ctheta;
262 ddR_dPsi_dPsi(2, 0) = 0.0;
263 ddR_dPsi_dPsi(0, 1) = -cpsi * stheta * sphi + spsi * cphi;
264 ddR_dPsi_dPsi(1, 1) = -cpsi * cphi - spsi * stheta * sphi;
265 ddR_dPsi_dPsi(2, 1) = 0.0;
266 ddR_dPsi_dPsi(0, 2) = -spsi * sphi - cpsi * stheta * cphi;
267 ddR_dPsi_dPsi(1, 2) = -spsi * stheta * cphi + cpsi * sphi;
268 ddR_dPsi_dPsi(2, 2) = 0.0;
271 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
278 Matrix4& transformation_matrix)
281 if (indices_src.size() < min_number_correspondences_) {
284 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
286 << min_number_correspondences_
287 <<
" points to estimate a transform! "
288 "Source and target have "
289 << indices_src.size() <<
" points!");
295 x[0] = transformation_matrix(0, 3);
296 x[1] = transformation_matrix(1, 3);
297 x[2] = transformation_matrix(2, 3);
300 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
301 x[4] = asin(-transformation_matrix(2, 0));
302 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
305 tmp_src_ = &cloud_src;
306 tmp_tgt_ = &cloud_tgt;
307 tmp_idx_src_ = &indices_src;
308 tmp_idx_tgt_ = &indices_tgt;
320 int inner_iterations_ = 0;
332 inner_iterations_ == max_inner_iterations_) {
333 PCL_DEBUG(
"[pcl::registration::TransformationEstimationBFGS::"
334 "estimateRigidTransformation]");
335 PCL_DEBUG(
"BFGS solver finished with exit code %i \n", result);
336 transformation_matrix.setIdentity();
337 applyState(transformation_matrix, x);
342 "[pcl::" << getClassName()
343 <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
344 "solver didn't converge!");
347 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
354 Matrix4& transformation_matrix)
357 if (indices_src.size() < min_number_correspondences_) {
359 "[pcl::GeneralizedIterativeClosestPoint::"
360 "estimateRigidTransformationNewton] Need "
362 << min_number_correspondences_
363 <<
" points to estimate a transform! "
364 "Source and target have "
365 << indices_src.size() <<
" points!");
371 x[0] = transformation_matrix(0, 3);
372 x[1] = transformation_matrix(1, 3);
373 x[2] = transformation_matrix(2, 3);
376 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
378 std::min<double>(1.0, std::max<double>(-1.0, -transformation_matrix(2, 0))));
379 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
382 tmp_src_ = &cloud_src;
383 tmp_tgt_ = &cloud_tgt;
384 tmp_idx_src_ = &indices_src;
385 tmp_idx_tgt_ = &indices_tgt;
389 Eigen::Matrix<double, 6, 6> hessian;
390 Eigen::Matrix<double, 6, 1> gradient;
391 double current_x_value = functor(x);
392 functor.
dfddf(x, gradient, hessian);
393 Eigen::Matrix<double, 6, 1> delta;
394 int inner_iterations_ = 0;
399 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> eigensolver(hessian);
400 Eigen::Matrix<double, 6, 6> inverted_eigenvalues =
401 Eigen::Matrix<double, 6, 6>::Zero();
402 for (
int i = 0; i < 6; ++i) {
403 const double ev = eigensolver.eigenvalues()[i];
405 inverted_eigenvalues(i, i) = 1.0 / eigensolver.eigenvalues()[5];
407 inverted_eigenvalues(i, i) = 1.0 / ev;
409 delta = eigensolver.eigenvectors() * inverted_eigenvalues *
410 eigensolver.eigenvectors().transpose() * gradient;
414 double candidate_x_value;
415 bool improvement_found =
false;
416 for (
int i = 0; i < 10; ++i, alpha /= 2) {
417 Vector6d candidate_x = x - alpha * delta;
418 candidate_x_value = functor(candidate_x);
419 if (candidate_x_value < current_x_value) {
420 PCL_DEBUG(
"[estimateRigidTransformationNewton] Using stepsize=%g, function "
421 "value previously: %g, now: %g, "
426 current_x_value - candidate_x_value);
428 current_x_value = candidate_x_value;
429 improvement_found =
true;
433 if (!improvement_found) {
434 PCL_DEBUG(
"[estimateRigidTransformationNewton] finishing because no progress\n");
437 functor.
dfddf(x, gradient, hessian);
438 if (gradient.head<3>().norm() < translation_gradient_tolerance_ &&
439 gradient.tail<3>().norm() < rotation_gradient_tolerance_) {
440 PCL_DEBUG(
"[estimateRigidTransformationNewton] finishing because gradient below "
441 "threshold: translation: %g<%g, rotation: %g<%g\n",
442 gradient.head<3>().norm(),
443 translation_gradient_tolerance_,
444 gradient.tail<3>().norm(),
445 rotation_gradient_tolerance_);
448 }
while (inner_iterations_ < max_inner_iterations_);
449 PCL_DEBUG(
"[estimateRigidTransformationNewton] solver finished after %i iterations "
452 max_inner_iterations_);
453 transformation_matrix.setIdentity();
454 applyState(transformation_matrix, x);
457 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
463 gicp_->applyState(transformation_matrix, x);
465 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
466 for (
int i = 0; i < m; ++i) {
469 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
472 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
473 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
477 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
478 p_trans_src[1] - p_tgt[1],
479 p_trans_src[2] - p_tgt[2]);
480 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
483 f +=
static_cast<double>(d.transpose() * Md);
488 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
494 gicp_->applyState(transformation_matrix, x);
499 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
500 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
501 for (
int i = 0; i < m; ++i) {
504 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
507 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
509 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
512 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
513 p_trans_src[1] - p_tgt[1],
514 p_trans_src[2] - p_tgt[2]);
516 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
522 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
523 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
524 dCost_dR_T += p_base_src * Md.transpose();
526 g.head<3>() *= 2.0 / m;
527 dCost_dR_T *= 2.0 / m;
528 gicp_->computeRDerivative(x, dCost_dR_T, g);
531 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
537 gicp_->applyState(transformation_matrix, x);
541 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
542 const int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
543 for (
int i = 0; i < m; ++i) {
546 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
549 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
550 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
553 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
554 p_trans_src[1] - p_tgt[1],
555 p_trans_src[2] - p_tgt[2]);
557 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
559 f +=
static_cast<double>(d.transpose() * Md);
564 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
565 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
567 dCost_dR_T += p_base_src * Md.transpose();
569 f /=
static_cast<double>(m);
570 g.head<3>() *= (2.0 / m);
571 dCost_dR_T *= 2.0 / m;
572 gicp_->computeRDerivative(x, dCost_dR_T, g);
575 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
583 gicp_->applyState(transformation_matrix, x);
584 const Eigen::Matrix4f transformation_matrix_float =
585 transformation_matrix.template cast<float>();
586 const Eigen::Matrix4f base_transformation_float =
587 gicp_->base_transformation_.template cast<float>();
592 Eigen::Matrix3d dR_dPhi;
593 Eigen::Matrix3d dR_dTheta;
594 Eigen::Matrix3d dR_dPsi;
595 gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
596 Eigen::Matrix3d ddR_dPhi_dPhi;
597 Eigen::Matrix3d ddR_dPhi_dTheta;
598 Eigen::Matrix3d ddR_dPhi_dPsi;
599 Eigen::Matrix3d ddR_dTheta_dTheta;
600 Eigen::Matrix3d ddR_dTheta_dPsi;
601 Eigen::Matrix3d ddR_dPsi_dPsi;
602 gicp_->getR2ndDerivatives(x[3],
611 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
612 Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero();
613 Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero();
614 Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero();
615 Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero();
616 Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero();
617 Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero();
618 Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero();
619 Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero();
620 Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero();
621 Eigen::Matrix<double, 9, 6> hessian_rot_tmp = Eigen::Matrix<double, 9, 6>::Zero();
623 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
624 for (
int i = 0; i < m; ++i) {
626 const auto& src_idx = (*gicp_->tmp_idx_src_)[i];
630 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
631 Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src);
634 const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
635 p_trans_src[1] - p_tgt[1],
636 p_trans_src[2] - p_tgt[2]);
637 const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx);
638 const Eigen::Vector3d Md(M * d);
639 gradient.head<3>() += Md;
640 hessian.topLeftCorner<3, 3>() += M;
641 p_trans_src = base_transformation_float * p_src;
642 const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
643 dCost_dR_T.noalias() += p_base_src * Md.transpose();
644 dCost_dR_T1b += p_base_src[0] * M;
645 dCost_dR_T2b += p_base_src[1] * M;
646 dCost_dR_T3b += p_base_src[2] * M;
647 hessian_rot_tmp.noalias() +=
648 Eigen::Map<const Eigen::Matrix<double, 9, 1>>{M.data()} *
649 (Eigen::Matrix<double, 1, 6>() << p_base_src[0] * p_base_src[0],
650 p_base_src[0] * p_base_src[1],
651 p_base_src[0] * p_base_src[2],
652 p_base_src[1] * p_base_src[1],
653 p_base_src[1] * p_base_src[2],
654 p_base_src[2] * p_base_src[2])
657 gradient.head<3>() *= 2.0 / m;
658 dCost_dR_T *= 2.0 / m;
659 gicp_->computeRDerivative(x, dCost_dR_T, gradient);
660 hessian.topLeftCorner<3, 3>() *= 2.0 / m;
662 dCost_dR_T1.row(0) = dCost_dR_T1b.col(0);
663 dCost_dR_T1.row(1) = dCost_dR_T2b.col(0);
664 dCost_dR_T1.row(2) = dCost_dR_T3b.col(0);
665 dCost_dR_T2.row(0) = dCost_dR_T1b.col(1);
666 dCost_dR_T2.row(1) = dCost_dR_T2b.col(1);
667 dCost_dR_T2.row(2) = dCost_dR_T3b.col(1);
668 dCost_dR_T3.row(0) = dCost_dR_T1b.col(2);
669 dCost_dR_T3.row(1) = dCost_dR_T2b.col(2);
670 dCost_dR_T3.row(2) = dCost_dR_T3b.col(2);
671 dCost_dR_T1 *= 2.0 / m;
672 dCost_dR_T2 *= 2.0 / m;
673 dCost_dR_T3 *= 2.0 / m;
674 hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace();
675 hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace();
676 hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace();
677 hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace();
678 hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace();
679 hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace();
680 hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace();
681 hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace();
682 hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace();
683 hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose();
685 int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}};
686 for (
int l = 0; l < 3; ++l) {
687 for (
int i = 0; i < 3; ++i) {
688 double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0;
689 for (
int j = 0; j < 3; ++j) {
690 for (
int k = 0; k < 3; ++k) {
691 phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k);
692 theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k);
693 psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k);
696 hessian_rot_phi(i, l) = phi_tmp;
697 hessian_rot_theta(i, l) = theta_tmp;
698 hessian_rot_psi(i, l) = psi_tmp;
701 hessian_rot_phi *= 2.0 / m;
702 hessian_rot_theta *= 2.0 / m;
703 hessian_rot_psi *= 2.0 / m;
704 hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() +
705 (ddR_dPhi_dPhi * dCost_dR_T).trace();
706 hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() +
707 (ddR_dPhi_dTheta * dCost_dR_T).trace();
708 hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() +
709 (ddR_dPhi_dPsi * dCost_dR_T).trace();
710 hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() +
711 (ddR_dTheta_dTheta * dCost_dR_T).trace();
712 hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() +
713 (ddR_dTheta_dPsi * dCost_dR_T).trace();
714 hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() +
715 (ddR_dPsi_dPsi * dCost_dR_T).trace();
716 hessian(4, 3) = hessian(3, 4);
717 hessian(5, 3) = hessian(3, 5);
718 hessian(5, 4) = hessian(4, 5);
721 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
727 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
729 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
733 auto translation_grad = g.head<3>().norm();
736 auto rotation_grad = g.tail<3>().norm();
738 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
744 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
753 const std::size_t N = indices_->size();
755 mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
757 if ((!target_covariances_) || (target_covariances_->empty())) {
759 computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
762 if ((!input_covariances_) || (input_covariances_->empty())) {
764 computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
767 base_transformation_ = Matrix4::Identity();
770 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
772 std::vector<float> nn_dists(1);
776 while (!converged_) {
782 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
783 for (std::size_t i = 0; i < 4; i++)
784 for (std::size_t j = 0; j < 4; j++)
785 for (std::size_t k = 0; k < 4; k++)
786 transform_R(i, j) +=
static_cast<double>(transformation_(i, k)) *
787 static_cast<double>(guess(k, j));
789 Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
791 for (std::size_t i = 0; i < N; i++) {
792 PointSource query = output[i];
793 query.getVector4fMap() =
794 transformation_.template cast<float>() * query.getVector4fMap();
796 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
797 PCL_ERROR(
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
798 "in the target dataset for point %d in the source!\n",
799 getClassName().c_str(),
806 if (nn_dists[0] < dist_threshold) {
807 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
808 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
809 Eigen::Matrix3d& M = mahalanobis_[i];
813 Eigen::Matrix3d temp = M * R.transpose();
817 source_indices[cnt] =
static_cast<int>(i);
818 target_indices[cnt] = nn_indices[0];
823 source_indices.resize(cnt);
824 target_indices.resize(cnt);
826 previous_transformation_ = transformation_;
829 rigid_transformation_estimation_(
830 output, source_indices, *target_, target_indices, transformation_);
833 for (
int k = 0; k < 4; k++) {
834 for (
int l = 0; l < 4; l++) {
837 ratio = 1. / rotation_epsilon_;
839 ratio = 1. / transformation_epsilon_;
841 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
847 PCL_DEBUG(
"[pcl::%s::computeTransformation] Optimization issue %s\n",
848 getClassName().c_str(),
854 if (update_visualizer_ !=
nullptr) {
857 update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
861 if (nr_iterations_ >= max_iterations_ || delta < 1) {
863 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence reached. Number of "
864 "iterations: %d out of %d. Transformation difference: %f\n",
865 getClassName().c_str(),
868 (transformation_ - previous_transformation_).array().abs().sum());
869 previous_transformation_ = transformation_;
872 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence failed\n",
873 getClassName().c_str());
875 final_transformation_ = previous_transformation_ * guess;
877 PCL_DEBUG(
"Transformation "
878 "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%"
879 "5f\t%5f\t%5f\t%5f\n",
880 final_transformation_(0, 0),
881 final_transformation_(0, 1),
882 final_transformation_(0, 2),
883 final_transformation_(0, 3),
884 final_transformation_(1, 0),
885 final_transformation_(1, 1),
886 final_transformation_(1, 2),
887 final_transformation_(1, 3),
888 final_transformation_(2, 0),
889 final_transformation_(2, 1),
890 final_transformation_(2, 2),
891 final_transformation_(2, 3),
892 final_transformation_(3, 0),
893 final_transformation_(3, 1),
894 final_transformation_(3, 2),
895 final_transformation_(3, 3));
901 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
908 AngleAxis(static_cast<Scalar>(x[4]), Vector3::UnitY()) *
909 AngleAxis(static_cast<Scalar>(x[3]), Vector3::UnitX()))
911 Matrix4 T = Matrix4::Identity();
912 T.template block<3, 3>(0, 0) = R;
913 T.template block<3, 1>(0, 3) =
Vector3(
914 static_cast<Scalar>(x[0]),
static_cast<Scalar
>(x[1]), static_cast<Scalar>(x[2]));
920 #endif // PCL_REGISTRATION_IMPL_GICP_HPP_
shared_ptr< KdTree< PointT, Tree > > Ptr
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
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...
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d >> MatricesVector
Matrix4 base_transformation_
base transformation
BFGSSpace::Status testGradient()
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
A base class for all pcl exceptions which inherits from std::runtime_error.
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
iterator begin() noexcept
void dfddf(const Vector6d &x, Vector6d &df, Matrix6d &ddf)
typename Eigen::Matrix< Scalar, 3, 3 > Matrix3
void applyState(Matrix4 &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
typename PointCloudSource::Ptr PointCloudSourcePtr
Eigen::Matrix< double, 6, 6 > Matrix6d
typename Eigen::AngleAxis< Scalar > AngleAxis
IndicesAllocator<> Indices
Type used for indices in PCL.
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.
Eigen::Matrix< double, 6, 1 > Vector6d
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
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...
An exception that is thrown when the number of correspondents is not equal to the minimum required...
An exception that is thrown when the non linear solver didn't converge.
shared_ptr< const PointCloud< PointT > > ConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.
BFGSSpace::Status minimizeOneStep(FVectorType &x)
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.
BFGSSpace::Status minimizeInit(FVectorType &x)
optimization functor structure
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
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.
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear...