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.

[duanbotu123]

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]

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!

[duanbotu123]

Thanks for the detailed review — I’ve addressed all actionable comments from your latest review.
Implemented updates:

• Switched updateCorrespondences to use pcl::search::Search<pcl::PointXYZ>&.
• Kept findKNearestMedian on KdTree and fixed the call site to pass the concrete tree_data object (this also resolves the test_registration_api compile error).
• Made computeWeighted3DCentroid templated on PointT and updated the implementation signature accordingly.
• Removed redundant registration/src/fricp.cpp and its CMake entry.
• Replaced local median helpers with pcl::computeMedian.
• Added documentation for the behavior-impact setters (including set_sigma semantics).
• Set Anderson acceleration default to disabled.
• Updated the file header to the short SPDX-style license text.

[larshg]

AI suggestions.patch

AI had suggestions to use () instead of [] at some eigen indexing.
Also change some types to Eigen::Index in for loops.
Lastly save the residuals as squared and update other methods accordingly, to avoid using sqrt and resquare again.

[mvieth]

Can you add the files to this list:
https://github.com/PointCloudLibrary/pcl/blob/master/.dev/whitelist.txt

And ensure clang-format is run on the files.

The whole registration module is already in that list, so clang-format should already be run on those files.

AI suggestions.patch

Out of interest, which LLM did you use?

AI had suggestions to use () instead of [] at some eigen indexing.

Not sure where the difference/advantage is?

Also change some types to Eigen::Index in for loops.
Lastly save the residuals as squared and update other methods accordingly, to avoid using sqrt and resquare again.

[larshg]

Can you add the files to this list:
https://github.com/PointCloudLibrary/pcl/blob/master/.dev/whitelist.txt
And ensure clang-format is run on the files.

The whole registration module is already in that list, so clang-format should already be run on those files.

Ahh, doh. I just thought the indexing of the method after :: was odd, but it seemingly is so.

AI suggestions.patch

Out of interest, which LLM did you use?

Copilot - but what specific model, unsure. Either GPT 5 mini, Haiku 4.5 or GPT 4.1.

AI had suggestions to use () instead of [] at some eigen indexing.

Not sure where the difference/advantage is?

AIs own reply when I asked about the same.

Short answer: use residuals(i) because () is the canonical, unambiguous Eigen element-access operator and is preferred for matrices/vectors and expression-proxy types.
Why:
• Eigen documentation and code style use () for matrix/vector element access; it works uniformly for both matrices and vectors.
• () interacts correctly with Eigen expression templates and proxy objects; [] can be ambiguous or absent for some Eigen types/versions.
• () is clearer and more consistent with other Eigen code in PCL, improving readability and future compatibility.
• No measurable performance difference in optimized builds.
Recommendation: replace residuals[i] with residuals(i) for clarity and robustness.

Also change some types to Eigen::Index in for loops.
Lastly save the residuals as squared and update other methods accordingly, to avoid using sqrt and resquare again.

[duanbotu123]

Thanks for the review and suggestions. I’ll update the PR accordingly, including the index type changes, Eigen access style cleanup, and the residual handling improvements.

[larshg]

I just asked GPT 5.3-codes - and it came up with the following issues(discussions):

Findings (ordered by severity)

High: FRICP ignores max correspondence distance from the Registration/ICP API.
The base API exposes setMaxCorrespondenceDistance/getMaxCorrespondenceDistance in [registration.h:336], but FRICP correspondence update always takes the nearest neighbor without threshold gating in [fricp.hpp:379].
Impact: users can set a distance threshold and see no effect, which is a contract mismatch and can hurt robustness with outliers.

High: max iterations can exceed user expectation by a lot.
FRICP runs a nested schedule: outer robust loop plus inner loop up to max_iterations each time in [fricp.hpp:239], [fricp.hpp:240], and decays nu in [fricp.hpp:310].
Impact: setMaximumIterations behaves like per-stage iterations, not total iterations, unlike common PCL expectations.

High: several inherited ICP controls appear effectively bypassed.
ICP uses convergence criteria wiring in [icp.hpp:157], [icp.hpp:158], [icp.hpp:159], [icp.hpp:161], [icp.hpp:238], plus reciprocal/rejector paths. FRICP starts with initComputeReciprocal in [fricp.hpp:122] but does not use the usual correspondence estimation/rejector infrastructure.
Impact: knobs like reciprocal correspondences, rejectors, euclidean fitness epsilon, rotation epsilon may not influence behavior as users expect from ICP-derived classes.

Medium: NaN/Inf handling is missing in source/target ingestion.
FRICP directly maps point coordinates in [fricp.hpp:165] and [fricp.hpp:173] with no finite-value checks.
Impact: non-finite points can poison means/residuals and destabilize or fail correspondence search.

Medium: setDynamicWelschDecay documentation and behavior disagree at boundary 0.
Header says valid range is [0, 1] in [fricp.h:102], but implementation maps very small values (including 0) back to 0.5 in [fricp.hpp:110] and [fricp.hpp:113].
Impact: surprising user behavior and harder parameter tuning.

Low (style/API consistency): class is missing common Ptr/ConstPtr aliases seen in most PCL registration classes.
FRICP class declaration at [fricp.h:45]; compare with ICP aliases in [icp.h:113].
Impact: minor ergonomics inconsistency with PCL conventions.

Low (cleanliness): redundant initialization of nu_end_ratio_.
Member default exists in [fricp.h:161] and is also set in constructor init-list in [fricp.hpp:33].
Impact: harmless but slightly noisy.

Overall style assessment

Structure, naming, docs style, and formatting are generally PCL-like and clean. The bigger issues are not formatting style, but API-contract consistency with ICP/Registration behavior.

Concrete improvements I recommend

Enforce correspondence distance threshold in updateCorrespondences, using corr_dist_threshold_ semantics (or clearly document FRICP-specific alternative).
Reinterpret max_iterations as a total budget across robust stages, or add a new parameter (for example robust_stages) and keep max_iterations total for compatibility.
Either integrate ICP convergence criteria/rejector paths, or explicitly document unsupported inherited knobs and consider disabling/hiding them for FRICP.
Add finite-point filtering (or sanitize to selected valid indices) before matrix assembly.
Fix setDynamicWelschDecay contract: either allow 0 as true edge case, or update docs and clamp to a documented minimum > 0.
Add Ptr/ConstPtr aliases and optional getters for dynamic Welsch parameters for API symmetry.
Replace Anderson dimension magic number 16 with a named constant and brief comment in [fricp.hpp:233].
Tests worth adding

A regression test that setMaxCorrespondenceDistance changes result/failure behavior for FRICP.
A test that max iteration budget is respected globally.
A NaN-containing cloud test to verify deterministic handling.
A parameter-boundary test for setDynamicWelschDecay(0.0).

I haven't reviewed them closely, but I think some of them should be looked at.

[larshg]

I have tested in a program that I'm currently working on and it works really well, so its a nice addition btw 👍

[duanbotu123]

The clang-tidy warning in concave_hull.cpp is a pre‑existing issue in the codebase and might be addressed separately.

[mvieth]

I haven't reviewed them closely, but I think some of them should be looked at.

@larshg So which of these points do you think make sense, and which ones do you think we should ignore (or postpone)?

For example I think "FRICP ignores max correspondence distance from the Registration/ICP API" -- this is fine because FRICP has its own mechanism of rejecting correspondences with high distance. At most, we could consider adding a note in the documentation or warning if the max correspondence distance has been set to a non-default value.

[mvieth]

The clang-tidy warning in concave_hull.cpp is a pre‑existing issue in the codebase and might be addressed separately.

I see no warning in concave_hull.cpp, but I see the following:

/__w/pcl/pcl/registration/include/pcl/registration/impl/fricp.hpp:470:3: error: use range-based for loop instead [modernize-loop-convert,-warnings-as-errors]
Suppressed 13227 warnings (13215 in non-user code, 12 NOLINT).
  470 |   for (std::size_t i = 0; i < cloud.size(); ++i) {
Use -header-filter=.* to display errors from all non-system headers. Use -system-headers to display errors from system headers as well.
      |   ^   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1 warning treated as error
      |       (auto i : cloud)
  471 |     if (tree.nearestKSearch(cloud[i], k, nn_indices, nn_sqr_dists) != k)
      |                             ~~~~~~~~
      |                             i

[duanbotu123]

The clang-tidy warning in concave_hull.cpp is a pre‑existing issue in the codebase and might be addressed separately.

I see no warning in concave_hull.cpp, but I see the following:

/__w/pcl/pcl/registration/include/pcl/registration/impl/fricp.hpp:470:3: error: use range-based for loop instead [modernize-loop-convert,-warnings-as-errors]
Suppressed 13227 warnings (13215 in non-user code, 12 NOLINT).
  470 |   for (std::size_t i = 0; i < cloud.size(); ++i) {
Use -header-filter=.* to display errors from all non-system headers. Use -system-headers to display errors from system headers as well.
      |   ^   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1 warning treated as error
      |       (auto i : cloud)
  471 |     if (tree.nearestKSearch(cloud[i], k, nn_indices, nn_sqr_dists) != k)
      |                             ~~~~~~~~
      |                             i

My apologies — Copilot gave me an incorrect explanation. I correct this issue and submit it. Sorry for the confusion and the trouble caused.

[larshg]

I haven't reviewed them closely, but I think some of them should be looked at.

@larshg So which of these points do you think make sense, and which ones do you think we should ignore (or postpone)?

For example I think "FRICP ignores max correspondence distance from the Registration/ICP API" -- this is fine because FRICP has its own mechanism of rejecting correspondences with high distance. At most, we could consider adding a note in the documentation or warning if the max correspondence distance has been set to a non-default value.

I agree, it was also my own initial thought. But maybe it should be overwritten and output a warning that it doesn't have any effect in this algorithm?

High: max iterations can exceed user expectation by a lot.
FRICP runs a nested schedule: outer robust loop plus inner loop up to max_iterations each time in [fricp.hpp:239], [fricp.hpp:240], and decays nu in [fricp.hpp:310].
Impact: setMaximumIterations behaves like per-stage iterations, not total iterations, unlike common PCL expectations.

I guess this is fine - it seems faster already anways.

High: several inherited ICP controls appear effectively bypassed.
ICP uses convergence criteria wiring in [icp.hpp:157], [icp.hpp:158], [icp.hpp:159], [icp.hpp:161], [icp.hpp:238], plus reciprocal/rejector paths. FRICP starts with initComputeReciprocal in [fricp.hpp:122] but does not use the usual correspondence estimation/rejector infrastructure.
Impact: knobs like reciprocal correspondences, rejectors, euclidean fitness epsilon, rotation epsilon may not influence behavior as users expect from ICP-derived classes.

Maybe just document that you can't add the other correspondence estimators/rejectors since they are not used.

Medium: NaN/Inf handling is missing in source/target ingestion.
FRICP directly maps point coordinates in [fricp.hpp:165] and [fricp.hpp:173] with no finite-value checks.
Impact: non-finite points can poison means/residuals and destabilize or fail correspondence search.

If it is expected not to handle this, its fine as is (maybe add assert for it?) and document that it can't handle NAN/Inf

Medium: setDynamicWelschDecay documentation and behavior disagree at boundary 0.
Header says valid range is [0, 1] in [fricp.h:102], but implementation maps very small values (including 0) back to 0.5 in [fricp.hpp:110] and [fricp.hpp:113].
Impact: surprising user behavior and harder parameter tuning.

Minor adjustment to documentation.

Low (style/API consistency): class is missing common Ptr/ConstPtr aliases seen in most PCL registration classes.
FRICP class declaration at [fricp.h:45]; compare with ICP aliases in [icp.h:113].
Impact: minor ergonomics inconsistency with PCL conventions.

Easy to add

Low (cleanliness): redundant initialization of nu_end_ratio_.
Member default exists in [fricp.h:161] and is also set in constructor init-list in [fricp.hpp:33].
Impact: harmless but slightly noisy.

We have changed many to field initialization, so lets do that here and remove the values from the init-list.

[duanbotu123]

Thanks for the thorough review! I've addressed all points in the latest commit:

1. maxCorrespondenceDistance ignored — Added a \note in the class documentation listing the inherited ICP settings that have no effect (setMaxCorrespondenceDistance, correspondence estimators/rejectors, setEuclideanFitnessEpsilon, setRotationEpsilon). A runtime warning could also be added if you'd prefer.
2. maxIterations exceeding expectations — Documented that setMaximumIterations controls inner-loop iterations per Welsch scale stage, and the total may exceed this value due to the outer loop.
3. Inherited ICP controls bypassed — Documented in the same \note block as (1).
4. NaN/Inf handling — Added assert(pcl::isFinite(pt)) for both source and target points during matrix construction, plus a \note in the class documentation.
5. setDynamicWelschDecay doc vs behavior at 0 — Updated the documentation from [0, 1] to (0, 1] and noted that 0 or very small values are silently replaced by 0.5.
6. Missing Ptr/ConstPtr aliases — Added to the class declaration.
7. Redundant nu_end_ratio_ initialization — Removed from constructor init-list and added = RobustFunction::WELSCH as in-class default for robust_function_ (consistent with the field initialization style used in anderson_acceleration.h and the other members of this class).

[larshg]

Do you have any strong opnion about your convergence criteria - your Matrix substraction delta, or would you consider using the Default Convergence Criteria instead?
It requires to add correspondences to a pcl::Correspondences list that can be mapped into it.

It stops quite earlier than your default implementation when I test with bun0 + bun4. I made the changes here: larshg@a80912a

FYI. its mostly AI generated.

[duanbotu123]

Sounds good, I'll switch to DefaultConvergenceCriteria to stay consistent with PCL.

[larshg]

getLastConvergenceTrigger - is not common PCL - it was my own testing to figure out when it finished on default criteria vs. your original matrix substraction.

So if you replace the matrix substraction with defaultcriteria, you don't need getLastConvergenceTrigger.

But you more or less just added my test code?

I think we should go with either of them - not both?

const Matrix4d delta_transform =
          transform_centered * previous_transform.inverse();
      this->transformation_ = delta_transform.template cast<Scalar>();

Should probably be moved under the Anderson acceleration step - same place where you currently calculate the matrix substraction delta?

[larshg]

Also DefaultConvergenceCritria already has CONVERGENCE_CRITERIA_NO_CORRESPONDENCES - so we should use that instead in the cases there is no corrspondences, instead of ConvergenceTrigger::NO_CORRESPONDENCES

[larshg]

Sorry for not being more specific in what I was expecting from showing my test code.

[larshg]

Looks like some formatting went horribly wrong 😄