Question about removal of the Center Of Mass in Symmetric Point-To-Plane ICP

[YoshuaNava]

Introduction

First of all, I want to say thank you for developing, maintaining and sharing Cilantro. It is an awesome library 🙏

My name is Yoshua Nava. I work in Point Cloud-based SLAM targeted to live robot localization.

Background

I have come to the realization that multiple point cloud registration libraries may not be handling a key aspect in the same way, and that is: estimating transformations when reference/target point clouds are displaced from the origin.

I looked at Cilantro's code, specially the computation of the error for Symmetric Point-To-Plane ICP:

cilantro/include/cilantro/registration/transform_estimation.hpp

Line 333 in cd80c1a

const Vector<ScalarT, 3> d = dst_p.col(corr.indexInFirst) - dst_mean;

cilantro/include/cilantro/registration/transform_estimation.hpp

Line 335 in cd80c1a

const Vector<ScalarT, 3> s = tform * (src_p.col(corr.indexInSecond) - src_mean);

And I observed that it is quite different to other implementations. I also digged a bit through Cilantro's PRs, and found two mentions (1, 2)

Moreover, I checked different point cloud libraries such as PCL, Open3D, KissICP, OpenCV, as well as Libpointmatcher (Point-To-Point & Point-To-Plane).

I'm surprised by the variety of approaches, and I wonder what is the best one. I found in practice that handling of the CoM can make a significant difference in the estimation of rotations with Point-To-Plane ICP, because the rotational term is quite sensitive to the position of the origin.

Questions

1. Do you have some references on the procedure to remove the CoM from the reading and reference point clouds?
2. Did you consider the alternative approaches implemented by the other libraries? If yes how was your experience with them?
3. How deeply have you battle-tested the approach implemented by Cilantro? Do you see any limitations?

Thank you in advance.

Best,
Yoshua Nava

[YoshuaNava]

(I also reported this issue in the repo of libpointmatcher, the library that I'm the most familiar with and that kickstarted my investigation. Issue link -> norlab-ulaval/libpointmatcher#507)

[wannesvanloock]

Thanks for your interest in cilantro. These are interesting research questions and I'll answer briefly from my own limited experience, but would be happy to discuss this into further detail.

1. Numerically, it makes a lot of sense to center the clouds. Perhaps even scaling could be meaningful to improve numerical stability of the ICP algorithm. From experience I can confirm that this makes a difference. You could evaluate the condition number of the linear system of equations (Ax = b) at each ICP step. Based on your question in libpointmatcher, it seems you notice a difference in the basin of attraction depending on whether you center the clouds. AFAICS, this can only be explained by numerical conditioning as theoretically the ICP steps should be identical.
2. There are many, many different ICP approaches of which symmetric Point-To-Plane ICP is only one. I've implemented this alongside the already existing ICP approaches in cilantro for two reasons. 1) It uses normals from both clouds. Other point-to-plane approaches typically only use one of the two normals. This intuitively introduces asymmetry to the solutions and ignores data that is anyways there. 2) It has proven better convergence properties than regular ICP. Check the publication for more details.
   I have positive experiences with other libraries and each of the ICP methods has its own merits and drawbacks. I can only recommend experimenting and see what suits your application best.
3. I've been using cilantro and symmetric ICP regularly with success (and stability) but not (yet) in SLAM.

Additionally, note that ICP is a local optimization, assuming an already reasonable initial guess. For a large part, the basin of attraction is also determined by the clouds and not the algorithm, e.g. a geometry with a dominant plane may have a worse basin of attraction for point-to-plane ICP as the objective will be close to zero once the dominant face is aligned regardless of the rotation around the normal of the dominant face.

[YoshuaNava]

Dear @wannesvanloock,

Thank you for your reply and suggestions 🙏

Topic 1 - ICP Conditioning

Numerically, it makes a lot of sense to center the clouds.

I resonate with your observation about conditioning. My hope is that improving this leads to more uniform precision, independent of the location of reference and reading point clouds.

Furthermore, during my experiments I observed cases where the bad conditioning fooled the optimization to slide 🏂 into a wrong local minimum. I wonder if bad conditioning could be blinding ICP to misalignments, and if by improving the conditioning of ICP, we could enable it to better detect mis-registrations (there is a ton of work like CorAl which is trying to address that outside ICP).

Perhaps even scaling could be meaningful to improve numerical stability of the ICP algorithm.

I found that Trimesh2 from Rusinkiewicz (the author of Symmetric Point-To-Plane) takes scaling into account. I linked part of the formulation in the libpointmatcher issue and the implementation from this fork of Trimesh2.

For a large part, the basin of attraction is also determined by the clouds and not the algorithm, e.g. a geometry with a dominant plane may have a worse basin of attraction for point-to-plane ICP as the objective will be close to zero once the dominant face is aligned regardless of the rotation around the normal of the dominant face.

What you mention is spot-on 🤘. In robot localization we encounter this problem in areas like long corridors, wide-open spaces, where the P2P cost function is blind due to lack of features in certain degrees of freedom. Rusinkiewicz studied this problem and identified some geometric primitives that lead to ill-formulated ICP:

(Image taken from this paper)

In a collaboration between ANYbotics (my employer) & ETH-Zurich's Robotics Systems Lab we developed a way to measure the "degree of Localizability' based on the rotational and translational components of the Point-To-Plane error, and proposed constrained optimization as a way to limit registration to the observable sub-space. We released this paper describing the approach.

Topic 2 - Alternative methods

I follow the introduction of Symmetric Point-To-Plane ICP with interest. A reason I haven't yet tried to integrate it is because for live robot localization we tend to get reading and reference point clouds with different sampling properties, which influence the quality of surface normals significantly.

While a reference point cloud map can provide enough geometric support to extrude good normals, the point clouds coming from spinning lidar sensors may offer little support to extrude normals in certain areas. See for example the concentric rings falling on the floor, where extruding a normal would require using a large radius for finding neighbors, or some prior knowledge (e.g. a local map):

Moving from regular to symmetric P2P for robot localization may require to A) improve the quality of surface normals in reading point clouds, B) be more selective when matching, or C) both. A paper that I often come back to, which predates Symmetric P2P, but contains many insights on how to sample surface normals for SLAM is NOctoSLAM.

Topic 3 - Applications of Symmetric Point-To-Plane ICP

I see. I haven't yet used Cilantro for robot localization, but I will try it. The depth and breadth of the algorithms in Cilantro is awesome. I got to know the library thanks to a colleague, @fevb.

---

Actions moving forward

The actions mentioned in norlab-ulaval/libpointmatcher#507 (comment).

Additionally, I could keep you updated about the outcome of my investigation in this issue or the libpointmatcher one.

---

(Sorry for the long reply)

[YoshuaNava]

Hi @wannesvanloock,

I read through the Cilantro code and noticed that when you compute the means of the point clouds to register, you don't take into account the weights from the point-to-plane correlator.

The impact I could see in that is that even if your correlator rejects a point pair, it would still be used in the estimation.

Is this something you had considered in the past?

Thank you!

[wannesvanloock]

Cannot say I have considered this :) It does make sense though!

[YoshuaNava]

Hi @wannesvanloock,

I've been doing some experiments, and got Point-To-Plane ICP (libpointmatcher's) to reach 1e-6 to 1e-8 precision with single-precision floating point. I was also to boost Point-To-Point accuracy a bit, although it doesn't reach the same level. In my path I found that Eigen has a super nice implementation of Umeyama with solid conditioning.

I implemented multiple modules (first one submitted) to be able to unit test the code (synthetic data generation, computing the precision over multiple test cases). I could support doing the same for cilantro.

On the other hand, I wanted to ask you about this line:

cilantro/include/cilantro/registration/transform_estimation.hpp

Line 356 in cd80c1a

const Eigen::Translation<ScalarT, 3> ta(std::cos(theta) * d_theta.template tail<3>());

Is there any specific reason why you multiple by the cosine of the angle of rotation?

Thank you!

Yoshua

[wannesvanloock]

Is there any specific reason why you multiple by the cosine of the angle of rotation?

See equation (11) in Szymon Rusinkiewicz. "A Symmetric Objective Function for ICP." ACM Transactions on Graphics (Proc. SIGGRAPH) 38(4), Article 85, July 2019.

In my path I found that Eigen has a super nice implementation of Umeyama with solid conditioning.

I'm aware of the Umeyama implementation in Eigen. I've explored similar paths in which we don't compute AtA but rather formulate the problem as Ax = b and solve it with Eigen in LSQ sense. IIRC, I found that the computational performance was dramatically worsened. However, I do think it will benefit the overall quality of the solution.
Perhaps you can conclude ICP with a few of such steps, or perform these when the condition number degrades?