From dd70aab2217863aa7ebb94fbfaea3d028e72e5df Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sat, 23 Sep 2023 16:36:33 +0200 Subject: [PATCH] GICP: add function to compute Hessian matrix --- registration/include/pcl/registration/gicp.h | 9 + .../include/pcl/registration/impl/gicp.hpp | 209 ++++++++++++++++-- 2 files changed, 202 insertions(+), 16 deletions(-) diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 2eae855abbf..945bf564b90 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -111,6 +111,7 @@ class GeneralizedIterativeClosestPoint using Matrix3 = typename Eigen::Matrix; using Matrix4 = typename IterativeClosestPoint::Matrix4; + using Matrix6d = Eigen::Matrix; using AngleAxis = typename Eigen::AngleAxis; PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -447,6 +448,8 @@ class GeneralizedIterativeClosestPoint df(const Vector6d& x, Vector6d& df) override; void fdf(const Vector6d& x, double& f, Vector6d& df) override; + void + dfddf(const Vector6d& x, Vector6d& df, Matrix6d& ddf); BFGSSpace::Status checkGradient(const Vector6d& g) override; @@ -459,6 +462,12 @@ class GeneralizedIterativeClosestPoint const pcl::Indices& tgt_indices, Matrix4& transformation_matrix)> rigid_transformation_estimation_; +private: + void + getRDerivatives(double phi, double theta, double psi, Eigen::Matrix3d& dR_dPhi, Eigen::Matrix3d& dR_dTheta, Eigen::Matrix3d& dR_dPsi) const; + + void + getR2ndDerivatives(double phi, double theta, double psi, Eigen::Matrix3d& ddR_dPhi_dPhi, Eigen::Matrix3d& ddR_dPhi_dTheta, Eigen::Matrix3d& ddR_dPhi_dPsi, Eigen::Matrix3d& ddR_dTheta_dTheta, Eigen::Matrix3d& ddR_dTheta_dPsi, Eigen::Matrix3d& ddR_dPsi_dPsi) const; }; } // namespace pcl diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index b3ce37af2f3..c7c045741e6 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -129,19 +129,10 @@ GeneralizedIterativeClosestPoint::computeCovar template void -GeneralizedIterativeClosestPoint::computeRDerivative( - const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const -{ - Eigen::Matrix3d dR_dPhi; - Eigen::Matrix3d dR_dTheta; - Eigen::Matrix3d dR_dPsi; - - double phi = x[3], theta = x[4], psi = x[5]; - - double cphi = std::cos(phi), sphi = sin(phi); - double ctheta = std::cos(theta), stheta = sin(theta); - double cpsi = std::cos(psi), spsi = sin(psi); - +GeneralizedIterativeClosestPoint::getRDerivatives(double phi, double theta, double psi, Eigen::Matrix3d& dR_dPhi, Eigen::Matrix3d& dR_dTheta, Eigen::Matrix3d& dR_dPsi) const { + const double cphi = std::cos(phi), sphi = std::sin(phi); + const double ctheta = std::cos(theta), stheta = std::sin(theta); + const double cpsi = std::cos(psi), spsi = std::sin(psi); dR_dPhi(0, 0) = 0.; dR_dPhi(1, 0) = 0.; dR_dPhi(2, 0) = 0.; @@ -177,10 +168,88 @@ GeneralizedIterativeClosestPoint::computeRDeri dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta; dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta; dR_dPsi(2, 2) = 0.; +} - g[3] = matricesInnerProd(dR_dPhi, dCost_dR_T); - g[4] = matricesInnerProd(dR_dTheta, dCost_dR_T); - g[5] = matricesInnerProd(dR_dPsi, dCost_dR_T); +template +void +GeneralizedIterativeClosestPoint::computeRDerivative( + const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const +{ + Eigen::Matrix3d dR_dPhi; + Eigen::Matrix3d dR_dTheta; + Eigen::Matrix3d dR_dPsi; + getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi); + + g[3] = (dR_dPhi * dCost_dR_T).trace(); + g[4] = (dR_dTheta * dCost_dR_T).trace(); + g[5] = (dR_dPsi * dCost_dR_T).trace(); +} + +template +void +GeneralizedIterativeClosestPoint::getR2ndDerivatives(double phi, double theta, double psi, Eigen::Matrix3d& ddR_dPhi_dPhi, Eigen::Matrix3d& ddR_dPhi_dTheta, Eigen::Matrix3d& ddR_dPhi_dPsi, Eigen::Matrix3d& ddR_dTheta_dTheta, Eigen::Matrix3d& ddR_dTheta_dPsi, Eigen::Matrix3d& ddR_dPsi_dPsi) const +{ + const double sphi = std::sin(phi), stheta = std::sin(theta), spsi = std::sin(psi); + const double cphi = std::cos(phi), ctheta = std::cos(theta), cpsi = std::cos(psi); + ddR_dPhi_dPhi(0, 0) = 0.0; + ddR_dPhi_dPhi(1, 0) = 0.0; + ddR_dPhi_dPhi(2, 0) = 0.0; + ddR_dPhi_dPhi(0, 1) = -cpsi*stheta*sphi+spsi*cphi; + ddR_dPhi_dPhi(1, 1) = -cpsi*cphi-spsi*stheta*sphi; + ddR_dPhi_dPhi(2, 1) = -ctheta*sphi; + ddR_dPhi_dPhi(0, 2) = -spsi*sphi-cpsi*stheta*cphi; + ddR_dPhi_dPhi(1, 2) = -spsi*stheta*cphi+cpsi*sphi; + ddR_dPhi_dPhi(2, 2) = -ctheta*cphi; + + ddR_dPhi_dTheta(0, 0) = 0.0; + ddR_dPhi_dTheta(1, 0) = 0.0; + ddR_dPhi_dTheta(2, 0) = 0.0; + ddR_dPhi_dTheta(0, 1) = cpsi*ctheta*cphi; + ddR_dPhi_dTheta(1, 1) = spsi*ctheta*cphi; + ddR_dPhi_dTheta(2, 1) = -stheta*cphi; + ddR_dPhi_dTheta(0, 2) = -cpsi*ctheta*sphi; + ddR_dPhi_dTheta(1, 2) = -spsi*ctheta*sphi; + ddR_dPhi_dTheta(2, 2) = stheta*sphi; + + ddR_dPhi_dPsi(0, 0) = 0.0; + ddR_dPhi_dPsi(1, 0) = 0.0; + ddR_dPhi_dPsi(2, 0) = 0.0; + ddR_dPhi_dPsi(0, 1) = -spsi*stheta*cphi+cpsi*sphi; + ddR_dPhi_dPsi(1, 1) = spsi*sphi+cpsi*stheta*cphi; + ddR_dPhi_dPsi(2, 1) = 0.0; + ddR_dPhi_dPsi(0, 2) = cpsi*cphi+spsi*stheta*sphi; + ddR_dPhi_dPsi(1, 2) = -cpsi*stheta*sphi+spsi*cphi; + ddR_dPhi_dPsi(2, 2) = 0.0; + + ddR_dTheta_dTheta(0, 0) = -cpsi*ctheta; + ddR_dTheta_dTheta(1, 0) = -spsi*ctheta; + ddR_dTheta_dTheta(2, 0) = stheta; + ddR_dTheta_dTheta(0, 1) = -cpsi*stheta*sphi; + ddR_dTheta_dTheta(1, 1) = -spsi*stheta*sphi; + ddR_dTheta_dTheta(2, 1) = -ctheta*sphi; + ddR_dTheta_dTheta(0, 2) = -cpsi*stheta*cphi; + ddR_dTheta_dTheta(1, 2) = -spsi*stheta*cphi; + ddR_dTheta_dTheta(2, 2) = -ctheta*cphi; + + ddR_dTheta_dPsi(0, 0) = spsi*stheta; + ddR_dTheta_dPsi(1, 0) = -cpsi*stheta; + ddR_dTheta_dPsi(2, 0) = 0.0; + ddR_dTheta_dPsi(0, 1) = -spsi*ctheta*sphi; + ddR_dTheta_dPsi(1, 1) = cpsi*ctheta*sphi; + ddR_dTheta_dPsi(2, 1) = 0.0; + ddR_dTheta_dPsi(0, 2) = -spsi*ctheta*cphi; + ddR_dTheta_dPsi(1, 2) = cpsi*ctheta*cphi; + ddR_dTheta_dPsi(2, 2) = 0.0; + + ddR_dPsi_dPsi(0, 0) = -cpsi*ctheta; + ddR_dPsi_dPsi(1, 0) = -spsi*ctheta; + ddR_dPsi_dPsi(2, 0) = 0.0; + ddR_dPsi_dPsi(0, 1) = -cpsi*stheta*sphi+spsi*cphi; + ddR_dPsi_dPsi(1, 1) = -cpsi*cphi-spsi*stheta*sphi; + ddR_dPsi_dPsi(2, 1) = 0.0; + ddR_dPsi_dPsi(0, 2) = -spsi*sphi-cpsi*stheta*cphi; + ddR_dPsi_dPsi(1, 2) = -spsi*stheta*cphi+cpsi*sphi; + ddR_dPsi_dPsi(2, 2) = 0.0; } template @@ -377,6 +446,114 @@ GeneralizedIterativeClosestPoint:: gicp_->computeRDerivative(x, dCost_dR_T, g); } +template +inline void +GeneralizedIterativeClosestPoint:: + OptimizationFunctorWithIndices::dfddf(const Vector6d& x, Vector6d& gradient, Matrix6d& hessian) +{ + //PCL_DEBUG("[OptimizationFunctorWithIndices::dfddf] x=(%g, %g, %g, %g, %g, %g)\n", x[0], x[1], x[2], x[3], x[4], x[5]); + Matrix4 transformation_matrix = gicp_->base_transformation_; + gicp_->applyState(transformation_matrix, x); + const Eigen::Matrix4f transformation_matrix_float = transformation_matrix.template cast(); + const Eigen::Matrix4f base_transformation_float = gicp_->base_transformation_.template cast(); + // Zero out gradient and hessian + gradient.setZero(); + hessian.setZero(); + // Helper matrices + Eigen::Matrix3d dR_dPhi; + Eigen::Matrix3d dR_dTheta; + Eigen::Matrix3d dR_dPsi; + gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi); + Eigen::Matrix3d ddR_dPhi_dPhi; + Eigen::Matrix3d ddR_dPhi_dTheta; + Eigen::Matrix3d ddR_dPhi_dPsi; + Eigen::Matrix3d ddR_dTheta_dTheta; + Eigen::Matrix3d ddR_dTheta_dPsi; + Eigen::Matrix3d ddR_dPsi_dPsi; + gicp_->getR2ndDerivatives(x[3], x[4], x[5], ddR_dPhi_dPhi, ddR_dPhi_dTheta, ddR_dPhi_dPsi, ddR_dTheta_dTheta, ddR_dTheta_dPsi, ddR_dPsi_dPsi); + Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero(); + + int m = static_cast(gicp_->tmp_idx_src_->size()); + //PCL_DEBUG("m=%i\n", m); + for (int i = 0; i < m; ++i) { + // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_src = + (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap(); + // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_tgt = + (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap(); + Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src); + // The last coordinate is still guaranteed to be set to 1.0 + // The d here is the negative of the d in the paper + const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0], + p_trans_src[1] - p_tgt[1], + p_trans_src[2] - p_tgt[2]); + //const Eigen::Vector3d d = (p_trans_src.head<3>()-(*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector3fMap()).template cast(); + const Eigen::Matrix3d& M = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]); + const Eigen::Vector3d Md(M * d); // Md = M*d + gradient.head<3>() += Md; // translation gradient + hessian.block<3, 3>(0, 0) += M; // translation-translation hessian + p_trans_src = base_transformation_float * p_src; + const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]); + dCost_dR_T.noalias() += p_base_src * Md.transpose(); + dCost_dR_T1b += p_base_src[0] * M; + dCost_dR_T2b += p_base_src[1] * M; + dCost_dR_T3b += p_base_src[2] * M; + hessian_rot_phi.noalias() += M * (dR_dPhi * p_base_src) * p_base_src.transpose(); + hessian_rot_theta.noalias() += M * (dR_dTheta * p_base_src) * p_base_src.transpose(); + hessian_rot_psi.noalias() += M * (dR_dPsi * p_base_src) * p_base_src.transpose(); + } + gradient.head<3>() *= 2.0 / m; // translation gradient + dCost_dR_T *= 2.0 / m; + gicp_->computeRDerivative(x, dCost_dR_T, gradient); // rotation gradient + hessian.block<3, 3>(0, 0) *= 2.0 / m; // translation-translation hessian + // translation-rotation hessian + dCost_dR_T1.row(0) = dCost_dR_T1b.col(0); + dCost_dR_T1.row(1) = dCost_dR_T2b.col(0); + dCost_dR_T1.row(2) = dCost_dR_T3b.col(0); + dCost_dR_T2.row(0) = dCost_dR_T1b.col(1); + dCost_dR_T2.row(1) = dCost_dR_T2b.col(1); + dCost_dR_T2.row(2) = dCost_dR_T3b.col(1); + dCost_dR_T3.row(0) = dCost_dR_T1b.col(2); + dCost_dR_T3.row(1) = dCost_dR_T2b.col(2); + dCost_dR_T3.row(2) = dCost_dR_T3b.col(2); + dCost_dR_T1 *= 2.0 / m; + dCost_dR_T2 *= 2.0 / m; + dCost_dR_T3 *= 2.0 / m; + hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace(); + hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace(); + hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace(); + hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace(); + hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace(); + hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace(); + hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace(); + hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace(); + hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace(); + hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose(); + // rotation-rotation hessian + hessian_rot_phi *= 2.0 / m; + hessian_rot_theta *= 2.0 / m; + hessian_rot_psi *= 2.0 / m; + hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() + (ddR_dPhi_dPhi * dCost_dR_T).trace(); + hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() + (ddR_dPhi_dTheta * dCost_dR_T).trace(); + hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() + (ddR_dPhi_dPsi * dCost_dR_T).trace(); + hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() + (ddR_dTheta_dTheta * dCost_dR_T).trace(); + hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() + (ddR_dTheta_dPsi * dCost_dR_T).trace(); + hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() + (ddR_dPsi_dPsi * dCost_dR_T).trace(); + hessian(4, 3) = hessian(3, 4); + hessian(5, 3) = hessian(3, 5); + hessian(5, 4) = hessian(4, 5); +} + template inline BFGSSpace::Status GeneralizedIterativeClosestPoint::