12
$\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$
8
  • $\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$ Nov 21, 2011 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, 2011 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, 2012 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, 2012 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, 2012 at 11:32

3 Answers 3

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


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

$\endgroup$
5
  • $\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, 2012 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, 2012 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, 2012 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, 2013 at 0:25
  • $\begingroup$ @Zhakshylyk Nurlanov, thanks for the edit. Super interesting. I have not found the time to review it in detailed line by line, but looks legit at first glance! $\endgroup$
    – B0rk4
    Apr 27, 2021 at 15:27
3
$\begingroup$

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.

$\endgroup$
2
$\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$

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .