位姿变换与李群、李代数

　　本博客是在学习高翔《视觉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