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

3. Joint optimization

在上一个博客中，我们分别介绍了IMU预积分，LiDAR Relative Measurements，接下来就是非常重要优化环节了，优化过后才可以获得里程计计算结果。在这篇论文中，优化的目标函数主要由三部分构成：marginalization factor， imu-preintegration，以及LiDAR relative measurements，我们首先来看第一个marginalization factor。

3.1 Marginalization

3.1.1 什么是Marginalization

在上一博客中，我们已经知道该篇论文中引入了一个滑窗(Sliding Window)，来限制待优化的状态向量数量。如下图所示，我们考虑这样一种情景，假设当前时刻窗口内的优化变量为$x_1,x_2,...,x_6$，且已经对里程计中的目标函数完成了优化，也就是相当于拟合出来了一个概率分布$p(x_1,x_2,...,x_6)$，即多维高斯分布对应的均值$\mu$和协方差矩阵$K$。此时在下一个优化时刻到来之前，我们需要将窗口往后移动一个格子，即开始优化此时窗口内的优化变量$x_2,x_3,...,x_7$，这种过程会贯穿整个估计过程，直至算法停止运行。那么一个明显的问题是，我们在优化拟合出来$p(x_2,x_3,...,x_7)$之前，该怎样处理概率分布$p(x_1,x_2,...,x_6)$呢？

为了更形象的说明这个问题，我们来假设这样一种情景。小明开车想从中国科学技术大学到之心城买件衣服，怕迷路，想时刻估计一下自己到哪了，因此搞了个滑窗，利用gps，轮速计来分析自己当前和过往几个时刻自己开车到哪了。通过各种融合算法，发现自己$x_1,x_2,...,x_6$时的位置分布为$p(x_1,x_2,...,x_6)$发现自己原来刚出东门。然后接着往前开啊，现在滑窗内的状态就是$x_2,x_3,...,x_7$了。那么如何估计我$x_2,x_3,...,x_7$时刻的位置呢？
一种方法就是继续像上一时刻那样用自身的gps和轮速计等融合$p(x_2,x_3,...,x_7)$，也就是说我才不管你$x_1,x_2,x_3,...,x_6$时在哪，我只关心$p(x_2,x_3,...,x_7)$，计算完发现我去怎么还在东门附近？不是刚路过了吗？是不是算错了？
这时，聪明的副驾驶提醒你，哎这孩子读书读傻了，笨不笨，明明$(x_1,x_2,...,x_6)$时刻都到$p(x_1,x_2,...,x_6)$了，你咋还给忘了？你这不是黑瞎子掰玉米掰一个扔一个吗？把$p(x_2,...,x_6)$也就是我们刚刚路过东门这个消息加到你的那个什么融合算法里，再算一算我们现在到哪了？小明这才计算出来原来已经到东门前的十字路口了。
这个例子说明，在更新计算$p(x_2,x_3,...,x_7)$时，总共有两种方法，第一种是直接丢掉之前窗口内拟合的分布$p(x_1,x_2,...,x_6)$，重新计算估计$p(x_2,x_3,...,x_7)$。但是这种方法有一个比较重要的缺陷就是信息损失，可能$p(x_2,x_3,...,x_6)$内的估计非常准确，而你却把这么重要的信息给直接扔了，这不是败家吗你说？第二种方法就是将$p(x_1,x_2,...,x_6)$中的$p(x_2,x_3,...,x_6)$计算出来，并将$p(x_2,x_3,...,x_6)$作为先验信息加入到新窗口的估计中。那么问题来了，怎样根据$p(x_1,x_2,...,x_6)$计算$p(x_2,x_3,...,x_6)$呢？这个过程就叫做Marginalization。下图能比较清晰严谨的说明这个问题，数理基础比较好的话也能想到我们好像学过这个公式：$p(x_a)={\int }_{x_b}p(x_a,x_b)$。接下来我们从数学的角度严谨的说明一下Marginalization。

3.1.2 Basics of Multi-variate Gauss Distribution

如下图所示，假设有一个系统长这个样子，其中$y_2$表示的是室外的温度，$y_1$表示的是房间1的温度，$y_3$表示的是房间3的温度，且他们之间的函数关系表示为：

$$\{\begin{array}{l}{y}_2=v_2\\ y_1=w_1{y}_2+v_1\\ y_3=w_3{y}_2+v_3\\ v_i-N(0,{\sigma }_i^2)，高斯噪声\end{array}$$
则状态向量$y=[y_1,y_{}$