(C++) rotation in transformation matrix is incorrect

[mitjap]

When building transformation matrix from 'alpha1', 'alpha2', 'alpha3', rotation part is not correct. 'alpha1', 'alpha2', 'alpha3' are Euler angles and can not be just inserted in matrix like this: https://github.com/pglira/simpleICP/blob/fd1a926bc0b605c658dad08acd842e433f5eacf7/c%2B%2B/src/corrpts.cpp#L133

Rotation part of the matrix should be calculated the same way it is done in python version: https://github.com/pglira/simpleICP/blob/fd1a926bc0b605c658dad08acd842e433f5eacf7/python/simpleicp/mathutils.py#L39

[pglira]

The difference between the C++ and the python implementation is:

• C++ uses a linearized rotation matrix, sometimes also denoted as infinitesimal rotation matrix [https://rotations.berkeley.edu/infinitesimal-rotations/]. Using the linear version has the main advantage that the equation system (ES) is linear too and thus no iterative solution of the ES is needed. It should be emphasized that using the linear version is completely fine in ICP, as only small rotations must be estimated, especially in the last few ICP iterations.
• Python, however, uses the normal rotation matrix, which is non-linear. Formerly, I used also here the linearized version. However, the extended feature "observation of rigid-body transformation parameters" (which currently is only available in the python implementation) required the introduction of the non-linear version.

[pglira]

Hopefully my answer was clear. I will close this issue.

[mitjap]

I'm sorry for late reply. I still don't think this is correct. IPC can also be used where rotations are significant. You say that towards the end rotations are small. That is true, but final solution is sum of all rotations. And each rotation also does some scaling so the end scale change is not insignificant. I've observed 20% scale change in our tests.

I've change our version to construct proper rotation matrix from Euler angles and it works as expected without any performance penalties.

[mitjap]

This is for comparison between linearized rotation matrix for provided bunny dataset. This first transformation alone changes scale for X axis by 0.5%. Final transformation changes scale for Y axis by 1%. This scale change is for small rotations in provided dataset. For larger rotations scale change is unacceptable.

Original transformation in first step:

H:
          1   -0.104052  0.00799438  -0.0470766
   0.104052           1   0.0191137    -1.22437
-0.00799438  -0.0191137           1  -0.0404911
          0           0           0           1

Modified transformation in first step:

H_:
    0.99456   -0.103861   0.0079943  -0.0470766
   0.103693    0.994426   0.0191119    -1.22437
-0.00993472   -0.018179    0.999785  -0.0404911
          0           0           0           1

[mitjap]

For this dataset I also observed significant speedup of 6x. Original version hit 100 iterations limit while improved version converged after 17 steps.

[pglira]

You're absolutely right with the scale issue, I was not aware of that.

One minor issue remains: The linearized rotation matrix is still used in the optimization, i.e. when building the Jacobian matrix A. This needs to be reworked. The major change thereby is that the equation system to be solved becomes non linear by using the non linear rotation matrix. Consequently, a non linear solver must be used instead of Eigen (maybe Ceres).