一种车辆移动轨迹修正方法及相关装置

摘要

本申请是关于一种车辆移动轨迹修正方法及相关装置。该方法包括：获取车辆行驶过程中拍摄的图片序列及其拍摄时间；利用图片序列构建车辆的SLAM移动轨迹；获取SLAM移动轨迹上的拍摄至少两帧目标图片时车辆的预测位置；根据上述至少两帧目标图片的拍摄时间，分别获得定位系统对应测得的定位位置；根据拍摄上述至少两帧目标图片时车辆的预测位置和定位系统对应测得的定位位置，计算得到尺度因子、旋转矩阵和平移矩阵；根据尺度因子对SLAM移动轨迹进行尺度变换，并将尺度变换后的SLAM移动轨迹按照旋转矩阵和平移矩阵进行旋转和平移，得到修正后的移动轨迹。本申请能够对SLAM相对轨迹进行修正，有利于提升车辆移动轨迹的精度。

权利要求

1.一种车辆移动轨迹修正方法，其特征在于，包括：获取位于车辆上的单目摄像装置在所述车辆行驶过程中拍摄的图片序列，以及获取所述图片序列中每一帧图片的拍摄时间；

利用所述图片序列构建所述车辆的SLAM移动轨迹，其中，所述SLAM移动轨迹包括拍摄所述每一帧图片时所述车辆的预测位置的组合；

获取所述SLAM移动轨迹上的拍摄至少两帧目标图片时所述车辆的预测位置；

根据所述至少两帧目标图片的拍摄时间，分别获得所述车辆上的定位系统对应测得的定位位置；

根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所述SLAM移动轨迹的修正参数，其中，所述修正参数至少包括：尺度因子、旋转矩阵、平移矩阵，所述旋转矩阵和所述平移矩阵分别为所述SLAM移动轨迹在根据所述尺度因子进行尺度变换后相对于真实移动轨迹的旋转矩阵和平移矩阵；

根据所述尺度因子对所述SLAM移动轨迹进行尺度变换，得到新的SLAM移动轨迹；

将所述新的SLAM移动轨迹按照所述旋转矩阵和所述平移矩阵进行旋转和平移，得到修正后的移动轨迹。

2.根据权利要求1所述的方法，其特征在于，当所述至少两帧目标图片仅包括两帧目标图片时，所述根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所述SLAM移动轨迹的修正参数，包括：根据拍摄所述两帧目标图片时所述车辆的预测位置，得到车辆的第一移动距离；

根据拍摄所述两帧目标图片时所述定位系统对应测得的定位位置，得到车辆的第二移动距离；

计算所述第二移动距离与所述第一移动距离的比值，将所述比值确定为尺度因子；

根据拍摄所述两帧目标图片时所述车辆的预测位置、所述定位系统对应测得的定位位置以及所述尺度因子，利用相似变换公式计算得到旋转矩阵和平移矩阵。

3.根据权利要求1所述的方法，其特征在于，当所述至少两帧目标图片包括多于两帧目标图片时，所述根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所述SLAM移动轨迹的修正参数，包括：根据拍摄所述多于两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，利用最小二乘优化算法计算定位误差，得到最小定位误差以及对应的修正参数，并将所述最小定位误差对应的修正参数确定为所述SLAM移动轨迹的修正参数。

4.根据权利要求1‑3任一所述的方法，其特征在于，所述利用所述图片序列构建所述车辆的SLAM移动轨迹，包括：

确定滑动窗口大小，所述滑动窗口内包括所述图片序列中拍摄时间相邻的至少两帧图片；

根据所述至少两帧图片构建三维空间坐标；

获取所述图片序列中位于所述至少两帧图片之后的下一帧图片；

根据所述下一帧图片和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿；

根据拍摄所述下一帧图片时所述单目摄像装置的位姿，获得拍摄所述下一帧图片时所述车辆的预测位置；

将所述滑动窗口以预设步长在所述图片序列中滑动，分别得到拍摄各帧图片时所述车辆的预测位置；

根据拍摄所述各帧图片时所述车辆的预测位置，生成所述车辆的SLAM移动轨迹。

5.根据权利要求4所述的方法，其特征在于，所述根据所述至少两帧图片构建三维空间坐标之前，所述方法还包括：

对所述至少两帧图片中的目标物体进行识别，并标定出所述至少两帧图片的每一帧图片中的所述目标物体；

其中，所述根据所述至少两帧图片构建三维空间坐标，包括：利用所述至少两帧图片的每一帧图片中的位于所述目标物体标定范围外的区域构建三维空间坐标；

所述获取所述图片序列中位于所述至少两帧图片之后的下一帧图片之后，所述方法还包括：

对所述下一帧图片中的所述目标物体进行识别，并标定出所述下一帧图片中的所述目标物体；

其中，所述根据所述下一帧图片和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿，包括：

根据所述下一帧图片中的位于所述目标物体标定范围外的区域和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿。

6.一种车辆移动轨迹修正装置，其特征在于，包括：第一获取单元，用于获取位于车辆上的单目摄像装置在所述车辆行驶过程中拍摄的图片序列，以及获取所述图片序列中每一帧图片的拍摄时间；

轨迹构建单元，用于利用所述图片序列构建所述车辆的SLAM移动轨迹，其中，所述SLAM移动轨迹包括拍摄所述每一帧图片时所述车辆的预测位置的组合；

第二获取单元，用于获取所述SLAM移动轨迹上的拍摄至少两帧目标图片时所述车辆的预测位置；

第三获取单元，用于根据所述至少两帧目标图片的拍摄时间，分别获得所述车辆上的定位系统对应测得的定位位置；

参数计算单元，用于根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所述SLAM移动轨迹的修正参数，其中，所述修正参数至少包括：尺度因子、旋转矩阵、平移矩阵，所述旋转矩阵和所述平移矩阵分别为所述SLAM移动轨迹在根据所述尺度因子进行尺度变换后相对于真实移动轨迹的旋转矩阵和平移矩阵；

尺度变换单元，用于根据所述尺度因子对所述SLAM移动轨迹进行尺度变换，得到新的SLAM移动轨迹；

轨迹修正单元，用于将所述新的SLAM移动轨迹按照所述旋转矩阵和所述平移矩阵进行旋转和平移，得到修正后的移动轨迹。

7.根据权利要求6所述的装置，其特征在于，当所述至少两帧目标图片仅包括两帧目标图片时，所述参数计算单元具体用于根据拍摄所述两帧目标图片时所述车辆的预测位置，得到车辆的第一移动距离；根据拍摄所述两帧目标图片时所述定位系统对应测得的定位位置，得到车辆的第二移动距离；计算所述第二移动距离与所述第一移动距离的比值，将所述比值确定为尺度因子；根据拍摄所述两帧目标图片时所述车辆的预测位置、所述定位系统对应测得的定位位置以及所述尺度因子，利用相似变换公式计算得到旋转矩阵和平移矩阵。

8.根据权利要求6所述的装置，其特征在于，当所述至少两帧目标图片包括多于两帧目标图片时，所述参数计算单元具体用于根据拍摄所述多于两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，利用最小二乘优化算法计算定位误差，得到最小定位误差以及对应的修正参数，并将所述最小定位误差对应的修正参数确定为所述SLAM移动轨迹的修正参数。

9.根据权利要求6‑8任一所述的装置，其特征在于，所述轨迹构建单元包括：窗口确定子单元，用于确定滑动窗口大小，所述滑动窗口内包括所述图片序列中拍摄时间相邻的至少两帧图片；

空间构建子单元，用于根据所述至少两帧图片构建三维空间坐标；

图片获取子单元，用于获取所述图片序列中位于所述至少两帧图片之后的下一帧图片；

位姿确定子单元，用于根据所述下一帧图片和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿；

位置获取子单元，用于根据拍摄所述下一帧图片时所述单目摄像装置的位姿，获得拍摄所述下一帧图片时所述车辆的预测位置；

窗口滑动子单元，用于将所述滑动窗口以预设步长在所述图片序列中滑动，分别得到拍摄各帧图片时所述车辆的预测位置；

轨迹生成子单元，用于根据拍摄所述各帧图片时所述车辆的预测位置，生成所述车辆的SLAM移动轨迹。

10.一种电子设备，其特征在于，包括：处理器；以及

存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如权利要求1‑5中任一项所述的方法。

说明书

一种车辆移动轨迹修正方法及相关装置

技术领域

[0001] 本申请涉及导航技术领域，尤其涉及一种车辆移动轨迹修正方法及相关装置。

背景技术

[0002] SLAM（Simultaneous Localization And Mapping，即时定位与地图构建）主要用于解决移动设备在未知环境中运行时进行定位导航与地图构建的问题。要进行定位和绘
图，首先需要采集数据，当利用单目摄像装置（如单目摄像头、单目行车记录仪等）采集到的
图像数据进行定位时，只能得到物体间相对的距离关系，使得通过单目视觉SLAM进行定位
导航得到的移动轨迹是相对的，而不是移动设备的真实移动轨迹。因此，要想得到高精度的
移动轨迹，需要对相对轨迹进行修正。

发明内容

[0003] 为克服相关技术中存在的问题，本申请提供一种车辆移动轨迹修正方法及相关装置，能够对SLAM相对轨迹进行修正，有利于提升车辆移动轨迹的精度。

[0004] 本申请第一方面提供一种车辆移动轨迹修正方法，包括：

[0005] 获取位于车辆上的单目摄像装置在所述车辆行驶过程中拍摄的图片序列，以及获取所述图片序列中每一帧图片的拍摄时间；

[0006] 利用所述图片序列构建所述车辆的SLAM移动轨迹，其中，所述SLAM移动轨迹包括拍摄所述每一帧图片时所述车辆的预测位置的组合；

[0007] 获取所述SLAM移动轨迹上的拍摄至少两帧目标图片时所述车辆的预测位置；

[0008] 根据所述至少两帧目标图片的拍摄时间，分别获得所述车辆上的定位系统对应测得的定位位置；

[0009] 根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所述SLAM移动轨迹的修正参数，其中，所述修正参数至少包括如下
之一：尺度因子、旋转矩阵、平移矩阵；

[0010] 根据所述尺度因子对所述SLAM移动轨迹进行尺度变换，得到新的SLAM移动轨迹；

[0011] 将所述新的SLAM移动轨迹按照所述旋转矩阵和所述平移矩阵进行旋转和平移，得到修正后的移动轨迹。

[0012] 优选的，当所述至少两帧目标图片仅包括两帧目标图片时，所述根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所
述SLAM移动轨迹的修正参数，包括：

[0013] 根据拍摄所述两帧目标图片时所述车辆的预测位置，得到车辆的第一移动距离；

[0014] 根据拍摄所述两帧目标图片时所述定位系统对应测得的定位位置，得到车辆的第二移动距离；

[0015] 计算所述第二移动距离与所述第一移动距离的比值，将所述比值确定为尺度因子；

[0016] 根据拍摄所述两帧目标图片时所述车辆的预测位置、所述定位系统对应测得的定位位置以及所述尺度因子，利用相似变换公式计算得到旋转矩阵和平移矩阵。

[0017] 优选的，当所述至少两帧目标图片包括多于两帧目标图片时，所述根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到
所述SLAM移动轨迹的修正参数，包括：

[0018] 根据拍摄所述多于两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，利用最小二乘优化算法计算定位误差，得到最小定位误差以及对应的修正
参数，并将所述最小定位误差对应的修正参数确定为所述SLAM移动轨迹的修正参数。

[0019] 优选的，所述利用所述图片序列构建所述车辆的SLAM移动轨迹，包括：

[0020] 确定滑动窗口大小，所述滑动窗口内包括所述图片序列中拍摄时间相邻的至少两帧图片；

[0021] 根据所述至少两帧图片构建三维空间坐标；

[0022] 获取所述图片序列中位于所述至少两帧图片之后的下一帧图片；

[0023] 根据所述下一帧图片和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿；

[0024] 根据拍摄所述下一帧图片时所述单目摄像装置的位姿，获得拍摄所述下一帧图片时所述车辆的预测位置；

[0025] 将所述滑动窗口以预设步长在所述图片序列中滑动，分别得到拍摄各帧图片时所述车辆的预测位置；

[0026] 根据拍摄所述各帧图片时所述车辆的预测位置，生成所述车辆的SLAM移动轨迹。

[0027] 优选的，所述根据所述至少两帧图片构建三维空间坐标之前，所述方法还包括：

[0028] 对所述至少两帧图片中的目标物体进行识别，并标定出所述至少两帧图片的每一帧图片中的所述目标物体；

[0029] 其中，所述根据所述至少两帧图片构建三维空间坐标，包括：

[0030] 利用所述至少两帧图片的每一帧图片中的位于所述目标物体标定范围外的区域构建三维空间坐标；

[0031] 所述获取所述图片序列中位于所述至少两帧图片之后的下一帧图片之后，所述方法还包括：

[0032] 对所述下一帧图片中的所述目标物体进行识别，并标定出所述下一帧图片中的所述目标物体；

[0033] 其中，所述根据所述下一帧图片和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿，包括：

[0034] 根据所述下一帧图片中的位于所述目标物体标定范围外的区域和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿。

[0035] 本申请第二方面提供一种车辆移动轨迹修正装置，包括：

[0036] 第一获取单元，用于获取位于车辆上的单目摄像装置在所述车辆行驶过程中拍摄的图片序列，以及获取所述图片序列中每一帧图片的拍摄时间；

[0037] 轨迹构建单元，用于利用所述图片序列构建所述车辆的SLAM移动轨迹，其中，所述SLAM移动轨迹包括拍摄所述每一帧图片时所述车辆的预测位置的组合；

[0038] 第二获取单元，用于获取所述SLAM移动轨迹上的拍摄至少两帧目标图片时所述车辆的预测位置；

[0039] 第三获取单元，用于根据所述至少两帧目标图片的拍摄时间，分别获得所述车辆上的定位系统对应测得的定位位置；

[0040] 参数计算单元，用于根据拍摄所述至少两帧目标图片时所述车辆的预测位置和所述定位系统对应测得的定位位置，计算得到所述SLAM移动轨迹的修正参数，其中，所述修正
参数至少包括如下之一：尺度因子、旋转矩阵、平移矩阵；

[0041] 尺度变换单元，用于根据所述尺度因子对所述SLAM移动轨迹进行尺度变换，得到新的SLAM移动轨迹；

[0042] 轨迹修正单元，用于将所述新的SLAM移动轨迹按照所述旋转矩阵和所述平移矩阵进行旋转和平移，得到修正后的移动轨迹。

[0043] 优选的，当所述至少两帧目标图片仅包括两帧目标图片时，所述参数计算单元具体用于根据拍摄所述两帧目标图片时所述车辆的预测位置，得到车辆的第一移动距离；根
据拍摄所述两帧目标图片时所述定位系统对应测得的定位位置，得到车辆的第二移动距
离；计算所述第二移动距离与所述第一移动距离的比值，将所述比值确定为尺度因子；根据
拍摄所述两帧目标图片时所述车辆的预测位置、所述定位系统对应测得的定位位置以及所
述尺度因子，利用相似变换公式计算得到旋转矩阵和平移矩阵。

[0044] 优选的，当所述至少两帧目标图片包括多于两帧目标图片时，所述参数计算单元具体用于根据拍摄所述多于两帧目标图片时所述车辆的预测位置和所述定位系统对应测
得的定位位置，利用最小二乘优化算法计算定位误差，得到最小定位误差以及对应的修正
参数，并将所述最小定位误差对应的修正参数确定为所述SLAM移动轨迹的修正参数。

[0045] 优选的，所述轨迹构建单元包括：

[0046] 窗口确定子单元，用于确定滑动窗口大小，所述滑动窗口内包括所述图片序列中拍摄时间相邻的至少两帧图片；

[0047] 空间构建子单元，用于根据所述至少两帧图片构建三维空间坐标；

[0048] 图片获取子单元，用于获取所述图片序列中位于所述至少两帧图片之后的下一帧图片；

[0049] 位姿确定子单元，用于根据所述下一帧图片和所述三维空间坐标，确定拍摄所述下一帧图片时所述单目摄像装置的位姿；

[0050] 位置获取子单元，用于根据拍摄所述下一帧图片时所述单目摄像装置的位姿，获得拍摄所述下一帧图片时所述车辆的预测位置；

[0051] 窗口滑动子单元，用于将所述滑动窗口以预设步长在所述图片序列中滑动，分别得到拍摄各帧图片时所述车辆的预测位置；

[0052] 轨迹生成子单元，用于根据拍摄所述各帧图片时所述车辆的预测位置，生成所述车辆的SLAM移动轨迹。

[0053] 本申请第三方面提供一种电子设备，包括：

[0054] 处理器；以及

[0055] 存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如上所述的方法。

[0056] 本申请第四方面提供一种非暂时性机器可读存储介质，其上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如上所述的方法。

[0057] 本申请提供的技术方案，可以获取单目视觉下车辆在行驶过程中拍摄的多帧图片组成的图片序列及其拍摄时间；利用图片序列可以构建出车辆的SLAM移动轨迹；在SLAM移
动轨迹上可以获取拍摄至少两帧目标图片时车辆的预测位置，以及在拍摄上述至少两帧目
标图片时定位系统对应测得的定位位置；进一步地，根据拍摄至少两帧目标图片时车辆的
预测位置和定位位置，计算得到尺度因子、旋转矩阵和平移矩阵，并根据尺度因子对SLAM移
动轨迹进行尺度变换，将尺度变换后的SLAM移动轨迹按照旋转矩阵和平移矩阵进行旋转和
平移，得到修正后的移动轨迹。本申请的技术方案，利用单目视觉下的图片实现车辆的即时
定位，以构建出车辆的相对移动轨迹，在对车辆的相对移动轨迹在尺度上进行缩放后，可以
再进行角度和位置上的旋转平移，将车辆的相对移动轨迹往真实移动轨迹上进行修正，从
而能够提升车辆移动轨迹的精度。

[0058] 应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。

附图说明

[0059] 通过结合附图对本申请示例性实施方式进行更详细的描述，本申请的上述以及其它目的、特征和优势将变得更加明显，其中，在本申请示例性实施方式中，相同的参考标号
通常代表相同部件。

[0060] 图1是本申请实施例示出的一种车辆移动轨迹修正方法的流程示意图；

[0061] 图2是本申请实施例示出的一种车辆的SLAM移动轨迹的示意图；

[0062] 图3是本申请实施例示出的一种图片上目标物体识别标定示意图；

[0063] 图4是本申请实施例示出的一种车辆移动轨迹修正装置的结构示意图；

[0064] 图5是本申请实施例示出的一种电子设备的结构示意图。

具体实施方式

[0065] 下面将参照附图更详细地描述本申请的优选实施方式。虽然附图中显示了本申请的优选实施方式，然而应该理解，可以以各种形式实现本申请而不应被这里阐述的实施方
式所限制。相反，提供这些实施方式是为了使本申请更加透彻和完整，并且能够将本申请的
范围完整地传达给本领域的技术人员。

[0066] 在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数
形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包
含一个或多个相关联的列出项目的任何或所有可能组合。

[0067] 应当理解，尽管在本申请可能采用术语“第一”、“第二”、“第三”等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，
在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以
被称为第一信息。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更
多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限
定。

[0068] 以下结合附图详细描述本申请实施例的技术方案。

[0069] 实施例一

[0070] 请参见图1，本申请实施例提供了一种车辆移动轨迹修正方法。如图1所示，该方法至少可以包括以下步骤：

[0071] 110、获取位于车辆上的单目摄像装置在车辆行驶过程中拍摄的图片序列，以及获取图片序列中每一帧图片的拍摄时间。

[0072] 本申请实施例中，单目摄像装置可以包括但不限于单目行车记录仪、单目摄像头、手机等等。该单目摄像装置可以设置于车辆的首部以采集其前方的视频，也可以设置于车
辆的尾部以采集其后方的视频，这里不作限定。为了获得图片，需要对单目摄像装置获取的
视频数据进行抽帧。通常，视频的帧率为每秒30帧，可以按照预置的规则对视频进行抽帧，
例如，每秒抽10帧、15帧、20帧或其他值，从而获得拍摄的多帧图片，上述多帧图片可以组成
图片序列。其中，图片序列中的任意相邻两帧图片的时间间隔为抽帧时间间隔。此外，单目
摄像装置在拍摄图片的同时，会记录下图片的拍摄时间。

[0073] 本申请实施例提供的车辆移动轨迹修正方法可以应用于车机。单目摄像装置可以设置于车机内，也可以设置于车机外，且与车机之间建立有通信连接。

[0074] 120、利用图片序列构建车辆的SLAM移动轨迹，SLAM移动轨迹包括拍摄每一帧图片时车辆的预测位置的组合。

[0075] 本申请实施例中，利用采集到的图片序列进行SLAM定位，可以得到拍摄每一帧图片时车辆的SLAM定位位置，这里将SLAM定位位置看作为预测位置。通过拍摄每一帧图片时
车辆的预测位置可以绘制出车辆的SLAM移动轨迹，如图2所示。车辆的SLAM移动轨迹为车辆
的相对轨迹，而不是车辆的真实移动轨迹，因此，要想得到车辆的真实移动轨迹，需要对车
辆的SLAM移动轨迹进行修正。

[0076] 在一可选的实施方式中，步骤120利用图片序列构建车辆的SLAM移动轨迹的具体实施方式可以包括以下步骤：

[0077] 11）确定滑动窗口大小，该滑动窗口内可以包括图片序列中拍摄时间相邻的至少两帧图片；

[0078] 12）根据上述至少两帧图片构建三维空间坐标；

[0079] 13）获取图片序列中位于上述至少两帧图片之后的下一帧图片；

[0080] 14）根据该下一帧图片和三维空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿；

[0081] 15）根据拍摄该下一帧图片时单目摄像装置的位姿，获得拍摄该下一帧图片时车辆的预测位置；

[0082] 16）将该滑动窗口以预设步长在图片序列中滑动，重复执行步骤11）至步骤15），分别得到拍摄各帧图片时车辆的预测位置；

[0083] 17）根据拍摄各帧图片时车辆的预测位置，生成车辆的SLAM移动轨迹。

[0084] 其中，步骤12）根据上述至少两帧图片构建三维空间坐标的具体实施方式可以包括以下步骤：

[0085] 12a）获取上述至少两帧图片中每一帧图片的特征点；

[0086] 12b）将上述至少两帧图片的特征点进行匹配，得到上述至少两帧图片中匹配成功的第一特征点集；

[0087] 12c）根据第一特征点集构建三维空间坐标。

[0088] 具体的，图片上的特征点可以用来标识图片上的一些标志物体，一般将图片上灰度值发生剧烈变化的点或在图片边缘上曲率较大的点（如两个边缘的交点）看作为图片的
特征点。为了后续更好的进行图片匹配，一般可以将图片中不会随着相机的移动、旋转或者
光照的变化而变化的稳定的点选取为特征点。优选的，可以选择固定的建筑物（如路边的房
屋）、固定的树木或广告牌等中的特征点，而不选择天空或地面上的特征点。

[0089] 上述至少两帧图片中可以包含有不同视角下的同一物体（如建筑物、广告牌、路牌等等）。通过对图片上的特征点进行匹配，可以将不同图片上的同一物体的某些特征点匹配
成功。其中，第一特征点集为上述至少两帧图片中每一帧图片上均匹配成功的特征点的集
合。例如，当滑动窗口大小为包含两帧图片，则上述至少两帧图片仅包含两帧图片（如A、B两
帧图片）时，第一特征点集为A、B两帧图片进行匹配并匹配成功的特征点；当滑动窗口大小
为包含三帧图片，则上述至少两帧图片包含A、B、C三帧图片时，第一特征点集为A、B、C三帧
图片同时进行匹配并匹配成功的特征点，即匹配成功的特征点要同时出现在A、B、C三帧图
片上，不能仅出现在其中一帧或其中两帧图片上。

[0090] 可选的，步骤12a）获取上述至少两帧图片中每一帧图片的特征点的具体实施方式可以为：

[0091] 利用brisk算子提取上述至少两帧图片中每一帧图片的特征点，并对每一帧图片的特征点进行描述，将描述后的特征点作为该帧图片的特征点。

[0092] 相应地，步骤12b）将上述至少两帧图片的特征点进行匹配，得到上述至少两帧图片中匹配成功的第一特征点集的具体实施方式可以为：

[0093] 将上述至少两帧图片描述后的特征点进行匹配，将匹配距离小于预设值的特征点确定为匹配成功的第一特征点集。

[0094] 具体的，brisk算法由于具有较好的旋转不变性、尺度不变性和较好的鲁棒性等特点，在图像配准应用中有很好的表现。图片的一个特征点可以由两部分构成：关键点和描述
子。brisk算法主要利用FAST9‑16进行特征点检测，取得分值较大的点作为特征点（即关键
点），即完成特征点的提取。仅仅利用关键点的信息还无法很好的进行特征点匹配，所以需
要进一步得到更详细的信息，将特征区分开来，因此，需要进行特征点描述，得到特征描述
子。通过特征描述子可以消除视角的变化带来图片的尺度和方向的变化，能够更好的在图
片间匹配。一张图片上的每一个特征描述子都是独特的，具有排他性，尽可能减少彼此间的
相似性。brisk特征描述子可以用二进制数来表示，如256bit或512bit的二进制数。

[0095] 将每一帧图片的特征描述子进行匹配，具体的是可以将一帧图片上的某一个特征描述子与其他帧图片上的所有特征描述子进行匹配，分别计算匹配距离（如汉明距离），取
其他帧图片上匹配距离最小且匹配距离小于预设值的特征点作为匹配点。按照上述方法，
可以逐一对每帧图片上的所有特征点进行匹配，找到匹配成功的特征点。可以理解的是，在
得出匹配距离后，可以再结合特征点在图片上的uv坐标来共同确定匹配的特征点，例如匹
配距离小于预设值，且特征点的uv坐标的差距也在允许的范围内时，才确定为匹配的特征
点，否则不匹配。

[0096] 当一帧图片上的某一个特征点只与其他帧图片中的其中一帧或几帧图片上的特征点匹配，而与某帧或某些帧图片上的特征点不匹配，则该特征点可以看作为无效特征点，
可以进行丢弃。当一帧图片上的某一个特征点在其他帧图片上均能找到匹配的特征点，则
该特征点可以看作为有效特征点。将所有的有效特征点集合起来，可以看作为第一特征点
集。

[0097] 举例来说，当滑动窗口大小为包含两帧图片，上述至少两帧图片仅包含先后采集的A、B两帧图片时，假设利用brisk算法从A帧图片中提取到100个特征点，从B帧图片中提取
到200个特征点。对A、B两帧图片中的特征点进行描述，得到对应的特征描述子。对A、B两帧
图片上的所有特征描述子进行逐一匹配后，得到匹配成功的特征点共50个，即A帧图片上有
50个特征点与B帧图片上的50个特征点一一匹配，则第一特征点集中可以包含匹配成功的A
帧图片上的50个特征点和B帧图片上的50个特征点，即可以将第一特征点集看作为50对特
征点。

[0098] 又举例来说，当滑动窗口大小为包含三帧图片，上述至少两帧图片包含先后采集的A、B、C三帧图片时，假设利用brisk算法从A帧图片中提取到100个特征点，从B帧图片中提
取到150个特征点，从C帧图片中提取到120个点。对A、B、C三帧图片中的特征点进行描述，得
到对应的特征描述子；对A、B、C三帧图片上的所有特征描述子进行逐一匹配后，得到匹配成
功的特征点共50个，即A帧图片上的50个特征点、B帧图片上的50个特征点、C帧图片上的50
个特征点均匹配成功，则第一特征点集中可以包含匹配成功的A帧图片上的50个特征点、B
帧图片上的50个特征点和C帧图片上的50个特征点，即可以将第一特征点集看作为50组特
征点。

[0099] 可以理解的是，也可以利用其他算法（如ORB、SURF或SIFT算法等等）来实现图片特征点的提取和描述，采用不同的算法可能得到不同的图片配准结果，即配准结果会有所差
异。

[0100] 可选的，步骤12c）根据第一特征点集构建三维空间坐标的具体实施方式可以为：

[0101] 利用第一特征点集，采用对极约束计算上述至少两帧图片之间的旋转矩阵和平移矩阵；

[0102] 根据上述至少两帧图片之间的旋转矩阵和平移矩阵，生成第一特征点集的三维空间坐标。

[0103] 具体的，可以利用八点法来求得图片之间的旋转矩阵和平移矩阵，也可以利用对极约束构建最小二乘法来求得图片之间的旋转矩阵和平移矩阵。其中，八点法和最小二乘
法均为较成熟的相关技术，这里将不再叙述其具体实现过程。

[0104] 另外，在利用第一特征点集求出各相邻图片之间的旋转矩阵和平移矩阵后，可以利用三角化计算出第一特征点集中各特征点的三维空间坐标（即特征点的3D位置）。

[0105] 在根据上述至少两帧图片构建出三维空间坐标后，可以从图片序列中获取位于上述至少两帧图片后的下一帧图片。进一步地，可以利用brisk算法提取该下一帧图片的特征
点，并对提取的特征点进行描述，以得到特征描述子。

[0106] 可选的，14）根据该下一帧图片和三维空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿的具体实施方式可以包括以下步骤：

[0107] 14a）将该下一帧图片与上述至少两帧图片中的每一帧图片进行匹配，分别得到该下一帧图片与每一帧图片匹配成功的特征点集；

[0108] 14b）根据该下一帧图片与每一帧图片匹配成功的特征点集，将该下一帧图片中同时与上述至少两帧图片中的至少预设数量帧图片匹配成功的特征点确定为第二特征点集；

[0109] 14c）根据第一特征点集的三维空间坐标确定第二特征点集的三维空间坐标；

[0110] 14d）利用第二特征点集的三维空间坐标以及第二特征点集中位于该下一帧图片上的特征点的位置，确定拍摄该下一帧图片时单目摄像装置的位姿。

[0111] 以滑动窗口大小包含两帧图片进行举例说明，假设上述至少两帧图片包含A、B两帧图片，下一帧图片为C帧图片，A帧图片中的特征点为100个、B帧图片中的特征点为200个，
A、B两帧图片匹配成功的特征点50个，即第一特征点集包含有50对点。提取到C帧图片中的
特征点200个，分别与A帧图片中的特征点匹配成功70个，与B帧图片中的特征点匹配成功60
个，将其中C帧图片中既与A帧图片中的特征点又与B帧图片中的特征点均匹配成功的特征
点划入第二特征点集。例如，C帧图片上编号为c1的特征点与A帧图片上编号为a3的特征点
匹配，又与B帧图片上编号为b2的特征点匹配，则特征点c1为有效特征点，特征点（a3，b2，
c1）为第二特征点集中的其中一组特征点。当C帧图片上编号为c1的特征点仅与A帧图片上
编号为a3的特征点匹配，而在B帧图片中没有找到相匹配的特征点，则特征点c1为无效特征
点（或噪点），将不被包含在第二特征点集内。按照上述方法，可以找到在三帧图片中都匹配
的特征点，以构成第二特征点集。

[0112] 假设C帧图片与A帧图片匹配成功的70个特征点以及C帧图片与B帧图片匹配成功的60个特征点中三帧都存在的特征点有30个，而这30个特征点包含在A、B两帧图片匹配成
功的50个特征点中，因此，可以从这50个特征点的三维空间坐标中提取出上述30个特征点
的三维空间坐标。当然，也可以直接利用三角化计算出上述30个特征点的三维空间坐标，这
里不作限定。进一步的，可以根据上述30个特征点的三维空间坐标，以及30个特征点在C帧
图片上的位置（即uv坐标），采用PnP优化方法可以计算得到拍摄C帧图片时单目摄像装置的
位姿。

[0113] 以滑动窗口大小包含三帧图片进行举例说明，假设上述至少两帧图片包含A、B、C三帧图片，下一帧图片为D帧图片，A帧图片中的特征点为100个、B帧图片中的特征点为200
个，C帧图片中的特征点为150个，A、B、C三帧图片匹配成功的特征点50个，即第一特征点集
包含有50组点。提取到D帧图片中的特征点200个，分别与A帧图片中的特征点匹配成功70
个，与B帧图片中的特征点匹配成功60个，与C帧图片中的特征点匹配成功65个。可以将D帧
图片中同时与A、B、C三帧图片中的至少两帧图片匹配成功的特征点划入第二特征点集，例
如D帧图片中某一特征点在A、B、C三帧图片中均能找到与之匹配的特征点，或者在A、B、C三
帧图片的其中两帧中能找到与之匹配的特征点，则可以认为该特征点为有效特征点，与其
他图片匹配成功的特征点组合起来作为第二特征点集中的一组特征点。而当D帧图片中某
一特征点仅在A、B、C三帧图片的其中一帧图片上找到与之匹配的特征点，则可以认为该特
征点为无效特征点（或噪点），将不被包含在第二特征点集内。按照上述方法逐一匹配，以找
到满足条件的匹配特征点构成第二特征点集。进一步地，通过第二特征点集的三维空间坐
标及其在D帧图片中的位置采用PnP优化方法来计算出拍摄D帧图片时单目摄像装置的位
姿。

[0114] 在实际应用中，滑动窗口大小还可以包含其他数量帧的图片，如4帧、5帧、6帧或其他值等。当滑动窗口包含的图片数量不同时，步骤14b）中的预设数量也随之发生变化，如滑
动窗口取4帧图片时，可以将预设数量设为2或3或4；当滑动窗口取5帧图片时，可以将预设
数量设为3或4或5。

[0115] 由于单目摄像装置设置于车辆上，因此可以将拍摄某帧图片时单目摄像装置的位姿看作为车辆当时的位姿，从而可以得到车辆的预测位置，即实现了车辆的SLAM定位。当
然，也可以是单目摄像装置与车辆之间预设有一种位置关系，根据位置关系对单目摄像装
置的位置进行转换，即可得到车辆的预测位置。

[0116] 可选的，在执行完步骤14c）之后，还可以包括以下步骤：

[0117] 利用该下一帧图片与每一帧图片匹配成功的特征点集中除去第二特征点集后的剩余特征点集，采用三角化计算剩余特征点集的三维空间坐标；

[0118] 利用剩余特征点集的三维空间坐标调整第一特征点集的三维空间坐标和第二特征点集的三维空间坐标。

[0119] 仍以前述滑动窗口大小包含两帧图片为例，C帧图片与A帧图片之间剩余特征点为70‑30=40个，C帧图片与B帧图片之间剩余特征点为60‑30=30个，分别利用三角化计算上述
40个剩余特征点和上述30个剩余特征点的三维空间坐标，从而利用剩余特征点的三维空间
坐标来调整第一特征点集的三维空间坐标和第二特征点集的三维空间坐标，以使第一特征
点集对应的三维空间范围以及第二特征点集对应的三维空间范围能够得以扩充，构建出包
含更多信息的三维地图，进而有利于后续图片配准，提高配准精度。

[0120] 将滑动窗口以预设步长1在图片序列中滑动，获得下一组拍摄时间相邻的至少两帧图片，重复执行上述步骤11）至步骤15）后，得到拍摄下下一帧图片时车辆的预测位置。按
照上述方法不断迭代，可以得到拍摄各帧图片时所述车辆的预测位置，从而可以构建出车
辆的SLAM移动轨迹。

[0121] 仍以前述滑动窗口大小包含两帧图片为例。迭代处理的具体过程可以为：当要对下下一帧（D帧）图片进行配准时，滑动窗口从包含A、B两帧图片滑动到包含B、C两帧图片，构
建B、C两帧图片的第一特征点集的三维空间坐标，并将D帧的特征点分别与B、C两帧图片的
特征点进行匹配，得到匹配成功的第二特征点集。利用第二特征点集的三维空间坐标及其
位于D帧图片上的位置，确定拍摄D帧图片时单目摄像装置的位姿，以得到拍摄D帧图片时车
辆的预测位置。当要对再下一帧（E帧）图片进行配准时，滑动窗口从包含B、C两帧图片滑动
到包含C、D两帧图片，构建C、D两帧图片的第一特征点集的三维空间坐标，并将E帧的特征点
分别与C、D两帧图片的特征点进行匹配，得到匹配成功的第二特征点集。利用第二特征点集
的三维空间坐标及其位于E帧图片上的位置，确定拍摄E帧图片时单目摄像装置的位姿，以
得到拍摄E帧图片时车辆的预测位置。按照上述过程往后迭代，一直到最后一帧图片，以得
到拍摄最后一帧图片时车辆的预测位置。

[0122] 仍以前述滑动窗口大小包含三帧图片为例。迭代处理的具体过程可以为：当要对下下一帧（E帧）图片进行配准时，滑动窗口从包含A、B、C三帧图片滑动到包含B、C、D三帧图
片，构建B、C、D三帧图片的第一特征点集的三维空间坐标，并将E帧的特征点分别与B、C、D三
帧图片的特征点进行匹配，得到匹配成功的第二特征点集。利用第二特征点集的三维空间
坐标及其位于E帧图片上的位置，确定拍摄E帧图片时单目摄像装置的位姿，以得到拍摄E帧
图片时车辆的预测位置。当要对再下一帧（F帧）图片进行配准时，滑动窗口从包含B、C、D三
帧图片滑动到包含C、D、E三帧图片，构建C、D、E三帧图片的第一特征点集的三维空间坐标，
并将F帧的特征点分别与C、D、E三帧图片的特征点进行匹配，得到匹配成功的第二特征点
集。利用第二特征点集的三维空间坐标及其位于F帧图片上的位置，确定拍摄F帧图片时单
目摄像装置的位姿，以得到拍摄F帧图片时车辆的预测位置。按照上述过程往后迭代，一直
到最后一帧图片，以得到拍摄最后一帧图片时车辆的预测位置。

[0123] 在一可选的实施方式中，在执行步骤12）根据上述至少两帧图片构建三维空间坐标之前，步骤120还可以包括以下步骤：

[0124] 对上述至少两帧图片中的目标物体进行识别，并标定出上述至少两帧图片的每一帧图片中的目标物体；

[0125] 相应地，步骤12）根据上述至少两帧图片构建三维空间坐标的具体实施方式可以为：

[0126] 利用上述至少两帧图片的每一帧图片中的位于目标物体标定范围外的区域构建三维空间坐标；

[0127] 在执行步骤13）获取图片序列中位于上述至少两帧图片之后的下一帧图片之后，步骤120还可以包括以下步骤：

[0128] 对该下一帧图片中的目标物体进行识别，并标定出该下一帧图片中的目标物体；

[0129] 相应地，步骤14）根据该下一帧图片和三维空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿的具体实施方式可以为：

[0130] 根据该下一帧图片中的位于目标物体标定范围外的区域和三维空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿。

[0131] 具体的，单目摄像装置在采集车辆前方或后方的视频图像时，往往会采集到一些动态物体，如其他行驶的车辆，人行道上或道路两边行走的人群、动物，或者天上飞行的飞
机或风筝等等移动的物体。目标物体可以看作为预先设置的一种或几种动态物体。由于动
态物体其位置和姿态可能会实时发生变动，如果利用动态物体的特征来构建三维空间的
话，则构建的三维空间精度差。因此，为了保证三维空间构建的准确性，有必要消除动态物
体对三维空间构建的影响，从而需要先找出图片上的所有动态物体，再利用除动态物体以
外的区域上的特征点去构建三维空间坐标。

[0132] 具体的，可以对图片中所有的动态物体进行识别，并将识别出的动态物体进行标定。以图3中示出的车辆在行驶过程中单目摄像装置采集到的其中一帧图片为例，对图3所
示的图片进行动态物体识别，并识别出图片上所有的车辆和行人，并将识别出的车辆和行
人用矩形框进行标定，以标定出每个动态物体所在的位置和大小。其中，可以根据矩形框的
四个顶点的坐标位置来确定动态物体的位置和大小，或者可以以其中一个顶点为原点通过
矢量确定矩形框的范围等。可以理解的是，也可以利用圆圈、椭圆形圈或其他规则或不规则
形状对动态物体进行标定，这里不作唯一限定。

[0133] 其中，可以利用yolo网络对上述至少两帧图片中的目标物体进行识别。yolo是一种基于深度神经网络的对象识别和定位算法。在进行识别操作之前，可以预先构造好训练
样本，具体的可以采集若干样本图片，将样本图片中的目标物体进行标定后，训练标定后的
样本图片，得到样本模型，再将上述至少两帧图片输入样本模型中进行学习，以识别出图片
上所有的目标物体。可以理解的是，还可以采用卷积神经网络CNN算法、R‑CNN算法或其他算
法对图片中的目标物体进行识别，这里不作唯一限定。

[0134] 可选的，在执行步骤12）根据上述至少两帧图片构建三维空间坐标之前，还可以对上述至少两帧图片中的每一帧图片进行分区处理，得到分区后的每一帧图片；从分区后的
每一帧图片中的每个区域提取预设个数的特征点，并将每个区域提取的特征点的集合作为
该帧图片的特征点；将上述至少两帧图片的特征点进行匹配，得到上述至少两帧图片中匹
配成功的第一特征点集，从而根据第一特征点集构建三维空间坐标。相应地，在步骤13）获
取图片序列中位于上述至少两帧图片之后的下一帧图片之后，对该下一帧图片进行分区处
理后提取该下一帧图片的特征点，从而根据该下一帧图片的特征点和第一特征点集的三维
空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿。

[0135] 具体的，单目摄像装置采集到的图片一般会出现道路两边特征比较多，而天空和路面上的特征比较少的情况。这样在提取图片上的特征点时，可能会集中提取某一处的特
征点，从而出现特征点集中在局部的现象，会导致局部特征权重过大，而其他地方的特征点
会被忽略，这种情况将不利于三维空间构建。基于此，对每一帧图片进行分区处理，以将每
一帧图片分割成若干个区域，该若干个区域可以为2个、4个、5个、6个、8个或其他值等。每个
区域中均可以包含有特征点，当提取图片中的特征点时，可以从该帧图片上的每个区域内
提取预设个数的特征点，这些特征点组合起来构成该帧图片的特征点，从而使得提取到的
特征点的位置能够尽可能分布均匀一些，更有利于构建三维空间，进而能够提高三维空间
的构建精度。

[0136] 可选的，可以是按照预设规则对上述至少两帧图片中的每一帧图片进行分区处理，得到分区后的每一帧图片；或者，获取上述至少两帧图片中的每一帧图片的图片信息，
根据每一帧图片的图片信息确定每一帧图片的分区策略，按照每一帧图片的分区策略对每
一帧图片进行分区处理，得到分区后的每一帧图片，其中，图片信息可以包括但不限于图片
的像素值、灰度值以及RGB值中的至少一种。

[0137] 具体的，可以根据单目摄像装置采集到的图片的情况事先设置好分区规则，进而按照该分区规则对各帧图片进行分区处理。例如，根据单目摄像装置采集到的图片一般会
出现道路两边特征比较多，而天空和路面上的特征比较少的情况，可以将图片按照“田”字
形分为4个区域。或者，为了更均匀的提取到特征点，可以将区域分割得更多。

[0138] 另外，也可以根据每帧图片自身的情况来对图片进行分区。具体的，可以根据图片的像素值、灰度值和/或RGB值等的分布来确定图片内容的分布情况，再根据图片内容的分
布情况确定图片的分区策略，并根据图片的分区策略对图片将进行分区处理。其中，分区策
略可以包含图片的区域个数和区域位置等信息。

[0139] 130、获取SLAM移动轨迹上的拍摄至少两帧目标图片时车辆的预测位置。

[0140] 140、根据上述至少两帧目标图片的拍摄时间，分别获得车辆上的定位系统对应测得的定位位置。

[0141] 本申请实施例中，要想对车辆的SLAM移动轨迹进行修正，可以获取SLAM移动轨迹上的拍摄至少两帧目标图片时车辆的预测位置。并且，可以获取定位系统在上述至少两帧
目标图片的拍摄时间测得的车辆的定位位置。其中，该定位系统可以包括但不限于GPS
（Global  Positioning  System，全球定位系统）、北斗卫星定位系统、RTK
（RealTimeKinematic，实时动态）定位系统等中的至少一种。

[0142] 其中，上述至少两帧目标图片可以是拍摄的起始至少两帧图片，对应的，定位系统获取的是起始至少两点的车辆定位位置；上述至少两帧目标图片也可以是拍摄的结尾至少
两帧图片，对应的，定位系统获取的是结尾至少两点的车辆定位位置；上述至少两帧目标图
片也可以是起始一帧或几帧图片和结尾一帧或几帧图片，对应的，定位系统获取的是起始
一点或几点的车辆定位位置和结尾一点或几点的车辆定位位置；上述至少两帧目标图片还
可以是拍摄的其中至少两帧或任意至少两帧图片，对应的，定位系统获取的是拍摄这至少
两帧图片时车辆定位位置。

[0143] 150、根据拍摄上述至少两帧目标图片时车辆的预测位置和定位系统对应测得的定位位置，计算得到SLAM移动轨迹的修正参数，该修正参数可以包括尺度因子、旋转矩阵和
平移矩阵中的至少一个。

[0144] 在一可选的实施方式中，当上述至少两帧目标图片仅包括两帧目标图片时，步骤150根据拍摄上述至少两帧目标图片时车辆的预测位置和定位系统对应测得的定位位置，
计算得到SLAM移动轨迹的修正参数的具体实施方式可以包括以下步骤：

[0145] 21）根据拍摄上述两帧目标图片时车辆的预测位置，得到车辆的第一移动距离；

[0146] 22）根据拍摄上述两帧目标图片时定位系统对应测得的定位位置，得到车辆的第二移动距离；

[0147] 23）计算第二移动距离与第一移动距离的比值，将该比值确定为尺度因子；

[0148] 24）根据拍摄上述两帧目标图片时车辆的预测位置、定位系统对应测得的定位位置以及该尺度因子，利用相似变换公式计算得到旋转矩阵和平移矩阵。

[0149] 举例来说，假设上述两帧目标图片分别为起始帧图片p1和结尾帧图片p2，且拍摄图片p1时车辆的预测位置为P1slam(x1、y1、z1)，拍摄图片p2时车辆的预测位置为P2slam(x2、
y2、z2)，根据上述两个预测位置坐标，可以得到两者之间的第一移动距离d1。拍摄图片p1时
车辆的定位位置为P1gps(x3、y3、z3)，拍摄图片p2时车辆的定位位置为P2gps(x4、y4、z4)，根
据上述两个定位位置坐标，可以得到两者之间的第二移动距离d2。根据第二移动距离d2与
第一移动距离d1的比值可以求得尺度因子s，即尺度因子s=d2/d1。进一步地，利用相似变换
公式可以求得旋转矩阵R和平移矩阵t，该旋转矩阵R和平移矩阵t分别为车辆的SLAM移动轨
迹在根据尺度因子s进行尺度变换后相对于真实移动轨迹的旋转矩阵和平移矩阵。

[0150] 相似变换公式如下：

[0151] (1)

[0152] 其中，Pislam是指拍摄第i帧图片时车辆的预测位置坐标，Pigps是指拍摄第i帧图片时车辆的定位位置坐标。将尺度因子s、拍摄图片p1时车辆的预测位置为P1slam(x1、y1、z1)
和拍摄图片p1时车辆的定位位置为P1gps(x3、y3、z3)代入上述公式，以及将尺度因子s、拍摄
图片p2时车辆的预测位置为P2slam(x2、y2、z2)和拍摄图片p2时车辆的定位位置为P2gps(x4、
y4、z4)代入上述公式，可以得到六个方程式，而旋转矩阵R和平移矩阵t共包含有六个自由
度，因此，可以利用六个方程式求解出六个自由度，从而得到旋转矩阵R和平移矩阵t。

[0153] 在一可选的实施方式中，当上述至少两帧目标图片包括多于两帧目标图片时，步骤150根据拍摄上述至少两帧目标图片时车辆的预测位置和定位系统对应测得的定位位
置，计算得到SLAM移动轨迹的修正参数的具体实施方式可以包括以下步骤：

[0154] 25）根据拍摄上述多于两帧目标图片时车辆的预测位置和定位系统对应测得的定位位置，利用最小二乘优化算法计算定位误差，得到最小定位误差以及对应的修正参数，并
将最小定位误差对应的修正参数确定为SLAM移动轨迹的修正参数。

[0155] 举例来说，当上述多于两帧目标图片为三帧目标图片时，假设上述三帧目标图片分别为图片p1、图片p2和图片p3，且拍摄图片p1时车辆的预测位置为P1slam(x1、y1、z1)，拍
摄图片p2时车辆的预测位置为P2slam(x2、y2、z2)，拍摄图片p3时车辆的预测位置为P3slam
(x3、y3、z3)。拍摄图片p1时车辆的定位位置为P1gps(x4、y4、z4)，拍摄图片p2时车辆的定位
位置为P2gps(x5、y5、z5)，拍摄图片p3时车辆的定位位置为P3gps(x6、y6、z6)。由于受定位误
差的影响，不能单纯通过距离比值来确定尺度因子s。因此可以利用最小二乘优化算法建立
误差方程：

[0156]  (2)

[0157] 将上述位置坐标分别代入上述误差方程（2），可以得到九个方程式，而尺度因子s未知，再加上旋转矩阵R和平移矩阵t包含的六个自由度，一共有七个未知数。利用九个方程
式求解七个未知数，可以得出最优解，该最优解包含了最小定位误差error，以及该最小定
位误差对应的修正参数（即s、R和t）。该最小定位误差对应的修正参数则用于后续SLAM移动
轨迹的修正。

[0158] 可以理解的是，上述多于两帧目标图片是以三帧目标图片为例进行举例说明的，但不局限于此，上述多于两帧目标图片可以是多于三帧目标图片，如4帧、5帧、6帧或其他
值，这里不作限定。

[0159] 160、根据该尺度因子对SLAM移动轨迹进行尺度变换，得到新的SLAM移动轨迹。

[0160] 170、将新的SLAM移动轨迹按照旋转矩阵和平移矩阵进行旋转和平移，得到修正后的移动轨迹。

[0161] 本申请实施例中，在求得尺度因子s后，可以将SLAM移动轨迹按照该尺度因子s进行伸缩，当尺度因子s为大于1的数时，可以将SLAM移动轨迹的长度拉伸为原来的s倍；当尺
度因子s为小于1的数时，可以将SLAM移动轨迹的长度缩小为原来的s倍。在进行完尺度变换
后，可以将尺度变化后的SLAM移动轨迹按照旋转矩阵R和平移矩阵t进行旋转和平移，使得
SLAM移动轨迹向真实移动轨迹进行修正。

[0162] 可以理解的是，本申请实施例虽然是以车辆为例进行说明的，但并不仅仅局限于此，本申请技术方案也可以应用于其他的移动设备，如安装有单目摄像装置和定位系统的
移动机器人中，这里不作限定。

[0163] 综上，本申请实施例可以获取单目视觉下车辆在行驶过程中连接拍摄的多帧图片组成的图片序列及其拍摄时间；利用图片序列可以构建出车辆的SLAM移动轨迹；在SLAM移
动轨迹上可以获取拍摄至少两帧目标图片时车辆的预测位置，以及在拍摄上述至少两帧目
标图片时定位系统对应测得的定位位置；进一步地，根据拍摄至少两帧目标图片时车辆的
预测位置和定位位置，计算得到尺度因子、旋转矩阵和平移矩阵，并根据尺度因子对SLAM移
动轨迹进行尺度变换，将尺度变换后的SLAM移动轨迹按照旋转矩阵和平移矩阵进行旋转和
平移，得到修正后的移动轨迹。本申请的技术方案，利用单目视觉下的图片实现车辆的即时
定位，以构建出车辆的相对移动轨迹，在对车辆的相对移动轨迹在尺度上进行缩放后，再进
行角度和位置上的旋转平移，将车辆的相对移动轨迹往真实移动轨迹上进行修正，从而能
够提升车辆移动轨迹的精度。

[0164] 实施例二

[0165] 与前述应用功能实现方法实施例相对应，本申请还提供了一种车辆移动轨迹修正装置、电子设备及相应的实施例。

[0166] 请参阅图4，本申请提供了一种车辆移动轨迹修正装置。该装置可以用于执行上述实施例一提供的车辆移动轨迹修正方法。具体的，如图4所示，该装置可以包括：

[0167] 第一获取单元41，用于获取位于车辆上的单目摄像装置在车辆行驶过程中拍摄的图片序列，以及获取图片序列中每一帧图片的拍摄时间；

[0168] 轨迹构建单元42，用于利用图片序列构建车辆的SLAM移动轨迹， SLAM移动轨迹包括拍摄每一帧图片时车辆的预测位置的组合；

[0169] 第二获取单元43，用于获取SLAM移动轨迹上的拍摄至少两帧目标图片时车辆的预测位置；

[0170] 第三获取单元44，用于根据上述至少两帧目标图片的拍摄时间，分别获得车辆上的定位系统对应测得的定位位置；

[0171] 参数计算单元45，用于根据拍摄上述至少两帧目标图片时车辆的预测位置和定位系统对应测得的定位位置，计算得到SLAM移动轨迹的修正参数，其中，该修正参数至少可以
包括如下之一：尺度因子、旋转矩阵、平移矩阵；

[0172] 尺度变换单元46，用于根据该尺度因子对SLAM移动轨迹进行尺度变换，得到新的SLAM移动轨迹；

[0173] 轨迹修正单元47，用于将新的SLAM移动轨迹按照旋转矩阵和平移矩阵进行旋转和平移，得到修正后的移动轨迹。

[0174] 可选的，当上述至少两帧目标图片仅包括两帧目标图片时，参数计算单元45具体可以用于根据拍摄上述两帧目标图片时车辆的预测位置，得到车辆的第一移动距离；根据
拍摄上述两帧目标图片时定位系统对应测得的定位位置，得到车辆的第二移动距离；计算
第二移动距离与第一移动距离的比值，将该比值确定为尺度因子；根据拍摄上述两帧目标
图片时车辆的预测位置、定位系统对应测得的定位位置以及该尺度因子，利用相似变换公
式计算得到旋转矩阵和平移矩阵。

[0175] 可选的，当上述至少两帧目标图片包括多于两帧目标图片时，参数计算单元45具体可以用于根据拍摄上述多于两帧目标图片时车辆的预测位置和定位系统对应测得的定
位位置，利用最小二乘优化算法计算定位误差，得到最小定位误差以及对应的修正参数，并
将最小定位误差对应的修正参数确定为SLAM移动轨迹的修正参数。

[0176] 可选的，轨迹构建单元42可以包括：

[0177] 窗口确定子单元，用于确定滑动窗口大小，该滑动窗口内可以包括图片序列中拍摄时间相邻的至少两帧图片；

[0178] 空间构建子单元，用于根据上述至少两帧图片构建三维空间坐标；

[0179] 图片获取子单元，用于获取图片序列中位于上述至少两帧图片之后的下一帧图片；

[0180] 位姿确定子单元，用于根据该下一帧图片和三维空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿；

[0181] 位置获取子单元，用于根据拍摄该下一帧图片时单目摄像装置的位姿，获得拍摄该下一帧图片时车辆的预测位置；

[0182] 窗口滑动子单元，用于将该滑动窗口以预设步长在图片序列中滑动，分别得到拍摄各帧图片时车辆的预测位置；

[0183] 轨迹生成子单元，用于根据拍摄各帧图片时车辆的预测位置，生成车辆的SLAM移动轨迹。

[0184] 可选的，轨迹构建单元42还可以包括：

[0185] 识别标定子单元，用于在空间构建子单元根据上述至少两帧图片构建三维空间坐标之前，对上述至少两帧图片中的目标物体进行识别，并标定出上述至少两帧图片的每一
帧图片中的目标物体；

[0186] 相应地，空间构建子单元具体可以用于利用上述至少两帧图片的每一帧图片中的位于目标物体标定范围外的区域构建三维空间坐标；

[0187] 识别标定子单元，还用于在图片获取子单元获取图片序列中位于上述至少两帧图片之后的下一帧图片之后，对该下一帧图片中的目标物体进行识别，并标定出该下一帧图
片中的目标物体；

[0188] 相应地，位姿确定子单元具体可以用于根据该下一帧图片中的位于目标物体标定范围外的区域和三维空间坐标，确定拍摄该下一帧图片时单目摄像装置的位姿。

[0189] 实施图4所示的装置，利用单目视觉下的图片实现车辆的即时定位，以构建出车辆的相对移动轨迹，在对车辆的相对移动轨迹在尺度上进行缩放后，再进行角度和位置上的
旋转平移，将车辆的相对移动轨迹往真实移动轨迹上进行修正，从而能够提升车辆移动轨
迹的精度。

[0190] 关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不再做详细阐述说明。

[0191] 请参阅图5，本申请提供了一种电子设备。该电子设备可以用于执行上述实施例一提供的车辆移动轨迹修正方法。具体的，如图5所示，该电子设备500可以包括：至少一个处
理器501、存储器502、至少一个通信接口503等组件。其中，这些组件可以通过一条或多条通
信总线504进行通信连接。本领域技术人员可以理解，图5中示出的电子设备500的结构并不
构成对本申请实施例的限定，它既可以是总线形结构，也可以是星型结构，还可以包括比图
示更多或更少的部件，或者组合某些部件，或者不同的部件布置。其中：

[0192] 处理器501可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器  (Digital Signal Processor，DSP)、专用集成电路 
(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列 (Field‑
Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器
件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理
器等。

[0193] 存储器502可以包括各种类型的存储单元，例如系统内存、只读存储器（ROM），和永久存储装置。其中，ROM可以存储处理器501或者计算机的其他模块需要的静态数据或者指
令。永久存储装置可以是可读写的存储装置。永久存储装置可以是即使计算机断电后也不
会失去存储的指令和数据的非易失性存储设备。在一些实施方式中，永久性存储装置采用
大容量存储装置（例如磁或光盘、闪存）作为永久存储装置。另外一些实施方式中，永久性存
储装置可以是可移除的存储设备（例如软盘、光驱）。系统内存可以是可读写存储设备或者
易失性可读写存储设备，例如动态随机访问内存。系统内存可以存储一些或者所有处理器
在运行时需要的指令和数据。此外，存储器502可以包括任意计算机可读存储媒介的组合，
包括各种类型的半导体存储芯片（DRAM，SRAM，SDRAM，闪存，可编程只读存储器），磁盘和/或
光盘也可以采用。在一些实施方式中，存储器502可以包括可读和/或写的可移除的存储设
备，例如激光唱片（CD）、只读数字多功能光盘（例如DVD‑ROM，双层DVD‑ROM）、只读蓝光光盘、
超密度光盘、闪存卡（例如SD卡、min SD卡、Micro‑SD卡等等）、磁性软盘等等。计算机可读存
储媒介不包含载波和通过无线或有线传输的瞬间电子信号。

[0194] 通信接口503可以包括有线通信接口、无线通信接口等，可以用于与单目摄像装置、定位系统或其他设备进行通信交互。

[0195] 存储器502上存储有可执行代码，当可执行代码被处理器501处理时，可以使处理器501执行上文述及的车辆移动轨迹修正方法中的部分或全部步骤。

[0196] 上文中已经参考附图详细描述了本申请的方案。在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述的部分，可以参见其他实施例的相关描述。
本领域技术人员也应该知悉，说明书中所涉及的动作和模块并不一定是本申请所必须的。
另外，可以理解，本申请实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删
减，本申请实施例装置中的模块可以根据实际需要进行合并、划分和删减。

[0197] 此外，根据本申请的方法还可以实现为一种计算机程序或计算机程序产品，该计算机程序或计算机程序产品包括用于执行本申请的上述方法中部分或全部步骤的计算机
程序代码指令。

[0198] 或者，本申请还可以实施为一种非暂时性机器可读存储介质（或计算机可读存储介质、或机器可读存储介质），其上存储有可执行代码（或计算机程序、或计算机指令代码），
当所述可执行代码（或计算机程序、或计算机指令代码）被电子设备（或电子设备、服务器
等）的处理器执行时，使所述处理器执行根据本申请的上述方法的各个步骤的部分或全部。

[0199] 本领域技术人员还将明白的是，结合这里的申请所描述的各种示例性逻辑块、模块、电路和算法步骤可以被实现为电子硬件、计算机软件或两者的组合。

[0200] 附图中的流程图和框图显示了根据本申请的多个实施例的系统和方法的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程
序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的
逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标记的功能也可
以以不同于附图中所标记的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，
它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流
程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作
的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

[0201] 以上已经描述了本申请的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技
术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨
在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的
其它普通技术人员能理解本文披露的各实施例。