[发明专利]一种单目视觉里程计定位方法

权利要求书

1.一种单目视觉里程计定位方法，其特征在于，所述方法包括如下步骤：

读取当前帧图像，判断所述当前帧图像的状态；

若所述当前帧图像处于初始状态，对所述当前帧图像进行初始化，得到所述当前帧图像中初始的三维地图点信息，完成初始化操作后进入稀疏图像对齐步骤；若所述当前帧图像处于正常状态，直接进入稀疏图像对齐步骤；

稀疏图像对齐，在所述当前帧图像上采用半直接法对前一帧图像建立的特征点进行图像对齐，得到前后两帧图像间的初始位姿；

局部地图跟踪包括如下步骤：

步骤6-1)、构建局部地图，遍历保存的所有关键帧图像，找到与所述当前帧图像共视的关键帧图像，并按与所述当前帧图像的距离由近及远取前5至15帧组成局部关键帧图像地图；

步骤6-2)、将所述当前帧图像按行列划分为尺寸为20至60像素的方形图像网格，局部地图点投影至当前帧图像，分别落在不同的图像网格中；

步骤6-3)、给每一个网格分配需要成功优化的投影点数量，设当前帧总的需要成功优化的数量为maxFts≥120，统计存在投影点的网格数量ncells，若满足条件ncells＞1.5×maxFts，则每个网格需优化1个投影点；反之则按照密度分配数量，表达式为：

其中n_i代表第i格的投影点数量；

步骤6-4)、按照由内而外的随机顺序遍历所述网格，对所述网格里的投影点根据Shi-Tomasi算子计算响应分数，按由高到低的顺序对投影点进行优化；

位姿及结构优化，在所述当前帧图像建立更准确的三维到二维的约束关系，并通过所述约束关系构建最小二乘优化，优化所述当前帧图像的位姿变量以使重投影误差平方和最小，得到所述当前帧图像最终的位姿信息。

2.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述对所述当前帧图像进行初始化包括如下步骤：

步骤2-1)、将获取的第一帧图像设为世界坐标系，对所述第一帧图像提取ORB点特征，判断所述ORB点特征数量是否大于最低阈值100，如果满足条件则进入步骤2-2)；

步骤2-2)、获取当前帧图像作为第二帧，采用金字塔Lucas-Kanade光流法对所述第一帧图像特征进行跟踪，判断匹配点数量是否大于最低阈值40，否则重启初始化流程；判断匹配点间的平均视差是否大于最低阈值40，否则重新进入步骤2-2)；

步骤2-3)、根据步骤2-2)中建立的二维到二维的匹配关系，恢复所述第一帧图像和所述第二帧图像的运动；其中所述运动包括两种运动模型，根据所述平均视差方差选择相应的模型恢复所述运动；

步骤2-4)、采用随机抽样一致性算法来避免误匹配点以及不符合所述运动模型的匹配点参与所述运动模型求解，通过重投影误差分析恢复出的位姿的精度；

步骤2-5)、设定尺度，对地图点的平均深度进行归一化；得到所述尺度下的运动模型及地图点，初始化完成并将所述第一帧图像和所述第二帧图像保存为关键帧。

3.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述稀疏图像对齐是基于灰度不变性构建帧间位姿T_k，k-1为优化变量的最小二乘优化，并通过高斯牛顿法算法或LM算法迭代求解。

4.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，在所述初始位姿上构建由关键帧图像组成的局部地图中的关键帧图像指通过关键帧策略筛选的图像序列中具有代表性的图像帧，基于关键帧的建图保存了地图信息及位姿。

5.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述局部地图跟踪中所述地图点的生成方法为：对新的关键帧图像提取新的ORB点特征，通过三角测量方法或深度滤波器方法估计特征深度建立所述地图点。

6.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述位姿及结构优化中，重投影误差表示为：

e(T，P)＝p-π(T(P))

其中T表示位姿，P表示地图点，函数T(·)表示位姿变换，函数π(·)表示相机的成像变换；

在所述位姿优化中，所述最小二乘优化表示为：

其中，T_k，w表示位姿，N为当前帧根据局部地图跟踪得到的地图点数量，P_i表示第i个地图点，函数e(·)表示地图点通过位姿变换后的重投影误差；

在所述结构优化，所述最小二乘优化表示为：

其中，P_w表示地图点，T_i，w表示跟踪到P_w的第i帧关键帧的位姿，函数e(·)表示地图点通过位姿变换后的重头误差。