视觉SLAM十四讲：从理论到实践（Chapter9：后端1）
前言
学习笔记，仅供学习，不做商用，如有侵权，联系我删除即可
一、目标
1.理解后端的概念。
2.理解以EKF为代表的滤波器后端的工作原理。
3.理解非线性优化的后端，明白稀疏性是如何利用的。
4.使用g2o和Ceres实际操作后端优化
二、BA和图优化
BA：Bundle Adjustment，是一种批量式的优化方法，给定很多个相机位姿、路标与观测数据，计 算最优的状态估计；定义每个运动/观测方程的误差，并从初始估 计开始寻找梯度下降；
2.1 投影模型和BA代价函数
BA代价函数：
待优化变量：相机的位姿和Landmark的三维空间坐标
2.2 BA的求解
把自变量定义成所有待优化的变量：
，Δx是对应的增量形式，把位姿和空间点单独放一起：
所以目标函数可简化表达为：
把F和E写到一起，即为：
2.3 稀疏性和边缘化
所以有：
图形化表示：
一般情况下的H矩阵：
G-N法求解HΔx=g时，使用了Schur消元（也叫边缘化法，Marginalization）：先求出一部分变量，再更新另一部分变量。即先求Δxc，再求Δxp。
把H表达为下列形式：
先求出Δxc，再去求Δxp。
从条件概率的角度来看：
2.4 鲁棒核函数
对误差点进行鲁棒性处理，常见的核为Huber核：
