[发明专利]一种基于ORBSLAM3的加速BA优化算法

说明书

本发明涉及后端非线性优化技术领域，且公开了一种基于ORBSLAM3的加速BA优化算法，为了加速局部BA和全局BA，采用ICE‑BA的方法，基于ORBSLAM3的油画算法分别改进，包括全局BA的改进：在ORBSLAM的全局优化中，每个代价函数f_k仅关联很小的一部分变量，例如f_k仅和2个类型变量(T_i,P_i)相关，相应的H矩阵和b矩阵仅具有4和3个非零元素，具有非常稀疏的结构，利用矩阵的稀疏结构和块结构可以得到增量方程HΔx＝b的有效构造，因为SLAM问题的性质，新状态和测量值是递增到达，所以每一个迭代中，只有一小部分的变量发生改变；本发明通过拆分特征点更新舒尔布和批量式的BA可以比原BA加速2倍以上，而精度仍在可接受的范围内。

技术领域

本发明涉及后端非线性优化技术领域，具体为一种基于ORBSLAM3的加速BA优化算法。

背景技术

ORBSLAM为了实现定位和建图，分为四大线程，分别为跟踪，局部建图，局部建图，和重定位。跟踪是为了获得相机的初步位姿和地图点。当来自跟踪线程的新的关键帧作为新的节点，加入到地图中，SLAM系统将更新共视图、地图点、关键帧及生长树，并将新的关键帧插入到地图中。随着关键帧不断增加，地图点在关键帧的投影信息也会越来越多，系统就会剔除不符合约束的地图点，留下更准确的地图点。然后检查关键帧中特征点的正向景深、视差、重投影误差和尺度一致性，没有错误创建一个新的地图点，随后对关键帧和地图点进行局部BA优化，剔除冗余关键帧，降低局部BA优化的复杂度。在全局BA中，把所有的关键帧与地图点进行批量BA优化，得到更精确的位姿和地图点。

在基于特征的视觉SLAM中，机器人的运动过程可以由运动方程、观测方程描述。描述的方程为

运动方程x_k是机器人从K-1时刻到K时刻的转变后的位姿，u表示机器人的运动数据，W表示输入的噪声；观测方程z_k，j是指机器人在x_k处观测到的路标点y_j产生的观测数据，y_j代表空间中的路标点，V表示观测中的噪声。

在机器人运动过程中，位姿和观测的路标点是待估计的状态变量，而u和z是输入变量。SLAM就是通过带噪声的运动数据和观测数据估计估计x和y。

定义x_k包含K时刻所有的未知量，它包含了相机当前时刻的位姿和m个路标点。

在第K时刻，用初始时刻到K时刻的数据来估计k时刻的状态分布为：

p(x_k|x_k,u_1:k,z_1:k)

按照贝叶斯法则：

p(x_k|x_k,u_1:k,z_1:k)∝p(z_k|x_k)p(x_k|x_k,u_1:k,z_1:k-1)

Local BA的代价函数f_k为：

其中t₀表示滑动窗口的第一帧，n是滑动窗口大小，v_i表示当前帧i上观测到的特征点集合。

Global BA的代价函数v_i为：

表示包含第i个相对位姿约束的关键帧集合。