Add a part of the new rigid registration method [FRICP] by yaoyx689 · Pull Request #6199 · PointCloudLibrary/pcl

yaoyx689 · 2024-12-11T12:09:12Z

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 · 2024-12-16T15:54:32Z

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 · 2024-12-17T19:16:49Z

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 · 2024-12-18T07:15:28Z

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.

duanbotu123 · 2025-11-11T12:42:13Z

Hello,

We have recently added a subclass FastRobustIterativeClosestPoint in fricp.hpp, which inherits from IterativeClosestPoint and implements our new method. The techniques used to improve speed and robustness are implemented respectively in anderson_acceleration.h and TransformationEstimationPointToPointRobust.h.

However, our PR is currently failing the automated checks because the server used for code verification runs out of disk space. Specifically, we are seeing the following error:

failed to register layer: write /usr/share/proj/CHENYX06_etrs.gsb: no space left on device

It seems that #6364encountered a similar issue before. Could you please clean up the disk space on the server corresponding to the Ubuntu 24.04 GCC build environment so that our PR can proceed successfully?

Thank you very much for your help.

mvieth · 2025-11-22T09:39:41Z

Hi, thanks for adding the new class. I will take a closer look and test your changes as soon as I have time, but that might take a few weeks.

mvieth

Hello, I have finally found some time to test your code in detail. With several point clouds, FRICP was indeed able to find a more accurate alignment than other registration algorithms. So I think it would be great to have it in PCL, but first, here are a few review comments I would ask you to address. Thank you!

mvieth · 2026-03-18T12:34:36Z

registration/include/pcl/registration/impl/fricp.hpp

+    updateCorrespondences(const Matrix4d& transform,
+                          const Matrix3Xd& source,
+                          const Matrix3Xd& target,
+                          pcl::search::KdTree<pcl::PointXYZ>& tree,


Suggested change

pcl::search::KdTree<pcl::PointXYZ>& tree,

pcl::search::Search<pcl::PointXYZ>& tree,

mvieth · 2026-03-21T10:44:18Z

registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h

+   * \ingroup common
+   */
+  inline unsigned int
+  computeWeighted3DCentroid(ConstCloudIterator<PointSource>& cloud_iterator,


Using PointSource here is a problem if PointSource and PointTarget are not the same. Please add a template parameter to this function. computeWeighted3DCentroid could also be moved out of the TransformationEstimationPointToPointRobust class and be a free function instead.

mvieth · 2026-03-21T10:46:24Z

registration/src/fricp.cpp

@@ -0,0 +1,37 @@
+/*


I think having these .cpp files that only include the .h files are not really necessary?

mvieth · 2026-03-21T10:55:00Z

registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h

+                              Matrix4& transformation_matrix) const override;
+
+  void
+  set_sigma(Scalar sigma)


Please add some documentation to this method, for example: what does this parameter do (short description), which values are allowed (positive/negative?), does the user have to call this function or is there a good default (I think there is a good default value, right?)

mvieth · 2026-03-21T11:02:38Z

registration/include/pcl/registration/impl/fricp.hpp

+
+template <typename PointSource, typename PointTarget, typename Scalar>
+double
+FastRobustIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeMedian(


PCL already has a computeMedian function in common.h: https://pointclouds.org/documentation/namespacepcl.html#aba79ef8875ca3f1085f257ca1530fffe Can you use that one?

mvieth · 2026-03-21T11:07:59Z

registration/include/pcl/registration/fricp.h

+  void
+  setDynamicWelschBeginRatio(double ratio);
+
+  void
+  setDynamicWelschEndRatio(double ratio);
+
+  void
+  setDynamicWelschDecay(double ratio);


Please add some documentation to these functions, e.g. what effect does it have when they are set to a smaller/larger value than before?

mvieth · 2026-03-21T11:21:06Z

registration/include/pcl/registration/fricp.h

+  matrixLog(const Matrix4d& transform) const;
+
+  RobustFunction robust_function_;
+  bool use_anderson_ = true;


I have tested several point clouds with and without Anderson acceleration enabled, and I have found that when it is enabled, the registration progress is often rather unpredictable and chaotic. Therefore I would rather make this option false by default and users can opt-in.

mvieth · 2026-03-21T15:57:55Z

registration/include/pcl/registration/impl/fricp.hpp

@@ -0,0 +1,581 @@
+/*


Please use the shorter license text as described here: https://pcl.readthedocs.io/projects/tutorials/en/latest/writing_new_classes.html#licenses

yaoyx689 added 2 commits December 11, 2024 19:36

add a part of the FRICP (robust transformation estimation)

aefe5db

update

213e84f

mvieth added changelog: new feature Meta-information for changelog generation module: registration labels Dec 16, 2024

Correct format

f2f6dea

duanbotu123 added 7 commits November 9, 2025 15:54

Merge branch 'PointCloudLibrary:master' into add_fricp

f5d0bda

add Anderson acceleration fricp.hpp etc.UPdate cmakelist

7c81280

fix bug

d87a99e

fix bug

574f036

fix bug in test

266ab88

fix the bug of parameter

b1a1a7b

Update test

d329abf

duanbotu123 added 8 commits November 17, 2025 08:49

repush

3f9a439

Merge branch 'PointCloudLibrary:master' into add_fricp

3d70839

fix bug in clang-tidy,merge the new update in pcl

8efff92

fix bug in clang-tidy,merge the new update in pcl

3fc1db5

fix bug in clang-tidy again

34d5dc3

fix bug in clang-tidy again

e8acb9f

fix bug in clang-tidy again

d5bdecc

fix bug in clang-tidy again

ee430dc

mvieth reviewed Mar 21, 2026

View reviewed changes

duanbotu123 added 3 commits March 22, 2026 09:00

Merge branch 'PointCloudLibrary:master' into add_fricp

0fe5fdc

Address all review comments in PR 6199

08d883c

Fix FRICP KdTree/Search mismatch in test_registration_api

397b7f3

	pcl::search::KdTree<pcl::PointXYZ>& tree,
	pcl::search::Search<pcl::PointXYZ>& tree,

Uh oh!

Conversation

yaoyx689 commented Dec 11, 2024

Uh oh!

mvieth commented Dec 16, 2024

Uh oh!

larshg commented Dec 17, 2024

Uh oh!

yaoyx689 commented Dec 18, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

duanbotu123 commented Nov 11, 2025

Uh oh!

mvieth commented Nov 22, 2025

Uh oh!

mvieth left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

yaoyx689 commented Dec 18, 2024 •

edited

Loading