From 180b6e0e4a56da16d51c368acee09b01dc028a1b Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sat, 23 Sep 2023 16:36:33 +0200 Subject: [PATCH 1/3] GICP: add Newton optimizer (as new default) It replaces the current BFGS optimizer, which is still available via `useBFGS`. The Newton optimizer uses the hessian matrix of the function, while BFGS only computes an approximation of the hessian. In direct comparison (same GICP cost function to minimize and same starting point/same initial transformation), the Newton optimizer (estimateRigidTransformationNewton) takes only about one third of the time needed by the BFGS optimizer (estimateRigidTransformationBFGS). Every time, the Newton optimizer finds a slightly better transformation (lower cost) than the BFGS optimizer. Note: GICP.align() as a whole becomes only a bit faster (not 3x faster) because it also spends time on nearest-neighbour search. Even in very difficult situations, e.g. when both point clouds are far away from the origin meaning even a small rotation change results in a large point-to-point distance change, the Newton optimizer still finds a good solution, while the BFGS optimizer often fails completely. --- registration/include/pcl/registration/gicp.h | 62 ++- .../include/pcl/registration/impl/gicp.hpp | 371 +++++++++++++++++- 2 files changed, 415 insertions(+), 18 deletions(-) diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 2eae855abbf..65041fbabd7 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -49,8 +49,9 @@ namespace pcl { * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf * The approach is based on using anisotropic cost functions to optimize the alignment * after closest point assignments have been made. - * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and - * FLANN. + * The original code uses GSL and ANN while in ours we use FLANN and Newton's method + * for optimization (call `useBFGS` to switch to BFGS optimizer, however Newton + * is usually faster and more accurate). * \author Nizar Sallem * \ingroup registration */ @@ -111,6 +112,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 @@ -135,7 +137,7 @@ class GeneralizedIterativeClosestPoint const PointCloudTarget& cloud_tgt, const pcl::Indices& indices_tgt, Matrix4& transformation_matrix) { - estimateRigidTransformationBFGS( + estimateRigidTransformationNewton( cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); }; } @@ -214,6 +216,23 @@ class GeneralizedIterativeClosestPoint const pcl::Indices& indices_tgt, Matrix4& transformation_matrix); + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using an iterative non-linear Newton approach. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing + * the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing + * the correspondences of the interest points from \a indices_src + * \param[in,out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformationNewton(const PointCloudSource& cloud_src, + const pcl::Indices& indices_src, + const PointCloudTarget& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix); + /** \brief \return Mahalanobis distance matrix for the given point index */ inline const Eigen::Matrix3d& mahalanobis(std::size_t index) const @@ -276,6 +295,21 @@ class GeneralizedIterativeClosestPoint return k_correspondences_; } + /** \brief Use BFGS optimizer instead of default Newton optimizer + */ + void + useBFGS() + { + rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src, + const pcl::Indices& indices_src, + const PointCloudTarget& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) { + estimateRigidTransformationBFGS( + cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); + }; + } + /** \brief Set maximum number of iterations at the optimization step * \param[in] max maximum number of iterations for the optimizer */ @@ -447,6 +481,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 +495,26 @@ 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..3e1bcb62ed6 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -129,19 +129,17 @@ GeneralizedIterativeClosestPoint::computeCovar template void -GeneralizedIterativeClosestPoint::computeRDerivative( - const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const +GeneralizedIterativeClosestPoint::getRDerivatives( + double phi, + double theta, + double psi, + Eigen::Matrix3d& dR_dPhi, + Eigen::Matrix3d& dR_dTheta, + Eigen::Matrix3d& dR_dPsi) 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); - + 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 +175,97 @@ 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.; +} + +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] = matricesInnerProd(dR_dPhi, dCost_dR_T); - g[4] = matricesInnerProd(dR_dTheta, dCost_dR_T); - g[5] = matricesInnerProd(dR_dPsi, dCost_dR_T); + 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 @@ -259,6 +344,116 @@ GeneralizedIterativeClosestPoint:: "solver didn't converge!"); } +template +void +GeneralizedIterativeClosestPoint:: + estimateRigidTransformationNewton(const PointCloudSource& cloud_src, + const pcl::Indices& indices_src, + const PointCloudTarget& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) +{ + // need at least min_number_correspondences_ samples + if (indices_src.size() < min_number_correspondences_) { + PCL_THROW_EXCEPTION(NotEnoughPointsException, + "[pcl::GeneralizedIterativeClosestPoint::" + "estimateRigidTransformationNewton] Need " + "at least " + << min_number_correspondences_ + << " points to estimate a transform! " + "Source and target have " + << indices_src.size() << " points!"); + return; + } + // Set the initial solution + Vector6d x = Vector6d::Zero(); + // translation part + x[0] = transformation_matrix(0, 3); + x[1] = transformation_matrix(1, 3); + x[2] = transformation_matrix(2, 3); + // rotation part (Z Y X euler angles convention) + // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations + x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2)); + x[4] = std::asin( + std::min(1.0, std::max(-1.0, -transformation_matrix(2, 0)))); + x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0)); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + tmp_idx_src_ = &indices_src; + tmp_idx_tgt_ = &indices_tgt; + + // Optimize using Newton + OptimizationFunctorWithIndices functor(this); + Eigen::Matrix hessian; + Eigen::Matrix gradient; + double current_x_value = functor(x); + functor.dfddf(x, gradient, hessian); + Eigen::Matrix delta; + int inner_iterations_ = 0; + do { + ++inner_iterations_; + // compute descent direction from hessian and gradient. Take special measures if + // hessian is not positive-definite (positive Eigenvalues) + Eigen::SelfAdjointEigenSolver> eigensolver(hessian); + Eigen::Matrix inverted_eigenvalues = + Eigen::Matrix::Zero(); + for (int i = 0; i < 6; ++i) { + const double ev = eigensolver.eigenvalues()[i]; + if (ev < 0) + inverted_eigenvalues(i, i) = 1.0 / eigensolver.eigenvalues()[5]; + else + inverted_eigenvalues(i, i) = 1.0 / ev; + } + delta = eigensolver.eigenvectors() * inverted_eigenvalues * + eigensolver.eigenvectors().transpose() * gradient; + + // simple line search to guarantee a decrease in the function value + double alpha = 1.0; + double candidate_x_value; + bool improvement_found = false; + for (int i = 0; i < 10; ++i, alpha /= 2) { + Vector6d candidate_x = x - alpha * delta; + candidate_x_value = functor(candidate_x); + if (candidate_x_value < current_x_value) { + PCL_DEBUG("[estimateRigidTransformationNewton] Using stepsize=%g, function " + "value previously: %g, now: %g, " + "improvement: %g\n", + alpha, + current_x_value, + candidate_x_value, + current_x_value - candidate_x_value); + x = candidate_x; + current_x_value = candidate_x_value; + improvement_found = true; + break; + } + } + if (!improvement_found) { + PCL_DEBUG("[estimateRigidTransformationNewton] finishing because no progress\n"); + break; + } + functor.dfddf(x, gradient, hessian); + if (gradient.head<3>().norm() < translation_gradient_tolerance_ && + gradient.tail<3>().norm() < rotation_gradient_tolerance_) { + PCL_DEBUG("[estimateRigidTransformationNewton] finishing because gradient below " + "threshold: translation: %g<%g, rotation: %g<%g\n", + gradient.head<3>().norm(), + translation_gradient_tolerance_, + gradient.tail<3>().norm(), + rotation_gradient_tolerance_); + break; + } + } while (inner_iterations_ < max_inner_iterations_); + PCL_DEBUG("[estimateRigidTransformationNewton] solver finished after %i iterations " + "(of max %i)\n", + inner_iterations_, + max_inner_iterations_); + transformation_matrix.setIdentity(); + applyState(transformation_matrix, x); +} + template inline double GeneralizedIterativeClosestPoint:: @@ -377,6 +572,152 @@ GeneralizedIterativeClosestPoint:: gicp_->computeRDerivative(x, dCost_dR_T, g); } +template +inline void +GeneralizedIterativeClosestPoint:: + OptimizationFunctorWithIndices::dfddf(const Vector6d& x, + Vector6d& gradient, + Matrix6d& hessian) +{ + 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(); + Eigen::Matrix hessian_rot_tmp = Eigen::Matrix::Zero(); + + int m = static_cast(gicp_->tmp_idx_src_->size()); + for (int i = 0; i < m; ++i) { + // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp + const auto& src_idx = (*gicp_->tmp_idx_src_)[i]; + Vector4fMapConst p_src = (*gicp_->tmp_src_)[src_idx].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::Matrix3d& M = gicp_->mahalanobis(src_idx); + 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_tmp.noalias() += + Eigen::Map>{M.data()} * + (Eigen::Matrix() << p_base_src[0] * p_base_src[0], + p_base_src[0] * p_base_src[1], + p_base_src[0] * p_base_src[2], + p_base_src[1] * p_base_src[1], + p_base_src[1] * p_base_src[2], + p_base_src[2] * p_base_src[2]) + .finished(); + } + 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 + int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}}; + for (int l = 0; l < 3; ++l) { + for (int i = 0; i < 3; ++i) { + double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0; + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 3; ++k) { + phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k); + theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k); + psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k); + } + } + hessian_rot_phi(i, l) = phi_tmp; + hessian_rot_theta(i, l) = theta_tmp; + hessian_rot_psi(i, l) = psi_tmp; + } + } + 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:: From 86e89c2f5e48f12ea085899a57d87cb9cbf1f86f Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Mon, 25 Sep 2023 11:20:32 +0200 Subject: [PATCH 2/3] Fix GICP6D test M_PI/0.1 would mean 10*M_PI or 5 times 360 degrees. GICP can't reliably find the correct solution for such large rotations (it might e.g. align the object upside down). When using 0.25*M_PI (45 degrees), the transformation is the correct one, and we can even tighten the error threshold to 1e-4 --- test/registration/test_registration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 0d3b7dcdb6c..8b90cab08b7 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -593,7 +593,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) PointCloud::Ptr src_full (new PointCloud); copyPointCloud (cloud_with_color, *src_full); PointCloud::Ptr tgt_full (new PointCloud); - sampleRandomTransform (delta_transform, M_PI/0.1, .03); + sampleRandomTransform (delta_transform, 0.25*M_PI, .03); pcl::transformPointCloud (cloud_with_color, *tgt_full, delta_transform); PointCloud output; @@ -616,7 +616,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) // Register reg.align (output); EXPECT_EQ (output.size (), src->size ()); - EXPECT_LT (reg.getFitnessScore (), 0.003); + EXPECT_LT (reg.getFitnessScore (), 1e-4); // Check again, for all possible caching schemes for (int iter = 0; iter < 4; iter++) @@ -637,7 +637,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) // Register reg.align (output); EXPECT_EQ (output.size (), src->size ()); - EXPECT_LT (reg.getFitnessScore (), 0.003); + EXPECT_LT (reg.getFitnessScore (), 1e-4); } } From 39286608163487d4b5eb7ff81b7e7f3139732f0a Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Tue, 24 Oct 2023 19:39:29 +0200 Subject: [PATCH 3/3] Add test for GICP with BFGS optimizer --- test/registration/test_registration.cpp | 62 +++++++++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 8b90cab08b7..0b2d33712aa 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -585,6 +585,68 @@ TEST (PCL, GeneralizedIterativeClosestPoint) EXPECT_LT (reg.getFitnessScore (), 0.0001); } +TEST (PCL, GeneralizedIterativeClosestPointBFGS) +{ // same as above, but uses BFGS optimizer + using PointT = PointXYZ; + PointCloud::Ptr src (new PointCloud); + copyPointCloud (cloud_source, *src); + PointCloud::Ptr tgt (new PointCloud); + copyPointCloud (cloud_target, *tgt); + PointCloud output; + + GeneralizedIterativeClosestPoint reg; + reg.useBFGS (); + reg.setInputSource (src); + reg.setInputTarget (tgt); + reg.setMaximumIterations (50); + reg.setTransformationEpsilon (1e-8); + + // Register + reg.align (output); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 0.0001); + + // Check again, for all possible caching schemes + for (int iter = 0; iter < 4; iter++) + { + bool force_cache = static_cast (iter/2); + bool force_cache_reciprocal = static_cast (iter%2); + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // Ensure that, when force_cache is not set, we are robust to the wrong input + if (force_cache) + tree->setInputCloud (tgt); + reg.setSearchMethodTarget (tree, force_cache); + + pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); + if (force_cache_reciprocal) + tree_recip->setInputCloud (src); + reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); + + // Register + reg.align (output); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 0.001); + } + + // Test guess matrix + Eigen::Isometry3f transform = Eigen::Isometry3f (Eigen::AngleAxisf (0.25 * M_PI, Eigen::Vector3f::UnitX ()) + * Eigen::AngleAxisf (0.50 * M_PI, Eigen::Vector3f::UnitY ()) + * Eigen::AngleAxisf (0.33 * M_PI, Eigen::Vector3f::UnitZ ())); + transform.translation () = Eigen::Vector3f (0.1, 0.2, 0.3); + PointCloud::Ptr transformed_tgt (new PointCloud); + pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); // transformed_tgt is now a copy of tgt with a transformation matrix applied + + GeneralizedIterativeClosestPoint reg_guess; + reg_guess.useBFGS (); + reg_guess.setInputSource (src); + reg_guess.setInputTarget (transformed_tgt); + reg_guess.setMaximumIterations (50); + reg_guess.setTransformationEpsilon (1e-8); + reg_guess.align (output, transform.matrix ()); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 0.0001); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, GeneralizedIterativeClosestPoint6D) {