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

[YoshuaNava]

Background

I'm debugging some behavior of Point-To-Plane ICP, which seems to estimate large rotations much better when

1. Reference point clouds have their Center Of Mass close to the origin, and
2. We zero the Reference Center Of Mass we computed in the code, enforcing that it is not removed,

This seems to increase the basin of convergence for rotations, and I can get up to 30-40° pure rotation estimations in one ICP call.

I checked different point cloud libraries such as PCL, Open3D, KissICP, OpenCV, as well as Cilantro, and every implementation seems to handle this aspect differently. I also digged a bit through Libpointmatcher's commit history, and found two mentions (1, 2) but no more details.

Questions

1. Are there are some references on the procedure to remove the CoM?
2. I noticed that we don't follow the same procedure for Point-To-Point (where we remove the CoM of reading and reference) and Point-To-Plane (where we only remove the CoM of the reference). Is there a specific reason for that?
3. Do you have any recommendations? I would be happy to follow any advice and submit a PR to address this broadly.

Thank you in advance 🙏

Best,
Yoshua Nava

[pomerlef]

Hi @YoshuaNava ,

1. I'm not sure what you mean by 'procedure'. Removing the center of mass is simply removing the mean of the coordinates. As for why we do it, it's because at some time I was applying ICP to match a trajectory with GPS coordinates. These GPS coordinates were expressed with very large numbers, which were not registering well. If you think about it for a point cloud very far from its origin, there is no difference between a small translation and a small translation to resolve the error. That being said, I would like to know what the other libraries are doing.
2. Substracting the mean is part of the classic solution for minimizing point-to-point error from [Arun et al., 1987],(see https://hal.science/hal-01178661/document, P. 43).
3. Keep on the good work ;) I'm not sure if there is something to follow up, maybe adding better comments in the source or adding to the documentation.

Cheers!

[YoshuaNava]

Hi @pomerlef,

Thank you for your prompt reply 🙂

My motivation for digging into this topic is because I noticed (empirically) that libpointmatcher's Point-To-Plane ICP implementation shows a high degree of sensitivity to the position of the map origin relative to where we are localizing.

During my investigations I implemented a rotation-only version of Point-To-Plane ICP, and I observed how, the closer to the origin (and the lower the norm of the reference CoM), the more accurate the rotations were estimated, and the larger rotations it could handle. Conversely, I also observed how, the farther we are from the origin, the higher the likelihood that the estimated rotation affects the translation component in an unexpected way.

My hypothesis is that the CoM computation could be reducing the basin of convergence of P2P ICP the farther away from the origin the CoM is.

That being said, I would like to know what the other libraries are doing.

Sure thing 🤘

I made a short summary of different approaches, from libraries and papers.

Review of Libraries (Click to display)

Libraries

In this section I'll include screenshots of the code and links to the code block, because GitHub does not support rendering Permalinks as code quotations across different repos.

PCL ⛔

The computation of the Point-To-Plane Error does not consider the CoM:

Link to code

Open3D ⛔

The computation of the Point-To-Plane Error does not consider the CoM:

KissICP ⛔

The computation of the Point-To-Point Error does not consider the CoM:

Link to code

OpenCV Surface Matching module ✅

The computation of the Point-To-Plane Error considers the CoM.

Steps followed:

1. Compute the CoM of both the reading and reference point clouds,
2. Compute the average of both CoMs,
3. Subtract the average CoM from both the reading and reference point clouds,
4. Subtract the average CoM from the translation component of the ICP-corrected pose,

Link to code from steps 1-3

Link to code from step 4

Cilantro ✅

The computation of the Point-To-Plane Error considers the CoM.

Steps followed:

1. Compute the CoM of both the reading (src_mean) and reference (dst_mean) point clouds,
2. Transform the reading point cloud to our "initial guess on its pose relative to the reference frame" (src_normals_trans_),
3. Estimate a transform between point clouds, which includes transforming the reading CoM to the reference frame (with the "initial guess on the reading pose in the reference frame"),
4. Pre-compute the transformation from reference frame to reference CoM (t_dst), and from reading CoM to reading (t_src).
5. Error computation:
   a. Subtract the reference CoM when computing the error per reference point.
   b. Subtract the reading CoM when computing the error per reading point, considering the delta transformation from the alignment,
6. Composing the ICP transformation with the transforms t_src and t_dst, to ensure that we bring it back from CoM to reference frame,

Link to code from step 1
Link to code from step 2
Link to code from step 3
Link to code from step 4
Link to code from step 5
Link to code from step 6

Trimesh2's implementation of Symmetric Point-To-Plane ICP ✅

Trimesh2 is a library implemented by Szymon Rusinkiewicz, one of the authors of Normal-Space Sampling and Symmetric P2P ICP. It is hosted on his website, but I found a recent fork on Github.

The computation of the Point-To-Plane Error considers the CoM.

Steps followed:

1. Compute the centroids of the reading and reference point clouds,
2. Compute the scale to make the average distance between points to the origin tend to 1,
3. Error computation:
   a. Subtract the CoM from the reading and reference point clouds,
   b. Multiply the CoM-shifted points by the scale factor,
4. (Similar to Cilantro) Composing the estimated rotation with the CoM translations from reference and reading, to ensure that we bring it back from CoM to reference frame,

Link to code from step 1
Link to code from step 2
Link to code from step 3
Link to code from step 4

Review of Papers (Click to display)

Papers

Geometrically Stable Sampling for the ICP Algorithm - Gelfand et al (Link)

In section 3.1 "A Measure of Stability", the mention that:

"However, the part of the transformation vector $[r^T, t^T]$ that corresponds to rotation depends on the term $p_i \times n_i$. This means rotations are dependent on distance of the point $p_i$ from the origin (which is the center of rotation when $r$ is applied to each $p_i$). As is common with PCA methods, we will shift the center of mass of the points $p$ to the origin. However, the magnitude of rotations can still be incompatible with the magnitude of translations, since a point $p_i$ can be arbitrarily far from the center of mass. Therefore, after shifting the center of mass, we will scale the point set so that the average distance of points $p_i$ from the origin is 1. This has the effect of equalizing the maximum amount of Displacement that can be contributed by a point due to “torque” (i.e. rotation) to the amount of displacement due to “force” (i.e. translation)."

Note that Trimesh2 implements the normalization proposed by this paper ❗

Improvements to the ICP Algorithm for Shape Registration in Manufacturing - Kwok et al (Link)

In section 4.1 "Rotation center" they describe that:

"As rotation is a geometric operation turning around a center, the analysis can give a correct result only when a correct center is given. The rotational component in Eq.(2) can be rewritten as

$$E = \sum_{i=1}^{k} \lVert (\delta r \times (p_i^* − c) + \delta t) \cdot n_i \rVert ^2$$

where $c = [cx, cy, cz]^T$ is the rotation center, and $p_i = p_i^* − c$ is assigned to distinguish the Euclidean position $p_i^*$ from the vector $p_i$, with the center $c$ as the start point. Similar to Principle Component Analysis (PCA), most of the previous works assume the rotation center $c$ to be the center of mass of the model. By shifting the center of mass to the origin of the coordinate system, the rotation center $c$ is then the origin $(0,0,0)$. That is exactly the reason why the center is not seen in the equations. However, the center of mass is not always equivalent to the rotation center, e.g., the shape in Fig.2. We find that when the rotation center is not correctly given, the stability analysis could report erroneous information. Therefore, it is crucial for the rotation center to be correct."

Analysis of Libpointmatcher (Click to display)

Analysis of Libpointmatcher

Point-To-Point ICP ❓

The computation of the Point-To-Point Error considers the CoM.

Steps followed:

1. Compute the CoM of the reading and reference point clouds,
2. Subtract each CoM to its corresponding point cloud,
3. (Similar to Cilantro) Composing the estimated rotation with the CoM translations from reference and reading, to ensure that we bring it back from CoM to reference frame,

https://github.com/ethz-asl/libpointmatcher/blob/e45710b421f757d7eadbf03cd93cb12e698098f8/pointmatcher/ErrorMinimizers/PointToPoint.cpp#L70-L72
https://github.com/ethz-asl/libpointmatcher/blob/e45710b421f757d7eadbf03cd93cb12e698098f8/pointmatcher/ErrorMinimizers/PointToPoint.cpp#L75-L77
https://github.com/ethz-asl/libpointmatcher/blob/e45710b421f757d7eadbf03cd93cb12e698098f8/pointmatcher/ErrorMinimizers/PointToPoint.cpp#L94

As mentioned in #507 (comment), subtracting the mean is part of the classic solution to this problem.

⚠️ Potential bug ⚠️

Before calling the Point-To-Point error minimizer, we remove the CoM from the reference point cloud:

https://github.com/ethz-asl/libpointmatcher/blob/e45710b421f757d7eadbf03cd93cb12e698098f8/pointmatcher/ICP.cpp#L292-L294

Is the mean of the reference being removed twice?

Point-To-Plane ICP ⛔

The computation of the Point-To-Plane Error does not explicitly consider the CoM.

We implement subtraction of the CoM in the ICP class, but the Point-To-Plane error minimizer is unaware of it:

https://github.com/ethz-asl/libpointmatcher/blob/e45710b421f757d7eadbf03cd93cb12e698098f8/pointmatcher/ICP.cpp#L292-L294

⚠️ Potential bug ⚠️

We subtract the reference CoM from the reference point cloud, and we transform the initial guess into the reference CoM frame. But we don't consider the reading CoM at all.

Should the reading CoM be considered more actively?

All ICP formulations

⚠️ Potential bug ⚠️

I observe that we are performing 1) Correspondence search, and 2) Optimization, in the Reference CoM frame.

I would like to validate if both operations can and should happen in this frame.

---

Actions moving forward

Get a better understanding of transformations

At the start I just want to get some clarity on how coordinates are/should be handled in ICP: transforms (initial guess, delta from ICP iterations, total ICP correction, corrected initial guess), frames (Reading, Reference, Reading CoM, Reference CoM).

I'll try to derive a bit the equations to understand whether the operations are running in the right coordinate frames.

Unit tests

I'm planning to write some unit tests that

1. Register geometric primitives with surface normals sampled in close-form (e.g. a rectangle, compositions of ellipsoids),
2. Offset from the origin in varying degrees (to validate that the CoM is handled properly),
3. Considering pure rotations, pure translations, and then combinations.

I'll add some flags to toggle ON/OFF:

1. Considering the reference and reading CoM for correspondence search,
2. Considering the reference CoM in the optimization,
3. Considering the reading CoM in the optimization,

I'll validate:

1. Whether the resulting transformations from ICP are correct,
2. The condition number of the optimization (thank you for the tip 🙏),
3. Whether the correspondences make sense (through visualization, I'll use Paraview for this)

---

Note: I used the tool UpMath to format LaTeX for displaying in Markdown

[YoshuaNava]

@pomerlef I implemented unit tests for ICP conditioning, as well as a fix.

In order to test the conditioning of the registration pipeline I put together new code, and I would like to submit a PR. To do that, I would like to know your opinion on where we should store new functionality. I follow the practice of 'shifting discussions to the left' (discussing design before submitting PRs, the 'what'), so that reviews can focus on the implementation (the 'how')

Some of the utilities I implemented are targeted to:

• Filesystem: the unit tests allow you to save the test data to disk, to enable debugging if a test fails (all functions relying on boost::filesystem)
• Geometry: comparing transformations, generating random translation/rotations, and converting radians/degrees (all functions relying on Eigen and cmath),
• Gtest: fetching the name of the current test, recording the random seed as a GTest Property,
• Registration benchmarking: structures for the input and output of registration tests, as well as for measuring the error.

Would you like me to add these utilities? If yes, should I do so under the utest directory, or make them generally available under the pointmatcher directory & namespace?

Thank you in advance.