9
$\begingroup$

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

$\endgroup$
  • $\begingroup$ 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. $\endgroup$ – Jitse Niesen Nov 21 '11 at 11:02
  • 3
    $\begingroup$ 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. $\endgroup$ – Damien Nov 21 '11 at 13:28
  • $\begingroup$ 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. $\endgroup$ – Tpofofn Apr 25 '12 at 10:22
  • 1
    $\begingroup$ @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. $\endgroup$ – B0rk4 May 4 '12 at 10:13
  • 1
    $\begingroup$ @Tpofofn: Excuse my passionate discussion style. You are right that quaternion are a great replacement for rotation matrices. But I just wanted to make clear that infinitesimal rotations (=axis-angle) are useful in either way, e.g. to formulate incremental updates in optimization. $\endgroup$ – B0rk4 May 5 '12 at 11:32
5
$\begingroup$

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

$\endgroup$
  • $\begingroup$ For the small rotation case, it looks like it's a version of the edit in the original question with fewer coefficients? $\endgroup$ – Damien Aug 27 '12 at 10:45
  • $\begingroup$ 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 :) $\endgroup$ – Damien Aug 27 '12 at 10:46
  • $\begingroup$ 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. $\endgroup$ – B0rk4 Aug 27 '12 at 13:41
  • $\begingroup$ Hey Hauke, shouldn't those inequalities be flipped on the second case, i.e. $3-\epsilon > t > -1+\epsilon$ ? $\endgroup$ – Alex Flint Feb 12 '13 at 0:25
  • $\begingroup$ Of couse, you are right. (Fixed!) $\endgroup$ – B0rk4 Feb 12 '13 at 22:39
0
$\begingroup$

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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.