Numerical Methods – Efficient Implementation of Inverse Rodrigues Rotation Formula

numerical methodsrotations

I want to implement the Inverse Rodrigues Rotation Formula (also known as Log map from SO(3) to so(3)), in double precision code (MATLAB is fine for the example) preferably as a 3-parameter vector with the unit direction vector scaled by the magnitude of rotation.

The analytical form is (from Wikipedia):

$\theta = \arccos\left( \frac{\mathrm{trace}(R) – 1}{2} \right)$

and then use it to find the normalised axis:

$\omega = \frac{1}{2 \sin(\theta)} \begin{bmatrix} R(3,2)-R(2,3) \\ R(1,3)-R(3,1) \\ R(2,1)-R(1,2) \end{bmatrix}$

which can then be used to find the scaled axis of rotation
$\rho = \theta \omega$

Of course, $\sin(\theta)$ will cause the denominator to approach zero, which is undefined. The rotation vector $\rho$ at zero rotation is $\rho = \begin{bmatrix}0 & 0 & 0\end{bmatrix}^T$. Furthermore, it will also be undefined at $n\pi$, though we may assign the rotation to be the desired sign ($\pm\pi$)

The naive implementation is to have if() statements for floating point values close to $n\pi$ rotations, but surely there is a better way than some dirty hacks around the singularities… right?

EDIT:

At rotations near zero, empirically, the following works well:

if (trace(R) > (3 - small_number))

    inverse_sinc = 1 + (1.0 / 6.0)      * theta_2 + ...
                       (7.0 / 360.0)    * theta_4 +
                       (31.0 / 15120.0) * theta_6;

    rho = 0.5 * inverse_sinc * r;

end

where $\theta$ (and powers thereof), and $\mathbf{r} = \begin{bmatrix} R(3,2)-R(2,3) \\ R(1,3)-R(3,1) \\ R(2,1)-R(1,2) \end{bmatrix}$ are pre-calculated, and inverse_sinc (i.e. x/sin(x))is calculated from the Taylor series. This is accurate to better than 10^-11 in each axis when unit tested across a range of values (0, eps, 10^-12 through to 10^-3 and negative values for each across all three axes).

A good solution for $\theta = \pi$ still eludes me…

Best Answer

Let $\mathbf{R}\in SO(3)$ be a rotation matrix, $t=R_{1,1} + R_{2,2} + R_{3,3}$ be the trace of $\mathbf{R}$, and $\mathbf{r}=\begin{bmatrix} R(3,2)-R(2,3) \\ R(1,3)-R(3,1) \\ R(2,1)-R(1,2) \end{bmatrix}$.

We can calculate the rotation vector $\omega$ (axis-angle representation) as follows:

$$\omega = \begin{cases} \left(\frac{1}{2} - \frac{t-3}{12}\right)\mathbf{r} & \text{if}\quad t\ge3-\epsilon\\ \frac{\theta}{2\sin(\theta)}\mathbf{r} & \text{if}\quad 3-\epsilon > t > -1+\epsilon\\ \pi\frac{\mathbf{v}}{|\mathbf{v}|} & \text{if }\quad t\le -1+\epsilon \end{cases} $$ with $$\theta = \arccos\left( \frac{t - 1}{2} \right)$$ and $(w,\mathbf{v})$ being a unit quaternion $$ v_a = \frac{s}{2},\quad v_b = \frac{1}{2s}(R_{b,a}+R_{a,b}),\quad v_c = \frac{1}{2s}(R_{c,a}+R_{a,c})\\ \quad\text{with} \quad s := \sqrt{R_{a,a}-R_{b,b}-R_{c,c} + 1}\\ \text{and}\quad a := \underset{i\in\{1,2,3\}}{\arg\max}\{R_{i,i}\},\quad b := (a+1)\text{ mod } 3, \quad c := (a+2)\text{ mod }3~.$$

Background: The last case for $\theta\approx \pm \pi$ (i.e. $t\approx-1$) is calculated using the route: rotation matrix $\Rightarrow$ unit quaternion $\Rightarrow$ axis-angle.*** Here, $\pi$ is the limit of $2\arctan\left(\frac{|\mathbf{v}|}{w}\right)$ with $w = \frac{1}{2s}(R_{c,b}-R_{b,c})$.

(*** rotation matrix to unit quaternion reference: Eigen library which again refers to Ken Shoemake, "Quaternion Calculus and Fast Animation", 1987; unit quaternion to axis-angle reference: C. Hertzberg et al.: "Integrating Generic Sensor Fusion Algorithms with Sound State Representation through Encapsulation of Manifolds" Information Fusion, 2011)

Edit: It would be nice to have a higher order approximation for the $t\le-1+\epsilon$ case. Please drop a comment or edit if you have a good solution...

Edit2: Actually, there are two possible solutions for the case when $\theta$ is close to $\pi$. In both of them, we first transform the rotation matrix to the unit quaternion $q = (w, \mathbf{v})$ without any numerical issues (because of case differentiation, see links above). Then the scalar part of quaternion $w = \cos(\theta/2)$ is close to 0, and norm of vector part $|\mathbf{v}| = \sin(\theta/2)$ is close to 1 for $\theta$ close to $\pi$.

First solution: using reciprocal arguments of $\arctan$ (see properties in wiki):

$$ \arctan\left(\frac{1}{x}\right) = \frac{\pi}{2} - \arctan(x) \text{, if } x > 0 \\ \arctan\left(\frac{1}{x}\right) = -\frac{\pi}{2} - \arctan(x) \text{, if } x < 0$$

We have: $$\omega = \theta \frac{\mathbf{v}}{|\mathbf{v}|} = 2 \arctan\left(\frac{|\mathbf{v}|}{w}\right) \frac{\mathbf{v}}{|\mathbf{v}|} = \left(\pm \pi - 2 \arctan\left(\frac{w}{|\mathbf{v}|}\right) \right) \frac{\mathbf{v}}{|\mathbf{v}|}$$

Second solution: use a variant of the $\text{atan2}(y, x)$ formula that avoids inflated rounding errors (last one in definitions sections). Moreover, if we choose the quaternion with a non-negative scalar part $w = \cos(\theta/2) \ge 0$, (we always can do it since two quaternions $q$ and $-q$ represent the same rotation), we simultaneously ensure that the angle $\theta$ will be in range [0, $\pi$], and we can use single "half-angle" formula everywhere: $$ \theta = 4 \arctan\left(\frac{|\mathbf{v}|}{w + \sqrt{w^2 + |\mathbf{v}|^2}} \right) = 4 \arctan\left(\frac{|\mathbf{v}|}{w + 1} \right) $$

Related Question