发明内容
本发明要解决的技术问题是:提供一种可以消除激光雷达扫描带来的随机误差,从而提高标注精度的激光雷达和相机的标定方法。
为了解决上述技术问题,本发明提出的技术方案是:一种激光雷达和相机的标定方法,包括以下步骤:
步骤一、将ArUco码贴于标定板表面,将标定板固定在相机前方;
步骤二、激光雷达获取N帧包括标定板的点云数据,并记录下每一帧点云数据中每一个点的坐标及其对应的射线id;
步骤三、对N帧点云数据进行处理,消除激光雷达的随机误差,方法如下:
1)读取第1帧点云数据的射线0所扫到的所有点,记为数组pc0_r0;
2)读取第2帧点云数据的射线0所扫到的所有点,记为数组pc1_r0;
3)对数组pc1_r0中的任一点pn,计算其与数组pc0_r0中的每一个点的距离,找到距离最小的点记为pm,完成配对;
4)完成数组pc0_r0和数组pc1_r0中的每个点的配对后,形成新的数组pc_r0,其中数组pc_r0中每一个点的坐标为对应两个配对点坐标的均值;
5)对第1帧点云和第2帧点云中的其他射线,重复上述步骤,最终得到新的点云数据pc;
6)以点云数据pc作为第1帧点云数据,从第3,4,5,...,N帧点云数据中依次取一帧点云数据作为第2帧点云数据,不断重复上述步骤1)-5),最终可得新的点云数据pc_final;
步骤四、从点云数据pc_final中,找出标定板的边缘点;
步骤五、设定一伪相机坐标系,所述伪相机坐标系的原点为相机坐标系的原点,所述伪相机坐标系的光轴水平设置,且所述伪相机坐标系的光轴与相机坐标系的光轴位于同一竖直平面内;所述伪相机坐标系与雷达坐标系之间的转换矩阵记为T1;
步骤六、将步骤四中找到的所述标定板的边缘点转换到伪相机坐标系下,并投影到伪相机的成像平面上,得到投影图像;从投影图像中找到标定板的角点位于伪相机坐标系下的2d坐标;
获取标定板的角点在相机坐标系下的3d坐标;
步骤七、移动标定板,重复步骤二至步骤六,记录下多组标定板的角点位于伪相机坐标系下的2d坐标,以及在相机坐标系下相对应的3d坐标;
步骤八、根据多组标定板的角点位于伪相机坐标系下的2d坐标,以及在相机坐标系下相对应的3d坐标,得到相机坐标系和伪相机坐标系之间的转换矩阵,记为T2,从而得到激光雷达坐标系与相机坐标系之间的转换矩阵T=T2-1*T1。
本发明通过对激光雷达获取的N帧包括标定板的点云数据进行处理,以概率统计的方式消除了激光雷达扫描时所固有的随机误差,从而使得在后续扫描图像上的标定板角点位置非常准确,为快速方便地寻找标定板的角点位于伪相机坐标系下的2d坐标扫除了障碍,克服了现有技术中采用平面拟合算法ransac寻找标定板的角点的2d坐标时,需要调整较多参数的问题,且迭代次数不确定,平面拟合的效果比较依赖于样本数据的质量以及参数的选取等缺陷。因此,本发明的标注方法不但简单可靠,而且标注精度高。
具体实施方式
本实施例的激光雷达和相机的标定方法,包括以下步骤:
步骤一、选择标定板,例如:选择49cm*49cm的标定板。将ArUco码贴于标定板表面,将标定板固定在相机前方。ArUco码标定板为为常用的标定板,通过ArUco码可以方便地得到标定板的四个角点。在使用时,固定好标定板,在相机前方,保证标定板周围较为空旷,杂点较少。
步骤二、激光雷达获取N帧包括标定板的点云数据,并记录下每一帧点云数据中每一个点的坐标及其对应的射线id。由于激光雷达固有的误差存在,对同一位置点,每次扫描都会有微小的偏差,在记录下每一个点的坐标及其对应的射线id,存储N帧点云数据后,开始处理。
为了减小计算量,本实施例优选的,在接收激光雷达的点云数据时,配置待处理的点云范围,保证大部分点为扫描到标定板上的点;通过点云范围对点云数据进行过滤,形成过滤后的点云数据。这样,步骤三在消除激光雷达的随机误差时,采用过滤后的点云数据。
在配置待处理的点云范围,过滤掉不需要处理的点,形成新的点云时,可通过点云可视化工具(比如ros系统中的rviz工具),观察过滤后的点云数据,保证大部分点为扫描到标定板上的点,如图1所示。如果过滤后的点云数据仍然含有较多杂点,则调整配置文件中的待处理点云范围,直至过滤后的点云中大部分为标定板的点;由于有可视化工具的同步观察,这一步只需调整极少的次数即可完成。
步骤三、对N帧点云数据进行处理,消除激光雷达的随机误差,方法如下:
1)读取第1帧点云数据的射线0所扫到的所有点,记为数组pc0_r0;
2)读取第2帧点云数据的射线0所扫到的所有点,记为数组pc1_r0;
3)对数组pc1_r0中的任一点pn,计算其与数组pc0_r0中的每一个点的距离,找到距离最小的点记为pm,完成配对;
4)完成数组pc0_r0和数组pc1_r0中的每个点的配对后,形成新的数组pc_r0,其中数组pc_r0中每一个点的坐标为对应两个配对点坐标的均值;
5)对第1帧点云和第2帧点云中的其他射线,重复上述步骤,最终得到新的点云数据pc;
6)以点云数据pc作为第1帧点云数据,从第3,4,5,...,N帧点云数据中依次取一帧点云数据作为第2帧点云数据,不断重复上述步骤1)-5),最终可得新的点云数据pc_final。
本实施例中,点云数据pc_final中的点的坐标由N帧点云不断匹配,求均值而得到,从而消除了激光雷达的随机误差。根据概率统计的原理,通常N越大,激光雷达的随机误差的消除效果越好。根据申请人的试验表明,N大于6时,激光雷达的随机误差就可以消除至能够接受的范围。
步骤四、从点云数据pc_final中,找出标定板的边缘点,如图2所示。
边缘点可以采用图像识别的方式找出,本实施例优选采用以下方式:对点云数据pc_final中的每一个点,计算该点到雷达激光原点的欧式距离,记为该点的第一距离;对每一条射线扫描到的点,依次计算该点的第一距离与相邻点的第一距离之差,并求其绝对值,记为该点的第一距离差;第一距离差大于预设阈值的点即为标定板的边缘点。
以射线0扫到的点p1,p2,...,pm,.. .,pn为例,计算各个点到激光雷达原点的欧氏距离,记为d1,d2,...,dm,...,dn,根据计算结果再次计算相邻点的距离差(第一个点和最后一个点不计算),得到d1,d1-d2,...,d(m-1)-dm,...,对其求绝对值,记为D1,D2,...,Dm,...,依次判断每一个点,判断Dm与某一阈值t的大小,当Dm>t时,则认为点pm是边缘点。对射线1,2....扫到的点重复上述过程,找到每一条射线扫描到的标定板的边缘点。其中,t为经验值,可根据自己的激光雷达灵活选取,也可根据可视化工具进行调整。
步骤五、获取标定板的边缘点后,现有技术中由于激光雷达误差的存在,这些点可能并不在一个平面上,因此通常使用ransac算法去拟合出一个平面,但是ransac拟合平面有很多参数要调整,迭代次数也不确定,平面拟合的效果比较依赖于样本数据的质量和参数的选取。
本实施例另辟蹊径,设定一与相机方向大致相同的伪相机坐标系,所述伪相机坐标系的原点为相机坐标系的原点,所述伪相机坐标系的光轴水平设置,且所述伪相机坐标系的光轴与相机坐标系的光轴位于同一竖直平面内;所述伪相机坐标系与雷达坐标系之间的转换矩阵记为T1。
比如以前视相机为例,假设伪坐标系记为C,C与雷达坐标系L之间的转换矩阵T1,转换矩阵T1由旋转矩阵和平移矩阵构成,其中旋转矩阵初始化成用欧拉角的表达的形式如下(1.57,-1.57,0),平移矩阵初始化为(0,0,0)。
步骤六、将步骤四中找到的所述标定板的边缘点转换到伪相机坐标系下,并投影到伪相机的成像平面上,得到投影图像;从投影图像中找到标定板的角点位于伪相机坐标系下的2d坐标。仍以前视相机为例,提取出标定板的边缘点云(记为P_edge)后,将其转换到伪相机坐标系下的坐标,记为P_edge_fakec,将P_edge_fakec投影到伪相机的成像平面上,得到投影图像,记为I,如图3所示。通过可视化工具,在投影图像I上选取两点A1,A2,其连线记为L1,使L1穿过尽可能多左上边的边缘点,同理可得连线L2穿过右上边缘点,L3穿过右下边缘点,L4穿过左下边缘点。求解L1和L2的交点记为A,求解L2和L3的交点记为B,求解L3和L4的交点记为C,求解L4和L5的交点,记为D。至此,得到标定板的角点位于伪相机坐标系下的2d坐标。当然,标定板的角点也可以图像识别,或者通过现有技术,将投影图像I拟合成四边形,寻找四边形的四个顶点即得到标定板的四个角点。
获取标定板的角点在相机坐标系下的3d坐标,此为现有技术,比如在实施时,可使用ros中的包aruco_mapping等得到标定板角点在相机坐标系下的3d坐标。
步骤七、移动标定板,重复步骤二至步骤六,记录下多组标定板的角点位于伪相机坐标系下的2d坐标,以及在相机坐标系下相对应的3d坐标。
步骤八、根据多组标定板的角点位于伪相机坐标系下的2d坐标,以及在相机坐标系下相对应的3d坐标,得到相机坐标系和伪相机坐标系之间的转换矩阵,记为T2,从而得到激光雷达坐标系与相机坐标系之间的转换矩阵T=T2-1*T1。在实施时,转换矩阵T2可通过OpenCV中的solvePnP函数求解得到,此为现有技术,中参考相关文献,不再赘述。
需要说明的是,本发明是通过先在步骤二中,消除激光雷达的随机误差后,再寻找标定板的四个角点。由于消除了激光雷达的随机误差,投影图像I中的边缘点非常接近规则的四边形,因此可通过描线或其他方式准确地找到标定板的四个角点。而如果不消除激光雷达的随机误差,则投影图像I中的边缘点处于不规则的分布状态,因此很难准确地找出标定板的四个角点,这也就是现有技术中需要使用ransac算法先拟合出一个平面再寻找边缘点的原因,但ransac算法非常复杂,且需要调整很多的参数,给准确快速地寻找边缘点带来了困难。
本发明不局限于上述实施例所述的具体技术方案,除上述实施例外,本发明还可以有其他实施方式。对于本领域的技术人员来说,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等形成的技术方案,均应包含在本发明的保护范围之内。