《视觉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    7.82e+00    1.03e+03   2.40e+00   1.76e+00  6.56e+07        1    1.25e-01    1.22e+00
   9  1.234934e+04    4.71e+00    5.04e+02   1.67e+00   1.87e+00  1.97e+08        1    1.25e-01    1.35e+00
  10  1.234610e+04    3.24e+00    4.31e+02   1.15e+00   1.88e+00  5.90e+08        1    1.24e-01    1.47e+00
  11  1.234386e+04    2.24e+00    3.27e+02   8.44e-01   1.90e+00  1.77e+09        1    1.26e-01    1.60e+00
  12  1.234232e+04    1.54e+00    3.44e+02   6.69e-01   1.82e+00  5.31e+09        1    1.26e-01    1.72e+00
  13  1.234126e+04    1.07e+00    2.21e+02   5.45e-01   1.91e+00  1.59e+10        1    1.24e-01    1.85e+00
  14  1.234047e+04    7.90e-01    1.12e+02   4.84e-01   1.87e+00  4.78e+10        1    1.26e-01    1.97e+00
  15  1.233986e+04    6.07e-01    1.02e+02   4.22e-01   1.95e+00  1.43e+11        1    1.26e-01    2.10e+00
  16  1.233934e+04    5.22e-01    1.03e+02   3.82e-01   1.97e+00  4.30e+11        1    1.26e-01    2.23e+00
  17  1.233891e+04    4.25e-01    1.07e+02   3.46e-01   1.93e+00  1.29e+12        1    1.24e-01    2.35e+00
  18  1.233855e+04    3.59e-01    1.04e+02   3.15e-01   1.96e+00  3.87e+12        1    1.23e-01    2.47e+00
  19  1.233825e+04    3.06e-01    9.27e+01   2.88e-01   1.98e+00  1.16e+13        1    1.24e-01    2.60e+00
  20  1.233799e+04    2.61e-01    1.17e+02   2.16e-01   1.97e+00  3.49e+13        1    1.24e-01    2.72e+00
  21  1.233777e+04    2.18e-01    1.22e+02   1.15e-01   1.97e+00  1.05e+14        1    1.23e-01    2.85e+00
  22  1.233760e+04    1.73e-01    1.10e+02   9.19e-02   1.89e+00  3.14e+14        1    1.23e-01    2.97e+00
  ...

使用 Meshlab 分别打开 g2o 执行文件夹下的 initial.ply 和 final.ply 两个文件，优化前后的效果如下：

bundle_adjustment_g2o ：

运行结果如下：

/home/wh/shenlan/slambook2/ch9/cmake-build-debug/bundle_adjustment_g2o /home/wh/shenlan/slambook2/ch9/problem-16-22106-pre.txt
Header: 16 22106 83718iteration= 0	 chi2= 8894422.962194	 time= 0.265725	 cumTime= 0.265725	 edges= 83718	 schur= 1	 lambda= 227.832660	 levenbergIter= 1
iteration= 1	 chi2= 1772145.543625	 time= 0.234101	 cumTime= 0.499826	 edges= 83718	 schur= 1	 lambda= 75.944220	 levenbergIter= 1
iteration= 2	 chi2= 752585.321418	 time= 0.232891	 cumTime= 0.732716	 edges= 83718	 schur= 1	 lambda= 25.314740	 levenbergIter= 1
iteration= 3	 chi2= 402814.285609	 time= 0.237176	 cumTime= 0.969892	 edges= 83718	 schur= 1	 lambda= 8.438247	 levenbergIter= 1
iteration= 4	 chi2= 284879.389455	 time= 0.235331	 cumTime= 1.20522	 edges= 83718	 schur= 1	 lambda= 2.812749	 levenbergIter= 1
iteration= 5	 chi2= 238356.210033	 time= 0.235521	 cumTime= 1.44074	 edges= 83718	 schur= 1	 lambda= 0.937583	 levenbergIter= 1
iteration= 6	 chi2= 193550.729802	 time= 0.231906	 cumTime= 1.67265	 edges= 83718	 schur= 1	 lambda= 0.312528	 levenbergIter= 1
iteration= 7	 chi2= 146861.192839	 time= 0.232932	 cumTime= 1.90558	 edges= 83718	 schur= 1	 lambda= 0.104176	 levenbergIter= 1
iteration= 8	 chi2= 122873.392728	 time= 0.231028	 cumTime= 2.13661	 edges= 83718	 schur= 1	 lambda= 0.069451	 levenbergIter= 1
iteration= 9	 chi2= 97812.478436	 time= 0.23297	 cumTime= 2.36958	 edges= 83718	 schur= 1	 lambda= 0.046300	 levenbergIter= 1
iteration= 10	 chi2= 80336.316621	 time= 0.233388	 cumTime= 2.60297	 edges= 83718	 schur= 1	 lambda= 0.030867	 levenbergIter= 1
iteration= 11	 chi2= 65654.850651	 time= 0.230989	 cumTime= 2.83396	 edges= 83718	 schur= 1	 lambda= 0.020578	 levenbergIter= 1
iteration= 12	 chi2= 55967.141021	 time= 0.232761	 cumTime= 3.06672	 edges= 83718	 schur= 1	 lambda= 0.013719	 levenbergIter= 1
iteration= 13	 chi2= 53270.115686	 time= 0.230372	 cumTime= 3.29709	 edges= 83718	 schur= 1	 lambda= 0.009146	 levenbergIter= 1
iteration= 14	 chi2= 35981.369897	 time= 0.276943	 cumTime= 3.57403	 edges= 83718	 schur= 1	 lambda= 0.006097	 levenbergIter= 2
iteration= 15	 chi2= 32092.173309	 time= 0.323072	 cumTime= 3.89711	 edges= 83718	 schur= 1	 lambda= 0.016259	 levenbergIter= 3
iteration= 16	 chi2= 31154.877381	 time= 0.276839	 cumTime= 4.17394	 edges= 83718	 schur= 1	 lambda= 0.021679	 levenbergIter= 2
iteration= 17	 chi2= 30773.690800	 time= 0.230582	 cumTime= 4.40453	 edges= 83718	 schur= 1	 lambda= 0.014453	 levenbergIter= 1
iteration= 18	 chi2= 29079.971263	 time= 0.276201	 cumTime= 4.68073	 edges= 83718	 schur= 1	 lambda= 0.012508	 levenbergIter= 2
iteration= 19	 chi2= 28481.944292	 time= 0.279615	 cumTime= 4.96034	 edges= 83718	 schur= 1	 lambda= 0.016678	 levenbergIter= 2
iteration= 20	 chi2= 28439.938323	 time= 0.232138	 cumTime= 5.19248	 edges= 83718	 schur= 1	 lambda= 0.011118	 levenbergIter= 1
iteration= 21	 chi2= 27171.835892	 time= 0.277235	 cumTime= 5.46972	 edges= 83718	 schur= 1	 lambda= 0.011153	 levenbergIter= 2
iteration= 22	 chi2= 26749.623597	 time= 0.277069	 cumTime= 5.74678	 edges= 83718	 schur= 1	 lambda= 0.014871	 levenbergIter= 2
iteration= 23	 chi2= 26674.555645	 time= 0.232376	 cumTime= 5.97916	 edges= 83718	 schur= 1	 lambda= 0.009914	 levenbergIter= 1
iteration= 24	 chi2= 26089.998120	 time= 0.276264	 cumTime= 6.25543	 edges= 83718	 schur= 1	 lambda= 0.010288	 levenbergIter= 2
iteration= 25	 chi2= 25877.861699	 time= 0.277238	 cumTime= 6.53266	 edges= 83718	 schur= 1	 lambda= 0.013717	 levenbergIter= 2
iteration= 26	 chi2= 25834.638622	 time= 0.230416	 cumTime= 6.76308	 edges= 83718	 schur= 1	 lambda= 0.009145	 levenbergIter= 1
iteration= 27	 chi2= 25570.298632	 time= 0.28266	 cumTime= 7.04574	 edges= 83718	 schur= 1	 lambda= 0.011127	 levenbergIter= 2
iteration= 28	 chi2= 25457.520755	 time= 0.277767	 cumTime= 7.32351	 edges= 83718	 schur= 1	 lambda= 0.011716	 levenbergIter= 2
iteration= 29	 chi2= 25380.650160	 time= 0.277565	 cumTime= 7.60107	 edges= 83718	 schur= 1	 lambda= 0.012090	 levenbergIter= 2
iteration= 30	 chi2= 25362.215118	 time= 0.230122	 cumTime= 7.83119	 edges= 83718	 schur= 1	 lambda= 0.008060	 levenbergIter= 1
iteration= 31	 chi2= 25202.452901	 time= 0.278364	 cumTime= 8.10956	 edges= 83718	 schur= 1	 lambda= 0.008800	 levenbergIter= 2
iteration= 32	 chi2= 25122.596797	 time= 0.276689	 cumTime= 8.38625	 edges= 83718	 schur= 1	 lambda= 0.009613	 levenbergIter= 2
iteration= 33	 chi2= 25115.107638	 time= 0.231352	 cumTime= 8.6176	 edges= 83718	 schur= 1	 lambda= 0.006408	 levenbergIter= 1
iteration= 34	 chi2= 24967.189622	 time= 0.278329	 cumTime= 8.89593	 edges= 83718	 schur= 1	 lambda= 0.006268	 levenbergIter= 2
iteration= 35	 chi2= 24910.138884	 time= 0.276907	 cumTime= 9.17283	 edges= 83718	 schur= 1	 lambda= 0.008358	 levenbergIter= 2
iteration= 36	 chi2= 24867.273980	 time= 0.280146	 cumTime= 9.45298	 edges= 83718	 schur= 1	 lambda= 0.006277	 levenbergIter= 2
iteration= 37	 chi2= 24848.840518	 time= 0.276981	 cumTime= 9.72996	 edges= 83718	 schur= 1	 lambda= 0.008370	 levenbergIter= 2
iteration= 38	 chi2= 24847.135479	 time= 0.233287	 cumTime= 9.96325	 edges= 83718	 schur= 1	 lambda= 0.005580	 levenbergIter= 1
iteration= 39	 chi2= 24798.075555	 time= 0.276943	 cumTime= 10.2402	 edges= 83718	 schur= 1	 lambda= 0.007249	 levenbergIter= 2

Process finished with exit code 0

课后习题

1. 证明式（10.25）成立。提示：你可能会用到SMW（Sherman-Morrison-Woodbury）公式，参考[76, 6]。

考察卡尔曼增益的理解。预测值与测量值的“加权平均”（高斯斑相乘）上的一个迭代过程。依旧是开篇的那篇博客

2. 对比g2o 和Ceres 的优化后目标函数的数值。指出为什么两者在Meshlab 中效果一样但为何数值却不同。

目标函数投影模型的代价函数？ 待更新

3. 给Ceres 当中的部分点云进行Schur 消元，看看结果会有什么区别。

Schur消元后，Ceres运行的时间会得到一定程度的提高。

4. 证明S 矩阵为半正定矩阵。

关于半正定的理解。正定、半正定矩阵的直觉代表一个向量经过它的变化后的向量与其本身的夹角小于等于90度。

5. 阅读[28]，看看g2o 对核函数是如何处理的。与Ceres 中的Loss function 有何联系？

g2o参考

6. *在两个示例中，我们优化了相机位姿、以 $f,k_1,k_2$为参数的相机内参以及路标点。请考虑使用第五章介绍的完整的相机模型进行优化，即，至少考虑 $f_x,f_y,p_1,p_2,k_1,k_2$这些量。修改现在的Ceres 和g2o 程序以完成实验。

待填坑。。。