1.BA的定义
BA的中文译为“光束法平差”,也有翻译为“束调整”、“捆绑调整”等,是一种用于计算机视觉和机器人领域的优化技术,主要用于精确优化相机参数(包括内参数和外参数)和三维空间中特征点的位置。BA的目标是通过最小化重投影误差来提高三维重建的精度和一致性。重投影误差是指实际观测到的图像点和通过估计的三维点投影到图像平面上的点之间的距离。重投影误差的示意图如图3所示,其中p1 、p2 和p3 是通过特征匹配知道的同一个空间点P的投影,但是相机的位姿是未知的。初值中P的投影p'1 与实际的p1 之间有距离e,此时可以 调整相机的位姿使之变小。由于这个调整要考虑到很多 个点,而每个点的误差不会为0,因此最终的效果应是整体误差的缩小,这种把相机和三维点放在一起进行最小化的问题统称为BA
2.BA的基本原理
BA的基本原理可以概括为以下几个步骤:
(1) 数学建模
假设有n个三维点和m个相机位置,定义一个误差函 数,该函数计算所有图像观测值与重投影值之间的误差和。 考虑空间中的第i个3D路标点坐标为Pi =[Xi ,Yi ,Zi ]T, 其投影的2D像素坐标为pi ,希望计算的相机位姿的李代 数用ξ表述,每个匹配好的特征点相应的深度为s,相机的内参矩阵为K,包含fx 、fy 、Cx 、Cy 四个参数,Pi 变换到相机坐标系下的空间点坐标为P'i ,则3D路标点到2D 特征点的转换关系如下:
(2) 误差函数构建
由于未知的相机位姿和观测中的噪声,这个等式存在 误差,无法找到一个准确的解。因此,将误差求和构造一 个最小二乘问题,然后找到最佳相机位姿以及特征点的位 置进行最小化,方程如下:
(3) 非线性优化
常用的解非线性最小二乘问题的方法有最速下降法、 高斯牛顿(GaussianNewton,GN)法、列文伯格马尔夸特(LevenbergMarquardt,LM)法等。然而,最速下降法 在计算时容易走出锯齿路线，反而增加了迭代次数，GN 法采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果。
LM法可以看作是GN法和最速下降法的结合。它 通过引入一个调整参数μ来平衡GN法和最速下降法,从 而提高算法的鲁棒性和收敛速度。LM法比GN法有更好的全局收敛性,对于不良条件化问题或远离最优解的初始估计,LM法表现更好。但是,LM方法的计算成本更高。通过增量求解方程,可以求解当前变量的一个改进, LM算法的增量方程如下:
在特征点优化的过程中,式(4)是一个三维线性方程组,在位姿估计过程中,式(4)是六维线性方程组。I为一 个单位矩阵,f(x)是在x处的目标函数值。当在FPGA上部署LM算法时,为了避免重复的λ更新操作,设置缩 放因子,并在每次迭代后,检查系统的误差情况,若误差减 小,则说明优化情况良好,λ除缩放因子,若误差增大,则λ乘缩放因子。
在进行矩阵求解时,针对矩阵的规模和类型,目前最 新的成果中有FSFICholesky、FIIterative和PCG等方 法。FSFICholesky是基于Cholesky分解的直接解法, 适用于稠密矩阵,通过将矩阵分解为下三角矩阵及其转置 来解矩阵。FSFICholesky优化了传统Cholesky分解中 的除法和平方根运算,因此在处理小规模矩阵时表现良 好,尤其在硬件实现中效率更高。FIIterative是基于迭 代法的求解方法,适合大规模矩阵,尤其是稀疏矩阵。它 通过迭代计算来逐步逼近线性方程组的解,计算复杂度较 低,适合高并行度的硬件实现。PCG也是一种迭代法,适 用于大规模稀疏对称正定矩阵。它通过引入预条件器来 加速收敛,特别适合解决复杂度较高的问题,在大规模矩 阵求解中常用。
3.VSLAM中的优化
VSLAM系统中的前端和后端均涉及BA优化,它 们各自承担不同的任务,并相互配合,最终完成机器人对 环境的建图和自我定位。前端优化主要处理图像数据,提 取视觉特征并建立初步的位姿估计;后端优化是全局优 化,主要对整个系统中的地图和位姿信息进行全局一致性 的调整和优化。
前端优化算法主要分为3类:对极几何法、迭代最近点算法(Iterative Closest Point, ICP)和透视n点算法(PerspectivenPoint, PnP)。对极几何法由于主要应用于单目相机,无法获取深度信息,因此具有一定的局限性。ICP算法通过旋转和平移操作来最小化两点集之间的距离,但其精度相对较低。PnP算法通过构建非线性优化问题, 采用最小二乘法进行求解，即通常所称的BA,它是最为关键的位姿估计方法之一。在机器人应用中，只有在获得物体的三维位置和方向后，机器人才能实现高效且精确的运动。在AR应用中，系统利用真实场景中的二维或三维对象作为标记,将虚拟信息与真实场景对齐，这一过程也与相机的姿态密切相关，从而为用户带来真实的体验。因此BA的应用范围更加广泛，合性能较为优异。
在后端优化中,BA是一种全局非线性优化方法,它优化的不仅是相机的位姿,还包括3D地图点的位置。BA是一个高度复杂的非线性最小二乘问题,涉及大量的计算资源。BA 的核心目标是最小化所有帧的重投影误差,从而实现整个系统的全局一致性
