视觉SLAM十四讲第四讲笔记

这一讲主要是要理解什么是 李群，李代数。 SO(3), SE(3)与对应李代数的表示方式。理解什么是BCH和 BCH近似的意义。学会在李代数上的 扰动模型。
上一讲中，已经了解到了旋转平移的表示方法，但是在SLAM中，除了表示之外，我们还需要进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。 一种典型的方式是把它构建成一个优化问题，求解最优的R,t ，使得误差最小化。
旋转矩阵自身是带有约束的(正交且行列式为1)。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群——李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

一、什么是李群，李代数。

回顾上一讲，我们把三维旋转矩阵构成了特殊正交群$SO(3)$,把变换矩阵构成了特殊欧式群$SE(3)$:

SO(3)={R∈Rx×3∣RRT=I,det(R)=1} SO(3) = \{R \in \R^{x \times 3} | RR^T=I, det(R)=1\} SO(3)={R∈Rx×3∣RRT=I,det(R)=1}

$$SE(3)=\left\{\begin{array}{c}T=\left[\begin{array}{c}Rt\\ 0^{T}1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\end{array}\right\}$$

这里有一个性质，就是旋转矩阵也好，变换矩阵也好，它们对加法是不封闭的。也就是对于任意两个旋转矩阵$R_1,R_2$，它们按照加法的定义，和不再是一个旋转矩阵，对于变换矩阵亦是如此。
$$R_1+R_2\notin SO(3)$$

但是它们对乘法是封闭的:

$$R_1{R}_2\in SO(3),T_1{T}_2\in SE(3)$$
我们知道乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个 运算的集合，我们把它叫做 群。

群是一种集合加上一种运算的代数结构。 我们把集合记作$A$，运算记作$\cdot$。那么群可以记作$G(A,\cdot )$。群的运算满足以下条件:

1. 封闭性: $\forall a_1,a_2\in A,a_1\cdot a_2\in A$
2. 结合律: $\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)$
3. 幺元: $\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆: $\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

李群是指具有连续(光滑)性质的群。 $SO(n)$和$SE(n)$，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。

我们再考虑一个任意旋转矩阵$R$，我们知道它满足:
$$R{R}^T=I$$

现在，我们给旋转矩阵加个时间的性质，也就是它会随时间连续变化，即为时间的函数$R(t)$。根据旋转矩阵的性质，我们有:
$$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=-(R(t)\dot{R}(t{)}^T)$$

也就是说，$\dot{R}(t)R(t{)}^T$是一个反对称矩阵。我们用上一讲的表示符号表示$\wedge$
$$\dot{R}(t)R(t{)}^T=\varphi (t{)}^{\wedge }$$
式子两边右乘$R(t)$，由于$R$是正交矩阵，有:
$$\dot{R}(t)=\varphi (t{)}^{\wedge }R(t)=[\begin{array}{c}0-{\varphi }_3{\varphi }_2\\ {\varphi }_{3}0-{\varphi }_1\\ -{\varphi }_2{\varphi }_{1}0\end{array}]R(t)$$

可以看到，每对旋转矩阵求一次导数，只需左乘一个$ ϕ^{\land}(t)$矩阵即可。如果把$R(t)$在0附近进行一阶泰勒展开:
$$R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)=I+\varphi (t_0{)}^{\wedge }(t)$$
设，在$t_0$附近，$\varphi$保持为常数$\varphi (t_0)={\varphi }_0$，
$$\dot{R}(t)=\varphi (t_0{)}^{\wedge }R(t)={\varphi }_0^{\wedge }R(t)$$
我们知道初始值$R(0)=I$，所以:
$$R(t)=exp({\varphi }_0^{\wedge }t)$$

我们看到， 旋转矩阵$R$与另一个反对称矩阵${\varphi }_0$通过指数关系发生了联系。那么给定某时刻的$R$，我们就能求得一个$\varphi$。 这个$\varphi$正是对应到$SO(3)$上的李代数$so(3)$。而$exp({\varphi }^{\wedge })$正是李群与李代数间的指数/对数映射。

每个李群都有与之对应的李代数。 李代数描述了李群的局部性质。

李代数的定义 如下：

上面说的$\varphi$就是对应$SO(3)$的李代数。每个$\varphi$都可以生成一个反对称矩阵:
$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{c}0-{\varphi }_3{\varphi }_2\\ {\varphi }_{3}0-{\varphi }_1\\ -{\varphi }_2{\varphi }_{1}0\end{array}\right]\in {\mathbb{R}}^{3\times 3}$$
在这个定义下，两个向量${\varphi }_1,{\varphi }_2$的李括号为:

$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$$

$so(3)$的元素是3维向量或者3维反对称矩阵。 它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。
so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3} so(3) = \{\phi \in \R^3, \Phi=\phi^{\land} \in \R^{3 \times 3} \} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}

现在讲了$SO(3)$，特殊欧式群$SE(3)$的李代数同理可得:
$$se(3)=\{ϵ=[\begin{array}{c}\rho \\ \varphi \end{array}]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in so(3),{ϵ}^{\wedge }=[\begin{array}{c}{\varphi }^{\wedge }\rho \\ 0^{T}0\end{array}]\in {\mathbb{R}}^{4\times 4}\}$$

我们把每个$se(3)$元素记作$ϵ$，它是一个六维向量。前三维为平移，记作$\rho$；后三维为旋转，记作$\varphi$， 实质上是$so(3)$元素。同时,我们拓展了$\wedge$符号的含义。在$se(3)$中， 同样使用$\wedge$符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称:
$${ϵ}^{\wedge }=\left[\begin{array}{c}{\varphi }^{\wedge }\rho \\ 0^{T}0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$
我们仍使用$\wedge$和$\vee$符号来指代“从向量到矩阵”和“从矩阵到向量”的关系。
${ϵ}_1,{ϵ}_2$的李括号为:

$$[{ϵ}_1,{ϵ}_2]=({ϵ}_1^{\wedge }{ϵ}_2^{\wedge }-{ϵ}_2^{\wedge }{ϵ}_1^{\wedge }{)}^{\vee }$$

二、指数与对数的映射

1. SO(3)上的指数映射

上面提到，$exp({\varphi }^{\wedge })$是$SO(3)$上的指数映射。那怎么计算呢？

任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。
$$exp(A)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n$$
所以，根据上面公式，我们有:
$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$
假设我们用$\theta ,a$分别定义$\varphi$的模长和方向，我们就可以用$\varphi =\theta a$表示。这里的$a$是一个长度为1的方向向量。首先对于$a^{\wedge }$有以下两个性质:

$$\begin{gathered} a^{\wedge }{a}^{\wedge }=a{a}^T-I \\ {a}^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge } \end{gathered}$$
所以我们把上面的指数映射转换为:

也就是 罗德里格斯公式。

$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }$$
这表明，$so(3)$实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。 通过它们，我们把$so(3)$中任意一个向量对应到了一个位于$SO(3)$中的旋转矩阵。

2. SE(3)上的指数映射

同理我们可以推导出$se(3)$上的指数映射。
$$\begin{array}{rl}exp({ϵ}^{\wedge })& =\left[\begin{array}{c}\begin{array}{rl}\sum _{n=0}^{\infty }\frac{1}{n!}& ({\varphi }^{\wedge }{)}^n\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ & 0^{T}1\end{array}\end{array}\right]\\ & \triangleq \left[\begin{array}{c}RJ\rho \\ 0^{T}1\end{array}\right]=T\end{array}$$

$ϵ$的指数映射左上角的$R$是我们熟知的欧式正交群$SO(3)$中的元素，与李代数$se(3)$当中的旋转部分$\varphi$对应。 而右上角的$J$则可整理为($\varphi =\theta a$):
$$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }$$

总结如下:

三、李代数求导与扰动模型

1. BCH公式与近似形式

使用李代数的一大动机是为了进行优化，而在优化过程中导数是非常必要的信息。 但是，当我们在$SO(3)$中完成两个矩阵乘法时， 李代数中$so(3)$上发生了什么改变呢？反过来说，当$so(3)$上做两个李代数的加法时，$SO(3)$上是否对应着两个矩阵的乘积？
两个李代数指数映射乘积的完整形式， 由Baker-Campbell-Hausdorf公式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]]+...$$
其中 [] 为李括号。BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑$SO(3)$上的李代数$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }$，当${\varphi }_1$或${\varphi }_2$为小量时，小量二次以上的项都可以被忽略掉。此时，BCH 拥有线性近似表达:

$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{c}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_{2}if{\varphi }_1\text{is small},\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_{1}if{\varphi }_2\text{is small},\end{array}$$
以第一个近似为例。该式告诉我们，当对一个旋转矩阵$R_2$(李代数为${\varphi }_2$)左乘一个微小旋转矩阵$R_1$(李代数为${\varphi }_1$)时，可以近似地看作，在原有的李代数${\varphi }_2$上， 加上了一 项$J_l({\varphi }_2{)}^{-1}{\varphi }_1$。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种。

以左乘为例，左乘BCH近似雅可比$J_l$:
$$J_l=J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$
它的逆为:
$$J_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }$$
而右乘雅可比仅需要对自变量取负号即可：
$$J_r(\varphi )=J_L(-\varphi )$$

我们就可以谈论李群乘法与李代数加法的关系了。

假定对某个旋转$R$，对应的李代数为$\varphi$。我们给它左乘一个微小旋转，记作$△R$，对应的李代数为$△\varphi$。 那么，在李群上，得到的结果就是$△R\cdot R$，而在李代数上，根据BCH近似，为:$J_l^{-1}(\varphi )△\varphi +\varphi$。合并起来，可以简单地写成：
$$exp(△{\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((\varphi +J_l^{-1}(\varphi )△\varphi {)}^{\wedge })$$

反之，如果我们在李代数上进行加法，让一个$\varphi$加上$\Delta \varphi$，那么可以近似为李群上带左右雅可比的乘法:
$$exp((\varphi +\Delta \varphi {)}^{\wedge })=exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })=exp({\varphi }^{\wedge })exp(J_r\Delta \varphi {)}^{\wedge })$$
同理对于$SE(3)$，有类似的BCH近似公式:
$$\begin{gathered} exp(\Delta {ϵ}^{\wedge })exp({ϵ}^{\wedge })\approx exp((J_l^{-1}\Delta ϵ+ϵ{)}^{\wedge }) \\ exp({ϵ}^{\wedge })exp(\Delta {ϵ}^{\wedge })\approx exp((J_r^{-1}\Delta ϵ+ϵ{)}^{\wedge }) \end{gathered}$$

2. SO(3)李代数上的求导

在SLAM中，我们要估计一个相机的位置和姿态，该位姿是由$SO(3)$上的旋转矩阵或$SE(3)$上的变换矩阵描述的。假设机器人的某个时刻位姿为$T$。它观察到了一个世界坐标位于$P$的点，产生了一个观测数据$z$。由变换关系可知:
$$z=Tp+w$$
由于观测噪声$w$的存在，$z$往往不可能精确地满足$z=Tp$的关系。所以，我们通常会计算理想的观测与实际数据的误差:
$$e=z-Tp$$
假设有$N$个这样的路标点和观测，于是就有$N$个上式。那么对于机器人的位姿估计，相当于是找一个最优的$T$，使得整体误差最小化:
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$

求解此问题，需要计算目标函数$J$关于变换矩阵$T$的导数。这里重点要说的是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。 然而，$SO(3)$,$SE(3)$上并没有良好定义的加法，它们只是群。如果我们把$T$当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：

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

第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。

首先考虑李代数的求导:
考虑$SO(3)$上的情况。设旋转矩阵$R$对应的李代数为$\varphi$:
$$\begin{array}{rl}\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{exp((\varphi +\delta \varphi {)}^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{exp((J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & \approx \underset{\delta \varphi \to 0}{\lim }\frac{(I+(J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{(J_l\delta \varphi {)}^{\wedge }exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-(exp({\varphi }^{\wedge })p{)}^{\wedge }{J}_l\delta \varphi }{\delta \varphi }=-(Rp{)}^{\wedge }{J}_l\end{array}$$
第二行的近似为BCH线性近似，第三行为泰勒展开舍去高阶项后近似，第四行至第五行将反对称符号看作叉积，交换之后变号。于是，我们推导了旋转后的点相对于李代数的导数:
$$\frac{\partial Rp}{\partial \varphi }=(-Rp{)}^{\wedge }{J}_l$$

不过，由于这里仍然含有形式比较复杂的$J_l$，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

再来看一下扰动模型(左乘)

另一种求导方式，是对$R$进行一次扰动$△R$。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动$△R$对应的李代数为$\phi$。然后，对$\phi$求导，即:
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }\\ & \approx \underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge }\end{array}$$
可见，扰动模型相比于直接对李代数求导，省去了一个雅可比$J_l$的计算。