search

PointCloudLibrary / pcl

Point Cloud Library (PCL)
https://pointclouds.org/
Other
9.88k stars 4.61k forks source link

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

Open KotaYonezawa opened 2 years ago

KotaYonezawa commented 2 years ago

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.

mvieth commented 2 years ago

I find it difficult to image how this problem would occur in practice. It sounds like it could theoretically happen, but I am not sure which inputs specifically would lead to it. Can you provide an example (clouds and guessed transform) where NDT fails to give the correct solution due to the gimbal lock? Did you experience this already?

KotaYonezawa commented 2 years ago

Thanks for your comment! > mvieth-san. Yes, I faced this issue on my project. To explain it, I created simple example based on following data. (based on my previous post) https://github.com/PointCloudLibrary/pcl/issues/5056#issuecomment-988864151

Red is target cloud, and green is source cloud. These point clouds are created from same mesh object, but for source data, shift 1m to X and Y direction, and rotate 10 degree around Z axis, and after that, rotated 90 degrees around Y axis. And I set -90 degrees rotation around Y axis for init guess. (Note that it's gimbal lock condition for eular angles used in NDT algo.)

BaseCloud

I wrote simple code based on NDT tutorial.

  // Loading first scan of room.
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("bunny1.pcd", *target_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file bunny1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from bunny1.pcd" << std::endl;

  // Loading second scan of room from new perspective.
  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("bunny2.pcd", *input_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file bunny2.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from bunny2.pcd" << std::endl;

  // Filtering input scan to roughly 10% of original size to increase speed of registration.
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setInputCloud (input_cloud);
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from bunny2.pcd" << std::endl;

  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.000001);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (1.0);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);
  // Outlier ratio.
  ndt.setOulierRatio (0.55);

  // Setting point cloud to be aligned.
  ndt.setInputSource (filtered_cloud);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);

  // Set initial alignment estimate found using robot odometry.
  Eigen::AngleAxisf init_rotation (-90 * M_PI / 180.0, Eigen::Vector3f::UnitY ());
  Eigen::Translation3f init_translation (0, 0, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  // Calculating required rigid transform to align the input cloud to the target cloud.
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt.align (*output_cloud, init_guess);

  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // Transforming unfiltered, input cloud using found transform.
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // Saving transformed input cloud.
  pcl::io::savePCDFileASCII ("bunny2_transformed.pcd", *output_cloud);

Without my wrapper, NDT matching was not OK like as follows. NGCase

After adding my wrapper (overloading align() function), NDT matching was OK like as follows. OKCase

Point cloud data used in this test is as follows. bunny_pcddata.zip

KotaYonezawa commented 2 years ago

I additionally checked 20, 40, 60, 80 degrees around Y axis cases. (Of course, init guess was also changed for each angles.)

20 40 60 80

  // Set initial alignment estimate found using robot odometry.
  Eigen::AngleAxisf init_rotation (-80 * M_PI / 180.0, Eigen::Vector3f::UnitY ());
  ...

The result was as follows. Only 90 degrees case (without modification) was failed.

rotation angles around Y axis Matching result (without modification) Matching result (with align() overload)
20 degrees OK OK
40 degrees OK OK
60 degrees OK OK
80 degrees OK OK
90 degrees NG OK

bunny_pcddata.zip

mvieth commented 2 years ago

Sorry for the delay, and thank you for your investigations. I am still looking into the problem, but I am not so familiar with NDT, so it is taking some time. I am trying to figure out where exactly inside ndt.hpp the problem with the gimbal lock happens, I suspect in computeAngleDerivatives. Your suggestion with the align-wrapper could be a workaround, but, as you said, ideally we would find a solution so that a wrapper is not necessary. Unfortunately, we have to keep using Euler angles and can't switch to another rotation representation like Quaternions -- that would be too much work and would probably require another doctoral thesis :smile: