IterativeClosestPoint with pcl does not give expected results

Question

I got two point clouds. To match them I try to do a registration with ICP. The point cloud's are not super similar but I want to at least get them very near together.

When using IterativeClosestPoint from the pcl library this works when I use my pointCloud A as a source and pointCloud B as a target. But it doesn't work when I use B as source and A as target. In the latter case it even increases the distance between my both clouds.

Does anyone know what I am doing wrong? Why should there be a difference in performance when changing the source/target?

This is my code:

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  
icp.setInputSource(A);
icp.setInputTarget(B);
icp.setMaximumIterations(50);
icp.setTransformationEpsilon(1e-8);
icp.setEuclideanFitnessEpsilon(1);
icp.setMaxCorrespondenceDistance(0.5); // 50cm
icp.setRANSACOutlierRejectionThreshold(0.03);
icp.align(aligned_model_cloud);

I am happy for any ideas and input.

Edit: here are the two clouds Cloud A Cloud B

Update: I tried my code using Cloud A as source and Cloud A* as target. Where Cloud A* is a copy of Cloud A with just a translation on the x-axis. I did the same experiment with Cloud B and both were able to successfully converge in icp.

But as soon as I use Cloud B as source and Cloud A as target, it doesn't work anymore and converges after moving the cloud only a tiny bit (even the wrong direction). I checked the convergecriteria and found that it is CONVERGENCE_CRITERIA_REL_MSE (when transfromationEpslion is almost zero). I tried reducing the relative MSE with icp.getConvergeCriteria()->setRelativeMSE(1e-15) but this didn't succeed. When checking the value of the relativeMSE after converging I get something like this: -124034642 which doesn't make any sense at all for me.

Update2: I moved the clouds quite near together first without ICP. When doing this ICP works fine.

Update3: I am doing an FPFH for a first estimation and afterwards ICP. Doing it like this works too.

Answer 1

This question is old and OP has already found a solution, but I'll just explain in case OP and someone find it useful.

First, ICP works by iteratively estimating correspondences between the two clouds and then minimize the overall distances between them. And ICP estimates correspondences using closest point data association (hence the name Iterative Closest Point).

And as you may know, closest neigbor graph is directed. That is to say, if point A has B as its closest neighbor, point B might not have A as its closest neighbor since C is closer to B than A!

Now that ICP uses closest point data association to estimate correspondences between the two clouds, specifying A as source will get a different correspondence set from specifying B. That explains the differences you observed.

Usually the difference is small and you may not notice after ICP. But in your case, I found the two clouds you provided are too different (one is extra large and the other small) and the relation becomes too asymmetric.

If you want to ensure the result is symmetric, you can just change the data association step (PCL might provide the option to do that) to make closest point correspondences come from both cloud (and this is just a variant of the classic ICP. For more information you can see my other answer).