多传感器融合定位十四-基于图优化的定位方法

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

1. 基于图优化的定位简介

1.1 核心思路

核心思路是把融合方法从滤波换成图优化，其元素不再是简单的惯性解算，而是预积分。
一个暴力的模型可以设计为：

缺陷：随着时间的进行，图模型会越来越大，导致无法达到实时性。

解决方法：不断删除旧的帧，只优化最新的几帧，即维持一个滑动窗口。
模型如下：

问题：直接从模型中删除，等于损失了信息。
解法：通过模型把旧帧的约束传递下来，即边缘化(后面讲具体细节)。

1.2 定位流程

整个流程：不断往滑窗里添加新信息，并边缘化旧信息。
需要注意的是：

1. 正常行驶时，不必像建图那样，提取稀疏的关键帧；
2. 停车时，需要按一定策略提取关键帧，但删除的是次新帧，因此不需要边缘化。

2. 边缘化原理及应用

2.1 边缘化原理

优化问题具有如下通用形式:
$$HX=b$$并可拆解成如下形式:
$$\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]\left[\begin{array}{c}{X}_m\\ X_r\end{array}\right]=\left[\begin{array}{l}{b}_m\\ b_r\end{array}\right]$$拆解的目的是通过一系列操作，把 $X_m$ 从状态量 里删除掉，并把它的约束保留下来。
在滑窗模式里，这个 $X_m$ 即为要边缘化掉的量。
回顾舒尔补：
给定矩阵
$$M=\left[\begin{array}{ll}\mathrm{A}& \mathrm{B}\\ \mathrm{C}& \mathrm{D}\end{array}\right]$$它可以通过如下变换，变成上三角矩阵，即
$$\left[\begin{array}{cc}\mathrm{I}& 0\\ -{\mathrm{CA}}^{-1}& \mathrm{I}\end{array}\right]\left[\begin{array}{cc}\mathrm{A}& \mathrm{B}\\ \mathrm{C}& \mathrm{D}\end{array}\right]=\left[\begin{array}{cc}\mathrm{A}& \mathrm{B}\\ 0& \Delta \mathrm{A}\end{array}\right]$$其中， $\Delta \mathrm{A}=\mathrm{D}-{\mathrm{CA}}^{-1}\mathrm{B}$ 称为 $\mathrm{A}$ 关于 $\mathrm{M}$ 的舒尔补。

拆解后的优化问题，可通过舒尔补对矩阵三角化， 即
$$\begin{array}{rl}& \left[\begin{array}{cc}I& 0\\ -H_{rm}{H}_{mm}^{-1}& I\end{array}\right]\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]\left[\begin{array}{c}{X}_m\\ X_r\end{array}\right]\\ =& \left[\begin{array}{cc}I& 0\\ -H_{rm}{H}_{mm}^{-1}& I\end{array}\right]\left[\begin{array}{l}{b}_m\\ b_r\end{array}\right]\end{array}$$
进一步化简得，
$$\begin{array}{rl}& \left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ 0& H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}\end{array}\right]\left[\begin{array}{c}{X}_m\\ X_r\end{array}\right]\\ =& \left[\begin{array}{c}{b}_m\\ b_r-H_{rm}{H}_{mm}^{-1}{b}_m\end{array}\right]\end{array}$$此时，可以利用等式第2行直接得到:
$$\left(H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}\right)X_r=b_r-H_{rm}{H}_{mm}^{-1}{b}_m$$
其含义为：此时可以不依赖 $X_m$ 求解出 $X_r$，若我们只关心 $X_r$ 的值，则可以把 $X_m$ 从模型里删除。

2.2 从滤波角度理解边缘化

kalman滤波是此前已经熟悉的，从边缘化的 角度重新看一遍滤波器的推导，更有利于深入 理解。
运动模型与观测模型分别为:
$$\begin{array}{rl}& {\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k\\ & {\mathbf{y}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k\end{array}$$
其中 $k=1\dots K$状态量的求解，可以等效为如下模型
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\min }J(\mathbf{x})$$
其中
$$\begin{array}{rl}& J(\mathbf{x})=\sum _{k=0}^K\left(J_{v,k}(\mathbf{x})+J_{y,k}(\mathbf{x})\right)\\ & J_{v,k}(\mathbf{x})=\{\begin{array}{c}\frac{1}{2}{\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right)}^T{\check{\mathbf{P}}}_0^{-1}\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right),k=0\\ \frac{1}{2}{\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right)}^T\\ \times {\mathbf{Q}}_k^{-1}\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right),k=1\dots K\end{array}\\ & J_{y,k}(\mathbf{x})=\frac{1}{2}{\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^T{\mathbf{R}}_k^{-1}\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right),k=0\dots K\end{array}$$将上述模型整理为更简洁的形式，令
$$\mathbf{z}=\left[\begin{array}{c}{\check{\mathbf{x}}}_0\\ {\mathbf{v}}_1\\ ⋮\\ {\mathbf{v}}_K\\ {\mathbf{y}}_0\\ {\mathbf{y}}_1\\ ⋮\\ {\mathbf{y}}_K\end{array}\right],\mathbf{x}=\left[\begin{array}{c}{\mathbf{x}}_0\\ ⋮\\ {\mathbf{x}}_K\end{array}\right]$$$$\begin{array}{rl}& \mathbf{H}=\left[\begin{array}{cccc}1& & & \\ -{\mathbf{A}}_0& 1& & \\ & \ddots & \ddots & \\ & & -{\mathbf{A}}_{K-1}& 1\\ {\mathbf{C}}_0& {\mathbf{C}}_1& & \\ & & \ddots & \\ & & & {\mathbf{C}}_K\end{array}\right]\\ & \mathbf{W}=\left[\begin{array}{lllllll}{\check{\mathbf{P}}}_0& & & & & & \\ & {\mathbf{Q}}_1& & & & & \\ & & \ddots & & & & \\ & & & {\mathbf{Q}}_K& & & \\ & & & & {\mathbf{R}}_0& & \\ & & & & & {\mathbf{R}}_1& \\ & & & & & {\mathbf{R}}_K& \end{array}\right]\\ & \end{array}$$此时，目标函数可以重新表示为
$$J(\mathbf{x})=\frac{1}{2}(\mathbf{z}-\mathbf{Hx}{)}^T{\mathbf{W}}^{-1}(\mathbf{z}-\mathbf{Hx})$$求解其最小值，即令其一阶导为零
$${\frac{\partial J(\mathbf{x})}{\partial {\mathbf{x}}^T}|}_{\hat{\mathbf{x}}}=-{\mathbf{H}}^T{\mathbf{W}}^{-1}(\mathbf{z}-\mathbf{H}\hat{\mathbf{x}})=0$$即
$$\left({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}\right)\hat{\mathbf{x}}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{z}$$然而，这是批量求解模型，当只关心当前时 刻(k时刻)状态时，应改为滤波模型。
假设上一时刻后验为
$$\left\{{\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}\right\}$$
目标是得到当前时刻后验
$$\left\{{\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1},{\mathbf{v}}_k,{\mathbf{y}}_k\right\}\mapsto \left\{{\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right\}$$由于马尔可夫性，仅与前一时刻有关，因此令
$$\begin{array}{rl}{\mathbf{z}}_k& =\left[\begin{array}{c}{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{v}}_k\\ {\mathbf{y}}_k\end{array}\right]\\ {\mathbf{H}}_k& =\left[\begin{array}{cc}1& \\ -{\mathbf{A}}_{k-1}& 1\\ & {\mathbf{C}}_k\end{array}\right]\\ {\mathbf{W}}_k& =\left[\begin{array}{ccc}{\hat{\mathbf{P}}}_{k-1}& & \\ & {\mathbf{Q}}_k& \\ & & {\mathbf{R}}_k\end{array}\right]\end{array}$$则模型的解为
$$\left({\mathbf{H}}_k^T{\mathbf{W}}_k^{-1}{\mathbf{H}}_k\right)\hat{\mathbf{x}}={\mathbf{H}}_k^T{\mathbf{W}}_k^{-1}{\mathbf{z}}_k$$
其中
$$\hat{\mathbf{x}}=\left[\begin{array}{c}{\hat{\mathbf{x}}}_{k-1}^{\prime }\\ {\hat{\mathbf{x}}}_k\end{array}\right]$$${\hat{\mathbf{x}}}_{k-1}$ 和 ${\hat{\mathbf{x}}}_{k-1}^{\prime }$ 有本质区别，下图可以明确展示

在此基础上，求解模型可以展开为
$$\left[\begin{array}{cc}{\hat{\mathbf{P}}}_{k-1}^{-1}+{\mathbf{A}}_{k-1}^T{\mathbf{Q}}_k^{-1}{\mathbf{A}}_{k-1}& -{\mathbf{A}}_{k-1}^T{\mathbf{Q}}_k^{-1}\\ -{\mathbf{Q}}_k^{-1}{\mathbf{A}}_{k-1}& {\mathbf{Q}}_k^{-1}+{\mathbf{C}}_k^T{\mathbf{R}}_k^{-1}{\mathbf{C}}_k\end{array}\right]\left[\begin{array}{c}{\hat{\mathbf{x}}}_{k-1}^{\prime }\\ {\hat{\mathbf{x}}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{\mathbf{P}}}_{k-1}^{-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{A}}_{k-1}^T{\mathbf{Q}}_k^{-1}{\mathbf{v}}_k\\ {\mathbf{Q}}_k^{-1}{\mathbf{v}}_k+{\mathbf{C}}_k^T{\mathbf{R}}_k^{-1}{\mathbf{y}}_k\end{array}\right]$$利用舒尔补，等式两边左乘如下矩阵，便可以直接求解出 ${\hat{\mathbf{x}}}_k$ ，且不需求解 ${\hat{\mathbf{x}}}_{k-1}^{\prime }$
$$\left[\begin{array}{cc}1& 0\\ {\mathbf{Q}}_k^{-1}{\mathbf{A}}_{k-1}{\left({\hat{\mathbf{P}}}_{k-1}^{-1}+{\mathbf{A}}_{k-1}^T{\mathbf{Q}}_k^{-1}{\mathbf{A}}_{k-1}\right)}^{-1}& 1\end{array}\right]$$可得:
$${\hat{\mathbf{P}}}_k^{-1}{\hat{\mathbf{x}}}_k={\check{\mathbf{P}}}_k^{-1}\left({\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{v}}_k\right)+{\mathbf{C}}_k^T{\mathbf{R}}_k^{-1}{\mathbf{y}}_k$$其中
$$\begin{array}{rl}& {\check{\mathbf{P}}}_k={\mathbf{Q}}_k+{\mathbf{A}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_{k-1}^T\\ & {\hat{\mathbf{P}}}_k={\left({\check{\mathbf{P}}}_k^{-1}+{\mathbf{C}}_k^T{\mathbf{R}}_k^{-1}{\mathbf{C}}_k\right)}^{-1}\end{array}$$以上过程，核心即为边缘化，因此滤波(IEKF)可以看做长度为1的滑动窗口。

3. 基于kitti的实现原理

3.1 基于地图定位的滑动窗口模型

1. 窗口优化模型构成
   在图优化模型中，优化模型也可写成如下形式：
   $${\mathbf{J}}^{\top }\mathbf{\Sigma }\mathbf{J}\delta \mathbf{x}=-{\mathbf{J}}^{\top }\mathbf{\Sigma }\mathbf{r}$$其中
   $r$ 是残差；
   $J$ 是残差关于状态量的雅可比；
   $\sum$ 是信息矩阵。
   在kitti工程中，基于地图定位的滑动窗口，其残差包括:

   • 地图匹配位姿和优化变量的残差
   • 激光里程计相对位姿和优化变量的残差
   • IMU预积分和优化变量的残差
   • 边缘化形成的先验因子对应的残差

   此处先介绍前3项，第4项待边缘化后介绍。

2. 地图匹配位姿和优化变量的残差
   该残差对应的因子为地图先验因子。
   一个因子仅约束一个位姿，其模型如下：
   
   残差关于优化变量的雅可比，可视化如下：
   
   因此，对应的Hessian矩阵的可视化为：
   

3. 激光里程计相对位姿和优化变量的残差
   该残差对应的因子为激光里程计因子。
   一个因子约束两个位姿，其模型如下：
   
   残差关于优化变量的雅可比，可视化如下：
   
   因此，对应的Hessian矩阵可视化为：
   

4. IMU预积分和优化变量的残差
   该残差对应的因子为IMU因子。
   一个因子约束两个位姿，并约束两个时刻 IMU
   的速度与 bias。
   
   残差关于优化变量的雅可比，可视化如下：
   
   因此，对应的Hessian矩阵可视化为：
   

5. 完整模型
   完整Hessian矩阵，即为以上各因子对应矩阵的累加。
   
   上述过程用公式可表示为:
   $$\underset{\mathbf{H}}{\underbrace{{\mathbf{J}}^{\top }\mathbf{\Sigma }\mathbf{J}}}\delta \mathbf{x}=\underset{\text{b }}{\underbrace{-{\mathbf{J}}^{\top }\mathbf{\Sigma }\mathbf{r}}}$$其中
   $$\mathbf{r}=\left[\begin{array}{l}{\mathbf{r}}_{Y0}\\ {\mathbf{r}}_{Y1}\\ {\mathbf{r}}_{Y2}\\ {\mathbf{r}}_{L0}\\ {\mathbf{r}}_{L1}\\ {\mathbf{r}}_{M0}\\ {\mathbf{r}}_{M1}\end{array}\right]$$
   $$\begin{array}{rl}& \mathbf{J}=\frac{\partial \mathbf{r}}{\partial \delta x}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_0}{\partial {\delta }_0}\\ \frac{\partial {\mathbf{r}}_1}{\partial \delta x}\\ \frac{\partial {\mathbf{r}}_2}{\partial \delta x}\\ \frac{\partial {\mathbf{r}}_{L0}}{\partial \delta x}\\ \frac{\partial {\mathbf{r}}_{L1}}{\partial \delta x}\\ \frac{\partial {\mathbf{r}}_{M0}}{\partial \delta x}\\ \frac{\partial {\mathbf{r}}_{M1}}{\partial \delta x}\end{array}\right]=\left[\begin{array}{l}{\mathbf{J}}_1\\ {\mathbf{J}}_2\\ {\mathbf{J}}_3\\ {\mathbf{J}}_4\\ {\mathbf{J}}_5\\ {\mathbf{J}}_6\\ {\mathbf{J}}_7\end{array}\right]\\ & {\mathbf{J}}^{\top }=\left[\begin{array}{lllllll}{\mathbf{J}}_1^{\top }& {\mathbf{J}}_2^{\top }& {\mathbf{J}}_3^{\top }& {\mathbf{J}}_4^{\top }& {\mathbf{J}}_5^{\top }& {\mathbf{J}}_6^{\top }& {\mathbf{J}}_7^{\top }\end{array}\right]\\ & \end{array}$$矩阵乘法写成累加形式为:
   $$\sum _{i=1}^7{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i{\mathbf{J}}_i\delta \mathbf{x}=-\sum _{i=1}^7{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i{\mathbf{r}}_i$$此累加过程，即对应前面可视化时各矩阵叠加。

3.2 边缘化过程

1. 移除老的帧
   假设窗口长度为3，在加入新的帧之前，需要先边缘化掉老的帧，即下图方框中的变量。
   用前述公式，可以表示为
   $$\left(H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}\right)\delta x_r=b_r-H_{rm}{H}_{mm}^{-1}{b}_m$$但是在实际代码中，会把它拆成两步实现。

   第一步：使用和要边缘化掉的量无关的因子，构建剩余变量对应的Hessian矩阵。
   
   上标 $a$ 代表是第一步中的变量，包含Hessian矩阵的完整表达式为
   $$H_{rr}^a\delta x_r=b_r^a$$第二步：挑出和要边缘化掉的量相关的因子，构建待边缘化的Hessian矩阵，并使用舒尔补做边缘化。
   
   上标 $b$ 代表是第二步中的变量，包含Hessian矩阵的完整表达式为
   $$\left[H_{rr}^b-H_{rm}^b{\left(H_{mm}^b\right)}^{-1}{H}_{mr}^b\right]\delta x_r=b_r^b-H_{rm}^b{\left(H_{mm}^b\right)}^{-1}{b}_m^b$$这一步形成的约束(上式)就叫先验因子，它包含了边缘化掉的量对剩余变量的约束关系。
   最终使用的是两步的叠加，Hessian矩阵叠加的可视化如下：
   
   对应的完整公式为
   $$H_{rr}\delta x_r=b_r$$其中
   $$\begin{array}{c}{H}_{rr}=H_{rr}^a+H_{rr}^b-H_{rm}^b{\left(H_{mm}^b\right)}^{-1}{H}_{mr}^b\\ b_r=b_r^a+b_r^b-H_{rm}^b{\left(H_{mm}^b\right)}^{-1}{b}_m^b\end{array}$$边缘化之后，模型如下：
   
   注意：边缘化先验因子只有在第一次边缘化之前是不存在的，完成第一次边缘化之后就一直存在，并且随着后续新的边缘化进行，其内容不断更新。

2. 添加新的帧
   添加新的帧之后，模型如下：
   
   此处直接给出新的Hessian矩阵可视化结果：
   
   此后，随着定位过程的进行，便不断循环“边缘化老帧->添加新帧”的过程，从而维持窗口长度不变。
   该过程的代码实现可参考后面lio-mapping的实现，理解后者便很容易实现前者。

4. lio-mapping 介绍

4.1 核心思想

基于滑动窗口方法，把雷达线/面特征、IMU预积分等的约束放在一起进行优化。

1. $o$ 到 $i$ 是滑窗；
2. 只有 $p$ 到 $i$ 的位姿在滑窗中优化；
3. $o$ 到 $p$ 是为了构建局部地图，防止地图过于稀疏；
4. 局部地图都投影到 $p$ 的位姿处；
5. 滑窗中点云约束是当前优化帧和局部地图特征匹配，因此特征对应的因子约束的是 $p$ 帧和 $k$ 帧($p$<$k$<=$j$)。

其优化模型为
min⁡X12{∥rP(X)∥2+∑m∈mLαα∈{p+1,⋯,j}∥rL(m,X)∥CLαm2+∑β∈{p,⋯,j−1}∥rB(zβ+1β,X)∥CBβ+1Bβ2}\begin{aligned} & \min _{\mathbf{X}} \frac{1}{2}\left\{\left\|\mathbf{r}_{\mathcal{P}}(\mathbf{X})\right\|^2+\sum_{\substack{m \in \mathbf{m}_{L_\alpha} \\ \alpha \in\{p+1, \cdots, j\}}}\left\|\mathbf{r}_{\mathcal{L}}(m, \mathbf{X})\right\|_{\mathbf{C}_{L_\alpha}^m}^2\right. \\ & \left.+\sum_{\beta \in\{p, \cdots, j-1\}}\left\|\mathbf{r}_{\mathcal{B}}\left(z_{\beta+1}^\beta, \mathbf{X}\right)\right\|_{\mathbf{C}_{B_{\beta+1}}^{B_\beta}}^2\right\} \end{aligned} ​Xmin​21​⎩⎨⎧​∥rP​(X)∥2+m∈mLα​​α∈{p+1,⋯,j}​∑​∥rL​(m,X)∥CLα​m​2​+β∈{p,⋯,j−1}∑​​rB​(zβ+1β​,X)​CBβ+1​Bβ​​2​⎭⎬⎫​​其中
${\mathbf{r}}_{\mathcal{P}}(\mathbf{X})$ 是边缘化产生的先验因子对应的残差；
${\mathbf{r}}_{\mathcal{L}}(m,\mathbf{X})$ 是点云特征匹配对应的残差；
${\mathbf{r}}_{\mathcal{B}}\left(z_{\beta +1}^{\beta },\mathbf{X}\right)$ 是IMU约束对应的残差。

4.2 具体流程

流程讲解思路：
• 以前述kitti中实现原理为基础，此处只是多了点云特征的约束；
• 只介绍可借鉴的内容，因此不介绍bias、外参初始化和外参优化等内容。

4.2.1 各类因子

1. 定义IMU 因子
   

2. 添加imu因子
   

3. 定义点云面特征因子
   

4. 添加点云面特征
   

5. 定义边缘化先验因子
   由于不确定边缘化后会和哪些量产生关联，因此没有固定size。
   其详细内容待讲解边缘化实现时再展开。

6. 添加边缘化先验因子
   

4.2.2 滑窗模型

• 帧与帧之间通过特征约束，因此没有了激光里程计因子。
• 当前模型中没有使用点云的线特征。

1. IMU预积分和优化变量的残差
   一个因子约束两个位姿，并约束两个时刻 IMU 的速度与 bias
   
   残差关于优化变量的雅可比可视化如下：
   
   因此对应的Hessian矩阵的可视化为：
   

2. 点云面特征对应的残差
   一个因子约束两个位姿
   
   残差关于优化变量的雅可比可视化如下：
   
   因此对应的Hessian矩阵的可视化为
   

3. 完整模型
   以上工程，就是前述代码中添加各类因子到模型的过程。

4.2.3 边缘化

1. 边缘化模型
   
   需要边缘化掉的为方框中的变量

2. 边缘化可视化
   第一步：使用和要边缘化掉的量无关的因子，构建剩余变量对应的Hessian矩阵。
   
   第二步：挑出和要边缘化掉的量相关的因子，构建待边缘化的Hessian矩阵，并使用舒尔补做边缘化。
   
   对应的完整Hessian矩阵就是他们合在一起的结果。
   

3. 边缘化实现
   核心思路是把要边缘化掉的变量，以及跟这些变量被同一个因子约束的变量，汇总在一起。

   

   在该例中，把和模型无关的量去除，剩余部分如下(设每帧有20个面特征)
   因此，放入MarginalizationInfo中的信息包括:
   5个变量: $T_0{M}_0{T}_1{M}_1{T}_2$
   41个因子： 40 个面特征因子、1个IMU因子
   （此处假设的是第一次进行边缘化，若不是第 一次，因子中还应该有边缘化先验因子)
   找出所有变量后，需要知道哪些是应该边缘化的，哪些是应该保留的。
   右图代码中，形参里_parameter_blocks包含所有相关参数，而_drop_set即为这些参数中要边缘化掉的参数的id。
   
   添加和IMU因子相关的ResidualBlockInfo
   
   添加和面特征因子相关的ResidualBlockInfo
   
   在添加以上ResidualBlockInfo的同时，核心变量parameter_block_size 就被赋值。
   
   第二个核心函数的作用是计算每个因子对应的变量(parameter_blocks)、误差项(residuals)、雅可比矩阵(jacobians)，并把变量数值放到parameter_block_data中。
   
   第三个核心函数的作用构建Hessian矩阵，Schur掉需要marg的变量，得到对剩余变量的约束，即为边缘化约束（先验约束）。
   函数的前半部分，对m、n和parameter_block_idx 这三个核心变量进行了赋值。
   
   函数的中间部分开始构建Hessian 矩阵，由于使用多线程，因此要给不同的线程平均分配因子。
   
   函数的最后便是执行边缘化，得到边缘化先验因子。
   
   上述过程是假设第一次执行边缘化，当不是第一次时，步骤上只是多了在AddResidualBlockInfo时把边缘化先验因子也加入进来，剩余过程不变。
   

4.2.4 添加新帧

边缘化之后，模型如下：

注意：由于面特征因子都是和第一帧($T_0$)关联，当它边缘化掉之后，该因子便不存在，因此在加入新的帧的同时，还需要构建所有帧和当前第一帧($T_1$)的面特征关联。

添加新的帧以后，模型如下：

Hessian矩阵可视化如下：

此后不断循环该过程，便可以实现lio功能。