Buggy axis-angle computation in GTSAM

[johnwlambert]

GTSAM returns angle's of well over 180 degrees when transforming a Rot3() object with .axisAngle().

Here's an example below, where the angle returned is 204 degrees. Scipy's Rotation package returns the same axis vector as GTSAM, but returns the angle as 179 degrees, which seems legitimate.

I believe the angle of the angle-axis representation should be confined to [0, pi] radians, per Duke and the notion of being smallest rotation to take us there (we would go the opposite way, instead, otherwise):

Given the following 3x3 matrix:

from gtsam import Rot3
(Pdb) p relative_rot
R: [
    -0.0309726, -0.789768, -0.612625;
    -0.787144, -0.358463, 0.501909;
    -0.615993, 0.497768, -0.610558
]

GTSAM provides an axis-angle decomposition:

(Pdb) p relative_rot.axisAngle()
(:-0.696212
 0.566258
 0.441181
, 3.576080425974451)

Angle is 204 degrees:

(Pdb) np.rad2deg( 3.576080425974451)
204.89431560768165

Scipy states the angle is 179.93 degrees:

(Pdb) from scipy.spatial.transform import Rotation
(Pdb) Rotation.from_matrix(relative_rot.matrix()).as_rotvec()
array([-2.18469647,  1.77759913,  1.38497925])
(Pdb) axisangle = Rotation.from_matrix(relative_rot.matrix()).as_rotvec()
(Pdb) np.linalg.norm(axisangle)
3.138618297175063
(Pdb) np.rad2deg(3.138618297175063)
179.8295819306683

Scipy's axis of rotation is identical:

(Pdb) axisangle /= np.linalg.norm(axisangle)
(Pdb) axisangle
array([-0.6960695 ,  0.56636359,  0.44127037])

[johnwlambert]

The angle can be 300 - 800 degrees in other cases:

>>> import numpy as np
>>> import gtsam
>>> R = np.array([[ -0.999957, 0.00922903, 0.00203116],[ 0.00926964, 0.999739, 0.0208927],[ -0.0018374, 0.0209105, -0.999781 ]])
>>> R.T @ R
array([[ 1.00000330e+00,  1.66519550e-07, -4.07243092e-07],
       [ 1.66519550e-07,  1.00000049e+00,  7.20413748e-08],
       [-4.07243092e-07,  7.20413748e-08,  1.00000268e+00]])
>>> np.linalg.det(R)
1.0000032373629155
>>> from gtsam import Rot3
>>> Rot3(R)
R: [
    -0.999957, 0.00922903, 0.00203116;
    0.00926964, 0.999739, 0.0208927;
    -0.0018374, 0.0209105, -0.999781
]
>>> Rot3(R).axisAngle()
(:0.00460089
  0.999934
 0.0104968
, 6.075185318083525)
>>> np.rad2deg(6.075185318083525)
348.08247848602855
>>> Rot3(R).matrix().T @ Rot3(R).matrix()
array([[ 1.00000330e+00,  1.66519550e-07, -4.07243092e-07],
       [ 1.66519550e-07,  1.00000049e+00,  7.20413748e-08],
       [-4.07243092e-07,  7.20413748e-08,  1.00000268e+00]])
>>> from scipy.spatial.transform import Rotation
>>> Rotation.from_matrix(Rot3(R).matrix()).as_rotvec()
array([0.01452083, 3.13945318, 0.03281409])
>>> scipy_angle_axis = Rotation.from_matrix(Rot3(R).matrix()).as_rotvec()
>>> np.linalg.norm(scipy_angle_axis)
3.139658247605024
>>> np.rad2deg(np.linalg.norm(scipy_angle_axis))
179.88916670120787
>>> scipy_angle_axis / np.linalg.norm(scipy_angle_axis)
array([0.00462497, 0.99993469, 0.01045148])

[ProfFan]

Hey @johnwlambert could you put up a PR with a unit test that reproduces this?

[ProfFan]

824

[johnwlambert]

Sure @ProfFan, just added one here: https://github.com/borglab/gtsam/pull/887/files

[dellaert]

Do you have an idea how to fix it?

[johnwlambert]

Not yet @dellaert. It seems like this issue rears its head when the true angle is right next to the limit of 180 degrees.

I see the issue occur 165 out of 64000 times in my most recent test. So a 0.3% failure rate is something most people might not discover.

So that suggests it's a numerical precision thing. Again, it seems to be an edge case from SO3::Logmap. So I'd say something in the 15 lines below is fishy:

  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
  // we do something special
  if (tr + 1.0 < 1e-10) {
    if (std::abs(R33 + 1.0) > 1e-5)
      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
    else if (std::abs(R22 + 1.0) > 1e-5)
      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
    else
      // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
      omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
  } else {
    double magnitude;
    const double tr_3 = tr - 3.0;  // always negative
    if (tr_3 < -1e-7) {
      double theta = acos((tr - 1.0) / 2.0);
      magnitude = theta / (2.0 * sin(theta));
    } else {
      // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
      // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
      // see https://github.com/borglab/gtsam/issues/746 for details
      magnitude = 0.5 - tr_3 / 12.0;
    }
    omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
  }

Scipy seems to handle this fine, and implements this as:

        cdef Py_ssize_t num_rotations = len(self._quat)
        cdef double angle, scale, angle2
        cdef double[:, :] rotvec = _empty2(num_rotations, 3)
        cdef double[:] quat

        for ind in range(num_rotations):
            if self._quat[ind, 3] < 0:  # w > 0 to ensure 0 <= angle <= pi
                quat = self._quat[ind, :].copy()
                for i in range(4):
                    quat[i] *= -1
            else:
                quat = self._quat[ind, :]

            angle = 2 * atan2(_norm3(quat), quat[3])

            if angle <= 1e-3:  # small angle
                angle2 = angle * angle
                scale = 2 + angle2 / 12 + 7 * angle2 * angle2 / 2880
            else:  # large angle
                scale = angle / sin(angle / 2)

            rotvec[ind, 0] = scale * quat[0]
            rotvec[ind, 1] = scale * quat[1]
            rotvec[ind, 2] = scale * quat[2]

[dellaert]

Yep... Good luck?

[ProfFan]

@dellaert @johnwlambert

I spent a day trying to figure this out. There are actually more than one issues in this problem.

The first issue is the incorrect calculation. The GTSAM one is incorrect because the if (tr + 1.0 < 1e-10) bound is too tight. It should at least be 1e-5 for the degenerate case to be correctly considered. However, even after properly considering this the result is still incorrect.

This is due to the second issue: when you construct a scipy rotation object, it actually checks if the matrix is properly orthogonal, and calculates the closest rotation at construction. If I supply the (fixed) GTSAM version with the "closest matrix", the values match.

This indicated that maybe you want to also use the ClosestTo? or similar mechanism in GTSAM to get a proper rot matrix before calculating the Axis angle form.

I will soon put up a PR fixed the first issue, with the justifications in math.

[ProfFan]

A PoC showing this idea:

comment R=scipy.xxxxxxx out and see the difference.

import unittest
import pytest

import logging

logging.basicConfig(level=logging.DEBUG)
logger = logging.getLogger()

from unittest import TestCase

import numpy as np
import scipy.spatial
from scipy.spatial.transform.rotation import Rotation

def get_axisangle(R: np.ndarray):
    tr = np.trace(R)
    # first case, theta = 0
    if tr > 3.0 - 1e-5:
        mag = 0.5 - (tr - 3.0) / 12.0
    elif tr < -1.0 + 1e-5:
        # second case, theta = pi
        # R = R
        R = scipy.spatial.transform.Rotation.from_matrix(R).as_matrix()  # renormalized

        max_dim = np.argmax(np.abs(R.diagonal()))
        da, db, dc = [max_dim, (max_dim + 1) % 3, (max_dim + 2) % 3]
        logger.warn(f"{ np.linalg.det(R) = }")
        logger.warn(f"{ R.diagonal() = }, { max_dim = }")
        r = np.sqrt(1 + R[da, da] - R[db, db] - R[dc, dc])
        quat = np.array(
            [
                0.5 / r * (R[dc, db] - R[db, dc]),
                0.5 * r,
                0.5 / r * (R[da, db] + R[db, da]),
                0.5 / r * (R[dc, da] + R[da, dc]),
            ]
        )
        if quat[0] < 0:
            quat *= -1

        angle = 2 * np.arctan2(np.linalg.norm(quat[1:4]), quat[0])

        if angle <= 1e-3:  # small angle
            angle2 = angle * angle
            scale = 2 + angle2 / 12 + 7 * angle2 * angle2 / 2880
        else:  # large angle
            scale = angle / np.sin(angle / 2)

        return scale * quat[1:4]
    else:
        raise NotImplementedError()

    omega = mag * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])

    return omega

class TestAxisAngle(TestCase):
    def test_scipy(self):
        axis = np.array([1, 0, 0])
        angle = 0.000001
        rot_sp: Rotation = scipy.spatial.transform.Rotation.from_rotvec(angle * axis)
        rot_recovered = rot_sp.as_rotvec()
        print(rot_recovered)
        np.testing.assert_allclose(angle * axis, rot_recovered)

    def test_failing_example(self):
        R = np.array(
            [
                [-0.999957, 0.00922903, 0.00203116],
                [0.00926964, 0.999739, 0.0208927],
                [-0.0018374, 0.0209105, -0.999781],
            ]
        )
        rot_sp: Rotation = scipy.spatial.transform.Rotation.from_matrix(R)
        rot_recovered = np.linalg.norm(rot_sp.as_rotvec())
        expected = 3.1396582
        print(rot_recovered)
        np.testing.assert_allclose(expected, rot_recovered)

    def test_failing_example(self):
        R = np.array(
            [
                [-0.999957, 0.00922903, 0.00203116],
                [0.00926964, 0.999739, 0.0208927],
                [-0.0018374, 0.0209105, -0.999781],
            ]
        )
        rot_recovered = np.linalg.norm(get_axisangle(R))
        expected = 3.1396582
        print(rot_recovered)
        np.testing.assert_allclose(expected, rot_recovered)

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

[dellaert]

I don’t understand what you’re doing here. The function you’re testing is not the GTSAM version, but a version of the scipy code? If possible we should fix the GTSAM version with a minimal edit, not rewrite it.

[dellaert]

Also, ClosestTo can be expensive, so the computational expectations of this piece of code would suddenly change, so we should be careful or find a computation less sensitive to small deviations from the manifold.

[johnwlambert]

Thanks for looking into this, @ProfFan.

It's interesting to see how this very small deviation from the manifold creates issues. When I looked at R^T @ R and det(R), they seemed close to I and 1, so I assumed they were still "on-manifold." But I guess the 6th or 7th decimal place is decisive?

>>> R.T @ R
array([[ 1.00000330e+00,  1.66519550e-07, -4.07243092e-07],
       [ 1.66519550e-07,  1.00000049e+00,  7.20413748e-08],
       [-4.07243092e-07,  7.20413748e-08,  1.00000268e+00]])
>>> np.linalg.det(R)
1.0000032373629155

I see the SO3::ClosestTo uses Eigen's SVD. Some say there is a closed form solution for eigenvectors of a 3x3 matrix (if we would want to get U,V as eigenvectors of 3x3 matrices R^TR and RR^T)

• https://dsp.stackexchange.com/questions/1911/analytical-expression-for-the-eigenvectors-of-a-3x3-real-symmetric-matrix
• https://twitter.com/jon_barron/status/1307356783082496000

But it may be numerically ill-behaved?

[ProfFan]

I have a Python version that cross checks with Scipy now:

================================================ test session starts ================================================
platform darwin -- Python 3.9.7, pytest-6.2.5, py-1.10.0, pluggy-1.0.0 -- /usr/local/opt/python@3.9/bin/python3.9
cachedir: .pytest_cache
rootdir: /Users/fan/Projects/FG/Reports
collected 6 items                                                                                                   

code/test_axisangle.py::TestAxisAngle::test_bruteforce_0 PASSED                                               [ 16%]
code/test_axisangle.py::TestAxisAngle::test_bruteforce_normal PASSED                                          [ 33%]
code/test_axisangle.py::TestAxisAngle::test_bruteforce_pi PASSED                                              [ 50%]
code/test_axisangle.py::TestAxisAngle::test_failing_example PASSED                                            [ 66%]
code/test_axisangle.py::TestAxisAngle::test_failing_example_handmade PASSED                                   [ 83%]
code/test_axisangle.py::TestAxisAngle::test_scipy_verysmall PASSED                                            [100%]

================================================= 6 passed in 8.52s =================================================

Still need to figure out if there is a good way to deal with the second problem.

@johnwlambert Yes the closed form is not numerically stable.

[dellaert]

I think there should be a way for GTSAM's current implementation to be more resistant to off-manifold matrices. Probably needs inspecting of math and sensitivity analysis of some sorts? Please also note that scipy uses quaternion, so that would be a good candidate in the QUATERNION conditional compilation branch.

[shteren1]

@ProfFan , @dellaert i just stumbled upon a similar issue, i have an incremental vslam optimization that runs hundreds of optimization cycles and uses results from previous iterations as initial state for new key frames i add, long story short i came across a very unpleasant event where the later Pose3 values i had in my problem were VERY far from orthogonality, as far as one of the entries of the diagonal on R*R.T was 43!

a very easy patch that solved the entire issue on my side was normalizing the rotation matrices using quaternions before initializing gtsam Pose3 objects. I think that enforcing SO3::ClosestTo on the constructor of Rot3 can prevent most of those problems without a very large run time overhead, calling it in every step of optimization is expensive, but if at least you start with orthogonal matrices at the end of an optimization cycle you don't stray too far away from it, perhaps as a last step when an optimization cycle ends its also worth calling it.