视觉SLAM—BA模型的理解

最新推荐文章于 2025-09-29 09:09:07 发布

原创最新推荐文章于 2025-09-29 09:09:07 发布 · 7.7k 阅读

24 ·

CC 4.0 BY-SA版权

学习笔记专栏收录该内容

20 篇文章

订阅专栏

本文深入探讨了视觉SLAM中的BA（Bundle Adjustment）优化方法，解析其在图像重建中的核心作用，包括代价函数定义、求解过程及非线性最小二乘优化应用。

参考高翔《视觉SLAM十四讲》

PnP是通过一组匹配好的3D点和2D点来求解两帧图像之间运动的一种算法。PNP的求解有DLT(直接线性变换)、P3P、EPNP和BA优化等方式。ICP的求解有两种方式：线性代数的求解(SVD),非线性优化方式(BA).本文主要讲解BA（Bundle Adjustment）。

基于图的SLAM算法里面使用图优化替代了原来的滤波器，这里图优化就是指BA。
BA的定义：从视觉重建中提炼出最优的3D模型和相机参数，从每个特征点反射出来的几束光线，把相机姿态和特征点空间位置做出最优调整之后，最后收束到相机光心的过程简称BA。它在视觉SLAM中起核心作用。
BA本质上就是最小化重投影残差同时优化位姿和路标点，以便从每个特征点反射出的光线通过调整最后都能通过相机光心，因此也称捆集优化或光束平差法。

a. BA的代价函数

视觉SLAM中的观测方程误差： $e=z-h(\xi ,p)$ (1)

代价函数为： $\frac{1}{2}\sum_{i=1}^{m} \sum _{j=1}^{n}\left \| e_{ij} \right \|^2=\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left \| z_{ij}-h(\xi _i,p_i) \right \|^2$ (2)

其中 $\frac{1}{2}\sum_{i=1}^{m} \sum _{j=1}^{n}\left \| e_{ij} \right \|^2$ 表示为第i个相机第j个特征点的重投影误差之和， $\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left \| z_{ij}-h(\xi _i,p_i) \right \|^2$ 中的 $z_{ij}$ 表示第i个相机第j个特征点在原始图像中的实际像素位置， $\xi _{i}$ 表示第i个相机的位姿， $p_{i}$ 表示第i个特征点的世界坐标系位置。

BA的就是获取使得公式(2)右边值最小化时， $\xi _{i}$ 的取值。其中需要用到非线性最小二乘优化，即通过最小化误差的平方和寻找数据的最佳函数匹配，其中最常用的四种数值优化方法：最速下降法、牛顿法、Gauss-Newton法和Levenberg-Marquardt法。

b.BA的求解

假定 $x=[\xi _1,\cdots ,\xi_m,p_1,\cdots,p_2],$ 则公式（2）的增量变为 $\frac{1}{2}\left \| f(x+\bigtriangleup x) \right \|^2\approx \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left \|e_{ij}+F_ij\bigtriangleup \xi_i +E_{ij}\bigtriangleup p_j \right \|^2$ 其中Fij表示整个代价函数在当前状态下对相机姿态的领导，Eij表示对路标点的偏导。此后便可用最速下降法、牛顿法、Gauss-Newton法和Levenberg-Marquardt法等求解最误差最小值时的位姿,具体解法可参考 https://blog.youkuaiyun.com/xranmail/article/details/101015295和 https://www.cnblogs.com/liuweixin/p/11901266.html 。