Pcl: ICP Transformation Matrix Accuracy Varies Based on YAW

[rwbot]

Good day to all,

I'm having difficulty getting the correct transformation matrix from ICP. I created a ROS node with the purpose of matching a target cloud to an input cloud of a polygonal shape. I'm currently working completely in simulation (Gazebo). I have a robot with a LaserScan as input, which I then convert into a sensor_msgs::PointCloud2 message. All the point clouds used are 2D. I obtained the target cloud by moving the robot a few centimeters from the shape and used rosrun pcl_ros pointcloud_to_pcd to capture the pointcloud into a PCD. The input cloud for ICP is the most recent 2D cloud published by the robot.

As shown below in the GIF and video, the pink ICP aligned cloud seems decently aligned with the blue target cloud so ICP _seems to be working_. But as shown by the Transform coordinate marker and ICP pose output (pink), the coordinate values in the transformation matrix returned by the ICP object don't align with the Gazebo ground truth coordinate values (white).

• When the YAW is zero, the transformation matrix gives very good estimates for the X and YAW values, however, the Y value seems to be the wrong sign.
• When the YAW is non-zero, the YAW output is mostly correct, however, the X and Y values become oddly warped.

It's very confusing because as I mentioned before, no matter the pose, the ICP aligned cloud (pink) is almost always in sync with the target cloud (blue) which suggests ICP is working, but somewhere in the process, the actual transformation matrix seems to be modified.

Here is a GIF of the visualization. If you prefer, here is the full video version with the ICP transformation matrix output in the terminal.

INPUT cloud is orange, TARGET cloud is blue, ICP aligned cloud is pink.

Here is the relevant code used:

///////////////// BEGIN ICP2D /////////////////

bool ICP2D(pcl::PointCloud<pcl::PointXYZRGB>::Ptr inputCloudPtr, 
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPCLPtr, 
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr outCloudPtr,
 {

  // ICP object.
  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> registration;
  Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());

  pcl::registration::WarpPointRigid3D<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr warp_fcn
        (new pcl::registration::WarpPointRigid3D<pcl::PointXYZRGB, pcl::PointXYZRGB>);

  // Create a TransformationEstimationLM object, and set the warp to it
  pcl::registration::TransformationEstimationLM<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr te 
        (new pcl::registration::TransformationEstimationLM<pcl::PointXYZRGB, pcl::PointXYZRGB>);
  te->setWarpFunction (warp_fcn);

  // Pass the TransformationEstimation objec to the ICP algorithm
  registration->setTransformationEstimation (te);


  registration->setInputSource(inputCloudPtr);
  registration->setInputTarget(targetPCLPtr);

  registration->align(*outCloudPtr);
  ROS_INFO_STREAM("ICP2D--CHECKING CONVERGENCE");
  if (registration->hasConverged())
  {
    t *= registration->getFinalTransformation ();
    std::cout << "ICP2D converged." << std::endl
          << "The score is " << registration->getFitnessScore() << std::endl;
    std::cout << "Transformation matrix:" << std::endl;
    std::cout << registration->getFinalTransformation() << std::endl;
    return true;
  }
  else
  {
    std::cout << "ICP2D did not converge." << std::endl;
    return false;
  }

}
///////////////// END ICP2D /////////////////

Environment

• Operating System and version: Ubuntu 18
• PCL Version: pcl 1.9
• ROS Version: Melodic

[rwbot]

@kunaltyagi @taketwo @SergioRAgostinho @shrijitsingh99
I'm using PCL for my capstone project, and the libraries been a huge help! But troubleshooting this problem has put me 2 weeks behind schedule. Is there any way you could give me an estimate as to when this bug will be addressed? If not, could you direct me to someone who could help with fixing and/or troubleshooting this bug? Any help is appreciated!

[SergioRAgostinho]

Is there any way you could give me an estimate as to when this bug will be addressed?

It likely won't in a meaningful time frame for you. GSoC administrative tasks are taking most of my/our time at the moment and following that there's 1.10.1 that needs to come out.

It would speed up debugging by providing a MVCE (cpp, cmake and data). PCL should be the only 3rd party dependency.

[shrijitsingh99]

@rwbot I am not a maintainer, so not sure if I will be able to solve your issue.
As mentioned by @SergioRAgostinho, please provide a reproducible example, after that, I can try helping you out.

[kunaltyagi]

The issue might be of frame transformation. Since you mentioned that the sign of Y is wrong, this seems like the best avenue to proceed for debugging.

If you could create a MCVE where you know expected and actual (wrapped) values, then it'll help a lot.

[rwbot]

@kunaltyagi @taketwo @SergioRAgostinho @shrijitsingh99
I created a MCVE using the Interactive PCL tutorial, as the same issue happens even when using the tutorial code. I made a repo with example output for your convenience:

https://github.com/rwbot/icp_tf_matrix_varies_with_yaw

[kunaltyagi]

Thanks a lot @rwbot, that was lovely.

The output Transformation Matrix from ICP seems to vary based on the initial YAW of the target

No, the transformation matrix is correct. As I suspected, the frame is different than what you'd expect. You transform cloud_in by [[Rot, T][0, 1]] to get cloud_icp. The align method will transform cloud_icp back to cloud_in (based on the source and target setting) giving you [[Rot, T]][0,1]]^{-1} aka the inverse of your matrix. You can verify that the ICP output is correct using numpy:

>>> rot_mat = np.array([[0.878, -0.479, 0., 1.],[0.479, 0.878, 0., 1.],[0., 0., 1., 1.],[0., 0., 0., 1.]])
>>> np.linalg.inv(mat)

So far so good. This implies that your usage is incorrect somewhere. The matrix is correct since you expect to find "What transformation will convert the modified cloud to my original cloud". Maybe you should flip the source and target to instead get "What transformation was used to convert my original cloud to the modified cloud".

PS: No need to tag repeatedly. Github sends notifications every-time once we participate unless someone opts-out

[rwbot]

Thanks a lot @rwbot, that was lovely.

The output Transformation Matrix from ICP seems to vary based on the initial YAW of the target

No, the transformation matrix is correct. As I suspected, the frame is different than what you'd expect. You transform cloud_in by [[Rot, T][0, 1]] to get cloud_icp. The align method will transform cloud_icp back to cloud_in (based on the source and target setting) giving you [[Rot, T]][0,1]]^{-1} aka the inverse of your matrix.

You were absolutely right. After verifying with numpy, I had to brush up on inverting matrices to make sense of it. Thank you for your help!