Skip to content

Commit

Permalink
GICP: add Newton optimizer (as new default)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mvieth
mvieth committed Sep 24, 2023
1 parent dd70aab commit 2196dbb
Show file tree
Hide file tree
Showing 2 changed files with 203 additions and 3 deletions.
38 changes: 35 additions & 3 deletions registration/include/pcl/registration/gicp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand Down Expand Up @@ -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);
};
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
*/
Expand Down
168 changes: 168 additions & 0 deletions registration/include/pcl/registration/impl/gicp.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -328,6 +328,174 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
"solver didn't converge!");
}

template <typename PointSource, typename PointTarget, typename Scalar>
void
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
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<double>(1.0, std::max<double>(-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<std::chrono::microseconds>(two-one).count() << " microseconds" << std::endl;
std::cout << "df took " << std::chrono::duration_cast<std::chrono::microseconds>(three-two).count() << " microseconds" << std::endl;
std::cout << "fdf took " << std::chrono::duration_cast<std::chrono::microseconds>(four-three).count() << " microseconds" << std::endl;
std::cout << "dfddf took " << std::chrono::duration_cast<std::chrono::microseconds>(five-four).count() << " microseconds" << std::endl;
}
#endif

//PCL_DEBUG_STREAM("function value is " << functor(x) << std::endl); // costly
Eigen::Matrix<double, 6, 6> hessian;
Eigen::Matrix<double, 6, 1> gradient;
Eigen::Matrix<double, 6, 1> 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<double, 6, 6> hessian = Eigen::Matrix<double, 6, 6>::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<Eigen::Matrix<double, 6, 6>> sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<double, 6, 1> 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<Eigen::Matrix<double, 6, 6>> 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<double, 6, 6> inverted_eigenvalues = Eigen::Matrix<double, 6, 6>::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<Eigen::Matrix<double, 6, 6>> 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)<current_x_value || i==9) {
PCL_DEBUG("Using alpha=%g\n", alpha);
x = candidate_x;
break;
}
}
//x -= delta;
//PCL_INFO_STREAM("new x:" << std::endl << x << std::endl);
//PCL_INFO_STREAM(functor(x) << " is the new function value" << std::endl); // costly
if(delta.head<3>().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<std::chrono::microseconds>(end-start).count() << " microseconds, " << functor(x) << std::endl;
}

template <typename PointSource, typename PointTarget, typename Scalar>
inline double
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
Expand Down

0 comments on commit 2196dbb

Please sign in to comment.