具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
图1示出了本发明实施例提供的一种多传感器的标定方法的流程示意图。该方法用于对第一传感器和第二传感器进行外参数标定,其中,第一传感器和第二传感器固定于所需的应用设备上,第一传感器可以为图像传感器,图像传感器包括但不限于单目相机、环视相机、双目相机、结构光相机等可以用的图像传感器;第二传感器可以为激光雷达传感器,激光雷达传感器包括但不限于多线激光雷达、毫米波雷达、微波雷达、Flash雷达、MEMS雷达、相控阵雷达等能够生成三维点云的传感器。
如图1所示,该方法包括:
步骤110、确定标定板上的第一点集在标定板坐标系下的第一坐标。
其中,标定板设置于第一传感器和第二传感器均能观测到的位置。标定板可以为棋盘格,棋盘格可以为黑白方格交替设置的标准尺寸棋盘平面标定板。可选地,标定板的形状可以为矩形,还可以为各行角点数有差异的其他形状,例如梯形、三角形等等。通过设置各行角点数有差异的标定板,能够使得标定板的特征点更明显,从而提高点云匹配的精度。
在本实施例中,标定板上的第一点集为棋盘格的角点。当然,在一些其他实施例中,第一点集还可以为棋盘格的方格的中点等等,可以根据实际使用进行设置。
在步骤110中,确定标定板上的第一点集在标定板坐标系下的第一坐标,具体可以为:建立三维的标定板坐标系,获取第一点集中各个点在标定板坐标系的坐标,从而得到第一坐标。例如,以标定板的左上角为圆点、长边延伸的方向为x轴、宽边延伸的方向为y轴、垂直标定板的方向为z轴建立标定板坐标系,获取的第一坐标包括(1,-1,1)、(2,-1,1)、(3,-1,1)等等。
步骤130、根据第一坐标和第一点集在第一传感器坐标系下的第二坐标,确定标定板坐标系与第一传感器坐标系之间的变换关系。
其中,需要先确定第一点集在第一传感器坐标系下的第二坐标,则在步骤130之前,该方法还包括:
步骤121、获取第一点集在第一传感器平面坐标系下的第六坐标;
步骤122、根据第六坐标以及第一传感器的内参数,确定第二坐标。
其中,第一传感器平面坐标系是指像素坐标系,例如以图像中心为原点建立的像素坐标系。则第六坐标为二维坐标。获取第一点集在第一传感器平面坐标系下的第六坐标,具体实施方式可以为:开启第一传感器,通过第一传感器拍摄标定板上的第一点集,获得拍摄图像,根据拍摄图像建立第一传感器平面坐标系,从而获得第六坐标。
其中,第一传感器坐标系是指考虑第一传感器的内参数后在第一传感器平面坐标系基础上建立的三维坐标系。例如,第一传感器平面坐标系以图像中心为原点建立,第一传感器坐标系以相机光心为原点建立。第一传感器的内参数可以包括焦距参数和光心参数。根据第六坐标和第一传感器的内参数,能够计算得到第二坐标。例如,假设像素坐标系上的第六坐标为Mab=[a,b],计算得到的第二坐标为N=[Xi,Yi,Zi],则第一传感器坐标系与第一传感器平面坐标系下的点的坐标的关系式为:
则求解得:
其中,fx、fy为焦距参数,分别为第一传感器x轴方向的焦距和y轴方向的焦距,cx、cy为光心参数,分别为第一传感器x轴方向的光心和y轴方向的光心;d为相机测出来的深度值,即真实点到相机平面的距离。
在步骤130中,在确定第一坐标和第二坐标后,可以通过PNP(pespective-n-point)方法,确定标定板坐标系与第一传感器坐标系之间的变换关系。具体可以为:
将第一坐标表示为第一点集的其中一个点的第二坐标表示为/>(即令d=1,Xi为μ1,Yi为ν1),则有:
假设:
则有:
当第一点集包括n个点时,则:
获取若干个第一点集的第一坐标和第二坐标,通过匹配第一坐标和第二坐标,并通过奇异值分解(Singular Value Decomposition,SVD)求解上述方程,以求得t1~t12,从而计算得到标定板坐标系与所述第一传感器坐标系之间的变换关系Twc:
其中,由于镜头的光轴(通过镜头中心垂直于传感器平面的直线)应该是穿过图像的正中间的,但是,实际由于安装精度的问题,会存在误差,这种误差需要用内参数进行描述。相机的x方向和y方向的缩小比例应该是一样的,但实际上,这两个方向的缩小比例不一致,内参数可以用来描述这两个方向的缩放比例。在本实施例中,fx、fy为焦距参数,分别为第一传感器x轴方向的焦距和y轴方向的焦距,cx、cy为光心参数,分别为第一传感器x轴方向的光心和y轴方向的光心。
需要说明的是,在本实施例中,第一点集可以包括多个角点,则相应地,第一坐标、第六坐标、第二坐标为第一点集中多个角点在不同坐标系的坐标的集合。
步骤140、扩展第一点集,并确定扩展后的第一点集在标定板坐标系下的第三坐标。
由于标定板上的角点比较稀疏,直接用于点云匹配误差会很大,因此对第一点集进行扩展。扩展第一点集,具体包括:按照预设分辨率扩展第一点集,并使扩展后的第一点集等间距分布。其中,预设分辨率为预先设置的分辨率,可以由用户根据实际需求进行设置,在一定范围内,设置的分辨率越高,则匹配误差越小。例如,如图2a和图2b所示,对标定板上的各个角点进行扩展,生成稠密的点集。第三坐标包括扩展后的第一点集中的若干扩展点在标定板坐标系下的坐标。
其中,由于扩展后的第一点集位于标定板上,因此,只改变了第一坐标中的x坐标和y坐标,而z坐标不变,并且扩展后的第一点集是等间距分布的,因此每个扩展点在标定板坐标系下的坐标是可知的。生成的扩展后的第一点集可以表示为:
Pw={x,y|x∈[0,length/res],y∈[0,width/res]}
其中,length为标定板实际长度,width为标定板实际宽度,res为扩展后点云的分辨率。
步骤150、根据变换关系和第三坐标,确定扩展后的第一点集在第一传感器坐标系下的第四坐标。
根据变换关系Twc和各个扩展点在标定板坐标系下的第三坐标Pw,可以计算得到各个扩展点在第一传感器坐标系下的第四坐标Pc,表示为:
Pc=Twc -1*Pw
步骤160、获取第二点集在第二传感器坐标系下的第五坐标,其中,第二点集部分或者全部位于标定板。
其中,当第二传感器为激光雷达时,第二点集可以为激光雷达对标定板或者标定板以及标定板以外区域进行扫描的激光点集(如图3所示)。当激光雷达只对标定板进行扫描时,第二点集全部位于标定板;当激光雷达对标定板以及标定板以外区域进行扫描时,第二点集部分位于标定板,另一部分位于标定板以外区域。
其中,第二传感器坐标系可以为以激光雷达中心建立的三维坐标系。可选地,在一些其他实施例中,第二传感器还可以为其他传感器,第二点集还可以为标定板上的角点。
在本实施例中,如图4所示,步骤160具体包括:
步骤161、通过第二传感器获取大于标定板区域的点云信息;
步骤162、从点云信息获取第五坐标。
在步骤161中,通过第二传感器获取大于标定板区域的点云信息,具体实施方式可以为:在激光雷达稳定运行时,使激光雷达扫描标定板,并且使所有激光线束覆盖整个标定板,激光雷达接收返回的激光线束,获取标定板以及标定板周边区域的点云信息,从而获取大于标定板区域的点云信息。
在步骤162中,由于激光雷达具有测距功能,点云信息携带有各个点的距离信息,因此从点云信息获取第五坐标。
步骤170、根据第四坐标和第五坐标,确定第一传感器和第二传感器之间的外参数。
其中,当第一传感器为相机、第二传感器为激光雷达时,第四坐标实际上为扩展后的点集在相机坐标系下的坐标,即,相机点云中的点集的坐标;第五坐标实际上为激光点云中的第二点集的坐标。则步骤170具体包括:根据第四坐标和第五坐标,基于迭代最近点算法计算得到第一传感器和第二传感器之间的变换矩阵;根据变换矩阵确定第一传感器和第二传感器之间的外参数。
其中,采用迭代最近点(Iterative Closest Point,ICP)算法进行第四坐标和第五坐标的匹配,从而确定第一传感器和第二传感器之间的外参数。ICP算法具体实施方式可以为:第一步,筛选点对,即在相机点云中选取点pi,并在第二点集中搜索出距离选取点pi最近的点qi(例如可以遍历第五坐标,计算第四坐标中的选取点与第五坐标中各个点的距离,通过不断比较,找到距离选取点最近的点),组成一个点对,找出两个点集中所有的点对,点对集合相当于进行有效计算的两个新点集;第二步,根据点对集,即两个新点集(即新的相机点云和新的第二点集),计算两个新点集的重心位置坐标;第三步,根据新点集计算旋转矩阵R和平移矩阵T;第四步,根据旋转矩阵R
′和平移矩阵T,计算第一点集中的点pi进行刚体变换之后的新第一点集中pi″的坐标,并计算从新的相机点云中的pi的坐标到新的第二点集中的qi的坐标的距离平方和,以连续两次距离平方和之差的绝对值作为是否收敛的依据,若小于设定的阈值,则停止迭代;第五步,重复第一步至第四步,直至收敛或达到既定的迭代次数。
在本实施例中,首先,在相机点云中选取点集pi∈Pc,并在第二点集中选取对应的点集qi∈Pl;然后,通过使得误差函数E(R,t)最小计算旋转矩阵R和平移矩阵T,其中,可以设置一个误差函数阈值,当误差函数小于误差函数阈值,则认为误差函数最小,其中:
再者,对pi使用旋转矩阵R和平移矩阵T进行旋转和平移转换,得到新的相机点云为:
Pi′={p′i=R·pi+t,pi∈Pc}
计算p′i与对应点集q′i的平均距离:
若d小于设定的阈值或者达到预设的迭代次数,则停止迭代,否则重复进行迭代,直至满足收敛条件。
其中,计算得到的旋转矩阵R和平移矩阵T,即可表述第一传感器坐标系和第二传感器坐标系之间的变换关系,即确定了第一传感器和第二传感器之间的外参数。
需要说明的是,步骤130和步骤140的顺序可以倒换,例如先执行步骤140再执行步骤130,或者先执行步骤130再执行步骤140,步骤160也可以在步骤110、步骤130、步骤140和步骤150前执行,均能实现本方案,并取得相同的计算效果。
本实施例通过确定标定板上的第一点集在标定板坐标系下的第一坐标;根据第一坐标和第一点集在第一传感器坐标系下的第二坐标,确定标定板坐标系与第一传感器坐标系之间的变换关系;扩展第一点集,并确定扩展后的第一点集在标定板坐标系下的第三坐标;根据变换关系和第三坐标,确定扩展后的第一点集在第一传感器坐标系下的第四坐标;获取第二点集在第二传感器坐标系下的第五坐标,其中,第二点集部分或者全部位于标定板;根据第四坐标和第五坐标,确定第一传感器和第二传感器之间的外参数,能够减少点云匹配误差,提高点云匹配精度,从而提高标定效果。
图5示出了本发明实施例提供的一种多传感器的标定方法的应用实例的流程示意图。如图5所示,该方法包括:
步骤201、确定标定板上的角点在标定板坐标系下的第一坐标。
其中,标定板长70厘米、宽80厘米,标定板上的每个格子的宽为10厘米。以标定板的左上角为原点建立标定板坐标系,角点在标定板坐标系的第一坐标为
步骤202、获取角点在图像传感器平面坐标系下的第六坐标。
步骤203、根据第六坐标以及图像传感器的内参数,确定第二坐标。
其中,通过角点检测的方法得到角点在图像传感器坐标系的第二坐标
步骤204、根据第一坐标和第二坐标,确定标定板坐标系与图像传感器坐标系之间的变换关系。
其中,根据第一坐标和第二坐标,计算得到标定板坐标系与图像传感器坐标系之间的变换关系Twc。
步骤205、扩展角点,并确定扩展后的角点在标定板坐标系下的第三坐标。
具体地,按照预设分辨率为1mm进行扩展,则扩展后的角点在标定板坐标系的坐标为:
Pw={x,y|x∈[0,700],y∈[0,800]}
步骤206、根据变换关系和第三坐标,确定扩展后的角点在图像传感器坐标系下的第四坐标。
具体地,图像传感器坐标系下的扩展后的角点的坐标为:Pc=Twc -1*Pw。
步骤207、使激光雷达传感器扫描标定板,通过激光雷达传感器获取激光点云在激光雷达传感器坐标系下的第五坐标。
其中,第五坐标记为Pl。
步骤208、根据第四坐标和第五坐标,通过ICP算法计算图像传感器和激光雷达传感器之间的变换矩阵[R,T],从而确定图像传感器和激光雷达传感器之间的外参数。其中,ICP算法在上文中已经详细描述,此处不再赘述。
本实施例通过确定标定板上的第一点集在标定板坐标系下的第一坐标;根据第一坐标和第一点集在第一传感器坐标系下的第二坐标,确定标定板坐标系与第一传感器坐标系之间的变换关系;扩展第一点集,并确定扩展后的第一点集在标定板坐标系下的第三坐标;根据变换关系和第三坐标,确定扩展后的第一点集在第一传感器坐标系下的第四坐标;获取第二点集在第二传感器坐标系下的第五坐标,其中,第二点集部分或者全部位于标定板;根据第四坐标和第五坐标,确定第一传感器和第二传感器之间的外参数,能够减少点云匹配误差,提高点云匹配精度,从而提高标定效果。
图6示出了本发明实施例提供的一种多传感器的标定装置的结构示意图。如图6所示,该装置300包括:第一坐标确定模块310、变换关系确定模块330、扩展模块340、第四坐标确定模块350、第五坐标确定模块360和标定模块370。
其中,第一坐标确定模块310用于确定标定板上的第一点集在标定板坐标系下的第一坐标;变换关系确定模块330用于根据所述第一坐标和所述第一点集在第一传感器坐标系下的第二坐标,确定所述标定板坐标系与第一传感器坐标系之间的变换关系;扩展模块340用于扩展所述第一点集,并确定扩展后的第一点集在所述标定板坐标系下的第三坐标;第四坐标确定模块350用于根据所述变换关系和所述第三坐标,确定所述扩展后的第一点集在所述第一传感器坐标系下的第四坐标;第五坐标确定模块360用于获取第二点集在第二传感器坐标系下的第五坐标,其中,所述第二点集部分或者全部位于所述标定板;标定模块370用于根据所述第四坐标和所述第五坐标,确定所述第一传感器和所述第二传感器之间的外参数。
在一种可选的方式中,该装置300还包括:第六坐标获取模块321和第二坐标确定模块322。第六坐标获取模块321用于获取所述第一点集在第一传感器平面坐标系下的第六坐标;第二坐标确定模块322用于根据所述第六坐标以及所述第一传感器的内参数,确定所述第二坐标。
在一种可选的方式中,扩展模块340具体用于按照预设分辨率扩展所述第一点集,并使所述扩展后的第一点集等间距分布。
在一种可选的方式中,第五坐标确定模块360具体用于:通过所述第二传感器获取大于所述标定板区域的点云信息;从所述点云信息获取所述第五坐标。
在一种可选的方式中,所述标定板为棋盘格,所述第一点集为所述棋盘格的角点。
在一种可选的方式中,所述标定板的形状为矩形或者各行角点数有差异的形状。
在一种可选的方式中,所述第一传感器为图像传感器,所述第二传感器为激光雷达、毫米波雷达或微波雷达。
在一种可选的方式中,标定模块370具体用于:根据所述第四坐标和所述第五坐标,基于迭代最近点算法计算得到所述第一传感器和所述第二传感器之间的变换矩阵;根据所述变换矩阵确定所述第一传感器和所述第二传感器之间的外参数。
需要说明的是,本发明实施例提供的多传感器的标定装置是能够执行上述多传感器的标定方法的装置,则上述基于多传感器的标定方法的所有实施例均适用于该装置,且均能达到相同或相似的有益效果。
本实施例通过确定标定板上的第一点集在标定板坐标系下的第一坐标;根据第一坐标和第一点集在第一传感器坐标系下的第二坐标,确定标定板坐标系与第一传感器坐标系之间的变换关系;扩展第一点集,并确定扩展后的第一点集在标定板坐标系下的第三坐标;根据变换关系和第三坐标,确定扩展后的第一点集在第一传感器坐标系下的第四坐标;获取第二点集在第二传感器坐标系下的第五坐标,其中,第二点集部分或者全部位于标定板;根据第四坐标和第五坐标,确定第一传感器和第二传感器之间的外参数,能够减少点云匹配误差,提高点云匹配精度,从而提高标定效果。
本发明实施例提供了一种计算机可读存储介质,所述存储介质中存储有至少一可执行指令,所述可执行指令使处理器执行上述任意方法实施例中的多传感器的标定方法。
本发明实施例提供了一种计算机程序产品,所述计算机程序产品包括存储在计算机存储介质上的计算机程序,所述计算机程序包括程序指令,当所述程序指令被计算机执行时,使所述计算机执行上述任意方法实施例中的多传感器的标定方法。
图7示出了本发明实施例提供的计算设备的结构示意图,本发明具体实施例并不对计算设备的具体实现做限定。
其中,计算设备包括:处理器和存储器。存储器用于存放至少一可执行指令,当计算设备运行时,处理器执行可执行指令,使处理器执行根据上述任意方法实施例中的多传感器的标定方法的步骤。
可选地,如图7所示,该计算设备可以包括:处理器(processor)402、通信接口(Communications Interface)404、存储器(memory)406、以及通信总线408。
其中:处理器402、通信接口404、以及存储器406通过通信总线408完成相互间的通信。通信接口404,用于与其它设备比如客户端或其它服务器等的网元通信。处理器402,用于执行程序410,具体可以执行上述任意方法实施例中的多传感器的标定方法。
具体地,程序410可以包括程序代码,该程序代码包括计算机操作指令。
处理器402可能是中央处理器CPU,或者是特定集成电路ASIC(ApplicationSpecific Integrated Circuit),或者是被配置成实施本发明实施例的一个或多个集成电路。计算设备包括的一个或多个处理器,可以是同一类型的处理器,如一个或多个CPU;也可以是不同类型的处理器,如一个或多个CPU以及一个或多个ASIC。
存储器406,用于存放程序410。存储器406可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
本实施例通过确定标定板上的第一点集在标定板坐标系下的第一坐标;根据第一坐标和第一点集在第一传感器坐标系下的第二坐标,确定标定板坐标系与第一传感器坐标系之间的变换关系;扩展第一点集,并确定扩展后的第一点集在标定板坐标系下的第三坐标;根据变换关系和第三坐标,确定扩展后的第一点集在第一传感器坐标系下的第四坐标;获取第二点集在第二传感器坐标系下的第五坐标,其中,第二点集部分或者全部位于标定板;根据第四坐标和第五坐标,确定第一传感器和第二传感器之间的外参数,能够减少点云匹配误差,提高点云匹配精度,从而提高标定效果。
在此提供的算法或显示不与任何特定计算机、虚拟系统或者其它设备固有相关。各种通用系统也可以与基于在此的示教一起使用。根据上面的描述,构造这类系统所要求的结构是显而易见的。此外,本发明实施例也不针对任何特定编程语言。应当明白,可以利用各种编程语言实现在此描述的本发明的内容,并且上面对特定语言所做的描述是为了披露本发明的最佳实施方式。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
类似地,应当理解,为了精简本发明并帮助理解各个发明方面中的一个或多个,在上面对本发明的示例性实施例的描述中,本发明实施例的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如下面的权利要求书所反映的那样,发明方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本发明的单独实施例。
本领域那些技术人员可以理解,可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件,以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外,本领域的技术人员能够理解,尽管在此的一些实施例包括其它实施例中所包括的某些特征而不是其它特征,但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如,在下面的权利要求书中,所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制,并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。上述实施例中的步骤,除有特殊说明外,不应理解为对执行顺序的限定。