Efficient and Accurate Numerical Implementation of the Inverse Rodrigues Rotation Formula (Rotation Matrix -> Axis-Angle)

Question

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...

Answer 1

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) $$

Answer 2

I think this might be worth writing up here. I already gave an implementation of the idea in this Mathematica Stack Exchange post, but expressing it in mathematical notation might make the algorithm more accessible.

As already noted in the OP, one can obtain the (unnormalized) axis in the nondegenerate case from the skew-symmetric part of a rotation matrix $\mathbf R$:

$$\mathbf x=\begin{pmatrix}r_{32}-r_{23}\\r_{13}-r_{31}\\r_{21}-r_{12}\end{pmatrix}$$

After normalizing, $\hat{\mathbf x}=\dfrac{\mathbf x}{\|\mathbf x\|}$, one can try to determine two vectors that are orthogonal to it and to each other, which I'll call $\hat{\mathbf y}$ and $\hat{\mathbf z}$. Then, one can compute the angle as

$$\theta=\arctan(\hat{\mathbf y}^\top\mathbf R\hat{\mathbf y},\hat{\mathbf z}^\top\mathbf R\hat{\mathbf y})$$

where $\arctan(x,y)$ is two-argument arctangent (sometimes referred to as atan2(y,x) in some languages).

A convenient set of orthogonal vectors is supplied by what I call the "Pixar method". Letting $\hat{\mathbf x}=\begin{pmatrix}x_1&x_2&x_3\end{pmatrix}^\top$, we can compute an appropriate $\hat{\mathbf y}$ and $\hat{\mathbf z}$ with the following procedure:

$$\begin{align*} s&=\operatorname{sign}(x_3)\\ a&=-\frac1{s+x_3}\\ b&=x_1 x_2 a\\ \hat{\mathbf y}&=\begin{pmatrix}1+sax_1^2&sb&-sx_1\end{pmatrix}^\top\\ \hat{\mathbf z}&=\begin{pmatrix}b&s+ax_2^2&-x_2\end{pmatrix}^\top \end{align*}$$

where $\operatorname{sign}(x)=\begin{cases}1&x\ge0\\-1&\text{otherwise}\end{cases}$.

In the degenerate case, when $\|\mathbf x\|=0$, we instead count the number of $1$ entries in the diagonal elements. If there are three $1$'s on the diagonal (i.e. the identity matrix), we set $\theta=0$; otherwise, we set $\theta=\pi$.

To get the axis, we evaluate $\mathbf S=\dfrac12(\mathbf I+\mathbf R)$, and then extract the column of $\mathbf S$ with the largest norm. (In my Mathematica implementation, I use $\|\cdot\|_\infty$ for convenience). User Jens explains in his answer how this works.

---

As an aside, you might want to read this paper by Palais and Palais, as well as this followup by Palais, Palais, and Rodi.

Answer 3

A slightly different take, that uses eigenvector calculation to determine the axis.

Given a $3\times3$ rotation matrix, $R$, find a unit vector, $v$, and the corresponding angle, $\theta$ such that (Rodrigues formula).

$R = I + \sin(\theta)\hat{v} + (1-\cos(\theta))\hat{v}^2$

where $\hat{v}$ denotes the antisymmetric matrix,

$ \begin{bmatrix} 0 & -v(3) & v(2) \\ v(3) & 0 & -v(1) \\ -v(2) & v(1) & 0 \end{bmatrix} $

$v$ is an (not "the" because $-v$ will also work in general, and any unit vector will work for $R=I$) axis of rotation, and $\theta$ is the corresponding angle of rotation. Thus, finding $v$ and $\theta$ answers the question asked above.

$v$ can be found as an ("the" if $R \ne I$) eigenvector of $R$ corresponding to eigenvalue 1.

$\theta$ satisfies the following.

1. $\DeclareMathOperator{\trace}{trace} \trace(R) = 1 + 2\cos(\theta)$.
2. $\trace(\hat{v}R) = -2\sin(\theta)$.

Both of these can be obtained by direct calculation from Rodrigues formula above.

Thus, $\DeclareMathOperator{\atantwo}{atan2} \theta = \atantwo(-\trace(\hat{v}R),\trace(R)-1)$.

In summary,

• $v$ is an eigenvector of $R$ corresponding to eigenvalue 1.
• $\theta = \atantwo(-\trace(\hat{v}R),\trace(R)-1)$

solves the problem without any special cases needed.