《视觉SLAM十四讲 第二版》笔记及课后习题（第九讲）

读书笔记：后端1

本讲开始，我们转入SLAM 系统的另一个重要模块：后端优化。
我们看到，前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。所以，在视觉里程计的基础上，我们还希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。不过，考虑到精度与性能的平衡，实际当中存在着许多不同的做法。

滤波器后端

关于卡尔曼滤波讲的非常好的一篇教程。
后端（Backend）

• 从带噪声的数据估计内在状态——状态估计问题
• Estimated the inner state from noisy data

渐进式（Incremental）

• 保持当前状态的估计，在加入新信息时，更新已有的估计（滤波）
• 线性系统+高斯噪声=卡尔曼滤波器
• 非线性系统+高斯噪声+线性近似=扩展卡尔曼
• 非线性系统+非高斯噪声+非参数化=粒子滤波器
• Sliding window filter & multiple state Kalman (MSCKF)

批量式（Batch）

• 给定一定规模的数据，计算该数据下的最优估计（优化）

我们又得到了后验均值的表达。总而言之，上面的两个步骤可以归纳为“预测”（Predict）和“更新”（Update）两个步骤：


EKF优点

• 推导简单清楚，适用各种传感器形式
• 易于做多传感器融合

EKF缺点

• 一阶马尔可夫性过于简单
• 可能会发散（要求数据不能有outlier）
• 线性化误差
• 需要存储所有状态量的均值和方差，平方增长

BA与图优化

如果对变量做好排序，例如所有相机位姿在前，路标在后，那么 H 有一定的特殊结构。

图模型与H矩阵存在对应关系：图模型中存在边=>H相应地方出现非零块
H 矩阵中非零矩阵块和图中边的对应关系。如左图当中的H 矩阵当中红色的矩阵块，表示在右图中其对应的变量C2 和P6 之间存在一条边e26。

实际当中的H，路标数量远大于位姿数量（箭头形矩阵或镐子形矩阵）



小结:
本讲比较深入地探讨了状态估计问题与图优化的求解。我们看到在经典模型中，SLAM可以看成状态估计问题。如果我们假设马尔可夫性，只考虑当前状态的话，则得到以EKF为代表的滤波器模型。如若不然，我们也可以选择考虑所有的运动和观测，它们构成一个最小二乘问题。在只有观测方程的情况下，这个问题称为BA，并可利用非线性优化方法求解。我们仔细讨论了求解过程中的稀疏性问题，指出了该问题与图优化之间的联系。

实践部分

bundle_adjustment_ceres：

代码运行如下：

/home/wh/shenlan/slambook2/ch9/cmake-build-debug/bundle_adjustment_ceres /home/wh/shenlan/slambook2/ch9/problem-16-22106-pre.txt
Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points. 
Forming 83718 observations. 
Solving ceres BA ... 
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.842900e+07    0.00e+00    2.04e+06   0.00e+00   0.00e+00  1.00e+04        0    6.47e-02    2.01e-01
   1  1.449093e+06    1.70e+07    1.75e+06   2.16e+03   1.84e+00  3.00e+04        1    1.40e-01    3.41e-01
   2  5.848543e+04    1.39e+06    1.30e+06   1.55e+03   1.87e+00  9.00e+04        1    1.30e-01    4.71e-01
   3  1.581483e+04    4.27e+04    4.98e+05   4.98e+02   1.29e+00  2.70e+05        1    1.27e-01    5.98e-01
   4  1.251823e+04    3.30e+03    4.64e+04   9.96e+01   1.11e+00  8.10e+05        1    1.24e-01    7.23e-01
   5  1.240936e+04    1.09e+02    9.78e+03   1.33e+01   1.42e+00  2.43e+06        1    1.23e-01    8.46e-01
   6  1.237699e+04    3.24e+01    3.91e+03   5.04e+00   1.70e+00  7.29e+06        1    1.26e-01    9.72e-01
   7  1.236187e+04    1.51e+01    1.96e+03   3.40e+00   1.75e+00  2.19e+07        1    1.26e-01    1.10e+00
   8  1.235405e+04