读书笔记:后端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与图优化![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/997be96fc5b39e1c14c418982574d1f5.png)
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/382cde703c9d61643299fe4df28f1593.png)
如果对变量做好排序,例如所有相机位姿在前,路标在后,那么 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 <