[发明专利]一种视觉定位方法和装置

权利要求书

1.一种视觉定位方法，其特征在于，该方法包括，

将来自相机的图像数据流的每帧图像输入直接法里程计进行前端处理，并选取出关键帧；

当累计的当前关键帧的数量达到第一阈值时，触发基于关键帧的视觉重定位；

将视觉重定位结果作为约束融合到直接法里程计的目标函数中，得到融合后的目标函数；

通过最小化所述融合后的目标函数，求解相机位姿。

2.如权利要求1所述的视觉定位方法，其特征在于，该方法进一步包括，

当累计的当前关键帧的数量未达到第一阈值时，禁止触发基于关键帧的视觉重定位，

所述触发所述视觉重定位进一步包括，将累计的关键帧的数量清零。

3.如权利要求1所述的视觉定位方法，其特征在于，所述将视觉重定位结果作为约束融合到直接法里程计的目标函数中包括，将所述视觉重定位结果作为约束，添加至直接法计里程后端优化的约束中。

4.如权利要求1所述的视觉定位方法，其特征在于，所述将视觉重定位结果作为约束融合到直接法里程计的目标函数中包括，在构建的融合重定位结果约束的目标函数中，重定位结果约束被加权，并且加权的权重根据视觉重定位结果的置信度而动态调整。

5.如权利要求4所述的视觉定位方法，其特征在于，所述权重为对视觉重定位结果进行置信度估计的结果，

所述置信度估计的结果由角速度、线速度、视觉重定位时特征点的数量、视觉重定位结果与直接法里程计定位结果的差值决定。

6.如权利要求5所述的视觉定位方法，其特征在于，所述置信度估计按照如下式子计算：

w＝αexp(β₅n-(β₁ω+β₂v+β₃||△T||+β₄||△θ||))

其中，ω为角速度，ν为线速度，n为视觉重定位时的特征点数量，△θ为重定位结果与里程计前端结果的角速度差值，△T为重定位结果与里程计前端结果的位移差值，β₁、β₂、β₃、β₄、β₅以及α为对应项的系数。

7.如权利要求4-6任一所述的视觉定位方法，其特征在于，所述将视觉重定位结果作为约束融合到直接法里程计的目标函数包括，

将相邻视觉重定位之间的所有关键帧进行局部图优化，得到相邻视觉重定位之间关键帧帧间相机位姿；

将得到的相邻视觉重定位之间关键帧帧间相机位姿作为约束，添加至直接法里程计的后端优化的窗口中；其中，所述约束为：视觉重定位结果经过局部图优化之后得到的关键帧帧间相机位姿与直接法里程计得到的该关键帧帧间相机位姿在没有误差的情况下相等；

对于任一图像点，按照如下式子构造融合的误差函数E_new为目标函数：

E_new＝E_photo+wE_{relocalization}

其中，E_photo为直接法里程计后端优化窗口内所有关键帧中该图像点的残差之和，E_{relocalization}为视觉重定位过程中后端优化窗口中所有关键帧的局部图优化结果与直接法里程计定位结果构成的残差之和，w为视觉重定位结果的置信度估计结果。

8.如权利要求7所述的视觉定位方法，其特征在于，所述相邻视觉重定位之间的所有关键帧为相邻两次视觉重定位之间的所有关键帧。