[Registration] NormalDistributionsTransform::align() can face gimbal lock depend on user specified guess argument

Describe the bug

Thanks a lot for great and useful library.

I found that NormalDistributionsTransform::align may not work correctly by gimbal lock.
It's depend on user specified "guess" argument. (the initial gross estimation of the transformation)

  inline void
  align(PointCloudSource& output, const Matrix4& guess);

Context

In NormalDistributionsTransform::computeTransformation() function in ndt.hpp,
user specified guess is converted to eular angles.

  if (guess != Eigen::Matrix4f::Identity()) {
    // Initialise final transformation to the guessed one
    final_transformation_ = guess;
    // Apply guessed transformation prior to search for neighbours
    transformPointCloud(output, output, guess);
  }

  // Initialize Point Gradient and Hessian
  point_jacobian_.setZero();
  point_jacobian_.block<3, 3>(0, 0).setIdentity();
  point_hessian_.setZero();

  Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
  eig_transformation.matrix() = final_transformation_;

  // Convert initial guess matrix to 6 element transformation vector
  Eigen::Matrix<double, 6, 1> transform, score_gradient;
  Eigen::Vector3f init_translation = eig_transformation.translation();
  Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
  transform << init_translation.cast<double>(), init_rotation.cast<double>();

So, if the user specified "guess" includes rotation near to gimbal lock condition,
the NDT algorithm can work incorrectly by gimbal lock.

I checked Dr. M.Magnusson's paper, in 6.2.2.

In the following, 3D Euler angles will be used, in spite of the potential problems associated with this rotation representation. The advantages — no constraint required for the numerical optimisation procedure, and slightly less complicated derivatives — are assessed to outweigh the risk of gimbal lock, which would only occur at such large angles that the local registration procedure would most likely fail anyway.

I think this part means that eular angles should be used to specify rotation from guessed pose to converged pose.
But in current implementation, eular angles specifies rotation from current input cloud pose to converged pose.

So, in current implementation, the user should take care about guess to avoid gimbal lock.
But many users will not understand it, and may face unexpected behavior by this issue.
Some of user will be very difficult to solve it, I supposed.

Now I implemented following "wrapper" function to avoid gimbal lock regardless of guess.
(Overloading align function.)

template <typename PointSource, typename PointTarget>
void
NormalDistributionsTransform<PointSource, PointTarget>::align(PointCloudSource& output,
                                                              const Matrix4& guess)
{
  PointCloudSourceConstPtr input_org;
  PointCloudSourcePtr input_replaced(new PointCloudSource);

  input_org = getInputSource();
  transformPointCloud(*input_org, *input_replaced, guess);

  setInputSource(input_replaced);
  Registration<PointSource, PointTarget>::align(output);

  final_transformation_ = final_transformation_ * guess;

  // Re-orthogonalization of Rotation Matrix
  // "3D rotations : parameter computation and Lie-Algebra based optimization", 4.5.
  Eigen::JacobiSVD<Eigen::Matrix3f> svd(final_transformation_.block<3, 3>(0, 0),
    Eigen::ComputeFullU | Eigen::ComputeFullV);
  const Eigen::Matrix3f& U = svd.matrixU();
  const Eigen::Matrix3f& V = svd.matrixV();
  Eigen::Matrix3f S = Eigen::Matrix3f::Identity();
  S(2, 2) = (U * V).determinant();
  final_transformation_.block<3, 3>(0, 0) = U * S * V.transpose();
  final_transformation_.block<1, 3>(3, 0).setZero();
  final_transformation_(3, 3) = 1.0f;

  setInputSource(input_org);
}

But I think it's temporal, not final solution.
If possible, it's better to fix in ndt.hpp algorithm internally...
What do you think?

Expected behavior

I think the user should be able to specify "guess" argument without any restriction.

Your Environment (please complete the following information):

OS: Windows10
Compiler: Compiler: Visual Studio 2019 Build Tools
PCL Version: HEAD

Possible Solution

As above, I implemented wrapper solution as temporal.
But I think it's better to fix in algorithm internal implementation in ndt.hpp if possible.

Start in ndt.hpp at NormalDistributionsTransform::align() and computeTransformation(), especially the conversion of final_transformation_ to Euler angles with Eigen. Reproduce alignment with a guess near gimbal lock and compare it with the reported wrapper approach. Done means a user can provide an unrestricted guess without incorrect alignment caused by the internal rotation representation.