From 2196dbb23c407f271f65d72cdea04378f6d9c02e Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 24 Sep 2023 18:15:24 +0200 Subject: [PATCH] GICP: add Newton optimizer (as new default) --- registration/include/pcl/registration/gicp.h | 38 +++- .../include/pcl/registration/impl/gicp.hpp | 168 ++++++++++++++++++ 2 files changed, 203 insertions(+), 3 deletions(-) diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 945bf564b90..8e128e19350 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -49,8 +49,8 @@ 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 change the optimizer). * \author Nizar Sallem * \ingroup registration */ @@ -136,7 +136,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); }; } @@ -215,6 +215,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 @@ -277,6 +294,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 */ diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index c7c045741e6..579cf0e1d62 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -328,6 +328,174 @@ 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) +{ + //std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); + // 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)); + PCL_DEBUG_STREAM("transformation_matrix=" << std::endl << transformation_matrix << std::endl); + PCL_DEBUG_STREAM("x=" << std::endl << x << std::endl); + + // 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); +#if 0 +{ // measure time + Vector6d df1, df2, df3; + Matrix6d ddf; + std::chrono::steady_clock::time_point one = std::chrono::steady_clock::now(); + const double value1=functor(x); + std::chrono::steady_clock::time_point two = std::chrono::steady_clock::now(); + functor.df(x, df1); + std::chrono::steady_clock::time_point three = std::chrono::steady_clock::now(); + double value2; + functor.fdf(x, value2, df2); + std::chrono::steady_clock::time_point four = std::chrono::steady_clock::now(); + functor.dfddf(x, df3, ddf); + std::chrono::steady_clock::time_point five = std::chrono::steady_clock::now(); + std::cout << "value1=" << value1 << "\nvalue2=" << value2 << "\ndf1=" << df1 << "\ndf2=" << df2 << "\ndf3=" << df3 << "\nddf=" << ddf << std::endl; + std::cout << "f took " << std::chrono::duration_cast(two-one).count() << " microseconds" << std::endl; + std::cout << "df took " << std::chrono::duration_cast(three-two).count() << " microseconds" << std::endl; + std::cout << "fdf took " << std::chrono::duration_cast(four-three).count() << " microseconds" << std::endl; + std::cout << "dfddf took " << std::chrono::duration_cast(five-four).count() << " microseconds" << std::endl; +} +#endif + + //PCL_DEBUG_STREAM("function value is " << functor(x) << std::endl); // costly + Eigen::Matrix hessian; + Eigen::Matrix gradient; + Eigen::Matrix delta, delta2; + int inner_iterations_ = 0; + do { + ++inner_iterations_; +#if 0 + { // compare with numerical differentiation + Vector6d x1 = x;//Vector6d::Zero(); + Vector6d g1 = Vector6d::Zero(), g2 = Vector6d::Zero(); + //double function1; + const double h[6] = {4e-5, 4e-5, 4e-5, 8e-7, 8e-7, 8e-7}; + /*function1 = */functor(x1); + for (int i=0; i<6; ++i) { + Vector6d x2 = x1; + x2[i] += h[i]/2.0; + double function2 = functor(x2); + x2[i] -= h[i]; + double tmp = 1.0/h[i]; + g2[i] = tmp*(function2-functor(x2)); + //g2[i] = function2/h-functor(x2)/h; + } + functor.df(x1, g1); + PCL_DEBUG("[estimateRigidTransformationNewton] num diff gradient=(%g, %g, %g, %g, %g, %g), diff=(%g, %g, %g, %g, %g, %g)\n", g2[0], g2[1], g2[2], g2[3], g2[4], g2[5], g1[0]-g2[0], g1[1]-g2[1], g1[2]-g2[2], g1[3]-g2[3], g1[4]-g2[4], g1[5]-g2[5]); + //functor.fdf(x1, function1, g1); + + // Estimate a hessian: + Eigen::Matrix hessian = Eigen::Matrix::Zero(); + for (int i=0; i<6; ++i) { + Vector6d x2 = x1; + x2[i] += h[i]/2.0; + Vector6d ga, gb; + functor.df(x2, ga); + x2[i] = x1[i]-h[i]/2.0; + functor.df(x2, gb); + double tmp = 1.0/h[i]; + hessian.col(i) = tmp*(ga-gb); + //hessian.col(i) = tmp*ga-tmp*gb; + } + //hessian = 0.5*(hessian+hessian.transpose()); + //for (int i=0; i<5; ++i) { + // for (int j=i+1; j<6; ++j) { + // hessian(j, i) = (hessian(j, i)+hessian(i, j))/2; + // hessian(i, j) = hessian(j, i); + // } + //} + PCL_DEBUG_STREAM("num diff hessian:" << std::endl << hessian << std::endl); + Eigen::JacobiSVD> sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix delta = sv.solve(g1); + PCL_DEBUG_STREAM("Newton step based on num diff hessian:" << std::endl << delta << std::endl); + } +#endif + functor.dfddf(x, gradient, hessian); + PCL_DEBUG_STREAM("hessian" << std::endl << hessian << std::endl); + PCL_DEBUG_STREAM("gradient" << std::endl << gradient << std::endl); + { + Eigen::SelfAdjointEigenSolver> eigensolver(hessian); + PCL_DEBUG_STREAM("Eigenvalues of hessian:" << std::endl << eigensolver.eigenvalues() << std::endl); + PCL_DEBUG_STREAM("Eigenvectors of hessian:" << std::endl << eigensolver.eigenvectors() << std::endl); + 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]; // TODO is this the best thing we can do if the hessian is not positive-definite? + else + inverted_eigenvalues(i, i) = 1.0/ev; + } + delta = eigensolver.eigenvectors() * inverted_eigenvalues * eigensolver.eigenvectors().transpose() * gradient; + PCL_DEBUG_STREAM("Corrected newton step:" << std::endl << delta << std::endl); + } +#if 0 + { + Eigen::JacobiSVD> sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); + PCL_DEBUG_STREAM("sv.rank()=" << sv.rank() << ", sv.info()=" << sv.info() << ", sv.singularValues()=" << std::endl << sv.singularValues() << std::endl << "sv.matrixU()=" << std::endl << sv.matrixU() << std::endl << "sv.matrixV()" << std::endl << sv.matrixV() << std::endl); + delta2 = sv.solve(gradient); + PCL_DEBUG_STREAM("Newton step with SVD:" << std::endl << delta2 << std::endl); + } +#endif + float alpha=1.0; + const float current_x_value = functor(x); + for(int i=0; i<10; ++i, alpha/=2) { + Vector6d candidate_x = x-alpha*delta; + if(functor(candidate_x)().norm() < translation_gradient_tolerance_ && delta.tail<3>().norm() < rotation_gradient_tolerance_) break; // TODO delta or gradient? + } 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); + //std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + //std::cout << "estimateRigidTransformationNewton took " << std::chrono::duration_cast(end-start).count() << " microseconds, " << functor(x) << std::endl; +} + template inline double GeneralizedIterativeClosestPoint::