SLAM中Bundle Adjustment与图优化

BA

Bundle Adjustment问题在后端用来优化相机位姿$\xi$(李代数形式)和观测到的路标点p(3D点)，实质上是一个最小二乘问题，下面进行推导

BA求解过程

在视觉里程计中可以利用pnp、icp、对极几何等方法求得相机的位姿$\xi$，也就是旋转矩阵R和平移向量t。
路标点与像素点的关系是:
$$z=\left(\genfrac{}{}{0ex}{}{u}{v}\right)=\frac{1}{s}K(Rp+t{)}_{1:2}$$
简写成
$$z=h(x,y)$$
$x$表示某一时刻相机位姿，对应的李代数为$\xi$，$y$为观测的路标点，即三维点p，由于存在噪声等的影响，求得的像素点与观测到的实际的像素点存在误差：
$$e=z-h(\xi ,p)$$
假设一共求得了m个时刻的相机位姿以及观测到n个路标点，把所有时刻观测到的路标点的误差都考虑进来，令$z_{i,j}$表示在位姿${\xi }_i$下观测路标$p_j$得到的像素点坐标，则构建整体的代价函数为：
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||z_{ij}-h({\xi }_i,p_j)|{|}^2$$
根据在视觉里程计中求得的相机位姿和路标点作为初值，不断寻找下降方向求得最优的相机位姿和路标点使得整体代价函数最小，这就是BA优化问题
把所有的待优化参数放在一个向量里：
$$x=[{\xi }_1,..,{\xi }_m,p_1,..,p_n{]}^T$$
则有
$$\frac{1}{2}||f(x)|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}|{|}^2$$
给自变量一个增量$\Delta x=(\Delta {\xi }_1$