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表示旋转的一种方法,若为三维空间中的旋转,则旋转矩阵 R R R为 R 3 × 3 R_{3\times3} R3×3;若为二维空间中的旋转,则旋转矩阵 R R R为 R 2 × 2 R_{2\times2} R2×2。其他表示旋转的方法有:欧拉角,四元数,轴角等
群(Group):所谓群,其实就是满足一定规则的元素的集合。例如我们常见的 S O ( 3 ) SO(3) SO(3)群和 S O ( 2 ) SO(2) SO(2)群。我们以 S O ( 3 ) SO(3) SO(3)群为例,其表达式就是:
[ R ∈ R 3 × 3 , R T R = I , d e t ( R ) = 1 ] \left[R\in R_{3\times3},R^TR=I,det(R)=1\right] [R∈R3×3,RTR=I,det(R)=1]
满足上式的 3 × 3 3\times3 3×3矩阵就构成了一个 S O ( 3 ) SO(3) SO(3)群。从上式中也可以看到旋转矩阵的一个非常重要的性质:就是旋转矩阵R的逆矩阵就是其转置,因此就不需要浪费时间计算R的旋转矩阵了,直接求转置就可以。
另外需要注意的是,群除了需要满足上式的要求以外,还应该具备以下性质:
- 首先是封闭性,即对于群内的任意两个元素 a a a和 b b b,和定义在该群上的操作符.,需要满足 a . b a.b a.b也一定要在群内。群就像是一个孤岛,无论你咋操作都逃离不出这个群,你说气不气。对应于 S O ( 3 ) SO(3) SO(3)群,即是任意两个旋转矩阵的乘积依然是一个旋转矩阵
- 其次是单位性,即存在唯一一个元素x,使群内的任意元素 a a a,满足 a . x = x . a = a a.x=x.a=a a.x=x.a=a。对于 S O ( 3 ) SO(3) SO(3)群来说,其实就是单位矩阵 I I I
- 最后是对于任意一个元素x,存在唯一一个y,使得 x . y = y . x = I x.y=y.x=I x.y=y.x=I。对于 S O ( 3 ) SO(3) SO(3)群来说,其实就是逆矩阵 R − 1 R = I R^{-1}R=I R−1R=I
流形(Manifold):所谓流形,就是高维空间中的曲面。一组 S O ( 3 ) SO(3) SO(3)群也构成了一个光滑流形。例如,假设现在有一个表示绕 z z z轴旋转10度的旋转矩阵 R R R:
R = [ 1 0 0 0 0.98 − 0.17 0 0.17 0.98 ] R=\left[ \begin{matrix} 1 & 0 & 0\\ 0 & 0.98 & -0.17\\ 0 & 0.17 & 0.98 \end{matrix} \right] R=⎣⎡10000.980.170−0.170.98⎦⎤
我们可以把旋转矩阵 R R R用向量 v v v来表示,即:
v = [ 1 , 0 , 0 , 0 , 0.98 , − 0.17 , 0 , 0.17 , 0.98 ] T v=\left[1,0,0,0,0.98,-0.17,0,0.17,0.98\right]^T v=[1,0,0,0,0.98,−0.17,0,0.17,0.98]T
这样表示一下是不是可以理解为是9维空间内的一点,而 S O ( 3 ) SO(3) SO(3)群里有无穷多个旋转矩阵,这些旋转矩阵共同构成了9维空间内的一个流形曲面,就像二维空间内一条直线是由其上的无穷多个点构成一样。
但是仔细想想好像不大对,明明三维空间内的旋转,怎么一下子搞到9维空间里了。那是因为9维空间里并不全是 S O ( 3 ) SO(3) SO(3),还有很多很多其他的9维向量, S O ( 3 ) SO(3) SO(3)只是其中的一个子空间。这么说可能有点抽象,可以参考一下下图。
3.2.2 切空间与李代数
切空间(tagent space):为解释什么是切空间,我们先来看 S O ( 3 ) SO(3) SO(3)群中的一个单位阵 I I I:
I = [ 1 0 0 0 1 0 0 0 1 ] I=\left[ \begin{matrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{matrix} \right] I=⎣⎡100010001⎦⎤
现在我们想找该单位阵附近小区间的一个旋转矩阵,为实现该目标,我们在矩阵 R R R的9个元素上分别添加一个无穷小量得到 R R R
R = [ 1 + a b c d 1 + e f g h 1 + i ] R=\left[ \begin{matrix} 1+a & b & c\\ d & 1+e & f\\ g & h & 1+i \end{matrix} \right] R=⎣⎡1+adgb1+ehcf