CN112801077B

CN112801077B - 用于自动驾驶车辆的slam初始化的方法及相关装置

Info

Publication number: CN112801077B
Application number: CN202110407070.9A
Authority: CN
Inventors: 单国航; 杨敏; 朱磊; 贾双成; 李倩; 李成军
Original assignee: Zhidao Network Technology Beijing Co Ltd
Current assignee: Zhidao Network Technology Beijing Co Ltd
Priority date: 2021-04-15
Filing date: 2021-04-15
Publication date: 2021-11-05
Anticipated expiration: 2041-04-15
Also published as: CN112801077A

Abstract

本申请是关于一种用于自动驾驶车辆的SLAM初始化的方法及相关装置。该方法包括：获取车辆行驶过程中采集的至少三帧图片；获取至少三帧图片中每一帧图片的特征点；将至少三帧图片的特征点按照预设匹配规则进行匹配，得到至少三帧图片中匹配成功的共同特征点集；按照预设筛选规则对共同特征点集进行筛选，得到优选特征点集；利用优选特征点集，计算至少三帧图片中的两帧目标图片之间的位姿关系；根据两帧目标图片之间的位姿关系，生成优选特征点集的初始三维空间坐标。本申请提供的方案，能够提升SLAM系统的稳定性和精度。

Description

用于自动驾驶车辆的SLAM初始化的方法及相关装置

技术领域

本申请涉及导航技术领域，尤其涉及一种用于自动驾驶车辆的SLAM初始化的方法及相关装置。

背景技术

SLAM（Simultaneous Localization And Mapping，即时定位与地图构建）主要用于解决移动设备在未知环境中运行时进行定位导航与地图构建的问题。在汽车自动驾驶领域，SLAM已成为关键技术，能够帮助车辆感知周围环境，更好的完成导航、避障、路径规划等高级任务。

目前，SLAM系统采用两帧图片进行特征匹配来构建初始三维空间坐标，并作为后续SLAM系统的初始化条件。两帧图片的匹配准确度决定了SLAM系统后续的即时定位及地图构建工作的稳定性和精度。

然而，仅依赖两帧图片所构建的初始三维空间坐标，其精度不足，导致整个SLAM系统的鲁棒性较低，难以保障SLAM系统后续的即时定位及地图构建工作的可靠性。

发明内容

为解决或部分解决相关技术中存在的问题，本申请提供一种用于自动驾驶车辆的SLAM初始化的方法及相关装置，能够提升SLAM系统的稳定性和精度。

本申请第一方面提供一种用于自动驾驶车辆的SLAM初始化的方法，包括：

获取车辆行驶过程中采集的至少三帧图片；

获取所述至少三帧图片中每一帧图片的特征点；

将所述至少三帧图片的特征点按照预设匹配规则进行匹配，得到所述至少三帧图片中匹配成功的共同特征点集；

按照预设筛选规则对所述共同特征点集进行筛选，得到优选特征点集；

利用所述优选特征点集，计算所述至少三帧图片中的两帧目标图片之间的位姿关系；

根据所述两帧目标图片之间的位姿关系，生成所述优选特征点集的初始三维空间坐标。

在一种实施方式中，所述利用所述优选特征点集，计算所述至少三帧图片中的两帧目标图片之间的位姿关系，包括：

利用所述优选特征点集，计算所述至少三帧图片中的末帧图片相对于首帧图片的位姿；

所述根据所述两帧目标图片之间的位姿关系，生成所述优选特征点集的初始三维空间坐标，包括：

根据所述末帧图片相对于首帧图片的位姿，生成所述优选特征点集的初始三维空间坐标。

在一种实施方式中，所述根据所述末帧图片相对于首帧图片的位姿，生成所述优选特征点集的初始三维空间坐标之后，还包括：

利用所述优选特征点集以及所述优选特征点集的初始三维空间坐标，计算所述至少三帧图片中任一中间帧图片相对于所述首帧图片的位姿；

根据至少一张所述中间帧图片相对于所述首帧图片的位姿、所述末帧图片相对于所述首帧图片的位姿以及所述初始三维空间坐标，利用预设优化算法生成所述优选特征点集的最终三维空间坐标。

在一种实施方式中，所述根据至少一张所述中间帧图片相对于所述首帧图片的位姿、所述末帧图片相对于所述首帧图片的位姿以及所述初始三维空间坐标，利用预设优化算法生成所述优选特征点集的最终三维空间坐标，包括：

根据至少一张所述中间帧图片相对于所述首帧图片的位姿、所述末帧图片相对于所述首帧图片的位姿以及所述初始三维空间坐标，通过建立投影关系，计算得到所述初始三维空间坐标中的各个特征点分别于每一帧图片中的投影像素坐标；

利用最小二乘优化算法构建各个特征点的投影像素坐标与真实像素坐标的误差方程，根据所述误差方程得到所述优选特征点集的最终三维空间坐标。

在一种实施方式中，所述按照预设筛选规则对所述共同特征点集进行筛选，得到优选特征点集，包括：

获取所述共同特征点集中的各特征点组分别在所述至少三帧图片中的像素坐标；

计算每一个特征点组在任意相邻两帧图片中的像素坐标向量；

计算所述每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小；

选取夹角大小均小于预设夹角的特征点组，生成优选特征点集。

在一种实施方式中，所述计算所述每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小，包括：

计算所述每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角余弦值；

所述选取夹角大小均小于预设夹角的特征点组，生成优选特征点集，包括：

选取夹角余弦值均大于预设阈值的特征点组，生成优选特征点集。

本申请第二方面提供一种用于自动驾驶车辆的SLAM初始化的装置，包括：

获取单元，用于获取车辆行驶过程中采集的至少三帧图片；

所述获取单元，还用于获取所述至少三帧图片中每一帧图片的特征点；

匹配单元，用于将所述至少三帧图片的特征点按照预设匹配规则进行匹配，得到所述至少三帧图片中匹配成功的共同特征点集；

筛选单元，用于按照预设筛选规则对所述共同特征点集进行筛选，得到优选特征点集；

构建单元，用于利用所述优选特征点集，计算所述至少三帧图片中的两帧目标图片之间的位姿关系；

所述构建单元，还用于根据所述两帧目标图片之间的位姿关系，生成所述优选特征点集的初始三维空间坐标。

在一种实施方式中，所述构建单元利用所述优选特征点集，计算所述至少三帧图片中的两帧目标图片之间的位姿关系的方式包括：

所述构建单元利用所述优选特征点集，计算所述至少三帧图片中的末帧图片相对于首帧图片的位姿；

所述构建单元根据所述两帧目标图片之间的位姿关系，生成所述优选特征点集的初始三维空间坐标的方式包括：

所述构建单元根据所述末帧图片相对于首帧图片的位姿，生成所述优选特征点集的初始三维空间坐标。

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

处理器；以及

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

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

本申请提供的技术方案可以包括以下有益效果：

本申请实施例提供的方法，通过获取车辆行驶过程中采集的至少三帧图片，分别提取各帧图片的特征点，并对各帧图片的特征点进行匹配，得到匹配成功的共同特征点集。按照预设筛选规则对共同特征点集进行筛选，以得到优选特征点集。利用优选特征点集，计算两帧目标图片之间的位姿关系，进而生成优选特征点集的初始三维空间坐标。通过采用至少三帧图片进行特征匹配，并按照预设筛选规则对各帧图片的共同特征点集进行筛选，以得到可靠程度更高的优选特征点集，进而能够提升由优选特征点集生成的初始三维空间坐标的稳定性及精度，提升SLAM系统的鲁棒性。

进一步的，本申请实施例提供的方法，可以计算至少三帧图片中的末帧图片相对于首帧图片的位姿，以生成优选特征点集的初始三维空间坐标，从而进一步的提升SLAM系统初始化的稳定性及精度。

进一步的，本申请实施例提供的方法，可以根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，利用预设优化算法生成优选特征点集的最终三维空间坐标，从而进一步的提升SLAM系统初始化的稳定性及精度。

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

附图说明

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

图1是本申请实施例示出的用于自动驾驶车辆的SLAM初始化的方法的流程示意图；

图2是本申请实施例示出的用于自动驾驶车辆的SLAM初始化的方法的另一流程示意图；

图3a是本申请实施例示出的单目摄像头采集的图片示意图；

图3b是本申请实施例示出的平移矩阵和旋转矩阵算法原理图；

图4是本申请实施例示出的用于自动驾驶车辆的SLAM初始化的装置的结构示意图；

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

具体实施方式

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

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

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

目前，SLAM已成为汽车自动驾驶领域的关键技术，SLAM地图构建的好坏直接影响到车辆的导航、避障和路线规划等。相关技术中，仅依赖两帧图片所构建的初始三维空间坐标，其精度不足，导致整个SLAM系统的鲁棒性较低，难以保障SLAM系统后续的即时定位及地图构建工作的可靠性。

针对上述问题，本申请实施例提供一种用于自动驾驶车辆的SLAM初始化的方法，能够提升SLAM系统的稳定性和精度。

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

图1是本申请实施例示出的用于自动驾驶车辆的SLAM初始化的方法的流程示意图。

参见图1，该方法包括：

步骤S101、获取车辆行驶过程中采集的至少三帧图片。

本申请实施例中，车辆在行驶过程中可以通过单目行车记录仪采集图片，单目行车记录仪可以设置于车辆的前挡风玻璃处。

其中，所述至少三帧图片中相邻两帧图片的采集时间间隔可以相同。

步骤S102、获取至少三帧图片中每一帧图片的特征点。

本申请实施例中，图片上的特征点可以用来标识图片上一些目标物体，一般将图片上灰度值发生剧烈变化的点或在图片边缘上曲率较大的点（如两个边缘的交点）看作为图片的特征点。

步骤S103、将至少三帧图片的特征点按照预设匹配规则进行匹配，得到至少三帧图片中匹配成功的共同特征点集。

本申请实施例中，上述至少三帧图片中可以包含有不同视角下的同一物体（如建筑物、广告牌、路牌等等）。通过对图片上的特征点进行匹配，可以将不同图片上的同一物体的某些特征点匹配成功。其中，共同特征点集为上述至少三帧图片中每一帧图片上均匹配成功的特征点的集合。

例如，当上述至少三帧图片包含A、B、C三帧图片时，共同特征点集为A、B、C三帧图片同时进行匹配并匹配成功的特征点；当上述至少三帧图片包含A、B、C、D四帧图片时，共同特征点集为A、B、C、D四帧图片同时进行匹配并匹配成功的特征点，即匹配成功的特征点要同时出现在A、B、C、D四帧图片上，不能仅出现在其中一帧或其中两帧图片上。

步骤S104、按照预设筛选规则对共同特征点集进行筛选，得到优选特征点集。

在该步骤中，首先可以获取共同特征点集中的各特征点组分别在至少三帧图片中的像素坐标。然后，计算每一个特征点组在任意相邻两帧图片中的像素坐标向量。最后，计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小。通过选取夹角大小均小于预设夹角的特征点组，以生成优选特征点集。

具体的，为了方便计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小。在其中一个实施例中，可以计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角余弦值，通过选取夹角余弦值均大于预设阈值的特征点组，以生成优选特征点集。

步骤S105、利用优选特征点集，计算至少三帧图片中的两帧目标图片之间的位姿关系。

在该步骤中，可以选取至少三帧图片中的末帧图片与首帧图片作为两帧目标图片，也可以选取至少三帧图片中的任一张中间帧图片与首帧图片作为两帧目标图片，还可以选取至少三帧图片中的任意两张中间帧图片作为两帧目标图片。也就是说，计算至少三帧图片中的任意两张图片之间的位姿关系。

步骤S106、根据两帧目标图片之间的位姿关系，生成优选特征点集的初始三维空间坐标。

从该实施例可以看出，本申请实施例提供的方法，通过获取车辆行驶过程中采集的至少三帧图片，分别提取各帧图片的特征点，并对各帧图片的特征点进行匹配，得到匹配成功的共同特征点集。按照预设筛选规则对共同特征点集进行筛选，以得到优选特征点集。利用优选特征点集，计算两帧目标图片之间的位姿关系，进而生成优选特征点集的初始三维空间坐标。通过采用至少三帧图片进行特征匹配，并按照预设筛选规则对各帧图片的共同特征点集进行筛选，以得到可靠程度更高的优选特征点集，进而能够提升由优选特征点集生成的初始三维空间坐标的稳定性及精度，提升SLAM系统的鲁棒性。

图2是本申请实施例的用于自动驾驶车辆的SLAM初始化的方法的另一流程示意图。图2相对图1更详细描述了本申请的方案。

步骤S201、获取车辆行驶过程中采集的至少三帧图片。

本申请实施例中，车辆在行驶过程中可以通过单目行车记录仪采集图片，单目行车记录仪可以设置于车辆的前挡风玻璃处。在车辆行驶过程中，可以利用单目行车记录仪采集车辆前方视频数据。为了获得图片，需要对获取的视频数据进行抽帧。通常，视频的帧率为每秒30帧，可以按照预置的规则对视频进行抽帧，从而获得图片。上述至少三帧图片可以为单目行车记录仪先后时间采集到的连续的三帧或三帧以上的图片。具体的，上述至少三帧图片可以为对单目行车记录仪在车辆行驶过程中采集的实时视频进行抽帧后得到的实时图片，也可以为对单目行车记录仪在车辆整个行驶过程中采集的整段视频进行抽帧后得到的一帧帧的图片序列中的其中几帧图片，这里不作限定。

可以理解的是，本申请实施例是以车辆上的单目行车记录仪为例进行说明的，也可以是车辆上的其他单目设备，如单目摄像头、手机等可以采集单目视频的设备。另外，该单目设备可以设置于车辆的首部以采集其前方的视频，也可以设置于车辆的尾部以采集其后方的视频，这里不作唯一限定。

步骤S202、获取至少三帧图片中每一帧图片的特征点。

本申请实施例中，图片上的特征点可以用来标识图片上一些目标物体，一般将图片上灰度值发生剧烈变化的点或在图片边缘上曲率较大的点（如两个边缘的交点）看作为图片的特征点。为了后续更好的进行图片匹配，一般可以将图片中不会随着相机的移动、旋转或者光照的变化而变化的稳定的点选取为特征点。图3a中示出了车辆在行驶过程中单目行车记录仪采集到的其中一帧图片，在3a图中可以选择固定的建筑物（如路边的房屋）、固定的树木或广告牌等中的特征点，而不选择天空或地面上的特征点。

步骤S203、将至少三帧图片的特征点按照预设匹配规则进行匹配，得到至少三帧图片中匹配成功的共同特征点集。

在一可选的实施方式中，步骤S202获取上述至少三帧图片中每一帧图片的特征点的具体实施方式可以包括以下步骤：

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

其中，步骤203将至少三帧图片的特征点按照预设匹配规则进行匹配，得到至少三帧图片中匹配成功的共同特征点集的具体实施方式可以包括以下步骤：

12）将上述至少三帧图片描述后的特征点进行匹配，至少三帧图片的特征点按照匹配距离小于预设值的预设匹配规则进行匹配，从而将匹配距离小于预设值的特征点确定为匹配成功的共同特征点集。

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

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

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

举例来说，当上述至少三帧图片包含先后采集的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组特征点。

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

步骤S204、按照预设筛选规则对共同特征点集进行筛选，得到优选特征点集。

为了方便计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小，可以通过计算两个像素坐标向量的夹角余弦值，以表征两个像素坐标向量的夹角大小。在一个具体的实施方式中，可以计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角余弦值，通过选取夹角余弦值均大于预设阈值的特征点组，以生成优选特征点集。在本实施例中，选取夹角余弦值均大于0.9的特征点组，生成优选特征点集。

需要说明的是，为了方便计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小，还可以通过计算两个像素坐标向量的夹角正弦值或正切值，以表征两个像素坐标向量的夹角大小。例如，可以计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角正弦值，通过选取夹角正弦值均小于预设阈值的特征点组，以生成优选特征点集；还可以计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角正切值，通过选取夹角正切值均小于预设阈值的特征点组，以生成优选特征点集。或者，也可以利用atan2函数来计算两个像素坐标向量的夹角大小，将夹角大小均小于预设夹角的特征点组，以生成优选特征点集，这里不作唯一限定。

步骤S205、利用优选特征点集，计算至少三帧图片中的两帧目标图片之间的位姿关系。

本申请实施例中，基于已匹配成功的优选特征点集，可以利用对极几何计算出单目摄像头在采集各帧图片时的位姿变化，即平移量和旋转量。

具体的，在一可选的实施方式中，步骤S205、利用优选特征点集，计算至少三帧图片中的两帧目标图片之间的位姿关系的具体实施方式可以包括以下步骤：

13）利用优选特征点集，采用对极约束计算上述两帧目标图片之间的旋转矩阵和平移矩阵；

举例来说，当上述至少三帧图片仅包含先后采集的A、B、C三帧图片时，对A、B、C三帧图片上的特征点进行匹配，得出8个匹配点，即优选特征点集中包含有8点组。根据这8点组可以计算出两帧目标图片之间的旋转矩阵和平移矩阵。

此处以两帧目标图片是C帧图片相对于A帧图片为例，展开具体阐述。

具体的，如图3b所示，在不同的位置拍摄同一个目标物体的两帧图片，图片中相同物体对应的像素点满足对极约束关系。其中P为世界坐标系中真实物体，例如建筑物上的某一点。O₁、O₂分别为拍摄A帧图片和C帧图片时单目行车记录仪的光心位置。I₁、I₂分别代表A帧图片和C帧图片。p₁、p₂分别为P点在A帧图片和C帧图片的投影，即A、C两帧图片中匹配成功的一对点。O₁P在C帧图片上投影为e₂p₂，记为l₂，O₂P在A帧图片上投影为e₁p₁，记为l₁，其中，l₁、l₂称为极线，e₁、e₂称为极点。根据对极约束:

得到：

其中：

E为本质矩阵，t为平移矩阵，R为旋转矩阵。

利用8点法求得E：

其中， (u₁，v₁)为p₁的图像像素坐标、 (u₂，v₂)为p₂的图像像素坐标。

得到：

其中：

对于其它的点对也使用同样的表示方法，这样将得到的所有方程放到一起，得到一个线性方程组，（uⁱ，vⁱ）表示第i个匹配的点对。

通过上述的线性方程组求得本质矩阵E。

对E奇异值分解，得到4组t、R值，分别为：

所得4组结果中只有一个深度值为正，深度值为正的t、R值的组合即为一目标帧图片相对另一目标帧图片的平移矩阵和旋转矩阵。

可以理解的是，上述过程是以八点法为例进行说明的，但并不局限于此。当A、C两帧图片上匹配的特征点不止八对时，可以利用对极约束构建最小二乘法来求得两帧之间的平移矩阵和旋转矩阵，其中最小二乘法为较为成熟的现有技术，这里将不再叙述其具体实现过程。

步骤S206、根据两帧目标图片之间的位姿关系，生成优选特征点集的初始三维空间坐标。

在该步骤中，可以根据上述至少三帧图片中的两帧目标图片之间的旋转矩阵和平移矩阵，生成优选特征点集的初始三维空间坐标。

具体的，在利用优选特征点集求出至少三帧图片中的两帧目标图片之间的旋转矩阵R和平移矩阵t后，可以利用三角化计算出优选特征点集中各特征点的初始三维空间坐标（即特征点的3D位置）。

优选的，在本实施例中，在步骤S205中，利用优选特征点集，计算至少三帧图片中的末帧图片相对于首帧图片的位姿，在步骤S206中，根据末帧图片相对于首帧图片的位姿，生成优选特征点集的初始三维空间坐标，从而提升初始三维空间坐标的精准程度。由于首末两帧图片相对于其他两帧图片（如任意相邻两帧图片），其图片中包含的特征信息变化大，利用首末两帧图片构建出的三维空间坐标相对更为准确。

步骤S207、利用优选特征点集以及优选特征点集的初始三维空间坐标，计算至少三帧图片中任一中间帧图片相对于首帧图片的位姿。

在该步骤中，上述任一中间帧图片为至少三帧图片中除去首帧图片和末帧图片外的任一帧图片。例如，当至少三帧图片包含A、B、C、D四帧图片时，任一中间帧图片为B帧图片或者C帧图片。

在该步骤中，可以根据优选特征点集以及优选特征点集的初始三维空间坐标，利用PnP算法计算至少三帧图片中任一中间帧图片相对于首帧图片的位姿。

步骤S208、根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，利用预设优化算法生成优选特征点集的最终三维空间坐标。

具体的，在一可选的实施方式中，步骤S208、根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，利用预设优化算法生成优选特征点集的最终三维空间坐标的具体实施方式可以包括以下步骤：

14）根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，通过建立投影关系，计算得到初始三维空间坐标中的各个特征点分别于每一帧图片中的投影像素坐标；

15）利用最小二乘优化算法构建各个特征点的投影像素坐标与真实像素坐标的误差方程，根据误差方程得到优选特征点集的最终三维空间坐标。

具体的，利用以下公式建立特征点的世界坐标到像素坐标的投影关系。

P_uv,i=KT_iP_w

其中，矩阵K为相机内参数，T_i为第i帧图片相对于首帧图片的位姿，P_w可以为优选特征点集内一个优选特征点的初始三维空间坐标，P_uv,i为该优选特征点在第i帧图片中的投影坐标。其中，当至少三帧图片为n帧图片时，i的取值为1、2、……、n。

也就是说，将优选特征点集中的各个优选特征点的初始三维空间坐标代入上述公式，即可得到该优选特征点在各目标帧图片中的投影坐标。

进一步的，利用最小二乘优化算法构建优选特征点集中的各个优选特征点的投影像素坐标与真实像素坐标的误差方程：

其中，P_uv,i'为该优选特征点在第i帧图片中的真实像素坐标。计算上述误差方程的最优解，该最优解包含了最小误差error以及对应的该优选特征点的三维空间坐标，并利用该三维空间坐标对该优选特征点的初始三维空间坐标进行修正。按照上述方式，进而修正优选特征点集中的各个优选特征点的三维空间坐标，即得到优选特征点集的最终三维空间坐标。

从该实施例可以看出，本申请实施例提供的方法，通过获取车辆行驶过程中采集的至少三帧图片，分别提取各帧图片的特征点，并对各帧图片的特征点进行匹配，得到匹配成功的共同特征点集。对各帧图片的共同特征点集进行筛选，并以每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小作为筛选参考值，以筛选得到可靠程度更高的优选特征点集，进而提升由优选特征点集生成的初始三维空间坐标的稳定性及精度，提升SLAM系统的鲁棒性。进一步的，根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，通过建立投影关系，计算得到初始三维空间坐标中的各个特征点分别于每一帧图片中的投影像素坐标；利用最小二乘优化算法构建各个特征点的投影像素坐标与真实像素坐标的误差方程，根据误差方程得到优选特征点集的最终三维空间坐标，从而进一步的提升由优选特征点集生成的最终三维空间坐标的稳定性及精度，提升SLAM系统的鲁棒性，保障SLAM系统后续运作的精准程度。

与前述应用功能实现方法实施例相对应，本申请还提供了一种用于自动驾驶车辆的SLAM初始化的装置的实施例。

图4是本申请实施例示出的用于自动驾驶车辆的SLAM初始化的装置的结构示意图。

参见图4，本申请实施例提供一种用于自动驾驶车辆的SLAM初始化的装置，包括：

获取单元401，用于获取车辆行驶过程中采集的至少三帧图片；

获取单元401，还用于获取至少三帧图片中每一帧图片的特征点；

匹配单元402，用于将至少三帧图片的特征点按照预设匹配规则进行匹配，得到至少三帧图片中匹配成功的共同特征点集；

筛选单元403，用于按照预设筛选规则对共同特征点集进行筛选，得到优选特征点集；

构建单元404，用于利用优选特征点集，计算至少三帧图片中的两帧目标图片之间的位姿关系；

构建单元404，还用于根据两帧目标图片之间的位姿关系，生成优选特征点集的初始三维空间坐标。

可选的，构建单元404利用优选特征点集，计算至少三帧图片中的两帧目标图片之间的位姿关系的方式可以包括：

构建单元404利用优选特征点集，计算至少三帧图片中的末帧图片相对于首帧图片的位姿；

构建单元404根据两帧目标图片之间的位姿关系，生成优选特征点集的初始三维空间坐标的方式可以包括：

构建单元404根据末帧图片相对于首帧图片的位姿，生成优选特征点集的初始三维空间坐标。

可选的，图4所示的装置还可以包括：

计算单元405，用于利用优选特征点集以及优选特征点集的初始三维空间坐标，计算至少三帧图片中任一中间帧图片相对于首帧图片的位姿。

再建单元406，用于根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，利用预设优化算法生成优选特征点集的最终三维空间坐标。

可选的，再建单元406根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，利用预设优化算法生成优选特征点集的最终三维空间坐标的方式可以包括：

再建单元406根据至少一张中间帧图片相对于首帧图片的位姿、末帧图片相对于首帧图片的位姿以及初始三维空间坐标，通过建立投影关系，计算得到初始三维空间坐标中的各个特征点分别于每一帧图片中的投影像素坐标；利用最小二乘优化算法构建各个特征点的投影像素坐标与真实像素坐标的误差方程，根据误差方程得到优选特征点集的最终三维空间坐标。

可选的，筛选单元403按照预设筛选规则对共同特征点集进行筛选，得到优选特征点集的方式可以包括：

筛选单元403获取共同特征点集中的各特征点组分别在至少三帧图片中的像素坐标；计算每一个特征点组在任意相邻两帧图片中的像素坐标向量；计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小；选取夹角大小均小于预设夹角的特征点组，生成优选特征点集。

可选的，筛选单元403计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小的方式可以包括：

筛选单元403计算每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角余弦值；

筛选单元403选取夹角大小均小于预设夹角的特征点组，生成优选特征点集的方式可以包括：

筛选单元403选取夹角余弦值均大于预设阈值的特征点组，生成优选特征点集。

实施图4所示的装置，能够提升SLAM系统的稳定性和精度。

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

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

请参阅图5，本申请实施例还提供了另一种电子设备500。该电子设备500可以用于执行上述实施例提供的用于自动驾驶车辆的SLAM初始化的方法。该电子设备500可以是任意具有计算单元的设备，如计算机、服务器、手持设备（如智能手机、平板电脑等）、或行车记录仪等等，本申请实施例不作限定。

参见图5，该电子设备500包括存储器501和处理器502。

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

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

存储器501上存储有可执行代码，当可执行代码被处理器502处理时，可以使处理器502执行上文述及的方法中的部分或全部步骤。

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

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

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

Claims

1.一种用于自动驾驶车辆的SLAM初始化的方法，其特征在于，包括：

获取车辆行驶过程中采集的至少三帧图片；

获取所述至少三帧图片中每一帧图片的特征点；

利用所述优选特征点集，计算所述至少三帧图片中的两帧目标图片之间的位姿关系；其中，包括利用所述优选特征点集，计算所述至少三帧图片中的末帧图片相对于首帧图片的位姿；

根据所述两帧目标图片之间的位姿关系，生成所述优选特征点集的初始三维空间坐标；其中，包括根据所述末帧图片相对于首帧图片的位姿，生成所述优选特征点集的初始三维空间坐标；

2.根据权利要求1所述的方法，其特征在于，所述根据至少一张所述中间帧图片相对于所述首帧图片的位姿、所述末帧图片相对于所述首帧图片的位姿以及所述初始三维空间坐标，利用预设优化算法生成所述优选特征点集的最终三维空间坐标，包括：

3.根据权利要求1或2所述的方法，其特征在于，所述按照预设筛选规则对所述共同特征点集进行筛选，得到优选特征点集，包括：

4.根据权利要求3所述的方法，其特征在于，所述计算所述每一个特征点组在任意相邻的三帧图片中的两个像素坐标向量的夹角大小，包括：

5.一种用于自动驾驶车辆的SLAM初始化的装置，其特征在于，包括：

获取单元，用于获取车辆行驶过程中采集的至少三帧图片；

构建单元，用于利用所述优选特征点集，计算所述至少三帧图片中的两帧目标图片之间的位姿关系；其中，包括利用所述优选特征点集，计算所述至少三帧图片中的末帧图片相对于首帧图片的位姿；

所述构建单元，还用于根据所述两帧目标图片之间的位姿关系，生成所述优选特征点集的初始三维空间坐标；其中，包括根据所述末帧图片相对于首帧图片的位姿，生成所述优选特征点集的初始三维空间坐标；

计算单元，用于利用所述优选特征点集以及所述优选特征点集的初始三维空间坐标，计算所述至少三帧图片中任一中间帧图片相对于所述首帧图片的位姿；

再建单元，用于根据至少一张所述中间帧图片相对于所述首帧图片的位姿、所述末帧图片相对于所述首帧图片的位姿以及所述初始三维空间坐标，利用预设优化算法生成所述优选特征点集的最终三维空间坐标。

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

处理器；以及

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

7.一种非暂时性机器可读存储介质，其上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如权利要求1-4中任一项所述的方法。