Bug in base.r2q

[PhilNad]

I get the following surprising result when using the UnitQuaternion class as the input

R = np.array([ [0, -1, 0], 
               [-1, 0, 0], 
               [0,  0, -1]])

UnitQuaternion(SO3(R)).SO3()

produces a different rotation matrix

SO3(array([[ 0.,  1.,  0.],
           [ 1.,  0.,  0.],
           [ 0.,  0., -1.]]))

and

UnitQuaternion(SO3(R)).vec_xyzs

produces

array([0.70710678, 0.70710678, 0.        , 0.        ])

If I try it with scipy, I get:

from scipy.spatial.transform import Rotation
Rotation.from_matrix(R).as_quat()

Output in (xyzs) format:

array([ 0.70710678, -0.70710678,  0.        ,  0.        ])

Also, the spatialmath.base.r2q_old method gives the same result as scipy (in sxyz format):

r2q_old(R)
array([ 0. ,  0.70710678, -0.70710678,  0.        ])

which indicates that spatialmath.base.r2q might be erroneous.

Is it possible that spatialmath.base.r2q be the culprit?

[petercorke]

Thanks for spotting this. Using base I get

>>> R
array([[ 0, -1,  0],
       [-1,  0,  0],
       [ 0,  0, -1]])
>>> r2q(R)
array([0.        , 0.70710678, 0.70710678, 0.        ])

and the sign of the third element is wrong. If I uncomment r2q_old as you did, I get

>>> r2q_old(R)
array([ 0.        ,  0.70710678, -0.70710678,  0.        ])

This also agrees with the native MATLAB implementation and my previous MATLAB Toolbox implementation which was translated to r2q_old.

The code is a faithful implementation of A Survey on the Computation of Quaternions from Rotation Matrices by Soheil Sarabandi and Federico Thomas (76) to (79). The problem is with the sign transfer mentioned below the equations: $(r_{13} - r_{31})$ is zero $(0-0)$ in this case so $e_2$ does not get set to a negative value, which it should be. In fact, for this example, all the expressions have a value of zero.

The solution isn't as simple as changing < to <= or adding a tolerance because it would flip the sign of both terms.

I should probably revert to r2q_old and maybe contact the authors of the paper. I made the change for performance reasons, and the cases I tested for gave the correct answer.

[petercorke]

I had a very helpful reply from Federico Thomas, coauthor of the referenced paper. This is fixed now on typing branch and I've added more extensive unit tests as well. Please have a play and let me know.

[PhilNad]

I read the paper you linked and clearly Cayley's method seems to be the best. The new implementation looks good but would it be possible to expand a bit on the logic behind how the choice of which of the four cases is made (depending on the result from np.argmax(...)?

[petercorke]

Federico Thomas sent me his MATLAB version of the Cayley code and I translated the sign transfer code across. There seems to be a bit more detail about the sign transfer in this paper.

[PhilNad]

Ah yes, I get it now: "assuming that an element of the quaternion close to zero is positive is a bad choice because a small perturbation in the rotation matrix might induce a chance of its sign leading to an inconsistency with the rest of signs. A better option is to assume that the largest component of the quaternion is positive".
Makes sense.