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

权利要求书

1.一种基于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问题的性质，新状态和测量值是递增到达，所以每一个迭代中，只有一小部分的变量发生改变，所有只有这小部分f_k需要重新线性化，利用这一规律，可以明显加速HΔx＝b的构建，在以往的增量BA中，也是逐步更新[H|b]，

[H_k+1|b_k+1]＝[H_k|b_k]+[∑_k∈LΔH_k|∑_k∈LΔb_k]，

其中，L是需要重新线性化的代价函数几何，这部分涉及至少一个Δx超过变化的阈值，

[ΔH_k|Δb_k]＝[H_k+1|b_k+1]-[H_k|b_k]，

求解BA问题，即求解HΔx＝b，常用的方法是边缘化路标点，先求相机位姿，在将求得的位姿带入原方程求解路标点，

通过schur complement消除第二行获得仅和Δx_c相关的方程，

SΔx_c＝s，

[S|s]＝(U-WV^-1W^T)Δx_c＝(u-WV^-1v)，

S表示相机两帧之间的共视关系，对应于S中的(i₁，i₂)两帧和在S中的第i个相机的块可以算出：

Robust keyframe-based dense SLAM with an RGB-D camera中提出的增量式BA可以逐步获取SLAM的测量值并将其应用加速上式的构造，逐步更新

其中，p是代价函数子集中有联系的点集，也就是需要重新线性化，仅S中的(i₁，i₂)两帧有共视关系时，他们之间是非零块，这种由共视关系引起的稀疏结构可以通过预处理的共轭梯度高效(PCG)的求解SΔx_c＝s，求解得到Δx_c后，路标部分的增量方程Δx_p＝V^-1(v-W^TΔx_c)，对于每个点j，分别可以解得：

其中，χ_j表示能看到点j的帧的集合；

局部BA的改进：

全局BA中运用的增量式BA可以明显加速全局BA，其中大多数的关键帧是没有共视关系的，及S矩阵稀疏，易于求解，但在局部BA中，主要对当前的关键帧K，以及在covisibilitygraph和K有共视关系的其他关键帧KC，还有这些关键帧对应的地图点，优化；因此局部BA的关键帧之间具有很高的共视关系；即S是个稠密矩阵，所以对于SΔx_c＝s则需要一个新的解决方案，根据ICE-BA中BA的大部分时间是消耗在Schur Complement的过程，所以这里把改进的增量BA求解的方法应用在ORBSLAM中的局部BA的Schur Comple ment的问题中，是将一个特征点拆分成多个，这样相机之间的共视关系就降低了，也就是对于的S矩阵变稀疏了，具体为把每一个特征点X_i分为3个子特征，分别为X_i1，X_i2，X_i3，每一子轨道X_ik跟踪3帧，这个子轨道包含了局部BA的关键帧，其对应的逆深度分别拷贝为ρ_j1，ρ_j2，ρ_i3；

相比较在边缘化ρ_j时引入了对于每一对相机间(i₁，i₂)∈χ_j×χ_j非零块这里用边缘化特征点ρ_jk后引入的相比于前者的更小的(i₁，i₂)∈χ_jk×χ_jk替换它，因此，只要有一个|χ_j|中的帧的数量大于滑动窗口的大小n，就会使得稠密矩阵S变为一个对角带状矩阵，接下来，对应的更新为：

是需要重新线性化的代价函数的拆分的点的集合，而且表示两帧(i₁，i₂)看到的路标点j的拆分后的特征点集合，通过这个方法，S会变得非常稀疏，同时，比要小的多，短的特征点中需要被重新线性化的概率相对低很多，其中也可以通过增量式的BA来加速计算，

相比较之前的直接求解S矩阵，代价函数是没有改变，只是在Schur Complent的更新过程运用了拆分特征点，完成Schur Complent之后，还是会使用用未拆分的特征点更新