Numerically vulnerable axis calculation in Rotation3

[IshitaTakeshi]

Abstract

nalgebra::Rotation3::axis relies on numerically vulnerable calculation.
It may return None in the place where it should return a non-None value.

Issue

In the current implementation of Rotation3, the rotation axis is calculated in the following way.

    #[inline]
    #[must_use]
    pub fn axis(&self) -> Option<Unit<Vector3<T>>>
    where
        T: RealField,
    {
        let rotmat = self.matrix();
        let axis = SVector::<T, 3>::new(
            rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone(),
            rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone(),
            rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone(),
        );

        Unit::try_new(axis, T::default_epsilon())
    }

If I initialize Rotation3 with a rotation of π radians around the y-axis, the code will look like this.

use nalgebra::{Rotation3, Vector3};

const PI: f64 = std::f64::consts::PI;

fn main() {
    let r = Rotation3::from_axis_angle(&Vector3::y_axis(), PI);
    println!("axis = {:?}", r.axis());
    println!("axis_angle = {:?}", r.axis_angle());
}

And it actually works.

axis = Some([[0.0, 1.0, 0.0]])
axis_angle = Some(([[0.0, 1.0, 0.0]], 3.141592653589793))

However, this is very vulnerable, because this behavior relies on the rounding error of the internal matrix representation.

I added the line to print the axis value in the axis function.

    #[inline]
    #[must_use]
    pub fn axis(&self) -> Option<Unit<Vector3<T>>>
    where  
        T: RealField,
    {
        let rotmat = self.matrix();
        let axis = SVector::<T, 3>::new(
            rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone(),
            rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone(),
            rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone(),
        );
          
        println!("intelnal axis value = {:?}", axis);
        Unit::try_new(axis, T::default_epsilon())    
    }

And it said

intelnal axis value = [[0.0, 2.4492935982947064e-16, 0.0]]

This is very close to [0, 0, 0]. If there was no rounding error, this code would return None in the place where it should return [0, 1, 0].

Why this happens

The matrix corresponding to the rotation of π radians around the y-axis is, without rounding error,

-1  0  0
 0  1  0
 0  0 -1

So if we calculate axis based on the matrix above, it becomes [0, 0, 0].
The same happens for the rotation around the x-axis, z-axis, or rotation of π radians around any axis. Actually, on my Ubuntu desktop, the code below printed None.

use nalgebra::{Rotation3, Vector3, Unit};

const PI: f64 = std::f64::consts::PI;

fn main() {
    let x = Unit::new_normalize(Vector3::new(1., 2., 1.));
    let r = Rotation3::from_axis_angle(&x, PI);
    println!("axis = {:?}", r.axis());
}

Possible solution

One possible solution is to handle these singular value cases as explained in this paper, section 9.4.1.2 "Logarithm map".
A tutorial on SE(3) transformation parameterizations and on-manifold optimization

Another solution is to replace the internal rotation representation in Rotation3 from a 3x3 matrix to a unit quaternion.
The conversion from a unit quaternion to a rotation vector is described in the same section of the paper.
This will improve the overall performance including rotation multiplication, but requires a large amount of modifications.