发明内容
本发明实施例的目的在于提供一种确定SFM地图绝对尺度的方法及电子设备,以提高确定的SFM地图绝对尺度的准确度。具体技术方案如下:
第一方面,本发明实施例提供了一种确定SFM地图绝对尺度的方法,所述方法包括:
获取运动恢复结构SFM视觉稀疏点云地图和目标激光点云;其中,SFM视觉稀疏点云地图基于相机拍摄指定场景得到的多帧图像构建,所述目标激光点云为所述相机拍摄所述多帧图像的其中一帧目标图像时,激光雷达扫描所述指定场景得到的激光点云;
将所述SFM视觉稀疏点云地图中的地图点与所述目标激光点云中的激光点进行匹配,获得多对匹配点;
根据各对匹配点的坐标,计算SFM地图尺度因子,所述SFM地图尺度因子用于表示SFM视觉稀疏点云地图中的地图点的相对坐标与绝对坐标之间的转换关系;
针对所述SFM视觉稀疏点云地图中的每个地图点,根据所述SFM尺度因子计算该地图点的绝对坐标,并将该地图点的相对坐标更新为该地图点的绝对坐标。
可选的,在所述根据各对匹配点的坐标,计算SFM地图尺度因子之后,所述方法还包括:
根据所述SFM尺度因子,分别确定所述SFM视觉稀疏点云地图中的每个地图点在指定坐标系下的坐标;
分别确定所述目标激光点云中的每个激光点在所述指定坐标系下的坐标;
针对每一对匹配点,判断该对匹配点的残差是否大于预设残差,其中,该对匹配点的残差为:该对匹配点包括的地图点在所述指定坐标系下的坐标与该对匹配点包括的激光点在所述指定坐标系下的坐标之间的残差;
若是,则忽略该对匹配点;
若否,则保留该对匹配点;
根据保留的各对匹配点的坐标,更新所述SFM地图尺度因子。
可选的,在所述针对所述SFM视觉稀疏点云地图中的每个地图点,根据所述SFM尺度因子计算该地图点的绝对坐标之前,所述方法还包括:
若更新所述SFM地图尺度因子的次数未达到预设次数,则将所述多帧图像中所述目标图像的下一帧图像作为所述目标图像,将所述目标图像对应的激光点云作为所述目标激光点云,并返回所述将所述SFM视觉稀疏点云地图中的地图点与所述目标激光点云中的激光点进行匹配,获得多对匹配点的步骤;
若更新所述SFM地图尺度因子的次数达到所述预设次数,则根据最近所述预设次数更新的所述SFM地图尺度因子,判断所述SFM地图尺度因子是否稳定;
若是,则执行所述针对所述SFM视觉稀疏点云地图中的每个地图点,根据所述SFM尺度因子计算该地图点的绝对坐标的步骤;
若否,则将所述多帧图像中所述目标图像的下一帧图像作为所述目标图像,将所述目标图像对应的激光点云作为所述目标激光点云,并返回所述将所述SFM视觉稀疏点云地图中的地图点与所述目标激光点云中的激光点进行匹配,获得多对匹配点的步骤。
可选的,所述根据最近所述预设次数更新的所述SFM地图尺度因子,判断所述SFM地图尺度因子是否稳定,包括:
确定最近所述预设次数更新的各SFM地图尺度因子的离散程度;
如果所述离散程度是否小于预设离散程度,则确定所述SFM地图尺度因子稳定;
如果所述离散程度大于等于所述预设离散程度,则确定所述SFM地图尺度因子不稳定。
可选的,所述将所述SFM视觉稀疏点云地图中的地图点与所述目标激光点云中的激光点进行匹配,获得多对匹配点,包括:
分别确定所述SFM视觉稀疏点云地图中的每个地图点在所述目标图像上的地图投影点,以及确定所述目标激光点云中的每个激光点在所述目标图像上的激光投影点;
针对每个地图投影点,确定以该地图投影点为中心的第一预设尺寸的第一图像块;
如果所述第一图像块内的激光投影点数量达到预设数量,且所述第一图像块内的激光投影点对应的激光点两两之间的距离小于预设距离,则确定所述第一图像块内的激光投影点对应的激光点的平均坐标;
将该地图投影点对应的地图点与所述目标激光点云中所述平均坐标处的激光点作为一对匹配点。
可选的,所述确定所述目标激光点云中的每个激光点在所述目标图像上的激光投影点,包括:
依次将所述目标激光点云中的每个激光点作为当前激光点;
将所述当前激光点投影至所述目标图像,确定所述当前激光点在所述目标图像上的当前激光投影点;
确定以所述当前激光投影点为中心的第二预设尺寸的第二图像块;
在所述第二图像块内存在除所述当前激光点以外的其他激光投影点时,判断所述第二图像块内,是否存在激光投影点对应的激光点与所述激光雷达之间的距离,小于等于所述当前激光点与所述激光雷达之间的距离;
若存在,则删除所述当前激光投影点,保留所述第二图像块内的其他激光投影点;
若不存在,则删除所述第二图像块内的其他激光投影点,保留所述当前激光投影点;
在所述第二图像块内不存在除所述当前激光点以外的其他激光投影点时,保留所述当前激光投影点。
可选的,所述根据各对匹配点的坐标,计算SFM地图尺度因子,包括:
确定每对匹配点的残差,并计算在确定的各残差总和最小的情况下的SFM地图尺度因子;其中,针对一对匹配点,该对匹配点的残差为:该对匹配点包括的地图点在指定坐标系下的坐标与该对匹配点包括的激光点在所述指定坐标系下的坐标之间的残差。
可选的,所述确定每对匹配点的残差,并计算在确定的各残差总和最小的情况下的SFM地图尺度因子,包括:
通过以下公式计算所述SFM地图尺度因子:
其中,
为所述SFM地图尺度因子,
为匹配点对数,
和
为从所述激光雷达的坐标系到所述相机的坐标系的变换外参数,
和
为从所述SFM视觉稀疏点云地图的坐标系到所述相机的坐标系的变换参数,
为第
对匹配点中的激光点的坐标,
为第
对匹配的点中的地图点的相对坐标。
可选的,在所述分别确定所述SFM视觉稀疏点云地图中的每个地图点在所述目标图像上的地图投影点,以及确定所述目标激光点云中的每个激光点在所述目标图像上的激光投影点之前,所述方法还包括:
对所述目标图像去畸变。
第二方面,本发明实施例提供了一种电子设备,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现上述任一确定SFM地图绝对尺度的方法的步骤。
第三方面,本发明实施例还提供了一种计算机可读存储介质,所述计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述任一确定SFM地图绝对尺度的方法的步骤。
第四方面,本发明实施例还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行上述任一确定SFM地图绝对尺度的方法的步骤。
本发明实施例提供的确定SFM地图绝对尺度的方法及电子设备,可以利用激光雷达扫描得到的激光点云,确定SFM地图尺度因子,由于激光点云具有绝对尺度信息,使得确定的SFM地图尺度因子具有绝对尺度信息。然后利用SFM地图尺度因子,得到SFM视觉稀疏点云地图中的地图点的绝对坐标,即相当于恢复了SFM地图的绝对尺度。而且激光雷达不存在零点偏置,不会引起尺度漂移,因此提高了确定SFM地图绝对尺度的准确度。
当然,实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员基于本申请所获得的所有其他实施例,都属于本发明保护的范围。
本发明实施例可以应用在无人驾驶领域,通过在车辆中安装激光雷达和相机,在车辆行驶过程中,利用相机拍摄车辆行驶过程中路过的场景,得到多帧图像;同时利用激光雷达扫描该场景,得到多个激光点云。其中,激光雷达旋转360°扫描得到的各激光点组成一个激光点云。利用相机拍摄的多帧图像构建运动恢复结构(Structure From Motion,SFM)视觉稀疏点云地图(也可以简称为SFM地图),利用激光雷达获得的激光点云恢复SFM地图的绝对尺度。
相机和激光雷达存在共视面,使得在同一时刻,相机拍摄的图像中与激光雷达获得的激光点云中存在同名点。一种实施方式中,激光雷达通过激光线束获得激光点,激光雷达的激光线束中,靠近中心的激光线束分布密集,远离中心的激光线束分布稀疏。密集分布的激光线束扫描得到的激光点分布密集,稀疏分布的激光线束扫描得到的激光点分布稀疏。可以设置相机拍摄的区域包括激光雷达扫描得到的部分密集分布的激光点对应的区域,即可以设置相机的拍摄角度与激光雷达的扫描角度之间的角度差小于预设角度差。
结合上述应用场景,为了提高确定的SFM地图绝对尺度的准确度,本发明实施例提供了一种确定SFM地图绝对尺度的方法,如图1所示,该方法包括以下步骤:
S101,获取SFM视觉稀疏点云地图和目标激光点云。
其中,SFM视觉稀疏点云地图基于相机拍摄指定场景得到的多帧图像构建。目标激光点云为:相机拍摄多帧图像的其中一帧目标图像时,激光雷达扫描指定场景得到的激光点云。可选的,指定场景可以是汽车行驶过程中所处的场景。
在本发明实施例中,在激光雷达的扫描周期与相机的拍摄周期相同时,可以确定同一个周期内获得的激光点云和图像对应。
在激光雷达的扫描周期与相机的拍摄周期不同时,由于激光点云和图像均存在时间戳,其中,激光点云的时间戳用于表示激光点云的获得时刻,图像的时间戳用于表示图像的获得时刻。在图像的时间戳与激光点云的时间戳之间的差值不超过预设差值时,确定该图像与该激光点云对应。例如,预设差值为30毫秒。
由于相机拍摄得到图像的时刻和激光雷达扫描360°得到激光点云的时刻可能不同,如果车辆处于行进中,可能导致相机的拍摄场景与激光雷达的扫描场景不同。为提高确定匹配点的准确度,进而提高确定SFM地图绝对尺度的准确度,可以设置在车辆静止时,相机拍摄图像以及激光雷达扫描得到激光点云。
S102,将SFM视觉稀疏点云地图中的地图点与目标激光点云中的激光点进行匹配,获得多对匹配点。
其中,一对匹配点包括激光点云中的一个激光点和SFM视觉稀疏点云地图中的一个地图点。
一种实施方式中,可以将SFM视觉稀疏点云地图中的地图点和目标激光点云中的激光点均转移到相同的坐标系中进行匹配,该坐标系可以是2维(dimension,D)坐标系,也可以是3D坐标系。例如,该坐标系可以是目标图像的坐标系,目标图像的坐标系是2D坐标系。或者,该坐标系可以是相机坐标系,相机坐标系是3D坐标系。或者该坐标系可以是激光雷达坐标系,激光雷达坐标系是3D坐标系。或者该坐标系也可以是其他坐标系,本发明实施例对此不作具体限定。
S103,根据各对匹配点的坐标,计算SFM地图尺度因子。
其中,SFM地图尺度因子用于表示SFM视觉稀疏点云地图中的地图点的相对坐标与绝对坐标之间的转换关系。
S104,针对SFM视觉稀疏点云地图中的每个地图点,根据SFM尺度因子计算该地图点的绝对坐标,并将该地图点的相对坐标更新为该地图点的绝对坐标。
一种实施方式中,可以针对每个地图点,将SFM尺度因子s乘以该地图点的相对坐标(x,y,z),得到该地图点的绝对坐标(sx,sy,sz)。其中,地图点的相对坐标和绝对坐标均是地图点在SFM地图的坐标系下的坐标。
可以理解的,根据地图点的相对坐标,可以得到地图点之间的相对距离。例如地图点1的相对坐标为(1,1,1),地图点2的相对坐标为(2,2,2),可以看出,地图点1与SFM地图原点之间的距离比地图点2与SFM地图原点之间的距离更短,但不能获得地图点所表示的物理位置之间的实际距离。
而一个地图点的绝对坐标可以表示该地图点所表示的物理位置与SFM地图原点所表示的物理位置之间的实际距离。例如,激光点云具有绝对尺度信息(即激光点的坐标能够表示激光点对应的物理位置与激光雷达之间的实际距离),假设激光点之间的实际距离以米为单位度量,则利用该激光点云恢复绝对尺度的SFM地图中地图点之间的距离可以以米为单位度量。假设一个地图点的绝对坐标为(1,2,3),则1表示该地图点与SFM地图原点的横向距离为1米,2表示该地图点与SFM地图原点的竖向距离为2米,3表示该地图点与SFM地图原点的高度距离为3米。SFM地图的坐标系为3D坐标系。
其中,SFM地图原点表示的物理位置为相机拍摄上述多帧图像中的第一帧图像时,相机的中心点所处的物理位置。地图点之间的实际距离可以利用常规的长度单位度量,例如可以使用单位米度量地图点之间的实际距离。
本发明实施例提供的确定SFM地图绝对尺度的方法,可以利用激光雷达扫描得到的激光点云,确定SFM地图尺度因子,由于激光点云具有绝对尺度信息,使得确定的SFM地图尺度因子具有绝对尺度信息。然后利用SFM地图尺度因子,得到SFM视觉稀疏点云地图中的地图点的绝对坐标,即相当于恢复了SFM地图的绝对尺度。而且激光雷达不存在零点偏置,不会引起尺度漂移,因此提高了确定SFM地图绝对尺度的准确度。
针对上述S102,参见图2,可以通过以下步骤获得匹配点:
S1021,分别确定SFM视觉稀疏点云地图中的每个地图点在目标图像上的地图投影点,以及确定目标激光点云中的每个激光点在目标图像上的激光投影点。
一种实施方式中,根据SFM视觉稀疏点云地图的坐标系到相机的坐标系的变换参数,确定SFM视觉稀疏点云地图中的每个地图点在相机坐标系下的坐标。根据相机的内参矩阵以及各地图点在相机坐标系下的坐标,确定SFM视觉稀疏点云地图中的每个地图点在目标图像上的地图投影点。
并根据激光雷达的坐标系到相机的坐标系的变换外参数,确定目标激光点云中的每个激光点在相机坐标系下的坐标。根据相机的内参矩阵以及各激光点在相机坐标系下的坐标,确定目标激光点云中的每个激光点在目标图像上的激光投影点。
在本发明实施例中,激光点云中的激光点的坐标为3D坐标,SFM视觉稀疏点云地图中的地图点的坐标为3D坐标,相机坐标系下的坐标为3D坐标,目标图像上的地图投影点和激光投影点的坐标为2D坐标。
相机经过镜头成像时,由于相机的镜头是凸透镜,使得相机的视场内不同区域形成的影像的放大倍率不同,形成成像画面的变形,越靠近成像边缘的画面变形越严重。因此在S102之前,还可以对目标图像去畸变,以减少目标图像中画面的变形对于确定匹配点的影响,提高确定匹配点的准确性。例如可以使用OpenCV中的矫正算法对目标图像去畸变,其中,OpenCV是一个开源发行的跨平台计算机视觉和机器学习软件库。
此时执行S1021时,分别确定SFM视觉稀疏点云地图中的每个地图点在去畸变后的目标图像上的地图投影点,以及确定目标激光点云中的每个激光点在去畸变后的目标图像上的激光投影点。
S1022,针对每个地图投影点,确定以该地图投影点为中心的第一预设尺寸的第一图像块。
例如,第一预设尺寸为5像素(pixel,px)×5px。
S1023,如果第一图像块内的激光投影点数量达到预设数量,且第一图像块内的激光投影点对应的激光点两两之间的距离小于预设距离,则确定第一图像块内的激光投影点对应的激光点的平均坐标。
其中,激光投影点对应的激光点两两之间的距离为:各激光投影点对应的激光点中,在激光雷达的坐标系下的两两之间的距离。例如,预设数量为3,预设距离为3cm。
S1024,将该地图投影点对应的地图点与目标激光点云中平均坐标处的激光点作为一对匹配点。
例如,如图3所示,图3中的白色点为SFM视觉稀疏点云地图中的各地图点在目标图像上的地图投影点。如图4所示,图4中的白色的尺寸较小的点为SFM视觉稀疏点云地图中的各地图点在目标图像上的地图投影点,图4中白色的尺寸较大的粗大点为与地图点匹配的激光点的激光投影点。
由于激光雷达和相机的安装位置存在一定距离,使得激光雷达得到的激光点云投影到目标图像上时,存在激光投影点之间相互遮挡的情况,因此在确定激光投影点时,还可以对激光投影点进行去遮挡处理,从而提升确定匹配点的准确性。基于此,参见图5,S1021确定激光投影点可以实现为以下步骤:
S10211,依次将目标激光点云中的每个激光点作为当前激光点。
S10212,将当前激光点投影至目标图像,确定当前激光点在目标图像上的当前激光投影点。
一种实施方式中,根据激光雷达的坐标系到相机的坐标系的变换外参数,确定当前激光点在相机坐标系下的坐标。根据相机的内参矩阵以及当前激光点在相机坐标系下的坐标,确定当前激光点在目标图像上的当前激光投影点。
S10213,确定以当前激光投影点为中心的第二预设尺寸的第二图像块。
在本发明实施例中,第二预设尺寸可以与第一预设尺寸相同,也可以与第一预设尺寸不同。例如第二预设尺寸为5px×5px。
S10214,在第二图像块内存在除当前激光点以外的其他激光投影点时,判断第二图像块内,是否存在激光投影点对应的激光点与激光雷达之间的距离,小于等于当前激光点与激光雷达之间的距离。若存在,则执行S10215;若不存在,则执行S10216。
激光点与激光雷达之间的距离,可以根据激光点在激光雷达的坐标系下的坐标确定。
S10215,删除当前激光投影点,保留第二图像块内的其他激光投影点。
可以理解的,当第二图像块内,存在激光投影点对应的激光点与激光雷达之间的距离,小于等于当前激光点与激光雷达之间的距离时,说明第二图像块内可能存在遮挡当前激光投影点的激光投影点,因此为避免激光投影点相互遮挡的情况,可以删除当前激光投影点,保留第二图像块内除当前激光投影点以外的其他激光投影点。
S10216,删除第二图像块内的其他激光投影点,保留当前激光投影点。
可以理解的,当第二图像块内,不存在激光投影点对应的激光点与激光雷达之间的距离,小于等于当前激光点与激光雷达之间的距离时,说明第二图像块内的激光投影点对应的激光点与激光雷达之间的距离,均大于当前激光点与激光雷达之间的距离,此时当前激光投影点可能遮挡其他激光投影点,因此为避免激光投影点相互遮挡的情况,可以删除第二图像块内除当前激光投影点以外的其他的激光投影点,保留当前激光投影点。
S10217,在第二图像块内不存在除当前激光点以外的其他激光投影点时,保留当前激光投影点。
可以理解的,在第二图像块内不存在除当前激光投影点以外的其他激光投影点时,不存在激光投影点之间相互遮挡的情况,因此可以保留当前激光投影点。
例如,对激光投影点进行去除遮挡处理的前后对比如图6所示,图6的左图中的白色点为未去除遮挡的激光投影点,图6的右图中的白色点为去除遮挡后的激光投影点。从图6的左图中可以看出,未去除遮挡时,左边的车辆中间位置对应的激光点,在目标图像上的激光投影点位于遮挡该车辆的柱子上,导致这些激光投影点的位置不准确。而从图6的右图中可以看出,在去除遮挡后,左边的车辆中间位置对应的激光点的激光投影点被删除,避免了这些激光投影点对于确定匹配点的影响。
由于本发明实施例能够在确定激光投影点时,对激光投影点进行去遮挡处理,有效地避免了激光投影点之间相互遮挡的情况,进而避免了相互遮挡的激光投影点对于确定匹配点的影响,提高了确定匹配点的准确性。
针对上述S103,计算SFM地图尺度因子的方式可以实现为:确定每对匹配点的残差,并计算在确定的各残差总和最小的情况下的SFM地图尺度因子。其中,针对一对匹配点,该对匹配点的残差为:该对匹配点包括的地图点在指定坐标系下的坐标与该对匹配点包括的激光点在指定坐标系下的坐标之间的残差。
在本发明实施例中,在相同的坐标系下计算匹配点的残差。因此指定坐标系可以是相机的坐标系,也可以是激光雷达的坐标系,或者可以是SFM视觉稀疏点云地图和激光雷达均可变换到的坐标系。
一种实施方式中,以指定坐标系为相机的坐标系为例,可以通过公式(1)计算SFM地图尺度因子s:
其中,
为SFM地图尺度因子,
为匹配点对数,
和
为从激光雷达的坐标系到相机的坐标系的变换外参数,
和
为从SFM视觉稀疏点云地图的坐标系到相机的坐标系的变换参数,
为第
对匹配点中的激光点的坐标,
为第
对匹配的点中的地图点的相对坐标。
可以看出,公式(1)的目的是最小化匹配的地图点与激光点之间的残差的平方和,即上述公式(1)为最小二乘问题,可以通过高斯牛顿迭代法(Gauss-Newton iterationmethod)或者列文伯格-马夸尔特(Levenberg-Marquardt)优化算法求解公式(1),得到SFM地图尺度因子s。
其中,高斯牛顿迭代法和列文伯格-马夸尔特优化算法的求解思想均是:选定SFM地图尺度因子的初始值,计算公式(1)在SFM地图尺度因子的初始值处的值,然后通过多次迭代,多次修正SFM地图尺度因子,使得公式(1)在SFM地图尺度因子处的值减小,在迭代过程中SFM地图尺度因子不断接近上述公式(1)的最优解,最终得到公式(1)的最优解。在SFM地图尺度因子为最优解时,匹配的地图点与激光点之间的残差的平方和最小。
可选的,由于确定的匹配点中可能存在错误的匹配点,为减少被误确认为匹配的匹配点对于计算SFM地图尺度因子s带来的影响,在计算SFM地图尺度因子s时,可以利用柯西损失函数减少离群点(即错误的匹配点)的影响。其中柯西损失函数的思想是:在求解公式(1)的迭代过程中,对残差较大的匹配点进行约束,减少残差较大的匹配点中地图点与激光点之间的残差的平方(例如可以取该平方的一半),即减少了错误的匹配点对于求解公式(1)的影响。
由于本发明实施例能够确定在匹配点的残差最小时的SFM地图尺度因子,使得利用该SFM地图尺度因子得到的地图点的绝对坐标,与该地图点匹配的激光点的坐标之间的残差最小,由于激光点云具有绝对尺度,使得激光点的坐标准确,因此利用该方式得到的地图点的绝对坐标更准确,即对SFM地图恢复的尺度更准确。
参见图7,为了进一步优化SFM地图尺度因子,提高SFM地图的绝对尺度的准确性,本发明实施例在上述S103计算SFM地图尺度因子之后,还可以通过以下步骤更新SFM地图尺度因子:
S701,根据SFM尺度因子,分别确定SFM视觉稀疏点云地图中的每个地图点在指定坐标系下的坐标。
S702,分别确定目标激光点云中的每个激光点在指定坐标系下的坐标。
S703,针对每一对匹配点,判断该对匹配点的残差是否大于预设残差。若是,则执行S704;若否,则执行S705。
其中,该对匹配点的残差为:该对匹配点包括的地图点在指定坐标系下的坐标与该对匹配点包括的激光点在指定坐标系下的坐标之间的残差。计算匹配点的残差的步骤与上述S103中计算残差的方式相同,可参考上述描述,此处不再赘述。
以指定坐标系为相机坐标系为例,相机坐标系是3D坐标系,可以判断一对匹配点在相机坐标系下的残差是否大于0.1米。
S704,忽略该对匹配点。
S705,保留该对匹配点。
S706,根据保留的各对匹配点的坐标,更新SFM地图尺度因子。
更新SFM地图尺度因子时,将当前的SFM地图尺度因子作为初始值,利用高斯牛顿迭代法或者列文伯格-马夸尔特优化算法,多次迭代并多次修正SFM地图尺度因子,得到上述公式(1)的最优解,将该最优解作为更新后的SFM地图尺度因子。
求解公式(1)最优解的具体过程可参考上述S103的相关描述,此处不再赘述。
由于本发明实施例能够将残差较大的匹配点剔除,即在计算SFM地图尺度因子时,不考虑误差较大的匹配点,避免了误差较大的匹配点对于计算SFM地图尺度因子的影响,因此提高了确定SFM地图尺度因子的准确性。
在上述S104针对SFM视觉稀疏点云地图中的每个地图点,根据SFM尺度因子计算该地图点的绝对坐标之前,还可以通过以下步骤确定是否利用当前计算的SFM地图尺度因子恢复SFM地图的绝对尺度:
步骤1,若更新SFM地图尺度因子的次数未达到预设次数,则将多帧图像中目标图像的下一帧图像作为目标图像,将目标图像对应的激光点云作为目标激光点云,并返回S102。
在执行S102及其之后的步骤时,基于当前确定的目标图像和目标激光点云执行。此时确定的目标激光点云为:相机拍摄当前确定的目标图像时,激光雷达扫描指定场景得到的激光点云。
步骤2,若更新SFM地图尺度因子的次数达到预设次数,则根据最近预设次数更新的述SFM地图尺度因子,判断SFM地图尺度因子是否稳定。若是,则执行S104;若否,则执行步骤3。
一种实施方式中,可以确定最近预设次数更新的各SFM地图尺度因子的离散程度。如果离散程度是否小于预设离散程度,则确定SFM地图尺度因子稳定;如果离散程度大于等于预设离散程度,则确定SFM地图尺度因子不稳定。
例如,预设次数为10次,可以计算最近10次更新的各SFM地图尺度因子的标准差,并判断计算的标准差是否大于预设标准差(例如预设标准差为10-3)。若是,则确定SFM地图尺度因子稳定;若否,则确定SFM地图尺度因子不稳定。
除了利用标准差确定SFM地图尺度因子的离散程度以外,还可以利用方差或者平均差等计算方式,本发明实施例对此不作具体限定。
步骤3,将多帧图像中目标图像的下一帧图像作为目标图像,将目标图像对应的激光点云作为目标激光点云,并返回S102。
在执行S102及其之后的步骤时,基于当前确定的目标图像和目标激光点云执行。
本发明实施例能够在SFM地图尺度因子稳定时,利用当前计算的SFM地图尺度因子恢复SFM地图的绝对尺度。由于SFM地图尺度因子稳定则表示当前计算的SFM地图尺度因子准确性较高,减少了匹配误差带来的影响。
针对S104,可以在SFM地图尺度因子稳定时,利用当前计算的SFM地图尺度因子,确定地图点的绝对坐标,得到SFM地图的绝对尺度。
在本发明实施例中,在SFM地图尺度因子稳定时,还可以针对构建SFM视觉稀疏点云地图的多帧图像,将相机拍摄这多帧图像的每张图像时位姿的平移部分乘以SFM地图尺度因子s,从而更准确地确定地图点在图像上的地图投影点。
参见图8,以下通过一个实例对本发明实施例提供的确定SFM地图绝对尺度的方法的整体流程进行说明:
S801,获取目标图像和目标激光点云。
S802,对目标图像去畸变,并确定SFM视觉稀疏点云地图中的每个地图点在去畸变后的目标图像上的地图投影点,以及确定目标激光点云中的每个激光点在去畸变后的目标图像上的激光投影点。
S802的实现方式与S1021记载的方式相同,可参考S1021的相关描述,此处不再赘述。
S803,将SFM视觉稀疏点云地图中的地图点与目标激光点云中的激光点进行匹配,获得多对匹配点。
S803的实现方式与上述S102记载的方式相同,可参考S102的相关描述,此处不再赘述。
S804,根据各对匹配点的坐标,利用列文伯格-马夸尔特(Levenberg-Marquardt)优化算法,计算SFM地图尺度因子。
S804实现方式与上述S103记载的方式相同,可参考S103的相关描述,此处不再赘述。
S805,忽略残差大于预设残差的匹配点,保留残差小于等于预设残差的匹配点,并根据保留的各对匹配点的坐标,更新SFM地图尺度因子。
更新SFM地图尺度因子的方式与上述步骤一至步骤六记载的方式相同,可参考上述描述,此处不再赘述。
S806,判断更新SFM地图尺度因子的次数是否达到预设次数。若否,则执行S807;若是,则执行S808。
S807,将多帧图像中目标图像的下一帧图像作为目标图像,将目标图像对应的激光点云作为目标激光点云,并返回S802。
S808,根据最近预设次数更新的SFM地图尺度因子,判断SFM地图尺度因子是否稳定。若否,则执行S809;若是,则执行S810。
S808的实现方式与上述步骤2记载的方式相同,可参考上述步骤2中的相关描述,此处不再赘述。
S809,将多帧图像中目标图像的下一帧图像作为目标图像,将目标图像对应的激光点云作为目标激光点云,并返回S802。
S810,针对SFM视觉稀疏点云地图中的每个地图点,根据SFM尺度因子计算该地图点的绝对坐标,并将该地图点的相对坐标更新为该地图点的绝对坐标。
S810的实现方式与上述S104记载的方式相同,可参考上述S104中的相关描述,此处不再赘述。
与利用轮速计恢复SFM地图绝对尺度的方式相比,轮速计利用车辆后轮转速,推测车辆行驶速度,进而利用车辆行驶速度恢复SFM地图绝对尺度。由于车辆在转向时,左右车轮的转速存在差异,使得轮速计确定的车辆速度不准确,且车辆长距离的行驶的过程中,轮速计还存在累计误差,导致确定SFM地图绝对尺度不准确。
与利用全球定位系统(Global Positioning System,GPS)恢复SFM地图绝对尺度的方式相比,利用GPS信号确定车辆行驶速度,进而利用车辆行驶速度恢复SFM地图绝对尺度。由于GPS信号的噪声较大,使得确定车辆的行驶速度不准确,进而导致确定SFM地图绝对尺度不准确。
而本发明实施例利用激光雷达恢复SFM地图绝对尺度,不用确定车辆的行驶速度,避免了确定车辆行驶速度不准确带来的误差,因此本发明实施例确定的SFM地图绝对尺度更准确。
基于相同的发明构思,对应于上述方法实施例,本发明实施例还提供了一种电子设备,如图9所示,包括处理器901、通信接口902、存储器903和通信总线904,其中,处理器901,通信接口902,存储器903通过通信总线904完成相互间的通信,
存储器903,用于存放计算机程序;
处理器901,用于执行存储器903上所存放的程序时,实现上述方法实施例中的方法步骤。
上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect,PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture,EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示,图中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory,RAM),也可以包括非易失性存储器(Non-Volatile Memory,NVM),例如至少一个磁盘存储器。可选的,存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)等;还可以是数字信号处理器(Digital SignalProcessor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中,还提供了一种计算机可读存储介质,该计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述任一确定SFM地图绝对尺度的方法的步骤。
在本发明提供的又一实施例中,还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行上述实施例中任一确定SFM地图绝对尺度的方法。
在上述实施例中,可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时,可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时,全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中,或者从一个计算机可读存储介质向另一个计算机可读存储介质传输,例如,所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质,(例如,软盘、硬盘、磁带)、光介质(例如,DVD)、或者半导体介质(例如固态硬盘Solid State Disk (SSD))等。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述,各个实施例之间相同相似的部分互相参见即可,每个实施例重点说明的都是与其他实施例的不同之处。尤其,对于装置实施例而言,由于其基本相似于方法实施例,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等,均包含在本发明的保护范围内。