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

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

-
Are you sure the formula from Wikipedia is correct? If R is a 3x3 orthogonal matrix then its trace is between -3 and 3 so the argument of the arccos in the formula is between -2 and 1, while I'd expect it to be between -1 and 1. – Jitse Niesen Nov 21 '11 at 11:02
R is special orthogonal so the trace will always be between 3 (theta = 0; R = I) and -1 (theta = pi; R = diag([-1 -1 1])) ). The formula is also on p27 of Ma et. al. and p30 of Groves. – Damien Nov 21 '11 at 13:28
I is impossible to encode a rotation in 3-space with only 3 parameters without running into these types of problems. You should consider the set of unit quaternions to encode the rotations. They have four parameters and one constraint yielding 3 degrees of freedom and they behave nicely everywhere. You can read about them at en.wikipedia.org/wiki/Quaternion. – Tpofofn Apr 25 '12 at 10:22
@Tpofofn: The exponential map from the Lie algebra so(3) (=axis-angle) to the Lie group SO(3) is surjective, so all possible rotations CAN nicely represented using three parameters only (math.stackexchange.com/questions/107609/…). Of course, if a group structure is desired, then unit quaternions are a nice replacement for rotation matrices. And yes, the conversion unit quaternion (SU(2)) <--> axis-angle (su(2)) is easier than rotation matrix (SO(3)) <--> axis-angle (so(3)); see below. – Hauke Strasdat May 4 '12 at 10:13
@Hauke, agree, one can always represent rotations with 3 parameters, but they all break down at some point (i.e. become singular configurations). – Tpofofn May 4 '12 at 22:24

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

-
For the small rotation case, it looks like it's a version of the edit in the original question with fewer coefficients? – Damien Aug 27 '12 at 10:45
Oh, by the way, sorry for how long it's taken me to taken a proper look at it. It's certainly much simpler than the last version :) – Damien Aug 27 '12 at 10:46
Yes, the case t=3/theta=0 is the same then in the original question, just in terms of "t" instead of "theta", so you need to do less computation. To be specific, it is what wolfram alpha gives me for "arccos( (t-1)/2)/(2*sin(arccos( (t-1)/2)))" as a series expansion around t=3. – Hauke Strasdat Aug 27 '12 at 13:41
Hey Hauke, shouldn't those inequalities be flipped on the second case, i.e. $3-\epsilon > t > -1+\epsilon$ ? – Alex Flint Feb 12 '13 at 0:25
Of couse, you are right. (Fixed!) – Hauke Strasdat Feb 12 '13 at 22:39