李群与李代数以及Sophus基本应用

开篇介绍

在上次博客中，我们介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等若干种方式。其中重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。于是我们把他归结为一种典型的优化问题，求解最优的$\mathbf{R}$，$\mathbf{t}$，使得误差最小化。

一、群和李群

在上次谈到的三维旋转矩阵和变换矩阵中，我们知道三维旋转矩阵构成了特殊正交群$SO(3)$，而变换矩阵构成了特殊欧氏群$SE(3)$：$$SO(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid \mathbf{R}{\mathbf{R}}^T=\mathbf{I},det(\mathbf{R})=1\right\}$$$$SE(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\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)$$
群的概念：群是一种集合加上一种运算的代数结构。集合记作$\mathbf{A}$，运算记作$\cdot$，那么群可以记作$G=(A,\cdot )$。
李群是指具有连续(光滑)性质的群。像整数群$\mathbb{Z}$那样的群没有连续性质，所以不是李群。而$SO(n)$和$SE(n)$，它们在实数空间上是连续的。我们能够直观的想象一个刚体能够在连续地空间中运动，所以它们是李群。

二、李代数

I.李代数的介绍

对于任意旋转矩阵，我们知道其也是一个正交矩阵，满足：$$\mathbf{R}{\mathbf{R}}^T=\mathbf{I}$$在实际应用场景下，$\mathbf{R}$是机器人或小车的旋转，会随着时间连续变化，于是旋转矩阵就为时间的连续函数$\mathbf{R}(t)$。于是有：$$\mathbf{R}(t)\mathbf{R}(t{)}^T=\mathbf{I}$$，由于李群是连续的，对时间求导后得：$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T+\mathbf{R}(t)\dot{\mathbf{R}}(t{)}^T=0$$$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T{)}^T$$
我们可以看出$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T$是一个反对称矩阵，根据先前介绍的叉乘运算，我们引入了向量转反对称矩阵的运算(也即${}^{\wedge }$)，对于任意反对称矩阵，我们都能找到一个与之对应的向量。于是我们令：$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=\varphi (t{)}^{\wedge }$$，根据正交阵的特性，其逆矩阵为自己的转置矩阵，于是等式两边右乘$\mathbf{R}(t)$有：$$\dot{\mathbf{R}}(t)=\varphi (t{)}^{\wedge }\mathbf{R}(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\mathbf{R}(t)$$可见，每对旋转矩阵求导一次，我们只需左乘一个$\varphi (t{)}^{\wedge }$矩阵即可。假设旋转矩阵满足$\mathbf{R}(0)=\mathbf{I}$，我们对$\mathbf{R}(t)$在$t=0$处进行一阶泰勒展开：$$\mathbf{R}(t)\approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\varphi (t_0{)}^{\wedge }(t)$$
在$t_0$附近，假设$\varphi$保持为常数$\varphi (t_0)={\varphi }_0$，那么通过求解$\mathbf{R}$的微分方程，解得$\mathbf{R}(t)=exp({\varphi }_0^{\wedge }t)$
不失一般性，我们找出了给定某时刻的$\mathbf{R}$，我们就能求得一个$\varphi$，它描述了$R$在局部的导数关系，那么这里的$\varphi$就是我们$SO(n)$群的李代数$\mathfrak{s}\mathfrak{o}(3)$。
在变换矩阵中，我们如此类推，也能得到$SE(n)$群的李代数$\mathfrak{s}\mathfrak{e}(3)$。

II.指数映射

在清楚了旋转矩阵或者变换矩阵能够构成对应的李群后，我们也清楚了它们各自的李代数，也就是旋转向量$\varphi =\theta \mathbf{a}$，其中，这里$\mathbf{a}$是长度为1的方向向量，$\theta$描述了旋转向量的长度。接下来我们就能通过以$e$为底数的指数函数的泰勒展开式来进行推导，最后得出$$\begin{array}{rl}\mathbf{R}& =exp({\varphi }^{\wedge })\\ & =exp(\theta {\mathbf{a}}^{\wedge })\\ & =\cos \theta \mathbf{I}+(1-\cos \theta ){\mathbf{a}\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }\end{array}$$这也就是我们之前提到过的罗德里格斯公式，详细的推导证明可以查看——罗德里格斯公式推导
而当我们知道了旋转矩阵，需要求出旋转向量的时候，我们可以通过旋转矩阵的迹公式求出旋转角$\theta$以及通过求$\mathbf{R}$的特征向量求出旋转向量的方向。
同样地，变换矩阵的指数映射求解也能从相同的推导中得出，过程大同小异，只是引入了平移量。

三、BCH公式及近似

我们使用李代数的一个动机是为了进行优化，而在优化问题中导数又是重中之重的任务。所以我们来考虑在$SO(3)$群中的两个矩阵的乘法，是否能对应到李代数$\mathfrak{s}\mathfrak{o}(3)$中的加法呢？如果成立的话，相当于：$$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })=exp(({\varphi }_1+{\varphi }_2{)}^{\wedge })$$
但是，很遗憾，该式只在变量是标量的情况下成立；在变量为矩阵时并不成立。
两个李代数指数映射乘积的完整形式，由$Baker-Campbell-Hausdorff$公式给出。在$BCH$公式下，当扰动量很小时，我们可以分别对应到左乘模型和右乘模型：$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{ if }{\varphi }_{1}issmall\\ {\mathbf{J}}_l({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{ if }{\varphi }_{2}issmall\end{array}$$
上式中第一个对应的是左乘模型(或左扰动模型)，第二个对应的是右乘模型。(或右扰动模型)

四、李代数求导

当一切准备就绪之后，我们就要考虑我们的优化问题了。回顾之前，我们引入了具有连续性质的李群，并知道了李代数的求解方法。由于实际场景应用当中，每个观测点得到的位姿数据都会存在噪声，于是我们要得到机器人或者小车在什么位置能够得到最佳观测数据，也就是归结到我们的优化问题上了。
于是，我们的目标就是，优化旋转矩阵，得到最佳的观测位置。$$z=Tp+w$$$$e=z-Tp$$
现在，我们通过对李代数求导，$$\begin{array}{rl}\frac{\partial (Rp)}{\partial R}& =\frac{\partial (\exp ({\varphi }^{\wedge })p)}{\delta \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}$$

五、扰动模型

以上述介绍过的左乘模型为例子，对$R$的一次扰动量设为$\Delta R$。这个扰动可以左乘或右乘，最后结果会由细微的差异。我在这里先以左扰动为例，设左扰动$\Delta 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{(1+{\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$的计算，这使得扰动模型更加实用。

六、Sophus实践

在了解到李群和李代数、李代数求导以及扰动模型后，我们在Kdevelop上进行基本的实践操作：(详情注释已经在代码中体现)

编译执行后得到如下结果：

这里使用的是非模板类的Sophus，在下载后非模板类Sophus后，我们需要在CMakeLists文本中加入寻找库的命令：

#为使用sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
project(useSophus)

include_directories(${Sophus_INCLUDE_DIRS})
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES})

另外要注意的一点是，在github源上的Sophus存在一些小的bug(https://github.com/strasdat/Sophus.git)
其中so2.cpp文件中有一个类出现了问题：

unit_complex_.real() = 1.;
unit_complex_.imag() = 0.;

应该修改为：

unit_complex_.real(1.);
unit_complex_.imag(0.);

最终，Sophus才能编译成功，开发者才能成功使用Sophus库开发项目。

七、总结

到目前为止，SLAM中的系统的位置姿态表述基础知识已经完全叙述完了，之前提到的旋转向量、旋转矩阵、四元数、欧拉角还有李代数和李群等等基本构件，都是SLAM中必不可少的元件，通过初步接触和学习，希望在后续开发中得到深入与场景应用。