位姿變換與李羣、李代數

　　本博客是在學習高翔《視覺SLAM十四講》過程中對位姿變換與李羣李代數相關知識點做的總結，不涉及公式的證明與推導。

一、位姿變換

1、旋轉矩陣與變換矩陣

旋轉矩陣是描述剛體旋轉最常見的一種形式，而變換矩陣通常是指旋轉矩陣與平移向量組成的齊次變換矩陣。對於三維空間的位姿變換，有旋轉矩陣：

R=[r1r2r3]=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥$$R=\left[\begin{array}{ccc}{\mathbf{r}}_{\mathbf{1}}& {\mathbf{r}}_{\mathbf{2}}& {\mathbf{r}}_{\mathbf{3}}\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

平移向量：

t=[t1t2t3]T$$\mathbf{t}={\left[\begin{array}{ccc}{t}_1& t_2& t_3\end{array}\right]}^T$$

則旋轉矩陣R$R$ 與平移向量t$t$ 組成的齊次變換矩陣爲：

T=[R0Tt1]=⎡⎣⎢⎢⎢⎢r11r21r310r12r22r320r13r23r330t1t2t31⎤⎦⎥⎥⎥⎥$$T=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\\ 0& 0& 0& 1\end{array}\right]$$

旋轉矩陣R$R$ 是一種單位正交陣，它具有單位正交陣所有的性質：

{r1T⋅r2=r1T⋅r3=r2T⋅r3=0∥r1∥=∥r2∥=∥r3∥=det(R)=1$$\{\begin{array}{l}{{\mathbf{r}}_{\mathbf{1}}}^{\text{T}}\cdot {\mathbf{r}}_{\mathbf{2}}={{\mathbf{r}}_{\mathbf{1}}}^{\text{T}}\cdot {\mathbf{r}}_{\mathbf{3}}={{\mathbf{r}}_{\mathbf{2}}}^{\text{T}}\cdot {\mathbf{r}}_{\mathbf{3}}=0\\ \Vert {\mathbf{r}}_{\mathbf{1}}\Vert =\Vert {\mathbf{r}}_{\mathbf{2}}\Vert =\Vert {\mathbf{r}}_{\mathbf{3}}\Vert =\mathbf{d}\mathbf{e}\mathbf{t}(R)=1\end{array}$$

重要的是，旋轉矩陣的逆等於旋轉矩陣的轉置，即R−1=RT$R^{-1}=R^T$ ，從而有RR−1=RRT=I$R{R}^{-1}=R{R}^T=I$ 。齊次變換矩陣T$T$ 的逆爲：

T−1=[R−10T−R−1t1]=[RT0T−RTt1]$$T^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathbf{-}\mathbf{1}}& \mathbf{-}{\mathbf{R}}^{\mathbf{-}\mathbf{1}}\mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}^{\text{T}}& {\mathbf{-}\mathbf{R}}^{\text{T}}\mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]$$

假設我們由座標系A經過變換矩陣TABT=[R0Tt1]${\phantom{T}}_B^{A}T=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]$ 得到了座標系B，那麼座標系B下的點PBP${\phantom{P}}_{}^B{P}_{}^{}$ 可在座標系A中表示爲：

PAP=TABTPBP=[R0Tt1][PBP1]=RPBP+t$${\phantom{P}}_{}^A{P}_{}^{}={\phantom{T}}_B^{A}T{\phantom{P}}_{}^B{P}_{}^{}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]\left[\begin{array}{c}{\phantom{P}}_{}^B{P}_{}^{}\\ 1\end{array}\right]=R{\phantom{P}}_{}^B{P}_{}^{}+\mathbf{t}$$

同樣，座標系A下的點PAP${\phantom{P}}_{}^A{P}_{}^{}$ 可在座標系B中表示爲：

PBP=TBATPAP=TABT−1PAP=[RT0T−RTt1][PAP1]=RTPAP−RTt=RT(PAP−t)$${\phantom{P}}_{}^B{P}_{}^{}={\phantom{T}}_A^{B}T{\phantom{P}}_{}^A{P}_{}^{}={\phantom{T}}_B^A{T}_{}^{-1}{\phantom{P}}_{}^A{P}_{}^{}=\left[\begin{array}{cc}{\mathbf{R}}^{\text{T}}& {\mathbf{-}\mathbf{R}}^{\text{T}}\mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]\left[\begin{array}{c}{\phantom{P}}_{}^A{P}_{}^{}\\ 1\end{array}\right]=R^T{\phantom{P}}_{}^A{P}_{}^{}-R^T\mathbf{t}=R^T({\phantom{P}}_{}^A{P}_{}^{}-\mathbf{t})$$

2、歐拉角與旋轉矩陣

歐拉角是描述剛體旋轉最直觀的一種形式，它往往只用於可視化的人機交互，而無法直接參與運算。

對於三維空間座標系，我們通常這樣規定：

⎧⎩⎨繞 X 軸旋轉的角度稱爲橫滾角，Roll繞 Y 軸旋轉的角度稱爲俯仰角，Pitch繞 Z 軸旋轉的角度稱爲偏航角，Yaw$$\{\begin{array}{l}\text{繞 X 軸旋轉的角度稱爲橫滾角，Roll}\\ \text{繞 Y 軸旋轉的角度稱爲俯仰角，Pitch}\\ \text{繞 Z 軸旋轉的角度稱爲偏航角，Yaw}\end{array}$$

所有的旋轉，沿着旋轉軸方向順時針旋轉的角度爲正，逆時針旋轉的角度爲負。對於平移，沿着該軸正方向平移的距離爲正，負方向平移的距離爲負。

設剛體繞着Z$Z$ 軸旋轉γ$\gamma$ 角度，那麼剛體上所有點的z$z$ 座標值不變，x$x$ 與y$y$ 的座標值分別變爲：

x′=ρcos(θ+γ)=ρ(cosθ⋅cosγ−sinθ⋅sinγ)=x⋅cosγ−y⋅sinγy′=ρsin(θ+γ)=ρ(cosθ⋅sinγ+sinθ⋅cosγ)=x⋅sinγ+y⋅cosγ$$\begin{gathered} x^{\prime }=\rho cos(\theta +\gamma )=\rho (cos\theta \cdot cos\gamma -sin\theta \cdot sin\gamma )=x\cdot cos\gamma -y\cdot sin\gamma \\ {y}^{\prime }=\rho sin(\theta +\gamma )=\rho (cos\theta \cdot sin\gamma +sin\theta \cdot cos\gamma )=x\cdot sin\gamma +y\cdot cos\gamma \end{gathered}$$

寫成矩陣的形式：

⎡⎣⎢x′y′z′⎤⎦⎥=⎡⎣⎢cosγsinγ0−sinγcosγ0001⎤⎦⎥⎡⎣⎢xyz⎤⎦⎥=RZ(γ)⎡⎣⎢xyz⎤⎦⎥$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=R_Z(\gamma )\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$

從而，我們總結出歐拉角與旋轉矩陣的關係：

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Roll：RX(α)=rotx(α)=⎡⎣⎢1000cosαsinα0−sinαcosα⎤⎦⎥Pitch：RY(β)=roty(β)=⎡⎣⎢cosβ0−sinβ010sinβ0cosβ⎤⎦⎥Yaw：RZ(γ)=rotz(γ)=⎡⎣⎢cosγsinγ0−sinγcosγ0001⎤⎦⎥$$\{\begin{array}{l}\text{Roll：}{R}_{\text{X}}(\alpha )=\text{rotx}(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]\\ \text{Pitch：}{R}_{\text{Y}}(\beta )=\text{roty}(\beta )=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]\\ \text{Yaw：}{R}_{\text{Z}}(\gamma )=\text{rotz}(\gamma )=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

有了歐拉角與旋轉矩陣之間的轉換關係，下面舉一個簡單的例子驗證一下：

如上圖所示，有位於立方體頂點上的三個座標系O1$O_1$ 、O2$O_2$ 、O3$O_3$ ，以及位於立方體中心的點P$P$ ，立方體的邊長分別爲6、8、10。下面先進行三個座標系之間的變換：

• O1→O2$O_1\to O_2$ ：

  相對變換：O1$O_1$ 先沿着座標軸O1X1$O_1{X}_1$ 的正方向平移10個單位到達O2$O_2$ ，然後繞着當前位置的O1Z1$O_1{Z}_1$ 軸順時針旋轉90°，此時新得到的O1$O_1$ 與O2$O_2$ 座標系完全重合，即完成了O1$O_1$ 到O2$O_2$ 的座標系變換。以上過程可以在MATLAB中用變換矩陣表述爲：
  

  T12T=transl(10,0,0)*trotz(pi/2)=⎡⎣⎢⎢⎢10000100001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥$${\phantom{T}}_2^{1}T=\text{transl(10,0,0)*trotz(pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

  絕對變換：O1$O_1$ 先繞着座標軸O1Z1$O_1{Z}_1$ 順時針旋轉90°，然後沿着原來O1X1$O_1{X}_1$ 軸的正方向平移10個單位到達O2$O_2$ ，此時新得到的O1$O_1$ 與O2$O_2$ 座標系完全重合，即完成了O1$O_1$ 到O2$O_2$ 的座標系變換。以上過程可以在MATLAB中用變換矩陣表述爲：
  

  T12T′=transl(10,0,0)*trotz(pi/2)=⎡⎣⎢⎢⎢10000100001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥=T12T$${{\phantom{T}}_2^{1}T}^{\prime }=\text{transl(10,0,0)*trotz(pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]={\phantom{T}}_2^{1}T$$

  你可能會問，兩次不同的操作爲什麼表達式是一樣的？原因是這裏牽涉到相對變換與絕對變換的區別：
  

  ⎧⎩⎨相對變換：每一步都以新得的坐標系爲參考，每一步得到的變換矩陣依次右乘；絕對變換：始終以最初的坐標系爲參考，通常是先旋轉再平移，每一步得到的變換矩陣依次左乘；相對變換與絕對變換不可同時使用！$$\{\begin{array}{l}\text{相對變換：每一步都以新得的座標系爲參考，每一步得到的變換矩陣依次右乘；}\\ \text{絕對變換：始終以最初的座標系爲參考，通常是先旋轉再平移，每一步得到的變換矩陣依次左乘；}\\ \text{相對變換與絕對變換不可同時使用！}\end{array}$$

  當然，O1→O2$O_1\to O_2$ 的相對變換還可以有很多種操作順序，比如：
  

  T12T′′=trotz(pi/2)*transl(0,-10,0)=⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢1000010000100−1001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥=T12T$${{\phantom{T}}_2^{1}T}^{″}=\text{trotz(pi/2)*transl(0,-10,0)}=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]={\phantom{T}}_2^{1}T$$

  等等，這裏不再一一列舉。

• O2→O1$O_2\to O_1$ ：

  相對變換：
  

  T21T=transl(0,10,0)*trotz(-pi/2)=⎡⎣⎢⎢⎢10000100001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥=trotz(-pi/2)*transl(-10,0,0)=⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢100001000010−10001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥$$\begin{gathered} {\phantom{T}}_1^{2}T=\text{transl(0,10,0)*trotz(-pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] \\ =\text{trotz(-pi/2)*transl(-10,0,0)}=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& -10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] \end{gathered}$$

  絕對變換：
  

  T21T′=transl(0,10,0)*trotz(-pi/2)=⎡⎣⎢⎢⎢10000100001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥=T21T$${{\phantom{T}}_1^{2}T}^{\prime }=\text{transl(0,10,0)*trotz(-pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]={\phantom{T}}_1^{2}T$$

  相信這些表達式的意義都很容易理解，接下來驗證一下T12T${\phantom{T}}_2^{1}T$ 與T21T${\phantom{T}}_1^{2}T$ 是不是互逆：
  

  T12TT21T=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢1000010000100001⎤⎦⎥⎥⎥=I$${\phantom{T}}_2^{1}T{\phantom{T}}_1^{2}T=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=I$$

  顯然，O1→O2$O_1\to O_2$ 與O2→O1$O_2\to O_1$ 互爲逆變換，T12T${\phantom{T}}_2^{1}T$ 與T21T${\phantom{T}}_1^{2}T$ 互逆。其實，由O1$O_1$ 到O2$O_2$ 的變換矩陣就是對座標系O2$O_2$ 在座標系O1$O_1$ 中位姿的一種描述，即T12T${\phantom{T}}_2^{1}T$ 描述了O2$O_2$ 在O1$O_1$ 中的位置和姿態。同理，T21T${\phantom{T}}_1^{2}T$ 描述了O1$O_1$ 在O2$O_2$ 中的位置和姿態。那麼，我們已經知道了點P$P$ 在O1$O_1$ 座標系下的座標爲P1P=(5,4,3)T${\phantom{P}}_{}^1{P}_{}^{}=(5,4,3{)}^T$ ，座標系O1$O_1$ 在座標系O2$O_2$ 下的位姿爲T21T${\phantom{T}}_1^{2}T$ ，順理成章地，我們就可以求得點P$P$ 在O2$O_2$ 座標系下的座標爲：
  

  P2P=T21TP1P=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢5431⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥$${\phantom{P}}_{}^2{P}_{}^{}={\phantom{T}}_1^{2}T{\phantom{P}}_{}^1{P}_{}^{}=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}5\\ 4\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right]$$

  很顯然，結果跟事實是一致的。

• O2↔O3$O_2\leftrightarrow O_3$ ：
  

  T23T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥T32T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥T23TT32T=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥2=⎡⎣⎢⎢⎢1000010000100001⎤⎦⎥⎥⎥=IP3P=T32TP2P=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥$$\begin{gathered} {\phantom{T}}_3^{2}T=\text{transl(8,0,6)*troty(pi)}=\text{troty(pi)*transl(-8,0,-6)}=\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right] \\ {\phantom{T}}_2^{3}T=\text{transl(8,0,6)*troty(pi)}=\text{troty(pi)*transl(-8,0,-6)}=\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right] \\ {\phantom{T}}_3^{2}T{\phantom{T}}_2^{3}T={\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right]}^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=I \\ {\phantom{P}}_{}^3{P}_{}^{}={\phantom{T}}_2^{3}T{\phantom{P}}_{}^2{P}_{}^{}=\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right] \end{gathered}$$

• O1→O3$O_1\to O_3$ ：
  

  T13T=transl(10,8,6)*trotz(pi/2)*troty(pi)=trotz(-pi/2)*trotx(pi)*transl(-8,-10,-6)=T12TT23T=⎡⎣⎢⎢⎢0−100−100000−1010861⎤⎦⎥⎥⎥P3P=T31TP1P=T13T−1P1P=⎡⎣⎢⎢⎢0−100−100000−1081061⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢5431⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥$$\begin{gathered} {\phantom{T}}_3^{1}T=\text{transl(10,8,6)*trotz(pi/2)*troty(pi)}=\text{trotz(-pi/2)*trotx(pi)*transl(-8,-10,-6)}={\phantom{T}}_2^{1}T{\phantom{T}}_3^{2}T=\left[\begin{array}{cccc}0& -1& 0& 10\\ -1& 0& 0& 8\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right] \\ {\phantom{P}}_{}^3{P}_{}^{}={\phantom{T}}_1^{3}T{\phantom{P}}_{}^1{P}_{}^{}={\phantom{T}}_3^1{T}^{-1}{\phantom{P}}_{}^1{P}_{}^{}=\left[\begin{array}{cccc}0& -1& 0& 8\\ -1& 0& 0& 10\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}5\\ 4\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right] \end{gathered}$$

3、旋轉向量與旋轉矩陣

旋轉向量（或稱軸角，Axis-Angle），是一種定義在R3${\mathbb{R}}^3$ 上的三維向量（對於三維空間的旋轉來說），它可以描述剛體在三維空間中繞任意旋轉軸發生的旋轉。旋轉向量的方向代表旋轉軸的方向，它的模代表旋轉的角度。對於一個旋轉軸爲n$\mathbf{n}$ （∥n∥=1$\Vert \mathbf{n}\Vert =1$ ），轉角爲θ$\theta$ 的旋轉向量θn$\theta \mathbf{n}$ （θ$\theta$ 單位爲弧度），它與旋轉矩陣R$R$ 的關係爲：

• 旋轉向量到旋轉矩陣
  

  R=I+sinθn∧+(1−cosθ)n∧2=cosθI+sinθn∧+(1−cosθ)nnT(羅德裏格斯公式)$$\begin{array}{}\text{(羅德里格斯公式)}& \begin{array}{rl}R& =I+sin\theta {\mathbf{n}}^{\wedge }+(1-cos\theta ){{\mathbf{n}}^{\wedge }}^2\\ & =cos\theta I+sin\theta {\mathbf{n}}^{\wedge }+(1-cos\theta ){\mathbf{n}\mathbf{n}}^T\end{array}\end{array}$$

  這裏的n∧${\mathbf{n}}^{\wedge }$ 爲n=[nxnynz]T$\mathbf{n}={\left[\begin{array}{ccc}{n}_x& n_y& n_z\end{array}\right]}^T$ 所對應的反對稱矩陣：
  

  n∧=⎡⎣⎢0nz−ny−nz0nxny−nx0⎤⎦⎥$${\mathbf{n}}^{\wedge }=\left[\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right]$$

  且n∧2=n∧n∧=nnT−I${{\mathbf{n}}^{\wedge }}^2={\mathbf{n}}^{\wedge }{\mathbf{n}}^{\wedge }=\mathbf{n}{\mathbf{n}}^T-I$ ，n∧3=−n∧${{\mathbf{n}}^{\wedge }}^3=-{\mathbf{n}}^{\wedge }$

• 旋轉矩陣到旋轉向量
  

  ⎧⎩⎨⎪⎪θ=arccos(tr(R)−12)Rn=n$$\{\begin{array}{l}\theta =\text{arccos}(\frac{\text{tr}(R)-1}{2})\\ R\mathbf{n}=\mathbf{n}\end{array}$$

  其中，tr(R)$\text{tr}(R)$ 表示旋轉矩陣R$R$ 的跡。旋轉軸n$\mathbf{n}$ 其實就是矩陣R$R$ 特徵值1所對應的特徵向量。

4、四元數與旋轉向量

不得不指出的是，旋轉矩陣具有冗餘性，而歐拉角和旋轉向量都具有奇異性，只有四元數（Quaternions）可以完美地描述一個剛體在三維空間的旋轉。

通常，我們用單位四元數q=qw+qxi+qyj+qzk$\mathbf{q}=q_w+q_x\mathbf{i}+q_y\mathbf{j}+q_z\mathbf{k}$ 表示三維空間中任意一個旋轉，寫成向量的形式：

q=[qwqxqyqz]T或q=[qxqyqzqw]T$$\mathbf{q}={\left[\begin{array}{cccc}{q}_w& q_x& q_y& q_z\end{array}\right]}^T或\mathbf{q}={\left[\begin{array}{cccc}{q}_x& q_y& q_z& q_w\end{array}\right]}^T$$

單位四元數q$\mathbf{q}$ 與旋轉向量θn$\theta \mathbf{n}$ 的關係：

θn=θ[nxnynz]T=2arccosqw[qxsinθ2qysinθ2qzsinθ2]T(四元數到旋轉向量)$$\begin{array}{}\text{(四元數到旋轉向量)}& \theta \mathbf{n}=\theta {\left[\begin{array}{ccc}{n}_x& n_y& n_z\end{array}\right]}^T=2\text{arccos}{q}_w{\left[\begin{array}{ccc}\frac{q_x}{sin\frac{\theta }{2}}& \frac{q_y}{sin\frac{\theta }{2}}& \frac{q_z}{sin\frac{\theta }{2}}\end{array}\right]}^T\end{array}$$

qw=cosθ2，qx=nxsinθ2，qy=nysinθ2，qz=nzsinθ2(旋轉向量到四元數)$$\begin{array}{}\text{(旋轉向量到四元數)}& q_w=cos\frac{\theta }{2}，q_x=n_{x}sin\frac{\theta }{2}，q_y=n_{y}sin\frac{\theta }{2}，q_z=n_{z}sin\frac{\theta }{2}\end{array}$$

單位四元數q$\mathbf{q}$ 與旋轉矩陣R$R$ 的關係：

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥=⎡⎣⎢1−2(q2y+q2z)2(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)1−2(q2x+q2z)2(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)1−2(q2x+q2y)⎤⎦⎥(四元數到旋轉矩陣)$$\begin{array}{}\text{(四元數到旋轉矩陣)}& R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}1-2(q_y^2+q_z^2)& 2(q_x{q}_y-q_w{q}_z)& 2(q_x{q}_z+q_w{q}_y)\\ 2(q_x{q}_y+q_w{q}_z)& 1-2(q_x^2+q_z^2)& 2(q_y{q}_z-q_w{q}_x)\\ 2(q_x{q}_z-q_w{q}_y)& 2(q_y{q}_z+q_w{q}_x)& 1-2(q_x^2+q_y^2)\end{array}\right]\end{array}$$

qw=tr(R)+1−−−−−−−−√2，qx=r23−r324qw，qy=r31−r134qw，qz=r12−r214qw(旋轉矩陣到四元數)$$\begin{array}{}\text{(旋轉矩陣到四元數)}& q_w=\frac{\sqrt{\text{tr}(R)+1}}{2}，q_x=\frac{r_{23}-r_{32}}{4q_w}，q_y=\frac{r_{31}-r_{13}}{4q_w}，q_z=\frac{r_{12}-r_{21}}{4q_w}\end{array}$$

二、李羣與李代數

1、什麼是李羣

羣（Group）是一種集合加上一種運算的代數結構，李羣（Lie Group）是指具有連續（光滑）性質的羣。三維空間中的旋轉矩陣R∈SO(3)$R\in \text{SO}(3)$ 和變換矩陣T∈SE(3)$T\in \text{SE}(3)$ 都屬於李羣，它們也都有各自的名字：SO(n)$\text{SO}(n)$ 稱爲特殊正交羣（Special Orthogonal Group，或稱旋轉矩陣羣），SE(n)$\text{SE}(n)$ 稱爲特殊歐式羣（Special Euclidean Group，或稱歐式變換羣），這裏的n$n$ 不限制爲3。

SO(3)={R∈R3×3|RRT=I,det(R)=1}SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}$$\begin{gathered} \text{SO}(3)=\left\{R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,\text{det}(R)=1\right\} \\ \text{SE}(3)=\left\{T=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in \text{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\right\} \end{gathered}$$

重要的是，李羣中的旋轉矩陣羣與歐式變換羣對加法是不封閉的，只對乘法封閉，即：

R1+R2∉SO(3),T1+T2∉SE(3)R1R2∈SO(3),T1T2∈SE(3)$$\begin{gathered} R_1+R_2\notin \text{SO}(3),T_1+T_2\notin \text{SE}(3) \\ {R}_1{R}_2\in \text{SO}(3),T_1{T}_2\in \text{SE}(3) \end{gathered}$$

所以，剛體之間的位姿變換都是用乘法而不是用加法。

2、什麼是李代數

簡單地說，李代數（Lie Algebra）就是李羣的導數，它描述了李羣的局部性質，是定義在李羣正切空間（Tangent Space）上的一組向量。旋轉矩陣羣SO(3)$\text{SO}(3)$ 與歐式變換羣SE(3)$\text{SE}(3)$ 對應的李代數分別爲：

so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),Ξ=ξ∧=[ϕ∧0Tρ1]∈R4×4}$$\begin{gathered} \mathfrak{s}\mathfrak{o}(3)=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\} \\ \mathfrak{s}\mathfrak{e}(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{s}\mathfrak{o}(3),\Xi ={\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ {\mathbf{0}}^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\} \end{gathered}$$

這裏，ϕ=θn$\varphi =\theta \mathbf{n}$ 其實就是定義在R3${\mathbb{R}}^3$ 上的旋轉向量，Φ=ϕ∧$\Phi ={\varphi }^{\wedge }$ 是其所對應的反對稱矩陣。

3、李羣與李代數之間的轉換

SO(3)⇔so(3)⎧⎩⎨⎪⎪⎪⎪SO(3)−→−−−−對數映射so(3)：ϕ=ln(R)∨so(3)−→−−−−指數映射SO(3)：R=exp(ϕ∧)=eΦ$$\text{SO}(3)\iff \mathfrak{s}\mathfrak{o}(3)\{\begin{array}{l}\text{SO}(3)\stackrel{對數映射}{\to }\mathfrak{s}\mathfrak{o}(3)：\varphi =\text{ln}(R{)}^{\vee }\\ \mathfrak{s}\mathfrak{o}(3)\underset{指數映射}{\overset{}{\to }}\text{SO}(3)：R=\text{exp}({\varphi }^{\wedge })=e^{\Phi }\end{array}$$

SE(3)⇔se(3)⎧⎩⎨⎪⎪⎪⎪SE(3)−→−−−−對數映射se(3)：ξ=ln(T)∨se(3)−→−−−−指數映射SE(3)：T=exp(ξ∧)=eΞ$$\text{SE}(3)\iff \mathfrak{s}\mathfrak{e}(3)\{\begin{array}{l}\text{SE}(3)\stackrel{對數映射}{\to }\mathfrak{s}\mathfrak{e}(3)：\xi =\text{ln}(T{)}^{\vee }\\ \mathfrak{s}\mathfrak{e}(3)\underset{指數映射}{\overset{}{\to }}\text{SE}(3)：T=\text{exp}({\xi }^{\wedge })=e^{\Xi }\end{array}$$

---

參考文獻
[1] https://blog.csdn.net/csxiaoshui/article/details/65437633
[2] 高翔, 張濤. 視覺SLAM 十四講[M]. 電子工業出版社, 2017.
[3] https://blog.csdn.net/tms_li/article/details/78599748?locationNum=4&fps=1