视觉SLAM十四讲(高翔版本)，ch4章节部分笔记

目标：理解slam的框架以及它的理论知识。供以后自己查阅。

这一章主要非常重要，也是理解后续优化的基础，它是将旋转矩阵和平移向量，转化为李代数的形式进行优化，因为它有很多好处。好处如下：

意思就是采用$R,t$的矩阵表达形式，有冗余。因为旋转向量内部的$R$的９个变量之间有约束，不利于优化。它可以转化为可以用三个无关变量表示的，转化为无约束的能量函数。这也是下面说的李代数。

1) 李代数：

这一章需要介绍一个基本的代数结构(大学的线性代数和离散数学里面有这方面的概念介绍)：群
集合：$A$, 运算为$\cdot$, 如果当运算满足以下性质时，称$(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$

上一章节讲了旋转矩阵$R$和平移向量$t$，总结如下它们都满足群的性质，且是一些特殊群。是群的一部分，如下：

三维旋转矩阵构成特殊正交群$SO(3)$:

$$SO(3)=\left\{\begin{array}{c}\mathbf{R}\in R^{3\times 3}|\mathbf{R}{\mathbf{R}}^T=I,det(\mathbf{R})=1\end{array}\right\}(1)$$

三维变换矩阵构成特殊特殊欧式群
$$SE(3)=\left\{\begin{array}{c}\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|\mathbf{R}\in SO(3),T\in R^3\end{array}\right\}(2)$$

可以验证：
旋转矩阵集合和矩阵乘法构成群。
变换矩阵和矩阵乘法构成群
可以统称旋转矩阵群，和变换矩阵群。
其它常见的群。
（私下可以验证上述４个性质）
在ＳＬＡＭ中，$SO(2),SO(3);SE(2),SE(3)$非常常见的群。
群论是一门课题，一般理论数学比较多。后续介绍其中的李群
李群(Lie Group)
具有连续的性质的群。
既有群也是流形
因为刚体运动中一般为空间连续运动，可以看到$SO(3),SE(3)$都是李群（连续性）。
群中的性质，只有乘法，没有加法，这个限制了很多运算。如求导和求极限。

为了解决没有加法的问题，引出李代数的概念。
李代数：和李群对应的结构，位于向量空间。是李群单位处的正切空间。(见下图使用小写的$so(3)$表示)

后面介绍上述图像表示的意思为啥成立。
引出李代数：
任意的旋转矩阵$R$，满足：
$$R{R}^T=I(3)$$

考虑到$R$随着时间的变化，有：
$$R(t)R(t{)}^T=I$$

两侧对时间求导：
$$\begin{gathered} \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(4) \end{gathered}$$

通过上述的公式$(4)$得到$\dot{R}(t)R(t{)}^T$是反对称矩阵。可以写成一个符号($a$为向量；$A$是矩阵)：
$$a^{\wedge }=A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];A^{\vee }=a$$

替换符号反对称矩阵（为了简便看）：
$$\dot{R}(t)R(t{)}^T={\varphi }^{\wedge }(5)$$

将公式$(5)$左右乘$R(t)$可以得到如下：

$$\dot{R}(t)={\varphi }^{\wedge }R(t)(6)$$

可以看到对$R(t)$求导其实是左乘一个反对称矩阵。

公式$(6)$是一个非常重要的公式，也是后续计算的基础。因为有泰勒展开这个法宝。假设在$t_0$时刻泰勒展开，得到如下：
$$R(t)=R(t_0)+\dot{R}(t_0)(t-t_0)+\cdot \cdot \cdot$$

一般情况下是求得一节求导比较多，省略后面的高阶项。
$$\begin{gathered} R(t)=R(t_0)+\dot{R}(t_0)(t-t_0) \\ ＝R(t_0)+\varphi (t_0{)}^{\wedge }(t)(7) \end{gathered}$$
可以看到${\varphi }^{\wedge }$它就是在$R(t)$的正切空间上(见上图)。
因此在连续的空间中，$t_0$附近，$\varphi$不变情况下，有微分方程：

$$\dot{R}(t)=\varphi (t_0{)}^{\wedge }R(t)=\varphi (0{)}^{\wedge }R(t)(8)$$

如果$R(0)=I$，代入到公式$(8)$,解方程得到（因为$exp(x)=exp(x)$，其它为常数项，很容易得到）：

$$R(t)=exp({\varphi }_0^{\wedge }t)(9)$$

上面的公式得到对于任意的$t$,可以找到对应的关系$R$和$\varphi$对应关系。这里称$\varphi$为李代数$so(3)$。

它也有很多性质和定义：
李代数由一个集合$V$,一个数域$F$和一个二元运算$[,]$组成，同事满足下列性质，称为$(V,F,[,])$为李代数，即为$G$。
1:封闭性：$\forall X,Y\in V,[X,Y]\in V$
2:双线性： $\forall X,Y,Z\in V;a,b\in F$,有：$[aX+bY,Z]=a[X,Y]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$
3:自反性：$\forall X\in V,[X,X]=0$
4:雅克比等价：$\forall X,Y,Z\in V,[X,[Y,Z]]+[Z,[Y,Z]]+[Y,[Z,X]]=0$

其中二元运算$[,]被称为李括号$
在日常的SLAM中，发现三维空间向量+差积的运算构成李代数。表示如下：
$$so(3)=\left\{\begin{array}{c}\varphi \in R^3,\Phi ={\varphi }^{\wedge }\in R^{3\times 3}\end{array}\right\}(10)$$

其中
$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in R^{3\times 3}$$

同理，对于李代数$se(3)$:

$$se(3)=\left\{\begin{array}{c}\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in R^6,\rho \in R^3,\varphi \in so(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 1\end{array}\right]\in R^{4\times 4}\end{array}\right\}(11)$$

其中$se(3)$是由三个平移分量和三个旋转分量组成。
旋转和$so(3)$相同
平移是一个普通的向量
这个上尖尖不再是反对称矩阵。
$${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 1\end{array}\right]\in R^{4\times 4}(12)$$

上面的公式是基本的概念，是后续优化的基础。

下面介绍优化的formulation

2)指数映射和对数映射

指数映射反映从李代数到李群的对应关系：
$$R=exp({\varphi }^{\wedge })$$

其中${\varphi }^{\wedge }$为一个矩阵，怎么定义矩阵的运算？直接用Taylor展开。
$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$

需要化简上面的泰勒公式（因为${\varphi }^{\wedge }$是矩阵）。
利用$\varphi$向量的一些性质来处理${\varphi }^{\wedge }$矩阵。
分开$\varphi$向量为方向和模长：$\varphi =\theta a$
因为$a$为单位向量，所以有以下性质：
$$\begin{gathered} a^{\wedge }{a}^{\wedge }=a{a}^T-I(13) \\ {a}^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }(14) \end{gathered}$$
利用上述的$(13),(14)$公式化解上面的泰勒公式。
泰勒展开后得到：
$$\begin{gathered} exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta a^{\wedge }{)}^n \\ =I+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2(a^{\wedge }{a}^{\wedge })+\frac{1}{3!}{\theta }^3(a^{\wedge }{a}^{\wedge }{a}^{\wedge })+... \\ =a{a}^T-a^{\wedge }{a}^{\wedge }+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2(a^{\wedge }{a}^{\wedge })-\frac{1}{3!}{\theta }^3(a^{\wedge })+... \\ =a{a}^T+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...)a^{\wedge }-(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...)a^{\wedge }{a}^{\wedge } \\ =a^{\wedge }{a}^{\wedge }+I+sin\theta a^{\wedge }-cos\theta a^{\wedge }{a}^{\wedge } \\ =(1-cos\theta )a^{\wedge }{a}^{\wedge }+I+sin\theta a^{\wedge } \\ =cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }(15) \end{gathered}$$

最后得到如下：
$$exp(\theta a^{\wedge })=cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }(16)$$

上述就是罗德里公式，可以看到$so(3)$的物理意义为旋转向量。
$$\varphi =In(R{)}^{\vee }=(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }(17)$$

CH3章节介绍了，从旋转矩阵到旋转向量的转化关系如下(可以参考上一个章节)：
$$\theta =arccos(\frac{tr(R)-1}{2});Rn=n(18)$$

上面就介绍了从$SO(3)$到$so(3)$的对应关系。

同样的方式可以获取$se(3)$到$SE(3)$的指数映射：

$$\begin{gathered} exp({\xi }^{\wedge })=\left[\begin{array}{cc}{\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}\right] \\ =\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]=T(19) \end{gathered}$$

上图中的李代数的平移部分到矩阵的平移部分相差一个线性变换(后续给出推导的过程)，由$J$给出
$$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }(20)$$

总结上述公式(见高翔老师的ＰＰＴ中的总结)：

因为李群没法求导，因此用李代数，在李代数上用加法，定义李群上导数（因为它们一一映射）。

为了获取上述过程，需要验证，在李代数上做加法的时候在李群上是啥意义？如下？

$$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })=exp(({\varphi }_1+{\varphi }_2{)}^{\wedge })(21)????(wrong)$$

因为${\varphi }_1^{\wedge },{\varphi }_2^{\wedge }$是矩阵，它们不成立。

但是在$exp(({\varphi }_1+{\varphi }_2{)}^{\wedge })$ 具体是啥样子的，可以查(BCH:Baker-Campbell-Hausdorff)，里面有详细的公式推导，我也没有具体看，所以略过。给出结果公式

$$In(exp(({\varphi }_1+{\varphi }_2{)}^{\wedge }))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...(22)$$

公式$22$中$[A,B]$是李括号。上述的公式中，如果一个是小量的时候，可以忽略高阶项.可以得到如下：

$$In(exp(({\varphi }_1+{\varphi }_2{)}^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{if }{\varphi }_{1}issmall\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{if }{\varphi }_{2}issmall\end{array}(23)$$

上述公式$(23)$中的$J_l,J_r$分别表示左乘雅可比，右乘雅可比。如下：

$$J_l=J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }(24)$$

$$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 }(25)$$

上述$(24)$公式是左乘雅可比，右乘雅可比如下：

$$J_r(\varphi )=J_l(-\varphi )(26)$$

介绍了上面的公式，因为其中一个是小量,用$\Delta \varphi$表示，通过公式$(23)$可以得到如下：

$$exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })(27)$$

通过基本的公式$(27)$得到，在李群上左乘小量时，李代数上的加法相差左雅可比的逆。

反之：
$$exp((\varphi +\Delta \varphi {)}^{\wedge })=exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })＝exp({\varphi }^{\wedge })exp((J_r\Delta \varphi {)}^{\wedge })(28)$$

通过公式$(28)$得到，在李代数上小量加法时，相当于李群上左乘一个左雅可比量(右乘一个右雅可比量)

上述是对$so(3)$运算，同样方式可以对$se(3)$运算。

$$exp(\Delta {\xi }^{\wedge })exp({\xi }^{\wedge })\approx exp((J_l^{-1}\Delta \xi +\xi {)}^{\wedge })(29)$$

$$exp({\xi }^{\wedge })exp(\Delta {\xi }^{\wedge })\approx exp((J_r^{-1}\Delta \xi +\xi {)}^{\wedge })(30)$$

总结，上述公式的作用是，通过BCH的线性近似，定义了李代数上的导数。

在slam过程中，会遇到旋转后的顶点对旋转矩阵进行求导，可以记为：

$$\frac{\partial Rp}{\partial R}$$

由于$R$没有加法，导数无法定义。
存在两种解决办法：
１：对$R$对应的李代数加上小量，求相对于小量的变化率（导数模型）
２：对$R$左乘或者右乘一个小量，求相对于小量的李代数的变化率（扰动模型）

下面具体介绍两种模型推导公式：

导数模型推导：

$$\begin{gathered} \frac{\partial \exp ({\varphi }^{\wedge })p}{\partial \varphi }=\underset{\delta \varphi ->0}{\lim }\frac{\exp ((\varphi +\delta \varphi {)}^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi ->0}{\lim }\frac{\exp ((J_l\delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi } \\ \approx \underset{\delta \varphi ->0}{\lim }\frac{(I+(J_l\delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi ->0}{\lim }\frac{(J_l\delta \varphi {)}^{\wedge }\exp ({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi ->0}{\lim }\frac{-(\exp ({\varphi }^{\wedge })p{)}^{\wedge }(J_l\delta \varphi )}{\delta \varphi }=-(Rp{)}^{\wedge }{J}_l(31) \end{gathered}$$

上述$31$公式的左乘的结果比较复杂，是否可以有其它简便的（试试扰动模型？）

扰动模型推导：
左边乘一个小量，且令其李代数趋于$0$

$$\begin{gathered} \frac{\partial (Rp)}{\partial \phi }=\underset{\phi ->0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi } \\ \approx \underset{\phi ->0}{\lim }\frac{(I+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi } \\ =\underset{\phi ->0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }=\underset{\phi ->0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge }(32) \end{gathered}$$

$(32)$这个公式可以看到它非常简约，对于三维顶点求导旋转部分，更加实用。

在$SE(3)$的扰动模型：

$$\begin{gathered} \frac{\partial (Tp)}{\partial \delta \xi }=\underset{\delta \xi ->0}{\lim }\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi } \\ \approx \underset{\delta \xi ->0}{\lim }\frac{(I+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi } \\ =li{m}_{\delta \xi ->0}\frac{(\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p}{\delta \xi } \\ =\underset{\delta \xi ->0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}(Rp+t)\\ 1\end{array}\right]}{\delta \xi } \\ =\underset{\delta \xi ->0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t{)}^{\wedge }+\delta \rho \\ 0^T\end{array}\right]}{\delta \xi } \\ =\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right](34) \end{gathered}$$

利用ＢＣＨ线性近似，可以推导$so(3),se(3)$上的导数和扰动模型。
其中扰动模型更加简单和常用。

它在SOPHUS库中都有实现，大家可以下载配置这个库，然后测试一下。