(12)发明专利申请
(10)申请公布号 CN 110749308 A(43)申请公布日 2020.02.04
(21)申请号 201910939643.5(22)申请日 2019.09.30
(71)申请人 浙江工业大学
地址 310014 浙江省杭州市下城区朝晖六
区潮王路18号(72)发明人 张剑华 刘儒瑜 刘嘉玲 尹科杰
陈胜勇 (74)专利代理机构 杭州斯可睿专利事务所有限
公司 33241
代理人 王利强(51)Int.Cl.
G01C 9/00(2006.01)G01S 19/53(2010.01)G01S 19/41(2010.01)G06T 17/00(2006.01)
权利要求书1页 说明书4页 附图1页
(54)发明名称
使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位方法(57)摘要
一种使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位方法，包括如下步骤：1)建立坐标系及坐标系之间的统一；2)在系统初始化阶段使用传感器估计相机位姿和2.5D建筑模型，获得图像深度图；再在图像上提取ORB特征点并在深度图上获取特征点对应的深度值，并用于创建3D地图点，从而构建SLAM初始化地图；3)在系统后端优化过程中，在成功跟踪地图点后，系统利用重投影误差同时优化地图点和相机位姿，最后当SLAM系统建立后，系统添加基于建筑物模型优化项和传感器优化项进行优化。本发明既能够快速初始化，又能提升室外相机定位和环境重建的精度。
CN 110749308 ACN 110749308 A
权 利 要 求 书
1/1页
1.一种使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位方法，其特征在于，所述方法包括如下步骤：
1)描述坐标系及其关系建立全局坐标系、2.5D地图坐标系和局部的SLAM坐标系，建立各个坐标系之间的关系，描述消费级GPS传感器数据的结构和表示；
2)室外RGBD快速初始化
利用传感器获得的相机姿态和2.5D地图，获得图像的深度图，进而完成SLAM系统的快速初始化，过程如下：
首先，根据传感器数据和2.5D建筑模型，计算建筑模型上每一点P2.5D＝（X2.5D，Y2.5D，Z2.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位姿之间的距离。
2
CN 110749308 A
说 明 书
1/4页
使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位
方法
技术领域
[0001]本发明涉及相机的室外姿态估计与跟踪，尤其是一种使用消费级GPS和2.5D 建筑物模型的面向SLAM的姿态估计与跟踪方法。
背景技术[0002]相机的姿态估计与跟踪技术是利用传感器或图像信息对相机的旋转方向和位置进行估计的过程。目前，相机的室外姿态估计和跟踪方法分为基于传感器的方法，基于视觉的方法以及多种传感器融合的方法。基于传感器的方法利用GPS (Global Positioning System，全球定位系统)和其他传感器直接获取和跟踪相机的绝对三维位置信息甚至方向信息。在这种方法中，姿态估计完全依赖于GPS等传感器，在传感器信息相对较差的环境中，精确度不高。基于视觉的方法，又分为基于图像数据库或重建地图的方法以及使用视觉和深度传感器的SLAM系统两种方法。第一种方法将输入图像和离线建立的图像数据库或者重建的点云地图进行匹配，从而得到输入图像的位姿。这种方法往往缺乏可拓展性，缺乏适当的更新机制来应对环境的变化。第二种方法又分为基于视觉的SLAM(Simultaneous Localization And Mapping)系统和基于RGBD深度传感器的SLAM。视觉SLAM 方法恢复的是相机的相对姿态，尺度是模糊的，此外，对于使用者而言初始化的操作是困难的。另外，由于硬件上的限制，基于RGBD深度传感器的SLAM系统只能用于近距离的室内场景。基于多模态融合的方法，根据不同的环境和任务选择传感器，利用不同传感器获取的不同的特征数据从而能获得更高的定位和跟踪精度。
发明内容
[0003]为了克服已有基于图像数据库和重建方法的相机姿态估计方法扩展性不足、单目SLAM初始化困难、尺度漂移以及RGBD深度传感器难以应用在室外场景等问题，本发明提出了一种使用消费级GPS传感器数据和2.5D建筑物模型的面向SLAM的室外单目相机定位与追踪方法，既能够快速初始化，又能提升室外相机定位和环境重建的精度。[0004]本发明解决其技术问题所采用的技术方案是：
[0005]一种使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位方法，所述方法包括如下步骤：
[0006]1)描述坐标系及其关系[0007]建立全局坐标系、2.5D地图坐标系和局部的SLAM坐标系，建立各个坐标系之间的关系，描述消费级GPS传感器数据的结构和表示；[0008]2)室外RGBD快速初始化[0009]利用传感器获得的相机姿态和2.5D地图，获得图像的深度图，进而完成 SLAM系统的快速初始化，过程如下：[0010]首先，根据传感器数据和2.5D建筑模型，计算建筑模型上每一点到相机中心的近
3
CN 110749308 A
说 明 书
2/4页
似距离
(1)
[0012]接着，根据深度通道信息，获得图像的深度掩码图。在提取图像中的ORB (Oriented Fast and Rotated BRIEF)关键点后，进一步用深度掩码保留属于建筑物上的ORB关键点和其对应的距离信息；[0013]最后，图像中的二维特征点坐标和距离信息被用于创建3D地图点坐标，从而构建了拥有正确的米制尺度的初始化的SLAM地图，并用于随后的跟踪环节；[0014]3)后端优化
[0015]在成功跟踪地图点后，系统加入地图点，利用重投影误差优化相机的姿态和地图点；在SLAM系统建立之后，系统添加建筑物模型优化项和传感器信息进行优化，过程如下：[0016]3.1)，基于建筑物模型的优化[0017]基于建筑物模型优化，通过关联视觉SLAM地图和2.5D地图，使得相机轨迹和SLAM重建的地图更接近真实的尺度，通过最小化重建地图点和关联的建筑物模型之间的距离达到上述目的，过程如下：[0018]首先，确定当前相机位置的可见建筑立面，通过获取当前相机位置的视野范围，计算视野范围内若干视线与建筑物立面的交点，从而确定当前相机位姿下所有的建筑物平面；
[0019]然后，关联视觉SLAM重建的地图点和其对应的建筑物立面。利用当前传感器位姿得到的深度掩码过滤掉不属于建筑物的特征点，对于剩下的特征三维点，本发明通过计算最小的点与平面的距离，找到地图点对应的建筑物平面，[0020]接着，对当前帧的每一个地图点执行基于g2o的非线性的图优化，将点和面关联的几何误差项作为连接3D地图点的边；[0021]3.2)，重投影误差
[0022]在优化了地图点之后，更新观测到地图点的关键帧位姿，地图点经过几何误差优化后拥有新的位置，进一步，通过重投影误差计算新的相机的轨迹；[0023]3.3)，基于传感器数据的优化
[0024]通过观察连续帧之间深度掩膜图的变化，判断GPS的飘移，由于传感器数据的不确定性，通过若约束误差方差最小化传感器位姿和SLAM位姿之间的距离。[0025]本发明的有益效果主要表现在：使用消费级GPS和2.5D建筑物模型的面向 SLAM的室外相机姿态估计技术，既能够快速初始化，又能够提升室外相机定位和环境重建的精度。附图说明
[0026]图1是本发明所涉及三个坐标系，分别是全局的世界坐标系，2.5D地图坐标系和SLAM坐标系
[0027]具体的实施方式
[0028]下面结合附图对本发明作进一步描述。[0029]参照图1，一种使用消费级GPS和2.5D建筑物模型的面向SLAM的室外定位方法，包括如下步骤：
[0030]1)坐标系的建立
4
[0011]
CN 110749308 A[0031]
说 明 书
3/4页
本发明涉及三个不同的坐标系统：全局坐标系，2.5D局部地图坐标系和SLAM 坐标
系。
由于一般的基于WGS84系统的经纬度坐标不是米制的，因此在本发明中使用 UTM
(Universal Transverse Mercator)全局坐标系。UTM全局坐标系是一个右手坐标系，其中y轴指向正北方向，x轴指向正东方向，z轴垂直指向天空。2.5D 地图坐标系是拥有米制单位的局部坐标系，其坐标中心是全局坐标系中的固定点。 SLAM坐标是相机重建地图的局部坐标系，没有全局尺度。
[0033]来自移动设备的传感器的姿态数据包括时间戳、3自由度的WGS84格式的GPS 位置信息以及来自罗盘和惯性测量单元IMU(Inertial Measurement Unit)以四元数形式表示的3自由度旋转信息。本发明采用的矩阵描述2.5D地图坐标系下来自传感器的相机位姿，它由旋转分量和平移分量组成，代表第帧对应的传感器数据。[0034](2)
[0035]2)室外RGBD快速初始化
[0036]本发明利用当前相机姿态看到的2.5D地图，计算图像中建筑物区域特征像素的距离信息，从而生成建筑物的深度图。具体方法如下：
[0037]计算建筑物模型上每一点到相机中心的近似距离[0038](4)
[0039]完整的初始化过程分为以下几个步骤：首先，利用传感器估计的相机位姿得到 2.5D建筑物模型的深度信息，进一步生成当前帧对应的深度图掩码。深度图仅包含建筑物模型覆盖区域的合理深度值，丢弃超过最大深度值的区域。然后，用深度图掩膜保留彩色图像帧上的对应的建筑物区域，提取ORB特征点。接着，估计每一个ORB特征点的近似距离信息。最后，图像中的特征点坐标和距离信息能够被用于创建3D的地图点坐标。从而所有的特征点3D坐标组成了初始化 SLAM地图，这个初始化地图拥有正确的米制尺度并用于接下来的SLAM跟踪过程。[0040]3)后端优化
[0041]本发明中使用的各模态数据为室外精确跟踪定位提供了补充线索。一方面， GPS和2.5D地图为大场景的室外环境提供了全局了米制尺度。现今，2.5D地图能够轻易从公共资源中获取。从实际角度出发，虽然传感器具有小型，低廉和低功耗的特点，但是却也存在不准确的问题。另一方面，视觉SLAM能够提供准确的局部注册和追踪，但是它难以直接用于室外环境。因此，本发明在一个优化方案中融合所有模态信息。[0042]由于系统不同阶段的不同状态，本发明建立了分段优化机制。根据系统在特定时间的活跃状态，在优化中不同阶段启用某些模态参数而忽略其他模态数据。在初始化阶段，计算特征点近似的距离，创建了初始化地图。由于初始地图来自单帧的观测，得到地图点的距离信息是近似值，因此我们只优化相机位姿。在成功跟踪各个地图点之后，即，已经从不同的相机位姿收集到了多视图的观测后，再在优化中启用地图点，使用重投影误差项同时优化相机姿态和地图点。最后当 SLAM系统建立后，系统添加基于建筑模型优化项和实际的传感器信息进行优化。
[0043]3.1)基于建筑物模型的优化
[0044]基于建筑物模型优化通过匹配SLAM重建地图和真实的2.5D地图，减小近似初始化
5
[0032]
CN 110749308 A
说 明 书
4/4页
引入的误差，提升包含7自由度位姿(平移、旋转和尺度)相机轨迹和SLAM 地图的精度，使其更接近真实的尺度。具体分为三步：[0045]第一步，确认可见的建筑物立面
[0046]当前关键帧的方向角和位置分为和。进而，利用相机内参计算相机的水平视角[0047](6)
[0048]表示相机的焦距，表示图像的宽度。在视野范围内，每隔4度，计算视线与建筑物立面的交点。最后，只保留属于建筑物的交点，如此确认当前帧位姿下所有可见的建筑物立面。代表2.5D地图中建筑物的立面。代表建筑物的个数，代表每个建筑物中立面的个数。通过交点，我们对建筑物立面建立布尔类型的变量, 如果建筑物立面可见，我们设置为真，否则为假。[0049](7)
[0050]第二步，点-面关联
[0051]为了恢复重建点云的真实尺度，3D地图点将关联至少一个对应的立面。本发明利用当前帧对应的深度图掩膜过滤掉不属于建筑物区域的特征点。[0052]对于保留下的3D地图点，计算每一个地图点与可见立面的距离，拥有最小距离的立面即为该地图点对应的建筑物立面，如此关联点和立面。令,表示一系列关键帧可见的三维点地图，属于所有的关键帧的结合。此外，让代表在地面上的正交投影。距离公式如下：[0053](8)[0054]第三步，迭代优化[0055]为了减小残差，方法中图优化将每个关键帧和其关联的地图点作为输入。优化包括几何误差项和关键帧的重投影。方法使用基于g2o框架的一元图优化，并将点和面关联的几何误差项作为连接3D地图点的边，对每一帧的每一个地图点执行非线性优化。[0056](9)[0057]其中，代表SLAM重建的3D地图点，,代表建筑物立面，。代表对应的可见建筑物立面上的一个三维点。几何误差项代表图优化中的第一类边连接3D的地图点。[0058]3.2)重投影误差
[0059]在优化了地图点后，地图点有了新的位置，系统进一步使用重投影误更新观测到该地图点的当前关键帧位姿。在重投影误差方程中，代表相机的内参矩阵，代表对应地图点的特征点。代表鲁棒的Huber损失函数。最后，表示关键帧的6 自由度的姿态。[0060](10)
[0061]3.3)基于传感器的优化
[0062]由于本发明在校准过程中连续使用来自实际传感器相机姿态的深度图像信息，因此可以通过比较连续帧间的建筑物深度图变化来识别GPS的漂移。当连续帧间建筑物深度图突然变化，说明GPS存在漂移，本发明将使用SLAM估计的相机姿态而不是传感器估计的相机姿态。由于传感器数据的不准确性，优化过程使用弱约束将传感器数据加入SLAM系统。[0063](11)
[0064]表示来自视觉SLAM计算的前后两帧之间的相对位姿，同样，表示来自传感器的前后两帧的相对位姿。作为图优化中待优化的顶点。若优化结果与来自SLAM 的姿态之间的差异超过一定阈值，系统将丢弃优化结果。
6
说 明 书 附 图
图1
7
1/1页
CN 110749308 A
因篇幅问题不能全部显示，请点此查看更多更全内容