How to get the matrix logarithm of the rotation

Question

I have the following rotation matrix

$$ R_{sb} = e^{[\hat{\omega}]\theta} = \begin{bmatrix} \cos\theta_1 & 0 & -\sin\theta_1 \\ \sin\theta_1&0&\cos\theta_1\\0&-1&0\end{bmatrix}, \quad \in SO(3) $$

In the book, it says

(a) if $R_{sb}=I$ then $\theta=0$ and $\hat{\omega}$ is undefined.

(b) If tr$R=-1$ then $\theta=\pi$. Set $\hat{\omega}$ equal to any of the following three vectors that is a feasible solution. $$ \hat{\omega} = \frac{1}{\sqrt{2(1+r_{33})}} \begin{bmatrix} r_{13} \\ r_{23} \\ 1+r_{33}\end{bmatrix}, \text{ or } \hat{\omega} = \frac{1}{\sqrt{2(1+r_{22})}} \begin{bmatrix} r_{12} \\ 1+r_{22} \\ r_{32}\end{bmatrix}, \text{ or } \hat{\omega} = \frac{1}{\sqrt{2(1+r_{11})}} \begin{bmatrix} 1+r_{11} \\ r_{21} \\ r_{31}\end{bmatrix} $$

(c) Otherwise $\theta = \cos^{-1} \left( \frac{1}{2}(trR-1) \right) \in [0,\pi)$ and $[\hat{\omega}] = \frac{1}{2\sin\theta} (R_{sb}-R_{sb}^T)$.

Since $R_{sb}$ varies depending on the value of $\theta_1$, I'm not able to extract $[\hat{\omega}]\theta$ so that I can get $[r]=[\hat{\omega}]\theta$ where [] indicates a skew-symmetric matrix. I need $r$ to compute $A^{-1}(r)$ where

$$ A(r) = I - \frac{1-\cos\lVert r \rVert}{\lVert r \rVert^2} [r] + \frac{\lVert r \rVert-\sin\lVert r \rVert}{\lVert r \rVert^3} [r]^2 $$

When I choose (c), the matrix has no inverse, Matlab freezes at computing the inverse. Any suggestions?

Answer 1

According to Terse Notes on Riemannian Geometry by Tom Fletcher, the logarithm of any matrix $R$ in $SO(3)$ is $$ \log(R)=\left\{\begin{array}{cc}~0\,,&\theta=0\,,\\\displaystyle\frac{\theta}{2\sin\theta}(R-R^\top)&|\theta|\in(0,\pi)\,,\end{array}\right. $$ where ${\rm tr}(R)=2\cos\theta+1\,.$ This worked very well for me in python.

Remarks.

1. For $\theta=0$ Fletcher seems to have a typo. It should be a zero $3\times 3$-matrix. Not the identity matrix $I$.

2. If your book says that when $R=I$ the matrix logarithm is undefined then this is wrong. The matrix log of the identity matrix is the zero matrix.

3. In your matrix, $$\tag{1} R_{sb}=\left(\begin{matrix}\cos\varphi &0&-\sin\varphi\\\sin\varphi&0&\cos\varphi\\0&-1&0\\\end{matrix}\right) $$ ${\rm tr}(R)=-1$ holds for $\varphi=\pm (2k-1)\pi\,,\,k\in\mathbb N\,.$ Fletcher's $\theta$ for such $\varphi$ will be $$\theta=\pi$$ but then $\sin\theta=0$ and the formula cannot be applied. This is really a singular case: for $\varphi\approx\pm (2k-1)\pi\,,\,k\in\mathbb N\,,$ we have $\cos\varphi\approx-1$ and $\sin\varphi\approx 0$ so that $$ R_{sb}\approx\left(\begin{matrix}-1&0&0\\0&0&-1\\0&-1&0\\\end{matrix}\right)\,. $$ This is a rotation around the axis $$ \left(\begin{matrix}0\\+1\\-1\end{matrix}\right) $$ by the angle $\pi\,.$ (Note that the $x$-axis flips its sign.) Numerically I found it acceptable to subtract a small number 1E-5 from $\theta=\pi$ and apply Fletcher's formula.

4. From $$ R_{sb}-R_{sb}^\top=\left(\begin{matrix}0&-\sin\varphi&-\sin\varphi\\\sin\varphi&0&1+\cos\varphi\\\sin\varphi&-1-\cos\varphi&0\\\end{matrix}\right) $$ we see that $R_{sb}-R_{sb}^\top$ is zero for $\varphi=\pm (2k-1)\pi\,,\,k\in\mathbb N\,,$ which are exactly the problematic cases where ${\rm tr}(R)=-1$ and $\theta=\pi\,.$ Using $$ {\rm tr}(R)=\cos\varphi=2\cos\theta+1 $$ we can write $$ R_{sb}-R_{sb}^\top=\left(\begin{matrix}0&-\omega&-\omega\\\omega&0&2\cos\theta+2\\\omega&-2\cos\theta-2&0\\\end{matrix}\right) $$ where $\omega=\sin(\arccos(2\cos\theta+1))\,.$ Experimentally in python it looks like $$ \lim_{\theta\to\pi}\frac{\omega}{\sin\theta}=\sqrt{2}\,,\quad\quad \lim_{\theta\to\pi}\frac{2+2\cos(\theta)}{\sin\theta}=0\,. $$ There is probably a way of proving this somehow. This would mean that, for this particular matrix $R_{sb}$ from (1), the matrix log for the singular case $\theta=\pi$ is $$ \log(R_{sb})=\frac{\pi}{\sqrt{2}}\left(\begin{matrix}0&-1&-1\\1&0&0\\1&0&0\\\end{matrix}\right)\,. $$