前言

学习笔记，仅供学习，不做商用，如有侵权，联系我删除即可

一、目标

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核：

总结