单目VSLAM的空间尺度恢复方法、装置、设备及介质

摘要

本申请公开了一种单目VSLAM的空间尺度恢复方法、装置、设备及介质，涉及辅助驾驶技术领域，包括：分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。综上可见，本申请基于车辆里程计信息恢复了单目VSLAM的空间尺度信息，从而实现了精准的定位和建图功能。

权利要求

1.一种单目VSLAM的空间尺度恢复方法，其特征在于，包括：

分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；

根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；

将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能；

其中，所述对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵，包括：根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵；

根据所述目标配准矩阵生成第一方向初始平移量、第二方向初始平移量、若干个旋转角度、若干个第一方向拉伸尺度和若干个第二方向拉伸尺度；

根据第一方向初始平移量、第二方向初始平移量、若干个旋转角度、若干个第一方向拉伸尺度和若干个第二方向拉伸尺度确定所述目标仿射矩阵；

其中，所述根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵，包括：从所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值中，确定出与所述第二点集中的每一坐标对应的每一时间戳差值的绝对值的最小值；

确定与每一时间戳差值的绝对值的最小值对应的所述第一点集中的每一目标坐标，并根据每一所述目标坐标构建所述目标配准矩阵。

2.根据权利要求1所述的单目VSLAM的空间尺度恢复方法，其特征在于，所述对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵，包括：根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵；

根据所述目标配准矩阵生成仿射矩阵初始值；其中，所述仿射矩阵初始值包括第一方向初始平移量、第二方向初始平移量、旋转初始角度、第一方向初始拉伸尺度和第二方向初始拉伸尺度；

通过所述旋转初始角度与第一预设增量值确定若干个旋转角度，并通过所述第一方向初始拉伸尺度和第二预设增量值确定若干个第一方向拉伸尺度，然后通过所述第二方向初始拉伸尺度和第三预设增量值确定若干个第二方向拉伸尺度；

根据所述第一方向初始平移量、所述第二方向初始平移量、若干个所述旋转角度、若干个所述第一方向拉伸尺度和若干个所述第二方向拉伸尺度确定若干个仿射矩阵，并从若干个所述仿射矩阵中确定所述目标仿射矩阵。

3.根据权利要求2所述的单目VSLAM的空间尺度恢复方法，其特征在于，所述根据所述目标配准矩阵生成仿射矩阵初始值，包括：根据所述目标配准矩阵中的第一个坐标与所述第二点集中的第一个坐标生成所述第一方向初始平移量和所述第二方向初始平移量；

根据所述目标配准矩阵、所述第一方向初始平移量、所述第二方向初始平移量和所述第二点集确定目标映射矩阵，并根据所述目标映射矩阵生成所述旋转初始角度、所述第一方向初始拉伸尺度和所述第二方向初始拉伸尺度。

4.根据权利要求3所述的单目VSLAM的空间尺度恢复方法，其特征在于，所述通过所述旋转初始角度与第一预设增量值确定若干个旋转角度，包括：通过所述旋转初始角度与所述第一预设增量值确定第一变化范围，并通过所述第一变化范围以及第一预设变化步长筛选得到若干个所述旋转角度；

相应的，所述通过所述第一方向初始拉伸尺度和第二预设增量值确定若干个第一方向拉伸尺度，包括：通过所述第一方向初始拉伸尺度和第二预设增量值确定第二变化范围，并通过所述第二变化范围以及第二预设变化步长筛选得到若干个所述第一方向拉伸尺度；

相应的，所述通过所述第二方向初始拉伸尺度和第三预设增量值确定若干个第二方向拉伸尺度，包括：通过所述第二方向初始拉伸尺度和第三预设增量值确定第三变化范围，并通过所述第三变化范围以及第三预设变化步长筛选得到若干个所述第二方向拉伸尺度。

5.根据权利要求4所述的单目VSLAM的空间尺度恢复方法，其特征在于，所述从若干个所述仿射矩阵中确定所述目标仿射矩阵，包括：确定与每一所述仿射矩阵对应的优化判据，并从所述优化判据中确定目标优化判据，然后将与所述目标优化判据对应的所述仿射矩阵确定为所述目标仿射矩阵。

6.根据权利要求5所述的单目VSLAM的空间尺度恢复方法，其特征在于，所述确定与每一所述仿射矩阵对应的优化判据，并从所述优化判据中确定目标优化判据，包括：基于每一所述仿射矩阵对所述第二点集进行尺度还原，得到与每一所述仿射矩阵对应的目标点集；

根据所述目标点集与所述目标配准矩阵中的相应坐标点之间的欧氏距离之和得到与每一所述仿射矩阵对应的所述优化判据；

将欧氏距离之和最小的所述优化判据确定为所述目标优化判据。

7.一种单目VSLAM的空间尺度恢复装置，其特征在于，包括：

目标仿射矩阵确定模块，用于分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；

空间尺度信息还原模块，用于根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；

车辆控制模块，用于将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能；

其中，所述目标仿射矩阵确定模块，具体用于：根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵；根据所述目标配准矩阵生成第一方向初始平移量、第二方向初始平移量、若干个旋转角度、若干个第一方向拉伸尺度和若干个第二方向拉伸尺度；根据第一方向初始平移量、第二方向初始平移量、若干个旋转角度、若干个第一方向拉伸尺度和若干个第二方向拉伸尺度确定所述目标仿射矩阵；

其中，所述目标仿射矩阵确定模块，具体还用于：从所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值中，确定出与所述第二点集中的每一坐标对应的每一时间戳差值的绝对值的最小值；确定与每一时间戳差值的绝对值的最小值对应的所述第一点集中的每一目标坐标，并根据每一所述目标坐标构建所述目标配准矩阵。

8.一种电子设备，其特征在于，包括：

存储器，用于保存计算机程序；

处理器，用于执行所述计算机程序，以实现如权利要求1至6任一项所述的单目VSLAM的空间尺度恢复方法。

9.一种计算机可读存储介质，其特征在于，用于保存计算机程序；其中，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述的单目VSLAM的空间尺度恢复方法。

说明书

单目VSLAM的空间尺度恢复方法、装置、设备及介质

技术领域

[0001] 本发明涉及辅助驾驶技术领域，特别涉及一种单目VSLAM的空间尺度恢复方法、装置、设备及介质。

背景技术

[0002] 单目VSLAM（Visual Simultaneous Localization and Mapping，基于视觉的同步建图定位技术）是结合相机参数、相机图像及其特征信息构成的实时建图、定位系统，该系统的配置成本较低，但无法确定图像像素的深度，并且推导、验算的过程中会不可避免的引入误差，因此在相同刻度下，单目VSLAM的输出结果和实际情况会相差很大的尺度，难以直接应用于如汽车、机器人的导航服务中。

[0003] 为此，如何恢复单目VSLAM的空间尺度以实现低成本的建图定位是本领域亟待解决的技术问题。

发明内容

[0004] 有鉴于此，本发明的目的在于提供一种单目VSLAM的空间尺度恢复方法、装置、设备及介质，能够恢复单目VSLAM的空间尺度，其具体方案如下：

[0005] 第一方面，本申请公开了一种单目VSLAM的空间尺度恢复方法，包括：

[0006] 分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；

[0007] 根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；

[0008] 将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。

[0009] 可选的，所述对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵，包括：

[0010] 根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵；

[0011] 根据所述目标配准矩阵生成仿射矩阵初始值；其中，所述仿射矩阵初始值包括第一方向初始平移量、第二方向初始平移量、旋转初始角度、第一方向初始拉伸尺度和第二方向初始拉伸尺度；

[0012] 通过所述旋转初始角度与第一预设增量值确定若干个旋转角度，并通过所述第一方向初始拉伸尺度和第二预设增量值确定若干个第一方向拉伸尺度，然后通过所述第二方向初始拉伸尺度和第三预设增量值确定若干个第二方向拉伸尺度；

[0013] 根据所述第一方向初始平移量、所述第二方向初始平移量、若干个所述旋转角度、若干个所述第一方向拉伸尺度和若干个所述第二方向拉伸尺度确定若干个仿射矩阵，并从若干个所述仿射矩阵中确定所述目标仿射矩阵。

[0014] 可选的，所述根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵，包括：

[0015] 从所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值中，确定出与所述第二点集中的每一坐标对应的每一时间戳差值的绝对值的最小值；

[0016] 确定与每一时间戳差值的绝对值的最小值对应的所述第一点集中的每一目标坐标，并根据每一所述目标坐标构建所述目标配准矩阵。

[0017] 可选的，所述根据所述目标配准矩阵生成仿射矩阵初始值，包括：

[0018] 根据所述目标配准矩阵中的第一个坐标与所述第二点集中的第一个坐标生成所述第一方向初始平移量和所述第二方向初始平移量；

[0019] 根据所述目标配准矩阵、所述第一方向初始平移量、所述第二方向初始平移量和所述第二点集确定目标映射矩阵，并根据所述目标映射矩阵生成所述旋转初始角度、所述第一方向初始拉伸尺度和所述第二方向初始拉伸尺度。

[0020] 可选的，所述通过所述旋转初始角度与第一预设增量值确定若干个旋转角度，包括：

[0021] 通过所述旋转初始角度与所述第一预设增量值确定第一变化范围，并通过所述第一变化范围以及第一预设变化步长筛选得到若干个所述旋转角度；

[0022] 相应的，所述通过所述第一方向初始拉伸尺度和第二预设增量值确定若干个第一方向拉伸尺度，包括：

[0023] 通过所述第一方向初始拉伸尺度和第二预设增量值确定第二变化范围，并通过所述第二变化范围以及第二预设变化步长筛选得到若干个所述第一方向拉伸尺度；

[0024] 相应的，所述通过所述第二方向初始拉伸尺度和第三预设增量值确定若干个第二方向拉伸尺度，包括：

[0025] 通过所述第二方向初始拉伸尺度和第三预设增量值确定第三变化范围，并通过所述第三变化范围以及第三预设变化步长筛选得到若干个所述第二方向拉伸尺度。

[0026] 可选的，所述从若干个所述仿射矩阵中确定所述目标仿射矩阵，包括：

[0027] 确定与每一所述仿射矩阵对应的优化判据，并从所述优化判据中确定目标优化判据，然后将与所述目标优化判据对应的所述仿射矩阵确定为所述目标仿射矩阵。

[0028] 可选的，所述确定与每一所述仿射矩阵对应的优化判据，并从所述优化判据中确定目标优化判据，包括：

[0029] 基于每一所述仿射矩阵对所述第二点集进行尺度还原，得到与每一所述仿射矩阵对应的目标点集；

[0030] 根据所述目标点集与所述目标配准矩阵中的相应坐标点之间的欧氏距离之和得到与每一所述仿射矩阵对应的所述优化判据；

[0031] 将欧氏距离之和最小的所述优化判据确定为所述目标优化判据。

[0032] 第二方面，本申请公开了一种单目VSLAM的空间尺度恢复装置，包括：

[0033] 目标仿射矩阵确定模块，用于分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；

[0034] 空间尺度信息还原模块，用于根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；

[0035] 车辆控制模块，用于将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。

[0036] 第三方面，本申请公开了一种电子设备，包括：

[0037] 存储器，用于保存计算机程序；

[0038] 处理器，用于执行所述计算机程序，以实现前述公开的单目VSLAM的空间尺度恢复方法。

[0039] 第四方面，本申请公开了一种计算机可读存储介质，用于保存计算机程序；其中，所述计算机程序被处理器执行时实现前述公开的单目VSLAM的空间尺度恢复方法。

[0040] 可见，本申请提出一种单目VSLAM的空间尺度恢复方法，包括：分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。综上可见，由于车辆里程计系统输出的第一点集是具有空间尺度信息的点集，因此本申请基于车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集的转换关系实现了对单目VSLAM的空间尺度信息的还原，具体的，本申请通过对车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集进行非刚性配准，得到目标仿射矩阵，并根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集，进一步的，本申请将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的低成本定位和建图功能。

附图说明

[0041] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

[0042] 图1为本申请公开的一种单目VSLAM的空间尺度恢复方法流程图；

[0043] 图2为本申请公开的一种具体的单目VSLAM的空间尺度恢复方法流程图；

[0044] 图3为本申请公开的另一种具体的单目VSLAM的空间尺度恢复方法流程图；

[0045] 图4为本申请公开的一种空间尺度恢复前VSLAM和实际航迹对比图；

[0046] 图5为本申请公开的一种空间尺度恢复后VSLAM和实际航迹对比图；

[0047] 图6为本申请公开的一种单目VSLAM的空间尺度恢复装置结构示意图；

[0048] 图7为本申请公开的一种电子设备结构图。

具体实施方式

[0049] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0050] 由于单目VSLAM无法确定图像像素的深度，并且推导、验算的过程中会不可避免的引入误差，因此在相同刻度下，单目VSLAM的输出结果和实际情况会相差很大的尺度，难以直接应用于如汽车、机器人的导航服务中。

[0051] 为此，本申请实施例提出一种单目VSLAM的空间尺度恢复方案，能够恢复单目VSLAM的空间尺度。

[0052] 本申请实施例公开了一种单目VSLAM的空间尺度恢复方法，参见图1和图2所示，该方法包括：

[0053] 步骤S11：分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵。

[0054] 本实施例中，首先启动VSLAM系统开始建图，并记录车辆里程计系统输出的第一点集 ，同时记录VSLAM系统输出的第二点集 。

[0055] 进一步的，结束VSLAM系统建图，并对第一点集和第二点集进行非刚性配准，得到目标仿射矩阵 ，所述目标仿射矩阵为2行6列矩阵，是由分别为2行2列的旋转矩阵 ，平移向量 ，尺度矩阵 拼接得到的。

[0056] 步骤S12：根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集。

[0057] 本实施例中，根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集，具体的， ，其中， 表示转置。

[0058] 步骤S13：将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。

[0059] 本实施例中，将 分发至目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能，其中，所述预设要求可以是记忆泊车（Home‑Zone Parking Assistant，HPA）过程中对于精度和稳定性的要求。

[0060] 可见，本申请提出一种单目VSLAM的空间尺度恢复方法，包括：分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。综上可见，由于车辆里程计系统输出的第一点集是具有空间尺度信息的点集，因此本申请基于车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集的转换关系实现了对单目VSLAM的空间尺度信息的还原，具体的，本申请通过对车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集进行非刚性配准，得到目标仿射矩阵，并根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集，进一步的，本申请将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的低成本定位和建图功能。

[0061] 本申请实施例公开了一种具体的单目VSLAM的空间尺度恢复方法，相对于上一实施例，本实施例对技术方案步骤S12作了进一步的说明和优化。参见图3所示，具体包括：

[0062] 步骤S121：根据所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值构建目标配准矩阵。

[0063] 本实施例中，从所述第二点集中的每一坐标的时间戳与所述第一点集中的所有坐标的时间戳的时间戳差值的绝对值中，确定出与所述第二点集中的每一坐标对应的每一时间戳差值的绝对值的最小值，并确定与每一时间戳差值的绝对值的最小值对应的所述第一点集中的每一目标坐标，然后根据每一所述目标坐标构建所述目标配准矩阵。

[0064] 具体的，首先载入具有时间标注的第一点集 和第二点集，其中， 表示o1时刻里程计系统输出的坐标， 表示s1时刻
VSLAM系统输出的坐标。

[0065] 进一步的，计算第二点集中的每一坐标的时间戳与第一点集中的所有坐标的时间戳的时间戳差值的绝对值，得到 ，其中， 表示第二点集 中第 个坐标的时间戳与第一点集中的所有坐标的时间戳的时间戳差值的绝对值的集合，例如， 中的 表示第二点集中第 个坐标的时间戳与第一点集中的第1个坐标的时间戳差值的绝对值， 中的 表示第二点集中第 个坐标的时间戳与第一点集中的第 个坐标的时间戳差值的绝对值。

[0066] 进一步的，取每个 中的最小值对应的 中的坐标，构建得到目标配准矩阵，其中， 表示 中的最小值对应的 中的坐标， 表示中的最小值对应的 中的坐标。

[0067] 综上可见，本申请通过构建所述目标配准矩阵，实现将所述第一点集和所述第二点集在时间上的对齐，如此一来，能够提高空间尺度还原的准确性。

[0068] 步骤S122：根据所述目标配准矩阵生成仿射矩阵初始值；其中，所述仿射矩阵初始值包括第一方向初始平移量、第二方向初始平移量、旋转初始角度、第一方向初始拉伸尺度和第二方向初始拉伸尺度。

[0069] 本实施例中，根据所述目标配准矩阵中的第一个坐标与所述第二点集中的第一个坐标生成所述第一方向初始平移量和所述第二方向初始平移量，并根据所述目标配准矩阵、所述第一方向初始平移量、所述第二方向初始平移量和所述第二点集确定目标映射矩阵，然后根据所述目标映射矩阵生成所述旋转初始角度、所述第一方向初始拉伸尺度和所述第二方向初始拉伸尺度。

[0070] 具体的，首先生成 ，其中， 为所述目标配准矩阵中的第一个坐标， 为所述第二点集中的第一个坐标， 为所述第一方向初始平移向量， 为所述第二方向初始平移向量， 为初始平移向量。

[0071] 进 一 步 ，获 取 第 二 点 集 对 应 的 映 射 集 ， ，，其中， 中包含n个 ， 与 行列维数相同，并基于 生成
目标映射矩阵 ， ，然后对目标映射矩阵进行变换，得到初始旋
转矩阵 和初始尺度矩阵 ，其中，对目标映射矩阵进行变换具体包括： 。

[0072] 最后，根据初始旋转矩阵生成初始旋转角度 ，也即 ；对初始尺度矩阵进行对角化变换得到所述第一方向初始拉伸尺度 和第二方向初始拉伸尺度 ，也即 ，其中， 表示正交描述行矩阵。

[0073] 步骤S123：通过所述旋转初始角度与第一预设增量值确定若干个旋转角度，并通过所述第一方向初始拉伸尺度和第二预设增量值确定若干个第一方向拉伸尺度，然后通过所述第二方向初始拉伸尺度和第三预设增量值确定若干个第二方向拉伸尺度。

[0074] 本实施例中，第一方面，通过所述旋转初始角度与所述第一预设增量值确定第一变化范围，并通过所述第一变化范围以及第一预设变化步长筛选得到若干个所述旋转角度；第二方面，通过所述第一方向初始拉伸尺度和第二预设增量值确定第二变化范围，并通过所述第二变化范围以及第二预设变化步长筛选得到若干个所述第一方向拉伸尺度；第三方面，通过所述第二方向初始拉伸尺度和第三预设增量值确定第三变化范围，并通过所述第三变化范围以及第三预设变化步长筛选得到若干个所述第二方向拉伸尺度。

[0075] 具体的，将所述第一预设增量值设置为 ，将所述第二预设增量值设置为 ，将所述第三预设增量值设置为 ，其中，上述三个预设增量值为正数，并且可以根据实际情况进行自行设定，在此不做具体赘述。

[0076] 进一步的，所述第一变化范围、所述第二变化范围和所述第三变化范围分别为：；其中， 、 和 分别表示旋转角度、第一方向拉伸尺度
和第二方向拉伸尺度。

[0077] 最后，根据所述第一预设变化步长、所述第二预设变化步长和所述第三预设变化步长分别从所述第一变化范围、所述第二变化范围和所述第三变化范围中等间距的生成若干个 、 和 。并且将生成的若干个 、 、 放置在对应的数组集合 ， ， 中，如下所示： ； 表示所述第一预设变化步长、 表示所述第二预设变化步长、 表示所述第三预设变化步长，集合 ，， 的元素个数分别为 、 和 ，  、 和 分别是由 ，
， 向下取整数获得， 的取值范围是[0， ]， 的取值范围是
[0， ]， 的取值范围是[0， ]。

[0078] 步骤S124：根据所述第一方向初始平移量、所述第二方向初始平移量、若干个所述旋转角度、若干个所述第一方向拉伸尺度和若干个所述第二方向拉伸尺度确定若干个仿射矩阵，并从若干个所述仿射矩阵中确定所述目标仿射矩阵。

[0079] 本实施例中，根据所述第一方向初始平移量和所述第二方向初始平移量构建；根据若干个所述旋转角度构建 ，其中， 表示旋转矩阵；根据若干个所述第一方向拉伸尺度和若干个所述第二方向拉伸尺度构建，其中， 表示尺度矩阵，进一步的，构建若干个仿射矩阵
，仿射矩阵 的总个数为 。

[0080] 进一步的，确定与每一所述仿射矩阵对应的优化判据，并从所述优化判据中确定目标优化判据，然后将与所述目标优化判据对应的所述仿射矩阵确定为所述目标仿射矩阵。其中，确定所述目标优化判据的过程具体可以包括：基于每一所述仿射矩阵对所述第二点集进行尺度还原，得到与每一所述仿射矩阵对应的目标点集；根据所述目标点集与所述目标配准矩阵中的相应坐标点之间的欧氏距离之和得到与每一所述仿射矩阵对应的所述优化判据；将欧氏距离之和最小的所述优化判据确定为所述目标优化判据。

[0081] 具体的，基于仿射矩阵 对所述第二点集 进行尺度还原得到所述目标点集，其中， ， 的每个元素表示为 ，进一步生成优化判据 ，其中， ，可以看出，优化判据是所述
目标点集与所述目标配准矩阵中对应点的欧氏距离之和， 总元素个数为 。

[0082] 进一步的，在 个判据 中，将欧氏距离之和最小的 作为所述目标优化判据，并将所述目标优化判据对应的 、 、 确定为最优仿射参数，其中， 表示旋转角度， 表示第一方向拉伸尺度， 表示第二方向拉伸尺度，并根据所述最优仿射参数构建目标仿射矩阵，如下所示， ， 。其中， 表
示旋转矩阵， 表示尺度矩阵，目标仿射矩阵为 ，其中， 。本实
施例中，欧式之和距离最小，表示目标点集与目标配准矩阵中相应坐标之间的距离最近，也表示还原效果最好。

[0083] 图4为本申请公开的一种空间尺度恢复前VSLAM（也即待还原轨迹）和实际航迹（也即与车辆里程计系统对应的航迹）对比图，图5为本申请公开的一种空间尺度恢复后VSLAM（也即还原后轨迹）和实际航迹对比图。参见图4所示，在对VSLAM系统输出的点集进行空间尺度恢复前，VSLAM轨迹与真实轨迹相差较大，说明VSLAM轨迹不具备真实的空间尺度信息。参见图5所示，在对VSLAM系统输出的点集进行空间尺度恢复后，VSLAM轨迹与真实轨迹接近于重合，说明经过空间尺度恢复的VSLAM轨迹具备真实的空间尺度信息。

[0084] 本申请基于车辆里程计系统输出的第一点集是具有空间尺度信息的点集这一特性，首先确定出车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集的转换关系，也即目标仿射矩阵，并基于目标仿射矩阵实现了对单目VSLAM的空间尺度信息的还原，解决了单目VSLAM存在的尺度不确定性。如此一来，本申请仅仅使用单目相机传感器，就可以实现准确的建图和定位，将其运用在实际的HPA系统和汽车高级辅助驾驶系统（ADAS：Advanced Driver Assistance System）中，可以稳定实现自动巡航功能，并且由于单目相机的成本较低，因此本申请大大降低了HPA功能量产落地的成本。

[0085] 相应的，本申请实施例还公开了一种单目VSLAM的空间尺度恢复装置，参见图6所示，该装置包括：

[0086] 目标仿射矩阵确定模块11，用于分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；

[0087] 空间尺度信息还原模块12，用于根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；

[0088] 车辆控制模块13，用于将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。

[0089] 其中，关于上述各个模块更加具体的工作过程可以参考前述实施例中公开的相应内容，在此不再进行赘述。

[0090] 可见，本申请提出一种单目VSLAM的空间尺度恢复方法，包括：分别获取车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集，并对所述第一点集和所述第二点集进行非刚性配准，得到目标仿射矩阵；根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集；将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的定位和建图功能。综上可见，由于车辆里程计系统输出的第一点集是具有空间尺度信息的点集，因此本申请基于车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集的转换关系实现了对单目VSLAM的空间尺度信息的还原，具体的，本申请通过对车辆里程计系统输出的第一点集与VSLAM系统输出的第二点集进行非刚性配准，得到目标仿射矩阵，并根据所述目标仿射矩阵对所述VSLAM系统输出的实时定位点集进行尺度还原，得到具有空间尺度信息的目标实时定位点集，进一步的，本申请将所述目标实时定位点集发送至所述车辆的目标控制系统，以便通过所述目标控制系统实现满足预设要求的低成本定位和建图功能。

[0091] 进一步的，本申请实施例还提供了一种电子设备。图7是根据一示例性实施例示出的电子设备20结构图，图中的内容不能认为是对本申请的使用范围的任何限制。

[0092] 图7为本申请实施例提供的一种电子设备20的结构示意图。该电子设备20，具体可以包括：至少一个处理器21、至少一个存储器22、显示屏23、输入输出接口24、通信接口25、电源26和通信总线27。其中，所述存储器22用于存储计算机程序，所述计算机程序由所述处理器21加载并执行，以实现前述任一实施例公开的单目VSLAM的空间尺度恢复方法中的相关步骤。另外，本实施例中的电子设备20具体可以为电子计算机。

[0093] 本实施例中，电源26用于为电子设备20上的各硬件设备提供工作电压；通信接口25能够为电子设备20创建与外界设备之间的数据传输通道，其所遵循的通信协议是能够适用于本申请技术方案的任意通信协议，在此不对其进行具体限定；输入输出接口24，用于获取外界输入数据或向外界输出数据，其具体的接口类型可以根据具体应用需要进行选取，在此不进行具体限定。

[0094] 另外，存储器22作为资源存储的载体，可以是只读存储器、随机存储器、磁盘或者光盘等，其上所存储的资源可以包括计算机程序221，存储方式可以是短暂存储或者永久存储。其中，计算机程序221除了包括能够用于完成前述任一实施例公开的由电子设备20执行的单目VSLAM的空间尺度恢复方法的计算机程序之外，还可以进一步包括能够用于完成其他特定工作的计算机程序。

[0095] 进一步的，本申请实施例还公开了一种计算机可读存储介质，用于存储计算机程序；其中，所述计算机程序被处理器执行时实现前述公开的单目VSLAM的空间尺度恢复方法。

[0096] 关于该方法的具体步骤可以参考前述实施例中公开的相应内容，在此不再进行赘述。

[0097] 本申请书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

[0098] 专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

[0099] 结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器（RAM）、内存、只读存储器（ROM）、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD‑ROM、或技术领域内所公知的任意其它形式的存储介质中。

[0100] 最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

[0101] 以上对本申请所提供的一种单目VSLAM的空间尺度恢复方法、装置、设备、存储介质进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。