Add a part of the new rigid registration method [FRICP]

[yaoyx689]

Hi, thanks for the suggestions of the contributor (#6061 (comment)), I added a class, TransformationEstimationPointToPointRobust, which contains the implementation of the robust ICP part of the FRICP (https://github.com/yaoyx689/Fast-Robust-ICP).

Compared with TransformationEstimation, I added a parameter setting function, because its adaptive setting is very important for the results. If there is no additional setting for the parameter, I define a default setting.

If possible, I hope to add a FastRobustIterativeClosestPoint class later, similar to IterativeClosestPoint, to perform adaptive parameter adjustment and pass it into the TransformationEstimationPointToPointRobust class.

Furthermore, for Anderson acceleration, it is not enough to just create an accelerated class of TransformationEstimation. The iterative process needs to be modified. If possible, these parts can also be organized in FastRobustIterativeClosestPoint.

[mvieth]

Hello, thanks for the pull request. A few things:

• Please make sure the formatting check passes. Formatting is done by clang-format-14 according to the rules in the .clang-format. You can use the script in .dev/format.sh, or run make format (at least on Linux, not sure if this also works on Windows or macOS)
• Some sort of test is necessary, to make sure the new class runs as expected. I think adding it in test/registration/test_registration_api.cpp would make sense. There are some tests for other transformation estimation methods you can use as inspiration.
• It has been some time since I read the paper: in which situations is the new estimation method better than reference methods? Basically, we have to make sure that the new method has a clear advantage over the other methods that are already implemented in PCL. Otherwise it would not make sense to add another method.

Compared with TransformationEstimation, I added a parameter setting function, because its adaptive setting is very important for the results. If there is no additional setting for the parameter, I define a default setting.

I assume you mean sigma. Do you have an estimate how sensitive the method is to this parameter? Does the default setting work okay in most situations, or does the user have to fine-tune sigma to their data?

If possible, I hope to add a FastRobustIterativeClosestPoint class later, similar to IterativeClosestPoint, to perform adaptive parameter adjustment and pass it into the TransformationEstimationPointToPointRobust class.

Furthermore, for Anderson acceleration, it is not enough to just create an accelerated class of TransformationEstimation. The iterative process needs to be modified. If possible, these parts can also be organized in FastRobustIterativeClosestPoint.

I am not completely sure what you mean with this. Would you have to duplicate code from this pull request to FastRobustIterativeClosestPoint? Or would you use TransformationEstimationPointToPointRobust in FastRobustIterativeClosestPoint? Code duplication needs to be avoided.

[larshg]

Please update the license as per https://pcl.readthedocs.io/projects/tutorials/en/latest/writing_new_classes.html#licenses and use the #pragma once instead of #ifdef xxx...

[yaoyx689]

Thanks for your reply. I will modify them as you said.

• It has been some time since I read the paper: in which situations is the new estimation method better than reference methods? Basically, we have to make sure that the new method has a clear advantage over the other methods that are already implemented in PCL. Otherwise it would not make sense to add another method.

In cases where the two point clouds used for registration have partial overlap or noise, our robust ICP algorithm can achieve higher accuracy compared to traditional ICP algorithms. On one hand, this is due to using the Welsch function instead of the L2-norm as the objective function. It reduces the impact of outliers, which is similar to the idea that ICP uses a distance threshold to filter outlier points, but it can achieve better results by giving each correspondence a different threshold instead of just 0,1.

On the other hand, the Welsch function can provide a coarse-to-fine result by adjusting parameters (sigma), leading to higher accuracy.

I assume you mean sigma. Do you have an estimate how sensitive the method is to this parameter? Does the default setting work okay in most situations, or does the user have to fine-tune sigma to their data?

By default, it will be set based on the maximum distance between corresponding points. This means it has the same effect as setting setMaxCorrespondenceDistance in the IterativeClosestPoint class that the user defines.
In this case, the new method performs slightly better than ICP, and it exhibits the same level of sensitivity to parameters as ICP.

I am not completely sure what you mean with this. Would you have to duplicate code from this pull request to FastRobustIterativeClosestPoint? Or would you use TransformationEstimationPointToPointRobust in FastRobustIterativeClosestPoint? Code duplication needs to be avoided.

In our method, an important part is that the parameters of the Welsch function can be adaptively adjusted. During the iteration process, the parameter sigma gradually decreases as the number of iterations increases. This adjustment plays a significant role in improving accuracy. Therefore, if possible, I would like to introduce a new class called FastRobustIterativeClosestPoint (It uses TransformationEstimationPointToPointRobust), which implements the process of adaptive parameter adjustment based on the positions of the point clouds. It does not require additional user settings. In our experiments, this method significantly enhances the accuracy of the ICP algorithm.