Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（三）

3. Joint optimization

3.2 IMU preintegration中的数学基础

流形上的IMU预积分涉及到一些群、流行、切空间、指数与对数映射的概念，可能笔者理解能力没那些大佬那么强，又喜欢刨根问底(wtf，这公式对吗，怎么推出来的啊)，因此刚开始接触IMU预积分时，对预积分的推导理解的不是很透彻。相信也有一些同学可能会遇到类似的问题。因此，在介绍IMU预积分理论之前，先对涉及到的数学知识进行了梳理，力求严谨且容易理解。
对于其中可能出现的错误与不足，还希望看到的同学帮忙指正。

3.2.1 旋转矩阵、群与流形

旋转矩阵(Rotation Matrix)：利用矩阵$R$表示旋转的一种方法，若为三维空间中的旋转，则旋转矩阵$R$为$R_{3\times 3}$；若为二维空间中的旋转，则旋转矩阵$R$为$R_{2\times 2}$。其他表示旋转的方法有：欧拉角，四元数，轴角等

群(Group)：所谓群，其实就是满足一定规则的元素的集合。例如我们常见的$SO(3)$群和$SO(2)$群。我们以$SO(3)$群为例，其表达式就是：
$$\left[R\in R_{3\times 3},R^{T}R=I,det(R)=1\right]$$
满足上式的$3\times 3$矩阵就构成了一个$SO(3)$群。从上式中也可以看到旋转矩阵的一个非常重要的性质：就是旋转矩阵R的逆矩阵就是其转置，因此就不需要浪费时间计算R的旋转矩阵了，直接求转置就可以。

另外需要注意的是，群除了需要满足上式的要求以外，还应该具备以下性质：

• 首先是封闭性，即对于群内的任意两个元素$a$和$b$，和定义在该群上的操作符.，需要满足$a.b$也一定要在群内。群就像是一个孤岛，无论你咋操作都逃离不出这个群，你说气不气。对应于$SO(3)$群，即是任意两个旋转矩阵的乘积依然是一个旋转矩阵
• 其次是单位性，即存在唯一一个元素x，使群内的任意元素$a$，满足$a.x=x.a=a$。对于$SO(3)$群来说，其实就是单位矩阵$I$
• 最后是对于任意一个元素x，存在唯一一个y，使得$x.y=y.x=I$。对于$SO(3)$群来说，其实就是逆矩阵$R^{-1}R=I$

流形(Manifold)：所谓流形，就是高维空间中的曲面。一组$SO(3)$群也构成了一个光滑流形。例如，假设现在有一个表示绕$z$轴旋转10度的旋转矩阵$R$:
$$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0.98& -0.17\\ 0& 0.17& 0.98\end{array}\right]$$
我们可以把旋转矩阵$R$用向量$v$来表示，即：
$$v={\left[1,0,0,0,0.98,-0.17,0,0.17,0.98\right]}^T$$
这样表示一下是不是可以理解为是9维空间内的一点，而$SO(3)$群里有无穷多个旋转矩阵，这些旋转矩阵共同构成了9维空间内的一个流形曲面，就像二维空间内一条直线是由其上的无穷多个点构成一样。
但是仔细想想好像不大对，明明三维空间内的旋转，怎么一下子搞到9维空间里了。那是因为9维空间里并不全是$SO(3)$，还有很多很多其他的9维向量，$SO(3)$只是其中的一个子空间。这么说可能有点抽象，可以参考一下下图。

3.2.2 切空间与李代数

切空间(tagent space)：为解释什么是切空间，我们先来看$SO(3)$群中的一个单位阵$I$:
$$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$
现在我们想找该单位阵附近小区间的一个旋转矩阵，为实现该目标，我们在矩阵$R$的9个元素上分别添加一个无穷小量得到$R$
$$R=[\begin{array}{ccc}1+a& b& c\\ d& 1+e& f\\ g& h& \end{array}$$