[发明专利]一种基于单目相机的车辆定位方法及系统

权利要求书

1.一种基于单目相机的车辆定位方法，其特征在于，所述方法包括：

利用所述车辆上的单目相机采集所述车辆移动时的图像；

提取所述图像中的特征点以及特征线；

在图像帧跟踪过程中，针对任一第一当前帧图像，对第一当前帧图像中的特征线与所述第一当前帧图像的前一帧图像中的特征线进行匹配，以得到第一匹配特征线并确定与所述第一匹配特征线对应的地图线；

根据所述第一当前帧图像中的特征线以及地图线之间的对应关系确定所述单目相机的第一相机位姿，所述第一相机位姿为采集到所述第一当前帧图像时所述单目相机相对于世界坐标系的位姿；

根据所述第一相机位姿将所述第一当前帧图像中的特征点与所述第一当前帧图像的前一帧图像中的特征点进行匹配，以得到第一匹配特征点并确定与所述第一匹配特征点对应的地图点；

非线性迭代优化所述第一相机位姿的取值，直至基于所述第一相机位姿的取值将所述第一匹配特征线对应的地图线以及所述第一匹配特征点对应的地图点反投影至所述第一当前帧图像之后得到的投影线以及投影点与对应的所述第一匹配特征线和所述第一匹配特征点之间的第一反投影误差最小；

以所述第一反投影误差最小时所述第一相机位姿的取值为第一优化相机位姿；

根据所述第一优化相机位姿确定所述车辆的第一位姿；其中，所述车辆的第一位姿为所述车辆相对于所述世界坐标系的位姿，所述世界坐标系以所述单目相机采集到初始帧图像时所述单目相机的光心点位置为坐标原点；

将所述第一当前帧图像的第一未匹配特征点进行地图点匹配，以获得所述第一未匹配特征点对应的地图点，所述第一未匹配特征点为所述第一当前帧图像中与所述第一当前帧图像的前一帧图像的地图点不匹配的特征点；

将所述第一当前帧图像的第一未匹配特征线进行地图线匹配，以获得所述第一未匹配特征线对应的地图线，所述第一未匹配特征线为所述第一当前帧图像中与所述第一当前帧图像的前一帧图像的地图线不匹配的特征线；

非线性迭代优化所述第一优化相机位姿的取值，直至基于所述第一优化相机位姿的取值将所述第一匹配特征线对应的地图线、所述第一匹配特征点对应的地图点、所述第一未匹配特征线对应的地图线以及所述第一未匹配特征点对应的地图点反投影至所述第一当前帧图像之后得到的投影线以及投影点与对应的所述第一匹配特征线、所述第一匹配特征点、所述第一未匹配特征线和所述第一未匹配特征点之间的第二反投影误差最小；

以所述第二反投影误差最小时所述第一优化相机位姿的取值为所述单目相机的第二优化相机位姿；

所述根据所述第一优化相机位姿确定所述车辆的第一位姿，包括：

根据所述第二优化相机位姿确定所述车辆的第二位姿；

所述根据所述第二优化相机位姿确定所述车辆的第二位姿之前，所述方法还包括：

统计所述第一当前帧图像中特征点对应的地图点的第一地图点数量和所述第一当前帧图像中特征线对应的地图线的第一地图线数量；

判断所述第一地图点数量是否小于第一预设地图点数量，并判断所述第一地图线数量是否小于第一预设地图线数量；

若所述第一地图点数量小于所述第一预设地图点数量，且所述第一地图线数量小于所述第一预设地图线数量，获取所述第一当前帧图像的特征线词袋；

根据所述特征线词袋将所述第一当前帧图像的特征线与所述第一当前帧图像的前一关键帧的特征线进行匹配，以得到第二匹配特征线并确定与所述第二匹配特征线对应的地图线，所述第二匹配特征线为所述第一当前帧图像中与所述第一当前帧图像的前一关键帧的地图线存在匹配关系的特征线；所述第一当前帧图像的前一关键帧为在所述第一当前帧图像之前的最接近所述第一当前帧图像的一个关键帧；

根据所述第二匹配特征线以及所述第二匹配特征线对应的地图线确定所述单目相机的第三相机位姿，所述第三相机位姿为采集到所述第一当前帧图像时所述单目相机相对于所述世界坐标系的位姿；

根据所述第三相机位姿将所述第一当前帧图像的特征点与所述第一当前帧图像的前一关键帧的特征点进行匹配，以得到第二匹配特征点并确定与所述第二匹配特征点对应的地图点；

非线性迭代优化所述第三相机位姿的取值，直至基于所述第三相机位姿的取值将所述第二匹配特征线对应的地图线以及所述第二匹配特征点对应的地图点反投影至所述第一当前帧图像之后得到的投影线以及投影点与对应的所述第二匹配特征线和第二匹配特征点之间的第三反投影误差最小；

以所述第三反投影误差最小时所述第三相机位姿的取值为所述单目相机的第三优化相机位姿。