Rot3 determinant!=1 after recursive operation

[Alexma3312]

Description

The determinant of a Rot3 variable no longer equals to 1 after 31 steps of recursive operations with Rot3.between() and Rot3.compose().

Steps to reproduce

1. run test_rot3_determinant_between in the attached file

"""Test Rot3 Determinant"""
import math
import unittest

import numpy as np
from gtsam import Rot3
from gtsam.utils.test_case import GtsamTestCase


class Test(GtsamTestCase):
    """Test methods."""

    @unittest.skip("test_rot3_determinant_compose")
    def test_rot3_determinant_compose(self):
        """Test rot3 determinant."""
        degree = 1
        R_w0 = Rot3()
        R = Rot3.Ry(math.radians(degree))
        for i in range(1,360):
            R_w1 = R_w0.compose(R)
            R_w0 = R_w1
            R_det = np.linalg.det(R_w1.matrix())
            expected_rotation = Rot3.Ry(math.radians(i*degree))
            expected_determinant = 1.0
            print(i, ' determinant error =',math.sqrt(R_det-expected_determinant))
            # self.assertAlmostEqual(R_det, expected_determinant, delta=1e-5)
            self.gtsamAssertEquals(R_w1, expected_rotation, 1e-5)

    def test_rot3_determinant_between(self):
        """Test rot3 determinant."""
        degree = 1
        R_w0 = Rot3()
        R_w1 = Rot3.Ry(math.radians(degree))
        for i in range(2,360):
            R_01 = R_w0.between(R_w1)
            R_w2 = R_w1.compose(R_01)
            R_w0 = R_w1
            R_w1 = R_w2
            R_det = np.linalg.det(R_w2.matrix())
            expected_rotation = Rot3.Ry(math.radians(i*degree))
            expected_determinant = 1.0
            print(i-1, ' determinant error =',math.sqrt(R_det-expected_determinant))
            self.assertAlmostEqual(R_det, expected_determinant, delta=1e-5)
            # self.gtsamAssertEquals(R_w2, expected_rotation, 1e-5)

if __name__ == "__main__":
    unittest.main()

Expected behavior

1 error = 1.4901161193847656e-08
2 error = 1.4901161193847656e-08
3 error = 1.4901161193847656e-08
4 error = 2.5809568279517847e-08
5 error = 4.2146848510894035e-08
6 error = 6.322027276634104e-08
7 error = 9.771344846937061e-08
8 error = 1.519626234052415e-07
9 error = 2.3513635751693695e-07
10 error = 3.656102462525754e-07
11 error = 5.682014216464048e-07
12 error = 8.830745452659758e-07
13 error = 1.37195922532322e-06
14 error = 2.1316995191139784e-06
15 error = 3.3122167562126646e-06
16 error = 5.1464477617739e-06
17 error = 7.99641350665854e-06
18 error = 1.2424628950122203e-05
19 error = 1.93050682493097e-05
20 error = 2.9995711773005583e-05
21 error = 4.6606554127121996e-05
22 error = 7.24160513973106e-05
23 error = 0.00011251817383636744
24 error = 0.00017482781131434593
25 error = 0.00027164290610608063
26 error = 0.0004220716887881555
27 error = 0.000655804046453184
28 error = 0.0010189714143876455
29 error = 0.0015832518450614864
30 error = 0.0024600176914758875
31 error = 0.0038223196428445

Environment

python3
gtsam 4.0.2

Additional information

1. Switching Rot3.between() into Rot3.inverse().compose() will also fail the unittest.
2. Unittest does not fail when the recursive operation only contains Rot3.compose(). This step can be proved by running test_rot3_determinant_compose().
3. This problem does not get fix with build option GTSAM_USE_QUATERNIONS on.

4. This problem can be temporally fix by running Rot3.ClosestTo() on each step.

[dellaert]

Wow. That loses precision much faster than I expected.

So, to be clear, if you replace

R_01 = R_w0.between(R_w1)
R_w2 = R_w1.compose(R_01) 

with

R_w2 = R_w1 * R_w0.inverse() * R_w1

things still fail? Could you give that a try?

[Alexma3312]

@dellaert When I use the (*) operation, I got an exception. I installed gtsam through pip install. Is this normal?:

Traceback (most recent call last):
  File "/home/sma96/Blimps/localization/tests/test_rot3_determinant.py", line 37, in test_rot3_determinant_between
    R_w2 = R_w1 * R_w0.inverse() * R_w1
TypeError: unsupported operand type(s) for *: 'gtsam.gtsam.Rot3' and 'gtsam.gtsam.Rot3'

I tried and replace

R_01 = R_w0.between(R_w1)
R_w2 = R_w1.compose(R_01) 

With

R_w2 = R_w1.compose(R_w0.inverse().compose(R_w1))

The result is the same:
1 determinant error = 1.4901161193847656e-08
2 determinant error = 1.4901161193847656e-08
3 determinant error = 1.4901161193847656e-08
4 determinant error = 2.5809568279517847e-08
5 determinant error = 4.2146848510894035e-08
6 determinant error = 6.322027276634104e-08
7 determinant error = 9.771344846937061e-08
8 determinant error = 1.519626234052415e-07
9 determinant error = 2.3513635751693695e-07
10 determinant error = 3.656102462525754e-07
11 determinant error = 5.682014216464048e-07
12 determinant error = 8.830745452659758e-07
13 determinant error = 1.37195922532322e-06
14 determinant error = 2.1316995191139784e-06
15 determinant error = 3.3122167562126646e-06
16 determinant error = 5.1464477617739e-06
17 determinant error = 7.99641350665854e-06
18 determinant error = 1.2424628950122203e-05
19 determinant error = 1.93050682493097e-05
20 determinant error = 2.9995711773005583e-05
21 determinant error = 4.6606554127121996e-05
22 determinant error = 7.24160513973106e-05
23 determinant error = 0.00011251817383636744
24 determinant error = 0.00017482781131434593
25 determinant error = 0.00027164290610608063
26 determinant error = 0.0004220716887881555
27 determinant error = 0.000655804046453184
28 determinant error = 0.0010189714143876455
29 determinant error = 0.0015832518450614864
30 determinant error = 0.0024600176914758875
31 determinant error = 0.0038223196428445

[dellaert]

Oh, yeah, * will (hopefully) only work in new pybind wrapper

[ProfFan]

I think it's quite hard to avoid this without some clever floatpoint-fu. (or we can use the quaternion at Rot3, since quaternion is always normalized)

[varunagrawal]

Do you think using a normalized quaternion would make more sense? It would be slightly more expensive, but hopefully Eigen's magic would ameliorate some of the run time costs, while ensuring correctness.

I wrote a test and made the changes on my local machine and it works great, error never goes greater than 1e-7.

[dellaert]

We have a separate compilation flag for quaternions. Sticking quaternions in the middle here is a solution, just not a very elegant one.

[dellaert]

BTW is the quaternion flag tested in CI?

[ProfFan]

@dellaert Nope :) We should have one though.

[varunagrawal]

Just added the special workflow via #476

[varunagrawal]

I found an elegant solution to this problem.
I can submit the PR later today.