一种车辆的SLAM建图方法及系统
技术领域
本发明涉及自动驾驶技术领域,具体涉及一种车辆的SLAM建图方法及系统。
背景技术
在自动驾驶领域,单目视觉同步定位与地图创建(Simultaneous Localizationand Mapping,SLAM)是指自动驾驶车辆利用单个视觉传感器(如摄像头)创建一个与真实环境相一致的地图,并同时确定自身在地图中的位置。
然而,传统的单目视觉SLAM只能通过动态方法进行初始化,即需要利用摄像头运动过程中拍摄到的图像才能成功初始化。因此,在制图时,需要先将车辆驶出一段距离,完成单目视觉SLAM的初始化之后才能开始进行建图。如果初始化成功的时机较晚,那么车辆需要驶出较长的距离才能开始建图,从而可能导致制图的成功率较低,而且也容易导致构建出的地图中缺少部分环境信息。
发明内容
本发明实施例公开了一种车辆的SLAM建图方法,能够在车辆静止时完成初始化,以提高建图的成功率。
本发明实施例第一方面公开一种车辆的SLAM建图方法,所述方法包括:
获取车辆的任意两个摄像模组在所述车辆静止时分别拍摄得到的两个初始帧;所述两个摄像模组的取景范围至少存在部分重叠;
根据所述两个摄像模组预先标定的内部参数和外部参数以及相互匹配的特征点在所述两个初始帧中的视差,确定所述相互匹配的特征点的三维空间位置,以得到所述相互匹配的特征点对应的地图点,构建出初始的SLAM地图,以完成初始化;
在初始化成功之后,获取所述两个摄像模组中的任意一个目标摄像模组拍摄到的图像;
以所述初始的SLAM地图为基础,结合所述目标摄像模组拍摄到的图像进行单目视觉SLAM的建图。
作为一种可选的实施方式,在本发明实施例第一方面中所述获取车辆的任意两个摄像模组在所述车辆静止时分别拍摄得到的两个初始帧,包括:
获取车辆的任意两个摄像模组在所述车辆静止时分别进行拍摄得到的第一图像和第二图像;其中,所述两个摄像模组的焦距不同;
按照使用相同焦距拍摄得到的图像效果对所述第一图像和所述第二图像进行处理,以处理后的所述第一图像和所述第二图像作为两个初始帧。
作为一种可选的实施方式,在本发明实施例第一方面中,所述以所述初始的SLAM地图为基础,结合所述目标摄像模组拍摄到的图像进行单目视觉SLAM的建图,包括:
对所述目标摄像模组拍摄到的图像中的特征点以及当前SLAM地图中的地图点进行匹配;其中,当所述图像为所述目标摄像模组拍摄到的第一帧图像时,所述当前SLAM地图为所述初始的SLAM地图;
根据相互匹配的特征点和地图点确定所述目标摄像模组拍摄到的每帧图像对应的摄像模组位姿,以得到所述目标摄像模组的第一摄像模组位姿序列;
跟踪未匹配到地图点的特征点在所述目标摄像模组拍摄到的图像中的位置,并结合所述未匹配到地图点的特征点所在的图像对应的摄像模组位姿,确定所述未匹配到地图点的特征点的三维空间位置,以构建出新的地图点加入至所述当前SLAM地图;
通过所述车辆的定位模块获取与所述第一摄像模组位姿序列相对应的所述车辆的第一车辆位姿序列;
迭代调整所述目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及所述当前SLAM地图中的地图点的三维空间位置的取值,直至所述第一摄像模组位姿序列与第一车辆位姿序列之间的投影误差最小;
以所述投影误差最小时所述目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及所述当前SLAM地图中的地图点的三维空间位置的取值构建全局SLAM地图。
作为一种可选的实施方式,在本发明实施例第一方面中,所述方法还包括:
在重定位时,获取所述目标摄像模组拍摄到的多帧目标图像以及所述全局SLAM地图;
针对所述多帧目标图像中的每帧目标图像,匹配该帧目标图像中的特征点以及所述全局SLAM地图中的地图点;
根据相互匹配的特征点和地图点确定该帧目标图像对应的所述目标摄像模组在所述全局SLAM地图中的摄像模组位姿,以得到所述目标摄像模组的第二摄像模组位姿序列;
通过所述车辆的定位模块获取与所述第二摄像模组位姿序列对应的所述车辆的第二车辆位姿序列;
基于多帧所述目标图像以及所述第二摄像模组位姿序列确定所述目标摄像模组在所述全局SLAM地图中的重定位位姿,以使利用所述目标摄像模组的所述重定位位姿将所述第二车辆位姿序列转换到所述全局SLAM地图的SLAM地图坐标系时,转换得到的第三车辆位姿序列与所述第二摄像模组位姿序列的误差最小;所述全局SLAM地图坐标系的坐标原点为所述目标摄像模组拍摄到所述初始帧时的光心位置;
根据所述目标摄像模组在所述全局SLAM地图中的重定位位姿,确定所述车辆在所述全局SLAM地图中的重定位位姿。
作为一种可选的实施方式,在本发明实施例第一方面中,在所述根据所述目标摄像模组在所述SLAM地图中的重定位位姿,确定所述车辆在所述SLAM地图中的重定位位姿之后,所述方法还包括:
获取当前时刻所述车辆的定位模块测量到的所述车辆的当前位姿;
根据所述当前位姿以及所述车辆在所述全局SLAM地图中的重定位位姿,确定所述当前位姿在所述全局SLAM地图中的定位位姿作为当前时刻所述车辆的定位结果。
本发明实施例第二方面公开一种车辆的SLAM建图系统,包括:
初始化单元,用于获取车辆的任意两个摄像模组在所述车辆静止时分别拍摄得到的两个初始帧;以及,根据所述两个摄像模组预先标定的内部参数和外部参数,以及相互匹配的特征点在所述两个初始帧中的视差,确定所述相互匹配的特征点的三维空间位置,以得到所述相互匹配的特征点对应的地图点,构建出初始的SLAM地图,以完成初始化;其中,所述两个摄像模组的取景范围至少存在部分重叠;
建图单元,用于在初始化成功之后,获取所述两个摄像模组中的任意一个目标摄像模组拍摄到的图像;以及,以所述初始的SLAM地图为基础,结合所述目标摄像模组拍摄到的图像进行单目视觉SLAM的建图。
作为一种可选的实施方式,在本发明实施例第二方面中,所述初始化单元用于获取车辆的任意两个摄像模组在所述车辆静止时分别拍摄得到的两个初始帧的方式具体包括:
所述初始化单元,用于获取车辆的任意两个摄像模组在所述车辆静止时分别进行拍摄得到的第一图像和第二图像;以及,按照使用相同焦距拍摄得到的图像效果对所述第一图像和所述第二图像进行处理,以处理后的所述第一图像和所述第二图像作为初始帧;其中,所述两个摄像模组的焦距不同。
作为一种可选的实施方式,在本发明实施例第二方面中,所述建图单元,包括:
匹配子单元,用于对所述目标摄像模组拍摄到的图像中的特征点以及当前SLAM地图中的地图点进行匹配;其中,当所述图像为所述目标摄像模组拍摄到的第一帧图像时,所述当前SLAM地图为所述初始的SLAM地图;
定位子单元,用于根据相互匹配的特征点和地图点确定所述目标摄像模组拍摄到的每帧图像对应的摄像模组位姿,以得到所述目标摄像模组的位姿序列;
建图子单元,用于跟踪未匹配到地图点的特征点在所述目标摄像模组拍摄到的图像中的位置,并结合所述未匹配到地图点的特征点所在的图像对应的摄像模组位姿,确定所述未匹配到地图点的特征点的三维空间位置,以构建出新的地图点加入至所述当前SLAM地图;
获取子单元,用于通过所述车辆的定位模块获取与所述目标摄像模组的位姿序列相对应的所述车辆的第一位姿序列;
优化子单元,用于迭代调整所述目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及所述当前SLAM地图中的地图点的三维空间位置的取值,直至所述目标摄像模组的位姿序列与所述第一位姿序列之间的投影误差最小;
所述建图子单元,还用于以所述投影误差最小时所述目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及所述当前SLAM地图中的地图点的三维空间位置的取值构建全局SLAM地图。
作为一种可选的实施方式,在本发明实施例第二方面中,所述系统还包括:
第一获取单元,用于在重定位时,获取所述目标摄像模组拍摄到的多帧图像以及所述全局SLAM地图;
匹配单元,用于针对所述多帧图像中的每帧图像,匹配该帧图像中的特征点以及所述全局SLAM地图中的地图点;以及,根据相互匹配的特征点和地图点确定该帧图像对应的所述目标摄像模组在所述全局SLAM地图中的摄像模组位姿,以得到所述目标摄像模组的第二摄像模组位姿序列;
第二获取单元,用于通过所述车辆的定位模块获取与所述第二摄像模组位姿序列对应的所述车辆的第二车辆位姿序列;
重定位单元,用于基于多帧目标图像以及所述第二摄像模组位姿序列确定所述目标摄像模组在所述全局SLAM地图中的重定位位姿,以使利用所述目标摄像模组的所述重定位位姿将所述第二车辆位姿序列转换到所述全局SLAM地图的SLAM地图坐标系时,转换得到的第三车辆位姿序列与所述第二摄像模组位姿序列的误差最小;以及,根据所述目标摄像模组在所述全局SLAM地图中的重定位位姿,确定所述车辆在所述全局SLAM地图中的重定位位姿;
其中,所述全局SLAM地图坐标系的坐标原点为所述目标摄像模组拍摄到所述初始帧时的光心位置。
作为一种可选的实施方式,在本发明实施例第二方面中,所述系统还包括:
第三获取单元,用于在所述重定位单元根据所述目标摄像模组在所述SLAM地图中的重定位位姿,确定所述车辆在所述SLAM地图中的重定位位姿之后,获取当前时刻所述车辆的定位模块测量到的所述车辆的当前位姿;
定位单元,用于根据所述当前位姿以及所述车辆在所述全局SLAM地图中的重定位位姿,确定所述当前位姿在所述全局SLAM地图中的定位位姿作为当前时刻所述车辆的定位结果。
本发明实施例第三方面公开一种车辆,所述车辆包括本发明实施例第二方面公开的任一项车辆的SLAM建图系统。
本发明实施例第四方面公开一种车辆的SLAM建图系统,包括:
存储有可执行程序代码的存储器;
与所述存储器耦合的处理器;
摄像模组及车辆定位模块;
所述摄像模组拍摄得到的图像以及所述车辆定位模块检测到的车辆位姿传输至所述处理器,所述处理器调用所述存储器中存储的所述可执行程序代码,执行本发明实施例第一方面公开的任一项方法。
本发明第五方面公开一种计算机可读存储介质,其存储计算机程序,其中,所述计算机程序使得计算机执行本发明实施例第一方面公开的任一项方法。
本发明实施例第六方面公开一种计算机程序产品,当所述计算机程序产品在计算机上运行时,使得所述计算机执行本发明实施例第一方面公开的任一项方法。
与现有技术相比,本发明实施例具有以下有益效果:
本发明实施例在车辆静止时,通过车辆的任意两个摄像模组分别拍摄得到两个初始帧;利用初始帧中相互匹配的特征点构建出初始的SLAM地图,从而可以通过双目摄像模组进行初始化;在初始化成功之后,利用两个摄像模组中的任意一个目标摄像模组拍摄图像,以进行单目视觉SLAM的建图,从而可以融合双目和单目的优点,在车辆静止时完成初始化,不需要车辆运动一定距离,进而可以提高建图的成功率,减少地图中的信息缺失。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明实施例公开的一种车辆的SLAM建图方法的流程示意图;
图2是本发明实施例公开的另一种车辆的SLAM建图方法的流程示意图;
图3是本发明实施例公开的又一种车辆的SLAM建图方法的流程示意图;
图4是本发明实施例公开的一种车辆的SLAM建图系统的结构示意图;
图5是本发明实施例公开的另一种车辆的SLAM建图系统的结构示意图;
图6是本发明实施例公开的又一种车辆的SLAM建图系统的结构示意图;
图7是本发明实施例公开的再一种车辆的SLAM建图系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明的是,本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
本发明实施例公开了一种车辆的SLAM建图方法及系统,能够在车辆静止时完成初始化,以提高建图的成功率。以下分别进行详细说明。
实施例一
请参阅图1,图1是本发明实施例公开的一种车辆的SLAM建图方法的流程示意图。其中,图1所描述的车辆的SLAM建图方法适用于车载电脑、电子控制单元(ElectronicControl Unit,ECU)等车载电子设备,本发明实施例不做限定。如图1所示,该车辆的SLAM建图方法可以包括以下步骤:
101、车载电子设备获取车辆的任意两个摄像模组在车辆静止时分别拍摄得到的两个初始帧。
本发明实施例中,两个摄像模组的取景范围至少存在部分重叠。作为一种可选的实施方式,上述的两个摄像模组可以为多目摄像系统中的任意两个摄像模组。多目摄像系统中可以包含三个或以上的摄像模组,每个摄像模组之间的设置距离在可以在10cm-20cm之间,均朝向车辆的行驶方向设置。
作为另一种可选的实施方式,由于多目摄像系统中相邻的摄像模组之间的相对距离并不远,为了使得特征点在两个初始帧中的视差足够大,可以选取多目摄像系统中相对距离最远的两个摄像模组(如分别位于两端的摄像模组)拍摄图像,将上述的相对距离最远的两个摄像模组分别拍摄得到的两张图像作为两个初始帧。
可以理解的是,初始化成功的前提条件是初始帧中存在相互匹配的特征点,其中,相互匹配的特征点为三维空间中同一个物体分别被两个摄像模组拍摄之后,分别投影到两个初始帧中得到的投影点。因此,两个摄像模组的取景范围应该至少存在部分重叠,以保证初始帧中可以存在相互匹配的特征点。优选的,还可以控制两个摄像模组同时拍摄图像,进一步保证初始帧中可以存在相互匹配的特征点。此外,初始帧应该满足一定要求。比如,可以设定初始帧中的特征点数目应该大于预设阈值(如100)。也就是说,上述的两个摄像模组拍摄到的图像中,如果摄像模组取景范围的重叠部分对应的图像中的特征点数目大于上述的预设阈值,那么将拍摄到的图像设置为初始帧;否则,两个摄像模组重新拍摄图像。
102、车载电子设备根据上述的两个摄像模组预先标定的内部参数和外部参数,以及相互匹配的特征点在两个初始帧中的视差,确定相互匹配的特征点的三维空间位置,以得到相互匹配的特征点对应的地图点,构建出初始的SLAM地图,以完成初始化。
本发明实施例中,基于立体视觉的原理,当两个摄像模组的各自的内部参数(如焦距、畸变系数等)以及两个摄像模组相对的外部参数(如基线、右侧摄像模组相对于左侧摄像模组的平移量和旋转量等)已知,结合相互匹配的特征点在两个初始帧中的视差,即可确定出上述的相互匹配的特征点的三维空间位置。
可以理解的是,摄像模组的内部参数可用于表征三维空间点与图像像素点之间的映射关系,因此基于摄像模组的内部参数、以及特征点在图像中的位置,即可恢复出特征点相对于摄像模组的相对位置,但缺少深度信息。进一步地,特征点的深度信息d的求取方式可以通过以下简化的数学模型表示:d=b*f/p;b为基线(即两个摄像模组的光心之间的距离),f为焦距,p为相互匹配的特征点在两帧图像中的视差。
假设以两个摄像模组中的任意一个摄像模组在拍摄到初始帧时的位置作为坐标系原点,通过上述的实施方式,可以确定出重复出现在两个初始帧中的特征点相对于坐标系原点的三维空间位置,从而得到对应的地图点,这些地图点构成了初始的SLAM地图,SLAM系统的初始化完成。
103、在初始化成功之后,车载电子设备获取两个摄像模组中的任意一个目标摄像模组拍摄到的图像。
本发明实施例中,为了方便计算,目标摄像模组可以为上述被选取为坐标系原点的摄像模组。作为一种可选的实施方式,在初始化成功之后,车辆可以开始行驶,目标摄像模组可以在车辆不断行驶的过程中,持续拍摄多帧图像。
104、车载电子设备以初始的SLAM地图为基础,结合目标摄像模组拍摄到的图像进行单目视觉SLAM的建图。
本发明实施例中,在初始化成功之后,即可转换为单目视觉SLAM,也就是说,只使用一个目标摄像模组拍摄图像。可以理解的是,单目SLAM的建图具体可以包括以下步骤:
当目标摄像模组拍摄到第一帧图像时,对第一帧图像中的特征点以及初始的SLAM地图中的地图点进行匹配(即2D-3D的匹配);
针对相互匹配的特征点和地图点,根据特征点在第一帧图像中的图像位置以及对应匹配的地图点的三维空间位置,可以确定出第一帧图像对应的相机(即摄像模组)位姿;
针对目标摄像模组在第一帧图像之后拍摄到的每一帧图像,重复上述的步骤,从而可以确定出每帧图像对应的相机位姿;同时,针对图像中未匹配到地图点的特征点,在目标摄像模组拍摄到的图像中跟踪这些特征点,根据这些特征点所在图像中的图像位置,以及这些特征点所在图像对应的相机位姿,可以计算出这些特征点的三维空间位置,从而得到新的地图点加入至SLAM地图中。
综上,通过不断添加地图点,可以构建出车辆途经环境的地图。
可见,在图1所描述的方法中,在初始化时,可以利用两个摄像模组构成双目摄像系统,与只有单个摄像模组的单目摄像系统相比,双目摄像系统在车辆静止时已经可以获取到存在视差的初始帧,因此无需车辆行驶一段距离以进行初始化,在车辆静止时即可完成初始化,从而可以减少建图时的数据丢失;而在初始化完成之后,只利用一个摄像模组拍摄得到的图像进行建图,与利用双目摄像系统进行建图的方案相比,只使用单个摄像模组可以减少需要处理的图像数据量。也就是说,图1所描述的方法可以融合单目和双目的优点,在提高建图成功率的同时还可以减少数据处理量。
此外,上述的步骤101~步骤102可以为初始启动建图时执行的步骤,也可以为建图过程中一旦检测到建图失败时执行的步骤。也就是说,当检测到建图失败时,可以启动车辆的任意两个摄像模组,优选的,可以包含目标摄像模组,然后通过这两个摄像模组拍摄图像,以作为车辆静止时获取到的初始帧。其中,建图失败可以包括目标摄像模组拍摄到的图像中存在的特征点数量较少,或者图像中与地图点相互匹配的特征点较少等情况,本发明实施例不做限定。因此,执行上述的步骤101~步骤102还可以在建图失败时快速进行再次初始化,从而再次进行建图,相较于需要车辆移动进行初始化的单目摄像系统,可以减少两次建图之间的数据缺失。
实施例二
请参阅图2,图2是本发明实施例公开的另一种车辆的SLAM建图方法的流程示意图。如图2所示,该车辆的SLAM建图方法可以包括以下步骤:
201、车载电子设备获取车辆的任意两个摄像模组在车辆静止时分别拍摄得到的两个初始帧。
本发明实施例中,上述的两个摄像模组可以为多目摄像系统中的任意两个摄像模组,两个摄像模组的焦距和视场角可以相同,也可以不同。然而,多目摄像系统由于摄像模组数量较多,为了减少图像数据处理量,加快处理时间,一般会选择采用固定焦距的摄像模组。进一步地,为了扩大能够覆盖的观察距离,不同的摄像模组一般会设定为不同的焦距,以减少单个摄像模组无法切换焦距带来的问题。此外,不同的摄像模组的视场角也可能不同,比如三个摄像模组分别采用120°、60°和30°的视场角,以使不同的摄像模组覆盖不同角度范围的场景。
当焦距和视场角相同时,两个摄像模组拍摄得到的图像效果相同,因此在执行下述的步骤202对初始帧进行特征点匹配的准确率比较高。当焦距和视场角不同时,两个摄像模组拍摄得到的图像中物体的形变程度不同,从而会降低特征点匹配的准确率。因此,基于双目立体视觉恢复特征点三维空间位置时,使用焦距相同的两个摄像模组拍摄初始帧,恢复出的三维空间位置的精度可能高于使用不同焦距的两个摄像模组拍摄初始帧。在本发明实施例中,在采用不同焦距的摄像模组的同时,为了减少不同焦距的摄像模组对三维空间位置计算精度的影响,可以执行以下步骤:
获取车辆的任意两个摄像模组在车辆静止时分别进行拍摄得到的第一图像和第二图像;
按照使用相同焦距拍摄得到的图像效果对第一图像和第二图像进行处理,以处理后的第一图像和第二图像作为两个初始帧。
其中,假设两个摄像模组中一个摄像模组的焦距为fA,另一个摄像模组的焦距为fC,那么上述的相同焦距fAC可以满足以下条件:fAC∈[fA,fC]。可以理解的是,在标定摄像模组的参数时,可以先独立标定其中一个摄像模组的内部参数,以得到上述的fA,再独立标定另一个摄像模组的内部参数,以得到上述的fC,而非同时标定上述的两个摄像模组。
执行上述的步骤,可以通过软调焦的方式使得两个不同焦距的摄像模组拍摄得到的图像调整为相同焦距拍摄得到的图像效果,从而可以减少不同焦距的摄像模组对三维空间位置计算精度的影响,从而减少对建图精度的影响。
此外,下述的步骤202~步骤203与步骤102~步骤103相同,以下内容不再赘述。
204、车载电子设备对目标摄像模组拍摄到的图像中的特征点以及当前SLAM地图中的地图点进行匹配,以获得目标摄像模组的第一摄像模组位姿序列,并构建出新的地图点加入至当前SLAM地图。
本发明实施例中,当前SLAM地图为当前时刻已经构建出的SLAM地图,当用于进行特征点匹配的图像为目标摄像模组拍摄到的第一帧图像时,当前SLAM地图为初始的SLAM地图。
具体地,可以根据相互匹配的特征点和地图点确定目标摄像模组拍摄到的每帧图像对应的摄像模组位姿,以得到目标摄像模组的第一摄像模组位姿序列;其中,第一摄像模组位姿序列中的各个位姿可以与目标摄像模组拍摄到的图像一一对应;
针对未匹配到地图点的特征点,可以跟踪未匹配到地图点的特征点在目标摄像模组拍摄到的图像中的位置,并结合未匹配到地图点的特征点所在的图像对应的摄像模组位姿,确定未匹配到地图点的特征点的三维空间位置,以构建出新的地图点加入至当前SLAM地图。
205、车载电子设备通过车辆的定位模块获取与第一摄像模组位姿序列相对应的车辆的第一车辆位姿序列。
本发明实施例中,车辆上可以装设有惯性测量单元(Inertial measurementunit,IMU)、轮脉冲计数器等传感器,这些传感器可以作为车辆的定位模块(如车身里程计)计算车辆的行驶距离,进一步地还可以对车辆进行位置定位。第一车辆位姿序列中可以包括多个由车辆的定位模块测量出的车辆位姿,并且每个车辆位姿与第一摄像模组位姿序列中的某一个摄像模组位姿对应。可以理解的是,假设某一时刻目标摄像模组拍摄到一张图像,根据该图像中的特征点与地图点进行匹配,确定出该图像对应的摄像模组位姿;同时,在该时刻车辆的定位模块测量出车辆位姿,那么该车辆位姿与上述的摄像模组位姿对应。
206、车载电子设备迭代调整目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及当前SLAM地图中的地图点的三维空间位置的取值,直至第一摄像模组位姿序列与第一车辆位姿序列之间的投影误差最小。
本发明实施例中,在迭代调整的过程中,摄像模组位姿的取值、地图点的三维空间位置的取值应该满足联合优化(Bundle Adjustment)所需的约束条件。本发明实施例中,可以认为车辆的定位模块测量出的车辆位姿相对准确,因此,以上述的第一车辆位姿序列为准,假设摄像模组位姿的取值以及地图点的三维空间位置的取值相对准确,那么第一摄像模组位姿序列与第一车辆位姿序列之间的差距较小。其中,可以理解的是,第一摄像模组位姿序列中的摄像模组位姿可以为相机坐标系下的位姿,第一车辆位姿序列中的车辆位姿可以为车辆坐标系下的位姿;相机坐标系以摄像模组的光心为坐标原点,车辆坐标系以车辆的某一部位(如后轴中心点)为坐标原点;相机坐标系与车辆坐标系之间的转换关系可以预先标定得到。第一摄像模组位姿序列与第一车辆位姿序列之间的差距较小,可以为第一摄像模组位姿序列与第一车辆位姿序列之间的差距为相机坐标系与车辆坐标系之间的转换关系,也就是说,如果基于相机坐标系与车辆坐标系之间的转换关系将第一摄像模组位姿序列转换至车辆坐标系下,转换得到的位姿序列与第一车辆位姿序列重合。
207、车载电子设备以投影误差最小时目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及当前SLAM地图中的地图点的三维空间位置的取值构建全局SLAM地图。
本发明实施例中,全局SLAM地图可以为车载电子设备停止执行建图程序时最终得到的SLAM地图。
可见,实施如图2所示的方法,可以通过不同焦距的摄像模组在有限的物理空间内使得摄像系统尽可能多地覆盖观察场景,同时通过软调焦的方式可以减少不同焦距的摄像模组对三维空间位置计算精度的影响,从而减少对建图精度的影响。此外,还可以结合车辆的定位模块,通过联合优化的方式进一步地提高构建出的SLAM地图的精度。
实施例三
请参阅图3,图3是本发明实施例公开的又一种车辆的SLAM建图方法的流程示意图。如图3所示,该车辆的SLAM建图方法可以包括以下步骤:
步骤301~步骤304与步骤101~步骤104相同,其中,步骤304的具体实现方式可以如步骤204~步骤206所示,以下内容不再赘述。
305、车载电子设备在重定位时,获取目标摄像模组拍摄到的多帧目标图像以及上述的全局SLAM地图。
本发明实施例中,全局SLAM地图为真实环境的数字化描述,在构建出全局SLAM地图之后,车载电子设备可以重用全局SLAM地图。类似于人类利用地图进行定位的方式,车载电子设备可以定位车辆在全局SLAM地图中的位置,从而获知车辆在真实环境中的位置。在车辆再次驶入全局SLAM地图所指示的真实环境,或者车辆在重定位成功之后的后续定位过程中发生特征点跟踪丢失的情况,都可以触发重定位,以再次确定车辆在全局SLAM地图中的位置。具体地,可以执行下述的步骤306~步骤309进行重定位。
306、车载电子设备针对多帧目标图像中的每帧目标图像,匹配该帧目标图像中的特征点以及全局SLAM地图中的地图点,并根据相互匹配的特征点和地图点确定该帧目标图像对应的目标摄像模组在全局SLAM地图中的摄像模组位姿,以得到目标摄像模组的第二摄像模组位姿序列。
本发明实施例中,可以理解的是,每帧目标图像对应第二摄像模组位姿序列中的一个摄像模组位姿,第二摄像模组位姿序列中的摄像模组位姿为目标摄像模组的相机坐标系下的位姿。
307、车载电子设备通过车辆的定位模块获取与第二摄像模组位姿序列对应的车辆的第二车辆位姿序列。
本发明实施例中,第二车辆位姿序列中包含的各个车辆位姿与第二摄像模组位姿序列中包含的摄像模组位姿对应,车载电子设备可以在目标摄像模组拍摄到目标图像的同一时刻,通过定位模块获取车辆位姿,从而构成与第二摄像模组位姿序列对应的第二车辆位姿序列,并且第二车辆位姿序列中的车辆位姿可以为车辆坐标系下的位姿。
308、车载电子设备基于多帧目标图像以及第二摄像模组位姿序列确定目标摄像模组在全局SLAM地图中的重定位位姿,以使利用目标摄像模组的重定位位姿将第二车辆位姿序列转换到全局SLAM地图的SLAM地图坐标系时,转换得到的第三车辆位姿序列与第二摄像模组位姿序列的误差最小。
本发明实施例中,目标摄像模组在全局SLAM地图中的重定位位姿,即当前目标摄像模组的位姿与全局SLAM地图坐标系原点之间的相对转换关系(平移量和旋转量),也就是目标摄像模组的相机坐标系与全局SLAM地图坐标系之间的转换关系;其中,全局SLAM地图坐标系原点可以为目标摄像模组拍摄上述的初始帧时的光心位置。如果目标摄像模组的重定位位姿已知,基于车辆坐标系与目标摄像模组的相机坐标系之间的转换关系,以及相机坐标系与全局SLAM地图坐标系之间的转换关系,可以将车辆坐标系下的第二车辆位姿序列转换至全局SLAM地图中,转换得到的第三车辆位姿序列可以认为是车辆在地图中的移动轨迹。而第二摄像模组位姿序列可以认为是目标摄像模组在地图中的移动轨迹,如果目标摄像模组的重定位位姿准确,目标摄像模组在地图中的移动轨迹与车辆在地图中的移动轨迹之间的差距比较小,具体可以为第三车辆位姿序列与第二摄像模组位姿序列之间的差距为相机坐标系与车辆坐标系之间的转换关系。
作为一种可选的实施方式,可以再次通过联合优化所限定的目标图像、目标图像对应的摄像模组位姿以及地图点的三维空间位置之间的约束关系,不断迭代调整第二摄像模组位姿序列中各个摄像模组位姿的取值,以第二摄像模组位姿序列中最后一个摄像模组位姿的取值作为目标摄像模组的重定位位姿,或者以第二摄像模组位姿序列中各个摄像模组位姿的平均值作为目标摄像模组的重定位位姿,将第二车辆位姿序列转换至全局SLAM地图中,直至第三车辆位姿序列与第二摄像模组位姿序列之间的误差最小。误差最小时目标摄像模组的重定位位姿的取值即为最终的重定位位姿。
309、车载电子设备根据目标摄像模组在全局SLAM地图中的重定位位姿,确定车辆在全局SLAM地图中的重定位位姿。
本发明实施例中,可以理解的是,利用车辆坐标系与目标摄像模组的相机坐标系之间的转换关系,即可通过目标摄像模组的重定位位姿确定出车辆在全局SLAM地图中的重定位位姿。
通过执行上述的步骤305~步骤309,可以重用全局SLAM地图,以对车辆进行定位,同时,还可以通过多帧联合重定位的方式,提高重定位的精度。
310、车载电子设备获取当前时刻车辆的定位模块测量到的车辆的当前位姿。
311、车载电子设备根据当前位姿以及车辆在全局SLAM地图中的重定位位姿,确定当前位姿在全局SLAM地图中的定位位姿作为当前时刻车辆的定位结果。
本发明实施例中,车辆的定位模块测量到的当前位姿为车辆坐标系下的位姿,车辆在全局SLAM地图中的重定位位姿可以理解为车辆坐标系与全局SLAM地图坐标系之间的转换关系,因此,基于车辆在全局SLAM地图中的重定位位姿,可以将车辆的当前位姿转换至全局SLAM地图中。
由于基于视觉SLAM的车辆定位依赖于特征点匹配,一旦摄像模组被遮挡或者摄像模组拍摄到白墙等特征点较少的图像,视觉SLAM系统将无法进行定位。因此在本发明实施例中,车载电子设备在重定位成功(即确定出车辆在全局SLAM地图中的重定位位姿)之后,以车辆的定位模块测量到的车辆的当前位姿为准,将当前位姿转换到SLAM地图中,作为最终的定位结果,从而可以降低车辆定位对视觉信息的依赖程度,提高车辆定位成功的概率,大幅度摆脱了视觉定位过程的不稳定因素,提高了系统在视觉遮挡或视觉特征丢失等情况下的稳定性。
可见,在图3所描述的方法中,可以通过双目摄像系统进行初始化,在初始化成功之后通过单目摄像模组进行建图,从而融合单目和双目的优点,在提高建图成功率的同时还可以减少数据处理量。进一步地,在建图过程中,通过结合车辆的定位模块,以联合优化的方式进一步地提高构建出的SLAM地图的精度。在重定位时,通过多帧联合重定位的方式,提高重定位的精度,在重定位之后,将车辆的定位模块测量得到的当前位姿转换至SLAM地图中作为定位结果,以降低车辆定位对视觉信息的依赖程度,提高车辆定位成功的概率。
实施例四
请参阅图4,图4是本发明实施例公开的一种车辆的SLAM建图系统的结构示意图。如图4所示,该系统包括:
初始化单元401,用于获取车辆的任意两个摄像模组在车辆静止时分别拍摄得到的两个初始帧;以及,根据两个摄像模组预先标定的内部参数和外部参数,以及相互匹配的特征点在两个初始帧中的视差,确定相互匹配的特征点的三维空间位置,以得到相互匹配的特征点对应的地图点,构建出初始的SLAM地图,以完成初始化;其中,两个摄像模组的取景范围至少存在部分重叠;
本发明实施例中,初始化单元401可以获取任意两个摄像模组拍摄得到的图像作为初始帧,也可以获取摄像模组所在的多目摄像系统中相对距离最远的两个摄像模组(如分别位于两端的摄像模组)拍摄到的图像作为初始帧。
此外,如果初始化单元401判断出摄像模组取景范围的重叠部分对应的图像中的特征点数目未大于上述的预设阈值,可以控制摄像模组重新拍摄图像,直至获取到上述的特征点数目大于上述的预设阈值的图像作为初始帧。
建图单元402,用于在初始化成功之后,获取两个摄像模组中的任意一个目标摄像模组拍摄到的图像;以及,以初始的SLAM地图为基础,结合目标摄像模组拍摄到的图像进行单目视觉SLAM的建图。
本发明实施例中,可以理解的是,建图单元402具体可以通过以下操作进行建图:
建图单元402用于对目标摄像模组拍摄到的第一帧图像中的特征点以及初始的SLAM地图中的地图点进行匹配;
针对相互匹配的特征点和地图点,建图单元402根据特征点在第一帧图像中的图像位置以及对应匹配的地图点的三维空间位置,可以确定出第一帧图像对应的相机(即摄像模组)位姿;
针对目标摄像模组在第一帧图像之后拍摄到的每一帧图像,建图单元402重复上述的操作,从而可以确定出每帧图像对应的相机位姿;同时,针对图像中未匹配到地图点的特征点,在目标摄像模组拍摄到的图像中跟踪这些特征点,根据这些特征点所在图像中的图像位置,以及这些特征点所在图像对应的相机位姿,可以计算出这些特征点的三维空间位置,从而得到新的地图点加入至SLAM地图中。
此外,建图单元402在建图的过程中一旦检测到建图失败,即可触发初始化单元401执行上述的操作,从而可以在建图失败时快速进行再次初始化,从而再次进行建图,相较于需要车辆移动进行初始化的单目摄像系统,可以减少两次建图之间的数据缺失。
实施如图4所示的系统,可以在初始化时,利用两个摄像模组构成双目摄像系统,从而无需车辆行驶一段距离以进行初始化,能够在车辆静止时即可完成初始化;在初始化成功之后,只利用一个摄像模组拍摄得到的图像进行建图,以减少需要处理的图像数据量;因此,可以融合单目和双目的优点,在提高建图成功率的同时还可以减少数据处理量。此外,还可以在建图失败时快速进行再次初始化,以减少两次建图之间的数据缺失。
实施例五
请参阅图5,图5是本发明实施例公开的另一种车辆的SLAM建图系统的结构示意图。其中,图5所示的车辆的SLAM建图系统是由图4所示的车辆的SLAM建图系统进行优化得到的。
其中,图5所示的系统中,两个摄像模组的焦距不同;相应地,上述的初始化单元401用于获取车辆的任意两个摄像模组在车辆静止时分别拍摄得到的两个初始帧的方式具体可以为:
初始化单元401,用于获取车辆的任意两个摄像模组在车辆静止时分别进行拍摄得到的第一图像和第二图像;以及,按照使用相同焦距拍摄得到的图像效果对第一图像和第二图像进行处理,以处理后的第一图像和第二图像作为两个初始帧。
其中,假设两个摄像模组中一个摄像模组的焦距为fA,另一个摄像模组的焦距为fC,那么上述的相同焦距fAC可以满足以下条件:fAC∈[fA,fC]。通过上述的实施方式,初始化单元401可以通过软调焦的方式使得两个不同焦距的摄像模组拍摄得到的图像调整为相同焦距拍摄得到的图像效果,从而可以减少不同焦距的摄像模组对三维空间位置计算精度的影响,从而减少对建图精度的影响。
可选的,在图5所示的系统中,上述的建图单元402,可以包括:
匹配子单元4021,用于对目标摄像模组拍摄到的图像中的特征点以及当前SLAM地图中的地图点进行匹配;其中,当前SLAM地图为当前时刻已经构建出的SLAM地图,当用于进行特征点匹配的图像为目标摄像模组拍摄到的第一帧图像时,当前SLAM地图为初始的SLAM地图;
定位子单元4022,用于根据相互匹配的特征点和地图点确定目标摄像模组拍摄到的每帧图像对应的摄像模组位姿,以得到目标摄像模组的位姿序列;
建图子单元4023,用于跟踪未匹配到地图点的特征点在目标摄像模组拍摄到的图像中的位置,并结合未匹配到地图点的特征点所在的图像对应的摄像模组位姿,确定未匹配到地图点的特征点的三维空间位置,以构建出新的地图点加入至当前SLAM地图;
获取子单元4024,用于通过车辆的定位模块获取与目标摄像模组的位姿序列相对应的车辆的第一位姿序列;其中,定位模块可以为惯性测量单元(Inertial measurementunit,IMU)、轮脉冲计数器等传感器,这些传感器可以作为车辆的定位模块(如车身里程计)计算车辆的行驶距离,进一步地还可以对车辆进行位置定位。
优化子单元4025,用于迭代调整目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及当前SLAM地图中的地图点的三维空间位置的取值,直至目标摄像模组的位姿序列与第一位姿序列之间的投影误差最小;
相应地,上述的建图子单元4023,用于以投影误差最小时目标摄像模组的位姿序列包含的各个摄像模组位姿的取值以及当前SLAM地图中的地图点的三维空间位置的取值构建全局SLAM地图。
实施如图5所示的系统,可以通过不同焦距的摄像模组在有限的物理空间内使得摄像系统尽可能多地覆盖观察场景,同时通过软调焦的方式可以减少不同焦距的摄像模组对三维空间位置计算精度的影响,从而减少对建图精度的影响。此外,还可以结合车辆的定位模块,通过联合优化的方式进一步地提高构建出的SLAM地图的精度。
实施例六
请参阅图6,图6是本发明实施例公开的又一种车辆的SLAM建图系统的结构示意图。其中,图6所示的车辆的SLAM建图系统是由图5所示的车辆的SLAM建图系统进行优化得到的。如图6所示,还可以包括:
第一获取单元403,用于在重定位时,获取目标摄像模组拍摄到的多帧图像以及全局SLAM地图;
本发明实施例中,第一获取单元403在车辆再次驶入全局SLAM地图所指示的真实环境,或者车辆在重定位成功之后的后续定位过程中发生特征点跟踪丢失的情况时,都可以执行上述的操作;
匹配单元404,用于针对多帧图像中的每帧图像,匹配该帧图像中的特征点以及全局SLAM地图中的地图点;以及,根据相互匹配的特征点和地图点确定该帧图像对应的目标摄像模组在全局SLAM地图中的摄像模组位姿,以得到目标摄像模组的第二摄像模组位姿序列;
第二获取单元405,用于通过车辆的定位模块获取与第二摄像模组位姿序列对应的车辆的第二车辆位姿序列;
重定位单元406,用于基于多帧目标图像以及第二摄像模组位姿序列确定目标摄像模组在全局SLAM地图中的重定位位姿,以使利用目标摄像模组的重定位位姿将第二车辆位姿序列转换到全局SLAM地图的SLAM地图坐标系时,转换得到的第三车辆位姿序列与第二摄像模组位姿序列的误差最小;以及,根据目标摄像模组在全局SLAM地图中的重定位位姿,确定车辆在全局SLAM地图中的重定位位姿;
其中,全局SLAM地图坐标系的坐标原点为目标摄像模组拍摄到初始帧时的光心位置。
可选的,图6所示的系统还可以包括:
第三获取单元407,用于在重定位单元405根据目标摄像模组在SLAM地图中的重定位位姿,确定车辆在SLAM地图中的重定位位姿之后,获取当前时刻车辆的定位模块测量到的车辆的当前位姿;
定位单元408,用于根据测量出的当前位姿以及车辆在全局SLAM地图中的重定位位姿,确定当前位姿在全局SLAM地图中的定位位姿作为当前时刻车辆的定位结果。
实施如图6所示的系统,可以通过双目摄像系统进行初始化,在初始化成功之后通过单目摄像模组进行建图,从而融合单目和双目的优点,在提高建图成功率的同时还可以减少数据处理量。进一步地,在建图过程中,通过结合车辆的定位模块,以联合优化的方式进一步地提高构建出的SLAM地图的精度。在重定位时,通过多帧联合重定位的方式,提高重定位的精度,在重定位之后,将车辆的定位模块测量得到的当前位姿转换至SLAM地图中作为定位结果,以降低车辆定位对视觉信息的依赖程度,提高车辆定位成功的概率。
实施例七
请参阅图7,图7是本发明实施例公开的再一种车辆的SLAM建图系统的结构示意图。如图7所示,该系统可以包括:
存储有可执行程序代码的存储器701;
与存储器701耦合的处理器702;
摄像模组703;
车辆定位模块704;
其中,摄像模组703拍摄得到的图像以及车辆定位模块704检测到的车辆位姿传输至处理器702,处理器702调用存储器701中存储的可执行程序代码,执行图1~图3所示的任一种车辆的SLAM建图方法。
本发明实施例公开一种车辆,包括图4~图7所示的车辆的SLAM建图系统。
本发明实施例公开一种计算机可读存储介质,其存储计算机程序,其中,该计算机程序使得计算机执行图1~图3所示的任一种车辆的SLAM建图方法。
本发明实施例公开一种计算机程序产品,该计算机程序产品包括存储了计算机程序的非瞬时性计算机可读存储介质,且该计算机程序可操作来使计算机执行图1~图3所示的任一种车辆的SLAM建图方法。
应理解,说明书通篇中提到的“一个实施例”或“一实施例”意味着与实施例有关的特定特征、结构或特性包括在本发明的至少一个实施例中。因此,在整个说明书各处出现的“在一个实施例中”或“在一实施例中”未必一定指相同的实施例。此外,这些特定特征、结构或特性可以以任意适合的方式结合在一个或多个实施例中。本领域技术人员也应该知悉,说明书中所描述的实施例均属于可选实施例,所涉及的动作和模块并不一定是本发明所必须的。
在本发明的各种实施例中,应理解,上述各过程的序号的大小并不意味着执行顺序的必然先后,各过程的执行顺序应以其功能和内在逻辑确定,而不应对本发明实施例的实施过程构成任何限定。
上述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物单元,即可位于一个地方,或者也可以分布到多个网络单元上。可根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。
另外,在本发明各实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可获取的存储器中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分,可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储器中,包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等,具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序可以存储于一计算机可读存储介质中,存储介质包括只读存储器(Read-Only Memory,ROM)、随机存储器(Random Access Memory,RAM)、可编程只读存储器(Programmable Read-only Memory,PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory,EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory,OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory,EEPROM)、只读光盘(CompactDisc Read-Only Memory,CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
以上对本发明实施例公开的一种车辆的SLAM建图方法及系统进行了详细介绍,本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。