[registration] icp with TransformationEstimationPointToPlane can not give right result.

[QiuYilin]

If icp uses TransformationEstimationPointToPlane as the transformation estimation method, meaningless results will be obtained

I am testing to compare the performance of different ICP methods.

Currently, PCL has an ICPWithNormals type, which is based on the PointToPlane principle of LLS. There are also two other transformation estimation methods based on PointToPlane that are based on LM, but I tried these two methods and could not get meaningful results , only an identity matrix.

My code:

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>

#include <iostream>

int main(int argc, char **argv) {
  pcl::PointCloud<pcl::PointNormal>::Ptr source_cloud(
      new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointNormal>::Ptr target_cloud(
      new pcl::PointCloud<pcl::PointNormal>);

  pcl::io::loadPCDFile<pcl::PointNormal>("D:\\program_on_git\\own\\test\\pcl_test\\source\\normalcloud1.pcd", *source_cloud);
  pcl::io::loadPCDFile<pcl::PointNormal>("D:\\program_on_git\\own\\test\\pcl_test\\source\\normalcloud2.pcd", *target_cloud) ;



  pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
  icp.setInputSource(source_cloud);
  icp.setInputTarget(target_cloud);

  icp.setTransformationEstimation(std::make_shared<pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal>>());

  std::cout << "transformation epsilon: " << icp.getTransformationEpsilon()
            << std::endl;
  std::cout << "euclidean fitness epsilon: " << icp.getEuclideanFitnessEpsilon()
            << std::endl;
  pcl::PointCloud<pcl::PointNormal> Final;
  icp.align(Final);

  std::cout << "has converged:" << icp.hasConverged() << std::endl;
  std::cout << "score: " << icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;

  return 0;
}

my file: https://ufile.io/f/vpq0a

result:

transformation epsilon: 0
euclidean fitness epsilon: -1.79769e+308
has converged:1
score: 24.1359
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

Your Environment (please complete the following information):

OS: Windows 10
Compiler: MSVC 2022
PCL Version 1.14.1.99(2d5101a)
GPU:1060 6GB

[mvieth]

You can enable more logging by adding pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);. This might give you more insight into what is happening.
Interestingly, for me your test program does not return the identity transformation:

has converged:1
score: 1.7481
    0.999993    0.0038051 -0.000476987      6.11586
  -0.0038055     0.999992 -0.000828988      10.9821
 0.000473829  0.000830797            1    -0.728087
           0            0            0            1

However in the second iteration of the transformation estimation, something seems to go wrong:

[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 4, having a residual norm of nan. 
Final solution: [0.000000 0.000000 0.000000 0.000000 0.000000 0.000000]

I will see if I can find out more.

[QiuYilin]

I failed on my first iteration.

[pcl::PCDReader::read] Loaded D:\program_on_git\own\test\pcl_test\source\windfield0_sample_binary.pcd as a non-dense cloud in 35.0415 ms with 104452 points. Available dimensions: x y z normal_x normal_y normal_z curvature.
[pcl::PCDReader::read] Loaded D:\program_on_git\own\test\pcl_test\source\windfield1_sample_binary.pcd as a non-dense cloud in 32.6549 ms with 104452 points. Available dimensions: x y z normal_x normal_y normal_z curvature.
4
FLANN is not optimized for current index type. Will incur extra allocations and copy
transformation epsilon: 0
euclidean fitness epsilon: -1.79769e+308
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 4, having a residual norm of nan. 
Final solution: [0.000000 0.000000 0.000000 0.000000 0.000000 0.000000]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 1 out of 10.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 1.000000 rotation (cosine) and 0.000000 translation.
Transformation is:
	1.000000	0.000000	0.000000	0.000000
	0.000000	1.000000	0.000000	0.000000
	0.000000	0.000000	1.000000	0.000000
	0.000000	0.000000	0.000000	1.000000

[mvieth]

@QiuYilin Apparently, there are three points in both the source and the target cloud where xyz are valid, but the normal is invalid/nan. How did you estimate the normals?

[QiuYilin]

@mvieth gpu version normalestimation #6055

parameter: radius 2; max results 100; view point(1000,1000,10000)

point xyz pcd: https://ufile.io/7c83dt1t

So, there is also a difference in the logic between PointToPlaneLM and PointToPlaneLLS. PointToPlaneLLS will ignore invalid values ​​while PointToPlaneLM will not?

[mvieth]

I checked the three points, they are kind of isolated. So they did not have enough neighbours within the radius to estimate the normal. You could remove these three invalid points, then the PointToPlaneLM should work correctly. By the way, I have been working on making PointToPlaneLM faster, but I am not sure yet when that will be ready for a pull request.
Regarding PointToPlaneLLS, I haven't really worked with that method, so I can't say how well it handles invalid normals.

[QiuYilin]

So how should the program handle point cloud data with valid xyz values ​​but invalid normal values? The RemoveNanFromPoint method will only remove invalid xyz points. Should it directly treat such point clouds as corrupted data?

[mvieth]

You can use pcl::isNormalFinite to test if a point has a valid normal. However perhaps pcl::isFinite should actually also check the normal, if the point type has a normal. I will think about whether this would make sense.