Exponential Coordinate与旋转矩阵的转换

Exponential Coordinate的旋转表示参考旋转表示， 即

\[e^{[w]\theta}=I+sin\theta[w]+(1-cos\theta)[w]^2\tag{1} \]

因为：

\[[w]=\begin{bmatrix} 0&-w_3&w_2\\ w_3&0&-w_1\\ -w_2&w_1&0 \end{bmatrix}\tag{2}\]

将（２）带入（１），所以有：

\[\begin{align*}e^{[w]\theta}&=I+sin\theta[w]+(1-cos\theta)[w]^2\\ &=I+sin\theta \begin{bmatrix}0&-w_3&w_2\\w_1&0&-w_1\\-w_2&w_3&0\end{bmatrix}+...\\ &=\begin{bmatrix}c_\theta+w_1^2(1-c_\theta)&w_1w_2(1-c_\theta)-w_3s_\theta&w_1w_3(1-c_\theta)+w2s_\theta\\ w_1w_2(1-c_\theta)+w_3s_\theta&c_\theta+w_2^2(1-c_\theta)&w_2w_3(1-c_\theta)-w_1s_\theta\\ w_1w_3(1-c_\theta)-w_2s_\theta&w_2w_3(1-c_\theta)+w_1s_\theta&c_\theta+w_3^2(1-c_\theta)\end{bmatrix}\end{align*}\]

上式中\(s_\theta =sin\theta,c_\theta=cos\theta\), 将上式的每个元素与旋转矩阵\(R\in SO(3)\)的元素比对，当\(sin\theta\ne 0\)则有：

\[r_{32}-r_{23}=2w_1sin\theta \]

\[r_{13}-r_{31}=2w_2sin\theta$$$$r_{21}-r_{12}=2w_3sin\theta \]

即：

\[w_1=\frac{1}{2sin\theta}(r_{32}-r_{23}) \]

\[w_2=\frac{1}{2sin\theta}(r_{13}-r_{31}) \]

\[w_3=\frac{1}{2sin\theta}(r_{21}-r_{12}) \]

所以有：

\[[w]=\begin{bmatrix}0&-w_3&w_2\\w_3&0&-w_1\\-w_2&w_1&0\end{bmatrix}=\frac{1}{2sin\theta}(R-R^T)\tag{3} \]

\[trR=r_{11}+r_{22}+r_{33} \tag{4} \]

(3)(4)满足\(w_1^2+w_2^2+w_3^2=1\)
只要在\(sin\theta\ne 0\)的情况下，根据（３）　（４）两式即可求得\(e^{[w]R}\)旋转表示．

现在讨论\(sin\theta=0\)的情况即\(\theta=k\pi\)的情况，当\(k= 0,\pm 2,\pm 4,...\),有：
$$tr R=3 \qquad R=I $$
当\(k=\pm 1,\pm 3,\pm 5,...\)时，有：

\[R=e^{[w]\pi}=I+2[w]^2 \]

再根据\(w_1^2+w_2^2+w_3^2=1\),所以：

\[w_i=\pm \sqrt{\frac{r_{ii}+1}{2}},i=1,2,3 \]

上式可能不总是返回一个单位化向量\(w\)，此时可以看看另一种解决方法是否可求得单位化\(w\):

\[2w_1W_2=r_{12} \]

\[2w_2W_3=r_{23} \]

\[2w_1W_3=r_{13} \]

一旦找到了单位\(w\), 旋转表示则可以得到：\(R=e^{[w]k\pi},k=\pm\pi,\pm 3\pi...\)

实际表示旋转的时候，我们将\(\theta\)限制在\([0,\pi]\),综合上面的推导，可以得出：
给定\(R\in SO(3),\gamma=w\theta\in R^3,\theta\in[0,\pi],w\in R^3, ||w||=1,R=e^{[w]\theta}=I+sin\theta[w]_(1-cos\theta)[w]^2\), \(w\)和\(\theta\)的求法如下：

1. 如果R=I,那么\(\theta=0,[w]\theta=0\)
2. 如果\(trR=-1\),那么\(\theta=\pi\), \(w\)可以有一下任意给出：
   \(w=\frac{1}{\sqrt{2(1+r_{33})}}\begin{bmatrix}r_{13}\\r_{23}\\1+r_{33}\end{bmatrix}\)
   或者
   \(w=\frac{1}{\sqrt{2(1+r_{22})}}\begin{bmatrix}r_{12}\\1+r_{22}\\r_{32}\end{bmatrix}\)
   或者
   \(w=\frac{1}{\sqrt{2(1+r_{11})}}\begin{bmatrix}1+r_{11}\\r_{21}\\r_{31}\end{bmatrix}\)
3. \(\theta=cos^{-1}(\frac{trR-1}{2})\in[0,\pi), \qquad [w]=\frac{1}{2sin\theta}(R-R^T)\)

旋转可以用下图形象的来表示，给定球内一点\(r\in R^3\), 让\(w=r/||r|| \qquad \theta=||r||\),则有\(r=w\theta\).对于任意个\(R\in SO(3) ， tr R \ne -1\),球内存在唯一一点使得\(e^[r]=R\)
而对于\(tr R =-1\)的情况，存在两点在球的表面上，两点连线通过圆心，\(||r|=\pi\),或者是\(R=e^{r}\)或者是\(R=e^{-r}\),两者表示同一个旋转．