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+ehcf1+i⎦⎤
为了确保该矩阵 R R R还是旋转矩阵，将其带入 R T R = I R^TR=I RTR=I得到：
[ 1 + a d g b 1 + e h c f 1 + i ] [ 1 + a b c d 1 + e f g h 1 + i ] = I \left[ \begin{matrix} 1+a & d & g\\ b & 1+e & h\\ c & f & 1+i \end{matrix} \right]\left[ \begin{matrix} 1+a & b & c\\ d & 1+e & f\\ g & h & 1+i \end{matrix} \right]=I ⎣⎡1+abcd1+efgh1+i⎦⎤⎣⎡1+adgb1+ehcf1+i⎦⎤=I
在忽略掉二阶无穷小项如 a 2 , e 2 , i 2 , a b a^2,e^2,i^2,ab a2,e2,i2,ab等后，可以得到：
[ 1 + 2 a b + d c + g b + d 1 + 2 e f + h c + g f + h 1 + 2 i ] = I \left[ \begin{matrix} 1+2a & b+d & c+g\\ b+d & 1+2e & f+h\\ c+g & f+h & 1+2i \end{matrix} \right]=I ⎣⎡1+2ab+dc+gb+d1+2ef+hc+gf+h1+2i⎦⎤=I
进一步化简得到：
a = e = i = 0 , b = − d , c = − g , f = − h R = [ 1 b c − b 1 f − c − f 1 ] a=e=i=0,b=-d,c=-g,f=-h\\ R=\left[ \begin{matrix} 1 & b & c\\ -b & 1 & f\\ -c & -f & 1 \end{matrix} \right] a=e=i=0,b=−d,c=−g,f=−hR=⎣⎡1−b−cb1−fcf1⎦⎤
上式可以进一步分解为：
R = I + α 1 G 1 + α 2 G 2 + α 3 G 3 R=I+\alpha_1G_1+\alpha_2G_2+\alpha_3G_3 R=I+α1G1+α2G2+α3G3
其中：
G 1 = [ 0 0 0 0 0 − 1 0 1 0 ] , G 2 = [ 0 0 1 0 0 0 − 1 0 0 ] , G 3 = [ 0 − 1 0 1 0 0 0 0 0 ] G_1=\left[ \begin{matrix} 0 & 0 & 0\\ 0 & 0 & -1\\ 0 & 1 & 0 \end{matrix} \right], G_2=\left[ \begin{matrix} 0 & 0 & 1\\ 0 & 0 & 0\\ -1 & 0 & 0 \end{matrix} \right],G_3=\left[ \begin{matrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0 \end{matrix} \right] G1=⎣⎡0000010−10⎦⎤,G2=⎣⎡00−1000100⎦⎤,G3=⎣⎡010−100000⎦⎤
看上式是不是有点熟悉，这不就是三个矩阵 G i G_i Gi的线性叠加吗。还记得我们线性代数中学过的基底吗，例如二维平面空间中的基底就是 i ⃗ = [ 1 0 ] T , j ⃗ = [ 0 1 ] T \vec{i}=[1\ 0]^T,\vec{j}=[0\ 1]^T i =[1 0]T,j =[0 1]T，然后就可以利用这两个基底表示二维平面空间中的任意一点 v ⃗ = x i ⃗ + y j ⃗ \vec{v}=x\vec{i}+y\vec{j} v =xi +yj 。只不过在现在的切空间中，基底不是向量，而是变成了矩阵。当三个系数 α i \alpha_i αi不再是无穷小时， R R R就构成了一个三维空间内的平面。这个平面就叫做 S O ( 3 ) SO(3) SO(3)群的切空间(tagent space)，记为 s o ( 3 ) so(3) so(3)。对应的数学运算为李代数(Lie Algebra)。

3.2.3 指数映射

指数映射(exponential map)：对于切空间 s o ( 3 ) so( 3) so(3)内的任意一点 A A A，即
A = ∑ i α i G i = [ 0 − α 3 α 2 α 3 0 − α 1 − α 2 α 1 0 ] A=\sum_{i} \alpha_iG_i=\left[ \begin{matrix} 0 & -\alpha_3 & \alpha_2\\ \alpha_3 & 0 & -\alpha_1\\ -\alpha_2 & \alpha_1 & 0 \end{matrix} \right] A=i∑αiGi=⎣⎡0α3−α2−α30α1α2−α10⎦⎤
我们来对这个点 A A A做一个有趣的操作，对它求一个指数，也就是指数映射 e A e^A eA了，我们来看一下会得到什么结果，首先我们可以利用泰勒展开求解：
e x = 1 + x + x 2 2 + x 3 6 + . . . + x n n ! e A = I + A + A 2 2 + . . . + A n n ! e^x=1+x+\frac{x^2}{2}+\frac{x^3}{6}+...+\frac{x^n}{n!}\\ e^A=I+A+\frac{A^2}{2}+...+\frac{A^n}{n!} ex=1+x+2x2+6x3+...+n!xneA=I+A+2A2+...+n!An
多么熟悉的泰勒展开一阶近似，但是好像也没发现什么有趣的东西，但是换个角度可能就不一样了哦。关于指数函数，我们之前在微积分/高等数学中肯定还学习过这样一个极限公式：
e x = lim ⁡ n → + ∞ ( 1 + x n ) n e^x=\lim_{n\rightarrow+\infty}(1+\frac{x}{n})^n ex=n→+∞lim(1+nx)n
把切空间中的 A A A带入到上式就可以得到：
e A = lim ⁡ n → + ∞ ( I + 1 n A ) n e^A=\lim_{n\rightarrow+\infty}(I+\frac{1}{n}A)^n eA=n→+∞lim(I+n1A)n
当 n n n趋于无穷大时， ( I + 1 n A ) (I+\frac{1}{n}A) (I+n1A)趋近于3.2.2节中所述单位阵附近的旋转矩阵 R R R了，我们已经在上一小节中证明了 R R R是属于 S O ( 3 ) SO(3) SO(3)群的，也就是说是一个旋转矩阵。又由于群是满足计算封闭性的，通俗来讲就是旋转矩阵相乘，无论怎么乘，乘多少次，最后的结果都还是一个旋转矩阵。因此，对 ( I + 1 n A ) (I+\frac{1}{n}A) (I+n1A)连乘 n n n次得到 ( I + 1 n A ) n (I+\frac{1}{n}A)^n (I+n1A)n，结果肯定还是一个旋转矩阵。

Amazing！我们好像发现了一个不得了的事情：
对切空间 s o ( 3 ) so(3) so(3)内的任意一点 A A A求指数函数 e A e^A eA，得到的结果 e A e^A eA居然映射到了SO(3)内，也即：
E x p o n e n t i a l m a p : s o ( 3 ) ↦ S O ( 3 ) Exponential\ map: so(3)\mapsto SO(3) Exponential map:so(3)↦SO(3)

3.2.4 叉积与对数映射

叉积(Cross product)：对于三维空间中的两个向量 v 1 , v 2 v_1,v_2 v1,v2，我们应该都知道可以对这两个向量求点积和叉积，点积是比较好求的，但是笔者之前每次求叉积的时候都要像在线性代数中求行列式的方法求，直到有一天发现原来还可以通过矩阵求。我们首先一起来看一下：
求两个向量的叉积其实可以化简为：
v 1 ∧ v 2 = [ a b c ] ∧ [ d e f ] = [ b f − c e c d − a f a e − b d ] = [ 0 − c b c 0 − a − b a 0 ] [ d e f ] v_1\wedge v_2=\left[\begin{matrix} a\\b\\c \end{matrix}\right]\wedge \left[\begin{matrix} d\\e\\f \end{matrix}\right]=\left[\begin{matrix} bf-ce\\ cd - af\\ ae-bd \end{matrix}\right]=\left[\begin{matrix} 0 & -c & b\\ c & 0 & -a\\ -b & a & 0 \end{matrix}\right]\left[\begin{matrix} d\\ e\\ f \end{matrix}\right] v1∧v2=⎣⎡abc⎦⎤∧⎣⎡def⎦⎤=⎣⎡bf−cecd−afae−bd⎦⎤=⎣⎡0c−b−c0ab−a0⎦⎤⎣⎡def⎦⎤
因此，我们可以把叉积看做是一种将向量 v 1 v_1 v1转为矩阵的操作符，即：
v 1 ∧ = [ 0 − c b c 0 − a − b a 0 ] = a 1 G 1 + a 2 G 2 + a 3 G 3 = A v_1\wedge=\left[ \begin{matrix} 0 & -c & b\\ c & 0 & -a\\ -b & a & 0 \end{matrix} \right]=a_1G_1+a_2G_2+a_3G_3=A v1∧=⎣⎡0c−b−c0ab−a0⎦⎤=a1G1+a2G2+a3G3=A
这就很有意思了，上一节我们把切空间中的一点表示为矩阵 A A A，现在我们利用叉积运算符，可以直接再把切空间内的一点表示为向量的叉积符 [ v ∧ ] [v\wedge] [v∧]。因此， S O ( 3 ) SO(3) SO(3)群内的一个旋转矩阵就可以表示为：
R = e A = e [ v ∧ ] R=e^A=e^{[v\wedge]} R=eA=e[v∧]
更有趣的是 v v v和 R R R的关系：向量 v v v的方向表示的就是旋转矩阵 R R R对应的旋转轴，而向量 v v v的模表示的则是绕旋转轴旋转的角度。Interesting！

根据上式我们就可以得到另外一个有趣的东西，对数映射：
l o g ( R ) = l o g ( e A ) = l o g ( e [ v ∧ ] ) = [ v ∧ ] log(R)=log(e^A)=log(e^{[v\wedge]})=[v\wedge] log(R)=log(eA)=log(e[v∧])=[v∧]
如果我们再定义一个叉积符 v ∧ v \wedge v∧的逆操作 A ∨ = v A\vee=v A∨=v，即将切空间内的一点(反对称阵)转为一个向量，则可以进一步得到：
l o g ( R ) ∨ = l o g ( e v ∧ ) ∨ = v log(R)\vee=log(e^{v\wedge})\vee=v log(R)∨=log(ev∧)∨=v
即将旋转矩阵转为轴角向量 v v v，最后总结一下，对数映射实现 S O ( 3 ) SO (3) SO(3)群内一点到切空间 s o ( 3 ) so(3) so(3)的映射，即：
L o g a r i t h m m a p : S O ( 3 ) ↦ s o ( 3 ) Logarithm\ map: SO(3)\mapsto so(3) Logarithm map:SO(3)↦so(3)

3.3 IMU preintegration

为避免篇幅过长，这部分的详细推导见下一篇博客。

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（三）相关推荐

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一) 1. LiDAR inertial odometry and mapping简介 ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(四) 3. Joint optimization 3.3 IMU preintegrat ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（二）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(二) 3. Joint optimization 3.1 Marginalization ...
激光SLAM系统Fast LOAM (Lidar Odometry And Mapping)源码解析
github地址:Fast LOAM (Lidar Odometry And Mapping) Fast LOAM提供了mapping和localization的两个节点,目前只使用其定位部分,以ve ...
Disruptor源码解析三 RingBuffer解析
目录系列索引前言主要内容 RingBuffer的要点源码解析系列索引 Disruptor源码解析一 Disruptor高性能之道 Disruptor源码解析二 Sequence相关类解析 D ...
并发编程与源码解析 (三)
并发编程 (三) 1 Fork/Join分解合并框架 1.1 什么是fork/join Fork/Join框架是JDK1.7提供的一个用于并行执行任务的框架,开发者可以在不去了解如Thread.R ...
OkHttp3源码解析(三)——连接池复用
OKHttp3源码解析系列 OkHttp3源码解析(一)之请求流程 OkHttp3源码解析(二)--拦截器链和缓存策略本文基于OkHttp3的3.11.0版本 implementation 'com ...
ReactiveSwift源码解析(三) Signal代码的基本实现
上篇博客我们详细的聊了ReactiveSwift源码中的Bag容器,详情请参见<ReactiveSwift源码解析之Bag容器>.本篇博客我们就来聊一下信号量,也就是Signal的的几种状 ...
前端入门之(vuex源码解析三)
上两节前端入门之(vuex源码解析二)我们把vuex的源码大概的撸了一遍,还剩下(插件.getters跟module),我们继续哈~ 插件童鞋们可以去看看vuex在各个浏览器的状态显示插件,小伙伴可以 ...

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