1.一种VI-SLAM的优化方法,其特征在于,包括:如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;
根据重新线性化后的观测方程,对法向方程进行增量式更新;
根据更新后的法向方程,对舒尔补方程进行增量式更新;
采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解;
一组观测量包括一组相机参数和一个三维点坐标,所述方法还包括:根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
所述如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化的步骤,包括:当相机参数差异|C i|大于设定阈值、三维点坐标差异|X j|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程。
2.根据权利要求1所述的方法,其特征在于,采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
3.根据权利要求2所述的方法,其特征在于,采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时,若更新后的舒尔补方程的上一次迭代结果Ci的绝对值大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
4.根据权利要求1所述的方法,其特征在于,所述根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj;
所述方法还包括:
将观测方程fij线性化为:
其中,eij为重投影误差项, 为相机参数的雅可比矩阵, 为三维点的雅可比矩阵;
根据线性化后的观测方程,构造法向方程:其中,
根据法向方程构造舒尔补方程:
其中,矩阵 向量
i1在1-m之间,i2在1-m之间。
5.根据权利要求4所述的方法,其特征在于,所述根据重新线性化后的观测方程,对法向方程进行增量式更新的步骤,包括:根据重新线性化的观测方程,更新法向方程中的 和 并采用更新后的和 更新法向方程。
6.根据权利要求5所述的方法,其特征在于,所述根据重新线性化的观测方程,更新法向方程中的 和 并采用更新后的 和 更新法向方程的步骤,包括:
如果重新线性化的观测方程为 则采用eij+、和 更新法向方程中的 和
如果 和 在更新前上标为-,更新后的上标为+,则根据更新前后的 和 增量式更新法向方程中的 和其中:
上标-表示更新前的值,上标+表示
更新后的值。
7.根据权利要求6所述的方法,其特征在于,所述根据更新后的法向方程,对舒尔补方程进行增量式更新,包括:如果观测方程 或 需要更新,或三维点j关联于需要更新的观测方程,则更新和采用更新后的 和 更新舒尔补方程的 和si;其中,上标-表示更新前的值,上标+表示更
新后的值。
8.一种VI-SLAM的优化装置,其特征在于,包括:重新线性化模块,用于如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;
法向方程更新模块,用于根据重新线性化后的观测方程,对法向方程进行增量式更新;
舒尔补方程更新模块,用于根据更新后的法向方程,对舒尔补方程进行增量式更新;
求解模块,用于采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解;
一组观测量包括一组相机参数和一个三维点坐标,所述装置还包括:优化方程构造模块,用于根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
所述重新线性化模块具体用于当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程。
9.根据权利要求8所述的装置,其特征在于,所述求解模块在求解过程中,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
10.根据权利要求9所述的装置,其特征在于,所述求解模块在求解过程中,若更新后的舒尔补方程的上一次迭代结果Ci的绝对值大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
11.根据权利要求8所述的装置,其特征在于,所述优化方程构造模块中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj;
所述装置还包括:
线性化模块,用于将观测方程fij线性化为:其中,eij为重投影误差项, 为相机参数的雅可比矩阵, 为三维点的雅可比矩阵;
法向方程构造模块,用于根据线性化后的观测方程,构造法向方程:其中,
舒尔补方程构造模块,用于根据法向方程构造舒尔补方程:其中,矩阵 向量
i1在1-m之间,i2在1-m之间。
12.根据权利要求11所述的装置,其特征在于,所述法向方程更新模块具体用于根据重新线性化的观测方程,更新法向方程中的 和 并采用更新后的和 更新法向方程。
13.根据权利要求12所述的装置,其特征在于,所述法向方程更新模块在更新法向方程的过程中,如果重新线性化的观测方程为 则采用eij+、和 更新法向方程中的 和
如果 和 在更新前上标为-,更新后的上标为+,则根据更新前后的 和 增量式更新法向方程中的 和其中:
上标-表示更新前的值,上标+表示
更新后的值。
14.根据权利要求13所述的装置,其特征在于,所述舒尔补方程更新模块在进行方程的更新过程中,如果观测方程 或 需要更新,或三维点j关联于需要更新的观测方程,则更新 和采用更新后的 和 更新舒尔补方程的 和si;其中,上标-表示更新前的值,上标+表示更
新后的值。
15.一种用于VI-SLAM的优化设备,其特征在于,所述设备包括:一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行时,使得所述一个或多个处理器实现如权利要求1-7中任一所述的VI-SLAM的优化方法。
16.一种计算机可读介质,其存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1-7中任一所述的VI-SLAM的优化方法。