[发明专利]使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位方法

权利要求书

1.一种使用消费级GPS和2.5D建筑物模型的单目相机室外定位和追踪方法，其特征在于，所述方法包括如下步骤：

1)描述坐标系及其关系

建立全局坐标系、2.5D地图坐标系和局部的SLAM坐标系，建立各个坐标系之间的关系，描述消费级GPS传感器数据的结构和表示；

2)室外RGBD快速初始化

利用传感器获得的相机姿态和2.5D地图，获得图像的深度图，进而完成SLAM系统的快速初始化，过程如下：

首先，根据传感器数据和2.5D建筑模型，计算建筑模型上每一点P_2.5D＝(X_2.5D,Y_2.5D,Z_2.5D)到相机中心的近似距离D

接着，根据深度通道信息，获得图像的深度掩码图，在提取图像中的ORB关键点后，进一步用深度掩码保留属于建筑物上的ORB关键点和其对应的距离信息；

最后，图像中的二维特征点坐标p＝(x,y)和距离信息被用于创建3D地图点坐标P＝(X,Y,Z)，从而构建了拥有正确的米制尺度的初始化的SLAM地图，并用于随后的跟踪环节；

3)后端优化

在成功跟踪地图点后，系统加入地图点，利用重投影误差优化相机的姿态和地图点；由于初始地图来自单帧的观测，得到地图点的距离信息是近似值，因此只优化相机位姿，在成功跟踪各个地图点之后，即，已经从不同的相机位姿收集到了多视图的观测后，再在优化中启用地图点，使用重投影误差项同时优化相机姿态和地图点；在SLAM系统建立之后，系统添加建筑物模型优化项和传感器信息进行优化，过程如下：

3.1)，基于建筑物模型的优化

基于建筑物模型优化，通过关联视觉SLAM地图和2.5D地图，使得相机轨迹和SLAM重建的地图更接近真实的尺度，通过最小化重建地图点和关联的建筑物模型之间的距离达到上述目的，过程如下：

首先，确定当前相机位置的可见建筑立面，通过获取当前相机位置的视野范围，计算视野范围内若干视线与建筑物立面的交点，从而确定当前相机位姿下所有的建筑物平面；

然后，关联视觉SLAM重建的地图点和其对应的建筑物立面，利用当前传感器获得的相机位姿得到的深度掩码过滤掉不属于建筑物的特征点，对于剩下的特征三维点，通过计算最小的点与平面的距离，找到地图点对应的建筑物平面，

接着，对当前帧的每一个地图点执行基于g2o的非线性的图优化，将点和面关联的几何误差项作为连接3D地图点的边；

3.2)，重投影误差

在优化了地图点之后，更新观测到地图点的当前帧位姿，地图点经过几何误差优化后拥有新的位置，通过重投影误差计算新的相机的轨迹；

3.3)，基于传感器数据的优化

通过观察连续帧之间深度掩膜图的变化，判断GPS的飘移，由于传感器数据的不确定性，通过若约束误差方差最小化传感器获得的相机位姿和SLAM位姿之间的距离。