vslam --- 基于滑动窗口的Local BA

orbslam中使用的 local BA 优化部分，是使用g2o图优化进行，基于共视图进行优化，虽然也是选取一部分满足一定共视条件的特征点与关键帧进行优化的，但是与基于滑动窗口的BA，是不同的。

vins-mono 中基于滑动窗口的local BA ，使用ceres ，以及需要定义相应的 costfuction。 在边缘化关键帧时，会将其残差与雅克比，进行保存，作为下一次 BA 的先验信息。

高博的十四讲slam中的滑动窗口，没有进行先验的处理，而是直接移除关键帧，以及与此关键帧相对应的观测值，这样会造成约束信息的缺失，所以在实验中，出现优化偏差（见实验效果图）

例：后续补充一下这个图片 以及对应的矩阵图

linearized_jacobians、
linearized_residuals，
分别指的是边缘化之后从信息矩阵恢复出来雅克比矩阵和残差向量

信息矩阵

最小二乘的求解可以使用上节课的解法：
$${\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{J}\delta \mathbf{\xi }=-{\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{r}$$

$\mathbf{H}\delta \mathbf{\xi }=b$

这里我们可以注意到和上一讲中的形式相比它多了一个协方差矩阵的逆 ${\mathbf{\Sigma }}^{-1}$,这是因为之前并没有考虑噪声，直接认为观测值$\mathbf{r}$和 $\mathbf{\xi }$ 相等，协方差矩阵的逆我们称之为信息矩阵，记为$\Lambda$

边缘化

1. 对于先验损失，其待边缘化优化变量是根据是否等于para_Pose[0]或者para_SpeedBias[0]，也2就是说和第一帧相关的优化变量都作为边缘化的对象

2. 与第一帧相关的变量都作为边缘化的对象

3. 这里值得注意的是和后端优化不同，这里只添加了第一帧和第二帧的相关变量作为优化变量，因此边缘化构造的信息矩阵会比后端优化构造的信息矩阵要小

4. VINS-mono的边缘化操作中会不仅仅会边缘化第一帧相关的优化变量，还会边缘化掉以第一帧为起始观察帧的路标点

5. it->Evaluate()这一句里面其实就是调用各个损失函数中的重载函数Evaluate()

这个函数通过传入的优化变量值parameters，以及先验值（对于先验残差就是上一时刻的先验残差last_marginalization_info，对于IMU就是预计分值pre_integrations[1]，对于视觉就是空间的的像素坐标pts_i, pts_j）可以计算出各项残差值residuals，以及残差对应个优化变量的雅克比矩阵jacobians

前期准备后，正式的边缘化 marginalize()

第一步，秉承这map数据结构没有即添加，存在即赋值的语法
（同时这里会计算出两个变量m和n，他们分别是待边缘化的优化变量的维度和以及保留的优化变量的维度和）

第二步，函数会通过多线程快速构造各个残差对应的各个优化变量的信息矩阵（雅克比和残差前面都已经求出来了），然后在加起来，如下图所示：

因为这里构造信息矩阵时采用的正是parameter_block_idx作为构造顺序，因此，就会自然而然地将待边缘化的变量构造在矩阵的左上方。

第三步，函数会通过shur补操作进行边缘化，然后再从边缘化后的信息矩阵中恢复出来雅克比矩阵linearized_jacobians和残差linearized_residuals，这两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去。