BA与图优化-SLAM十四讲学习笔记

• BA

• 稀疏性

• 边缘化

---

BA

BA是指视觉图像中提炼出最优的3D模型和相机参数。意思就是从特征点发射的光束，通过调整相机位姿或者特征点的空间位置使光线汇聚到相机的光心，这个过程叫做BA。

 

 

 对于这个用到相机外参(R，t),以及如何投至归一化平面以及考虑畸变的详细情况笔者在这里就不再做赘述。上面的这个流程图描述了路标从世界坐标系转换为相机坐标系的过程只不过把之前的相机畸变的过程考虑进来了。其实这个过程就是观测方程。

 以观测模型为例，上式的x指代相机位姿，即外参R，t,它对应的是李群为T，李代数为。路标y即为三维点p，观测数据的像素坐标为。则从最小二乘角度，观测误差为

 上式只是一个误差模型，现在我们将之前其他时刻观测量考虑起来，那么整体的代价函数为：

 这里需要注意的是BA是把两者全部都放在一起进行优化。因此误差函数的值受两个因素影响：一个是相机姿态，一个是所谓路标点（也就是特征点）。因此可以求误差函数关于二者的偏导数，也就是之前所述的两个偏导数，2*6的那个雅克比矩阵是误差对于扰动的小量位姿的偏导数；2*3的那个雅克比矩阵是误差对于特征点相机坐标系下坐标的偏导数，它再乘以一个3*3的R得到一个2*3的雅克比矩阵就是误差关于特征点世界坐标系下坐标的偏导数。

推荐一个博主感觉总结的很到位：

SLAM14讲学习笔记（七）后端（BA与图优化，Pose Graph优化的理论与公式详解、因子图优化）

 上式是表示整个代价函数在i,j状态下的误差，以及偏导，现在我们我们把相机位姿变量和空间点的变量都放在一起：

 那么对之前的目标函数简化可得：

 由于我们把变量归类成了位姿和空间点两种，（注意E和F必须是整体目标函数对整体变量的导数，它是一个很大的矩阵，里面都是有每个误差项的导数和拼凑起来的）则雅克比矩阵可以分块为：

 现在以高斯牛顿法为例，则H矩阵为：

 现在的问题是，这个H矩阵太大了，如果一幅图上有上百个特征点，这个矩阵就是上万*上万的矩阵了，这时通过来求x则计算量太大了。所以我们得分析这个H矩阵，利用他的特殊结构来加速求解过程。

稀疏性

这个J表示误差对于第个位姿看到第个路标点的误差，前面的是对于位姿的，后面的是对于路标点的。只涉及第i个相机位姿和第j个路标点，对其余部分的变量导数为0；

现在我们来讨论H，因为Jij只有在ij处有非零块那么则使得H矩阵也仅有4个非零块位于(i,i),(i,j),(j,i),(j,ji)。（图中角标写错了应该将jk换回ij）

 

 

 边缘化

现在我们搞清楚H这种稀疏结构，用这个性质来求。Schur消元

 到目前为止，还没有利用它的稀疏特性。虽然C是稀疏的，但是ET这里不稀疏啊，这个矩阵求解会十分困难。高博处理的方式是把左上角E的东西统统处理到B那里：

 整理得：

 方程第一行整理一下就可以得到关于位姿部分的增量方程：