[发明专利]一种基于视觉地图的视觉定位方法、装置

权利要求书

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

采集当前图像，得到当前帧；

提取当前帧的特征点，

将当前帧的特征点与地图中的地图点进行匹配，得到匹配成功的第一匹配特征点，

将当前帧增加至用于位姿图优化的约束帧集合中，基于第一匹配特征点与该第一匹配特征点匹配的第一匹配地图点所构成的地图匹配约束，利用当前约束帧集合，对第一目标函数进行当前帧位姿优化，得到优化后的位姿，作为定位结果，并优化当前约束帧集合，以作为下一帧位姿优化的约束帧集合；

其中，

地图匹配约束为：第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该第一匹配地图点匹配的第一匹配特征点的像素位置之间的误差，或者，当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与该第一匹配特征点匹配的第一匹配地图点在世界坐标系中的空间位置之间的误差；

第一目标函数为：当前约束帧集合中所有帧的所有第一匹配特征点的地图匹配误差之和；

所述优化当前约束帧集合包括，

判断当前帧中第一匹配特征点的数量是否小于设定的第一阈值，如果是，则判定当前帧为关键帧，否则，判定为非关键帧，

若当前帧为非关键帧时，则在约束帧集合中删除该当前帧，

若当前帧为关键帧，则判断当前约束帧集合中帧数量是否达到设定的帧阈值，如果是，则删除约束帧集合中最早添加的关键帧，否则，则不删除约束帧集合中最早添加的关键帧。

2.如权利要求1所述的方法，其特征在于，所述基于第一匹配特征点与该第一匹配特征点匹配的第一匹配地图点所构成的约束，对当前帧位姿进行优化，得到优化后的位姿，作为定位结果，进一步包括，

基于地图匹配约束、以及至少帧间匹配约束、惯性约束之一，对当前帧位姿进行优化，得到优化后的位姿，作为定位结果，

其中，

帧间匹配约束为：当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与当前帧的上一关键帧中与该第一匹配特征点匹配的第二匹配特征点投影至世界坐标系中的空间位置之间的误差，或者，与第二匹配特征点匹配的第二匹配地图点反投影到当前帧的像素位置与该第二匹配地图点反投影到上一关键帧的像素位置之间的误差；

惯性约束为：根据当前里程计数据和上一关键帧的里程计数据所得到的帧间里程计相对位姿与根据当前帧位姿和上一关键帧位姿所得到的帧间相对位姿之间的误差。