Skip to content

Commit

Permalink
GICP: add function to compute Hessian matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@mvieth
mvieth committed Sep 23, 2023
1 parent 90fda41 commit dd70aab
Show file tree
Hide file tree
Showing 2 changed files with 202 additions and 16 deletions.
9 changes: 9 additions & 0 deletions registration/include/pcl/registration/gicp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ class GeneralizedIterativeClosestPoint
using Matrix3 = typename Eigen::Matrix<Scalar, 3, 3>;
using Matrix4 =
typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
using AngleAxis = typename Eigen::AngleAxis<Scalar>;

PCL_MAKE_ALIGNED_OPERATOR_NEW
Expand Down Expand Up @@ -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;

Expand All @@ -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

Expand Down
209 changes: 193 additions & 16 deletions registration/include/pcl/registration/impl/gicp.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,19 +129,10 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeCovar

template <typename PointSource, typename PointTarget, typename Scalar>
void
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::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<PointSource, PointTarget, Scalar>::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.;
Expand Down Expand Up @@ -177,10 +168,88 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::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 <typename PointSource, typename PointTarget, typename Scalar>
void
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::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 <typename PointSource, typename PointTarget, typename Scalar>
void
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::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 <typename PointSource, typename PointTarget, typename Scalar>
Expand Down Expand Up @@ -377,6 +446,114 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
gicp_->computeRDerivative(x, dCost_dR_T, g);
}

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
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<float>();
const Eigen::Matrix4f base_transformation_float = gicp_->base_transformation_.template cast<float>();
// 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<int>(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<double>();
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 <typename PointSource, typename PointTarget, typename Scalar>
inline BFGSSpace::Status
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
Expand Down

0 comments on commit dd70aab

Please sign in to comment.