第四章

李群和李代数

群

群（Group）是一种集合加上一种运算的代数结构
若集合记作$A$,运算记作$\cdot$,那么群就可以记作$G=(A,\cdot)$
这个运算需要满足以下条件：

1. 封闭性： $\forall a_{1},a_{2}\in A, a_{1} \cdot a_{2}\in A$
1. 结合率： $\forall a_{1},a_{2},a_{3}\in A,(a_{1}\cdot a_{2})\cdot a_{3}=a_{1}\cdot (a_{2}\cdot a_{3})$
1. 幺元： $\exists a_{0} \in A,$ $s.t.$ $a\cdot a^{-1}=a_{0}$
1. 逆： $\forall a\in A,\exists a^{-1} \in A$, $s.t.$ $a\cdot a^{-1}=a_{0}$

可以记为：“凤姐咬你”

$SO(3)与SE(3)$

三维旋转矩阵构成特殊正交群(Special Orthogonal Group)$SO(3)$
变换矩阵构成特殊欧氏群(Special Euclidean Group)$SE(3)$

$$SO(3)=\{R\in \mathbb{R}^{3\times 3} | R R^T =I ,\det(R) = 1 \}$$ $$SE(3)=\{ T=\left[\begin{matrix} R & t \\ \mathbf{0}^T & 1 \end{matrix}\right] \in \mathbb{R^{4 \times 4}} | R \in SO(3) , t\in \mathbb{R^3} \}$$

李群

李群是指具有连续（光滑）性质的群
对于整数$\mathbb{Z}$,由于是离散的，所以就不是李群

$SO(n)和SE(n)$都是李群

李代数

我们从旋转矩阵$R$引出

由于旋转矩阵是正交矩阵，故有：$$R R^T=I$$
令其随时间变化，记为：$$R(t) R(t)^T =I$$
两边对时间求导有:$$\dot{R(t)} R(t)^T + R(t) \dot{R}(t)^T =0$$
整理得：$$\dot{R(t)}R(t)^T = -(\dot{R}(t)R(t)^T)^T$$
故$\dot{R}(t)R(t)^T$是一个反对称矩阵，而反对称矩阵可以使用一个三维向量转换得到，这个转换符号为^，在这里我们把这个三维向量记为$\phi$，即：$$\dot{R}(t)R(t)^T = \phi(t)^{\wedge} \;\;\;\;\;(-)$$
等价的是：$$(\dot{R}(t)R(t)^T)^{\vee} = \phi(t)$$
对（-）式两边同乘$R(t)$，得到：$$\dot{R}(t) = \phi(t)^{\wedge} R(t)$$
可以看到，虽然$R(t)$对加法不封闭，我们不能直接使用无穷小增量去定义它的导数，但是我们得到了对$R(t)$求导是就相当于左成一个$\phi^{\wedge}$

$\phi$反映了$R$的导数性质，我们称它在$SO(3)$原点附近的正切空间(Tangent Space)上，同时假设在$t_{0}$附近，$\phi$保持为常数，值为$\phi_{0}$，带入（-）式有：$$\dot{R}(t)=\phi(t_{0})^{\wedge} R(t) = \phi_{0}^{\wedge} R(t)$$

上式是一个关于$R$的微分方程，由初始条件$R(0)=I$
解得：$$R(t)=\exp(\phi_{0}^{\wedge}t)$$

$\phi$正是对应到$SO(3)$上的李代数

每个李群都有与之对应的李代数，李代数描述了李群的局部性质，准确的说，是单位元附近的正切空间

李代数的定义直接粘贴到这里（写公式太麻烦了）

其中二元运算被称为李括号
例子：三维向量$\mathbb{R^3}$上定义的叉积$\times$是一种李括号，因此$\mathfrak{g}=(\mathbb{R^3},\mathbb{R},\times)$构成了一个李代数

如刚才所说，$\phi$是一种李代数的元素，记$\Phi=\phi^{\wedge} \in \mathbb{R^{3 \times 3}}$，$\phi_{1}和\phi_{2}$的李括号是：$$[\phi_{1},\phi_{2}]=(\Phi_{1} \Phi_{2} - \Phi_{2} \Phi_{1})^{\vee}$$
可以验证这个李括号符合上述4条性质

$\mathfrak{so}(3)$的元素是$\phi$，下面给出李代数：$$\mathfrak{so}=\{ \phi \in \mathbb{R^3} ,\Phi=\phi^{\wedge} \in \mathbb{R^{3 \times 3}} \}$$

有时也使用$\hat{\phi}$表示反对称

下面直接给出$SE(3)$对应的李代数$\mathfrak{se}(3) \in \mathbb{R^6}$ $$\mathfrak{se}(3)=\left\{ \xi = \left[\begin{matrix} \rho \\ \phi \end{matrix} \right] \in \mathbb{R^6}, \rho \in \mathbb{R^3}, \phi \in \mathfrak{so}(3), \xi^{\wedge} = \left [\begin{matrix} \phi^{\wedge} & \rho \\ \mathbf{0^T} & 0 \end{matrix} \right] \in \mathbb{R^{4 \times 4}} \right\}$$

$\mathfrak{se}(3)$的元素记作$\xi$，它是一个六维向量，前三位为平移（含义与变换矩阵中的平移不同），记作$\rho$；后三维为旋转，记作$\phi$，实质上是$\mathfrak{so}(3)$的元素。同时，在这里，^不再表示反对称，而是表示从6维向量变为4维矩阵，依然保持的是^表示由向量变矩阵，$^\vee$表示又矩阵到向量。

它的李括号是：$$[\xi_{1},\xi_{2}]=(\xi_{1}^{\wedge} \xi_{2}^{\wedge}-\xi_{2}^{\wedge} \xi_{1}^{\wedge})^{\vee}$$
和$\mathfrak{so}(3)$基本一样

指数映射和对数映射

这块直接给结论了，能大概理解就可以
注意：$\phi=\theta \mathbf{a}$

李代数求导与扰动模型

费了半天劲看了李群李代数内容，下面看看具体是做什么的吧

实际当中，我们常常需要对位姿进行估计。
使用李代数的一大动机就是进行优化，在优化过程中导数是非常重要的，一开始我们说李群加法不封闭，所以引入了李代数，李代数是加法封闭的，所以类似的，我们可以在李代数部分使用极限的思想来求导，但是在正式开始之前，我们还是要看看，$SO(3)$做两个矩阵乘法时候，李代数中是否就是做的加法，即下式是否成立：$$\exp(\phi_{1}^{\wedge}) \exp(\phi_{2}^{\wedge})=\exp((\phi_{1}+\phi_{2})^{\wedge})$$
如果是标量，无疑是成立的，但是现在是矩阵，两边取对数，得到：$$\ln(\exp(A) \exp(B))=A+B$$
就是研究这个式子是否成立。
Baker-Campbell-Hausdorff(BCH)公式告诉我们，这个式子不成立，成立的具体式子如下：$$\ln (\exp(A) \exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]] -\frac{1}{12}[B,[A,B]] + ··· $$
其中[ ]为李括号。

虽然不相等，但是在实际工程中，我们好像也不需要一定相等，只要足够精确就够了，特别地，当$\phi_{1}$或$\phi_{2}$为小量时，小量二次以上的项都可以被忽略。从而，BCH有线性近似表达：

BCH线性近似表达

公式为：

这里的雅可比和上面的一样：

它的逆为：

右乘雅可比只需要对自变量取符号：$$\mathbf{J_{r}(\phi)}=\mathbf{J_{l}(-\phi)}$$
现在根据这个近似公式，我们就可以利用刚才我们希望的等式了，至少现在是近似相等了

比如，某个旋转矩阵$R$，设它对应的李代数是$\phi$，我们对它做乘一个微小旋转，记作$\Delta R$，对应的李代数为$\Delta \phi$，根据BCH近似有；$$\exp(\Delta \phi^{\wedge}) \exp(\phi^{\wedge})=\exp((\phi+\mathbf{J_{l}^{-1}(\phi)} \Delta \phi)^{\wedge})$$
反之，如果在李代数上进行微小的加法，可近似为李群上带左右雅可比的乘法:$$\exp((\phi+ \Delta \phi)^{\wedge})=\exp((\mathbf{J_{l}} \Delta \phi)^{\wedge}) \exp(\phi^{\wedge})=\exp(\phi ^{\wedge})\exp((\mathbf{J_{r}} \Delta \phi)^{\wedge})$$

$SO(3)$李代数求导

使用李代数解决求导问题的两种思路：

用李代数表示姿态，然后根据李代数加法对李代数求导
对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动或右扰动模型

直接求导

假设对一个空间点$p$进行了旋转，得到$Rp$，现在要求旋转后的点坐标相对于旋转的导数，（非正式的）记为：$$\frac{\partial(Rp)}{\partial R}$$

$R$没有加法，无法直接求导，设其对应的李代数是$\phi$，有：$$R=\exp(\phi^{\wedge})$$

带入有：$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}$$

$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}=\lim_{ \delta \to 0 } \frac{\exp((\phi + \delta \phi)^{\wedge})p-\exp(\phi ^{\wedge})p}{\delta \phi}$$

再带入刚才的BCH近似公式：$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}=\lim_{ \delta \to 0 } \frac{\exp((\mathbf{J_{l}} \delta \phi)^{\wedge}) \exp(\phi^{\wedge})-\exp(\phi ^{\wedge})p}{\delta \phi}$$
把$\exp((\mathbf{J_{l}} \Delta \phi)^{\wedge})$进行线性近似（泰勒展开）：$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}=\lim_{ \delta \to 0 } \frac{(I+(\mathbf{J_{l}} \delta \phi)^{\wedge}) \exp(\phi^{\wedge})-\exp(\phi ^{\wedge})p}{\delta \phi}$$
拆开有：$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}=\lim_{ \delta \to 0 } \frac{(\mathbf{J_{l}} \delta \phi)^{\wedge} \exp(\phi ^{\wedge})p}{\delta \phi}$$
我们知道^就相当于叉乘，所以有：$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}=\lim_{ \delta \to 0 } \frac{-(\exp(\phi ^{\wedge})p)^{\wedge} \mathbf{J_{l}} \delta \phi }{\delta \phi}$$
将$\delta \phi$消去，把$R$带回来，得到：$$\frac{\partial(\exp(\phi^{\wedge})p)}{\partial \phi}=-(Rp)^{\wedge} \mathbf{J_{l}}$$
最后得到旋转后的点相对于李代数的导数：$$\frac{\partial(Rp)}{\partial R}=-(Rp)^{\wedge} \mathbf{J_{l}}$$
缺点是结果中含有$\mathbf{J_{l}}$，这个雅可比的形式很复杂，而扰动模型就好很多

扰动模型（左乘）

$R$不能做加法，但是可以做乘法，下面对它进行左乘运算，左乘一个$\Delta R$，看这个$\Delta R$对$R$的影响有多大（左乘和右乘的结果有一点区别）

同样需要引入李代数进行运算，设$\Delta R$对应的李代数为$\varphi$，对$\varphi$求导：$$\frac{\partial(Rp)}{\partial R}=\lim_{ \varphi \to 0 } \frac{\exp(\varphi^{\wedge}) \exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi}$$
这次更简单，不需要使用BCH的结论了，直接使用泰勒展开：$$\frac{\partial(Rp)}{\partial R}=\lim_{ \varphi \to 0 } \frac{(I+ \varphi ^{\wedge}) \exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi}$$
拆开得到：$$\frac{\partial(Rp)}{\partial R}=\lim_{ \varphi \to 0 } \frac{\varphi ^{\wedge} R p}{\varphi}$$
依然利用叉乘性质：$$\frac{\partial(Rp)}{\partial R}=\lim_{ \varphi \to 0 } \frac{-(Rp)^{\wedge} \varphi}{\varphi}=-(Rp)^{\wedge}$$
这个结果相对于直接对李代数求导，省去了一个雅可比的计算，所以扰动模型更加常用。

$SE(3)$李代数求导

这里就没有直接对李代数求导了，因为本身就麻烦，再加上$SE(3)$也麻烦，就更麻烦了

扰动模型

假设某空间点$p$经过一次变换$T$(对应的李代数是$\xi$)，得到$Tp$，现在给$T$左乘一个扰动$\Delta T=\exp(\xi^{\wedge})$，设扰动项的李代数为$\delta \xi = [\delta \rho ,\delta \phi]^{T}$

就有：$$\frac{\partial(Tp)}{\partial \delta \xi}=\lim_{ \delta \to 0 } \frac {\exp(\delta \xi ^{\wedge}) \exp(\xi ^{\wedge})p-\exp(\xi ^{\wedge})p}{\delta \xi}$$
下面的推导就直接放到这里了：

最后的结果也使用一个算符$\odot$表示，把一个齐次坐标空间点，变为一个4 $\times$ 6的矩阵

补充：矩阵求导
假设$\mathbf{a,b,x,y}$都是列向量，那么有：

性质证明

下面做两道课后题：

1.证明：$Rp^{\wedge}R^{T}=(Rp)^{\wedge}$
这道题目关键在于利用旋转向量叉乘的性质

$$R(a\times b)=Ra\times Rb\;\;\;\;\;\;a,b \in \mathbb{R^{3}}$$

于是有（$w$是一个三维向量）：

$$Rp^{\wedge}R^{T} w=Rp \times R^T w=R(p\times R^{T}w)=Rp\times R R^{T}w=Rp \times w =(Rp)^{\wedge}w$$

得证

2.证明：$R \exp(p^{\wedge}) R^{T}=\exp((Rp))^{\wedge}$
先带入刚刚证明的性质：$$\exp((Rp)^{\wedge})=\exp(R p^{\wedge} R^{T})$$
即证明：$$Rp^{\wedge}R^{T}=\exp(Rp^{\wedge}R^{T})$$
我们又根据无穷级数：$$\exp(A)=\sum_{k=0}^{\infty} \frac{A^{k}}{k!}$$
展开有：$$\exp(Rp^{\wedge}R^{T})=\sum_{k=0}^{\infty} \frac{(Rp^{\wedge}R^{T})^{k}}{k!}=\sum_{k=0}^{\infty} \frac{R(p^{\wedge})^{k}R^{T}}{k!}=R \sum_{k=0}^{\infty} \frac{(p^{\wedge})^{k}}{k!} R^{T}=R \exp(p^{\wedge})R^{T}$$
得证