CN115856849B - 一种深度相机与2d激光雷达标定方法及相关设备 - Google Patents
一种深度相机与2d激光雷达标定方法及相关设备 Download PDFInfo
- Publication number
- CN115856849B CN115856849B CN202310177045.5A CN202310177045A CN115856849B CN 115856849 B CN115856849 B CN 115856849B CN 202310177045 A CN202310177045 A CN 202310177045A CN 115856849 B CN115856849 B CN 115856849B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- plane
- equation
- radar
- camera
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本申请涉及图像标定技术领域，公开了一种深度相机与2D激光雷达标定方法及相关设备，深度相机与2D激光雷达标定方法包括：分别获取由深度相机和2D激光雷达采集的竖直墙面的第一点云数据和第二点云数据；根据第一点云数据计算第一平面方程；并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；根据第二点云数据计算第一直线方程；根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定，简单且易使用。
Description
技术领域
本申请涉及图像标定技术领域，具体而言，涉及一种深度相机与2D激光雷达标定方法及相关设备。
背景技术
随着科技的发展与进步，一些激光雷达与深度相机的标定算法逐渐出现，现有的一种差速机器人的实物示意图如图4所示，该差速机器人上设置有2D激光雷达和深度相机,2D激光雷达和深度相机设置在差速机器人的正前方且均设置在差速机器人的同一个竖直对称面上（即2D激光雷达的雷达坐标系OXYZ原点与深度相机的相机坐标系O1X1Y1Z1原点在同一竖直对称面上），其中，2D激光雷达的扫描平面为雷达坐标系的XOY平面，该扫描平面平行于水平面，雷达坐标系的Y坐标轴垂直于所述竖直对称面，且深度相机的相机坐标系的Y1坐标轴平行于水平面，从而相机坐标系原点相对雷达坐标系在Y轴方向上的位置偏差近似为0，但由于该差速机器人在不同高度位置的径向尺寸不同，导致深度相机的相机坐标系原点相对雷达坐标系在X轴方向上具有比较明显的位置偏差。这种差速机器人一般在地面比较平坦的区域使用，在进行导航时，只需要使用XOY平面的二维坐标，因此一般会把在深度相机测得的三维坐标数据映射到XOY平面，以与雷达坐标系测得的二维坐标数据进行融合导航。
这种差速机器人在使用前，需要先对2D激光雷达和深度相机进行标定，以标定得到从相机坐标系的三维坐标数据转换为雷达坐标系的二维坐标数据所需的旋转参数和平移参数；但是现有技术一般由激光雷达与双目相机进行标定，通过多次记录激光雷达与双目相机对同一标定板不同角度的拍摄照片，并计算得到激光雷达与双目相机的转换关系，达到标定的目的，而这种标定方法的计算比较复杂，且一般是应用在3D激光雷达和双目相机，而这种方法应用在2D激光雷达时会有较大的误差。
针对上述问题，目前尚未有有效的技术解决方案。
发明内容
本申请的目的在于提供一种深度相机与2D激光雷达标定方法及相关设备，适用于差速机器人，可快速建立深度相机与2D激光雷达之间的转换关系，简单且易使用。
第一方面，本申请提供了一种深度相机与2D激光雷达标定方法，应用于差速机器人，所述差速机器人上设置有2D激光雷达和深度相机，所述2D激光雷达和所述深度相机设置在所述差速机器人的正前方，所述2D激光雷达的雷达坐标系原点和所述深度相机的相机坐标系原点均设置在所述差速机器人的同一个竖直对称面上，所述2D激光雷达的扫描平面为雷达坐标系的XOY平面且平行于水平面，所述雷达坐标系的Y坐标轴垂直于所述竖直对称面，且所述相机坐标系的Y1坐标轴平行于水平面，所述深度相机与2D激光雷达标定方法包括步骤：
A1.获取由所述深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由所述2D激光雷达采集的所述竖直墙面在所述雷达坐标系下的第二点云数据；
A2.根据所述第一点云数据计算所述竖直墙面在所述相机坐标系下的第一平面方程；
A3.根据所述第一平面方程计算所述雷达坐标系的XOY平面相对所述相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算所述竖直墙面绕Y1坐标轴旋转所述第一旋转角度后在所述相机坐标系下的第二平面方程；
A4.根据所述第二点云数据计算所述2D激光雷达在所述竖直墙面上的扫描线在所述雷达坐标系下的第一直线方程；
A5.根据所述第二平面方程，计算第二平面方程对应的第二平面在所述2D激光雷达的扫描平面上的投影直线的投影直线方程；
A6.根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕所述雷达坐标系的Z轴旋转的第二旋转角度和所述相机坐标系相对所述雷达坐标系在X轴方向上的距离，完成标定。
本申请仅需将差速机器人放置于水平面上，使差速机器人的正前方面向竖直墙面，通过上述方法即可快速建立深度相机与2D激光雷达之间的转换关系，实现标定，简单且易使用。
优选地，步骤A2包括：
A201.循环执行多次以下步骤：
随机选取所述第一点云数据中的三个点来计算第三平面方程；
计算所述第一点云数据中的各点到所述第三平面方程对应的第三平面的第一距离，并根据所述第一距离统计内点的数量，记为第一数量；所述内点是指所述第三平面内的点；
A202.选取最大的所述第一数量对应的所述第三平面方程作为所述第一平面方程。
优选地，所述第一平面方程为：
；
式中，
、
、
、 为所述第一平面方程的系数，、、为所述相机坐标系下的坐标；
步骤A3包括：
根据以下公式计算所述第一旋转角度：
；
式中，为所述第一旋转角度。
优选地，根据以下公式计算所述第二平面方程：
；
式中，
、
、 为所述第二平面方程的系数。
优选地，步骤A4包括：
根据所述第二点云数据，采用最小二乘法计算所述2D激光雷达在所述竖直墙面上的扫描线在雷达坐标系下的第一直线方程。
本申请通过采用最小二乘法，可简单快捷计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程，以提高标定效率。
优选地，所述投影直线方程为：
。
优选地，步骤A6包括：
根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕Z轴旋转的第二旋转角度；
根据所述投影直线方程和所述第二旋转角度，计算所述投影直线绕Z轴旋转第二旋转角度后的第三直线的第三直线方程；
根据所述第三直线方程和所述第一直线方程，计算所述扫描线相对所述第三直线沿X轴方向的截距，作为所述相机坐标系相对所述雷达坐标系在X轴方向上的距离。
第二方面，本申请提供了一种深度相机与2D激光雷达标定的装置，应用于差速机器人，所述差速机器人上设置有2D激光雷达和深度相机，所述2D激光雷达和所述深度相机设置在所述差速机器人的正前方，所述2D激光雷达的雷达坐标系原点和所述深度相机的相机坐标系原点均设置在所述差速机器人的同一个竖直对称面上，所述2D激光雷达的扫描平面为雷达坐标系的XOY平面且平行于水平面，所述雷达坐标系的Y坐标轴垂直于所述竖直对称面，且所述相机坐标系的Y1坐标轴平行于水平面，包括：
获取模块，用于获取由所述深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由所述2D激光雷达采集的所述竖直墙面在所述雷达坐标系下的第二点云数据；
第一计算模块，用于根据所述第一点云数据计算所述竖直墙面在所述相机坐标系下的第一平面方程；
第二计算模块，用于根据所述第一平面方程计算所述雷达坐标系的XOY平面相对所述相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算所述竖直墙面绕Y1坐标轴旋转所述第一旋转角度后在所述相机坐标系下的第二平面方程；
第三计算模块，用于根据所述第二点云数据计算所述2D激光雷达在所述竖直墙面上的扫描线在所述雷达坐标系下的第一直线方程；
第四计算模块，用于根据所述第二平面方程，计算第二平面方程对应的第二平面在所述2D激光雷达的扫描平面上的投影直线的投影直线方程；
第五计算模块，用于根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕所述雷达坐标系的Z轴旋转的第二旋转角度和所述相机坐标系相对所述雷达坐标系在X轴方向上的距离，完成标定。
第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述深度相机与2D激光雷达标定方法中的步骤。
第四方面，本申请提供了一种计算机存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述深度相机与2D激光雷达标定方法中的步骤。
有益效果：
本申请提供的深度相机与2D激光雷达标定方法及相关设备，获取由所述深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由所述2D激光雷达采集的所述竖直墙面在所述雷达坐标系下的第二点云数据；根据所述第一点云数据计算所述竖直墙面在所述相机坐标系下的第一平面方程；根据所述第一平面方程计算所述雷达坐标系的XOY平面相对所述相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算所述竖直墙面绕Y1坐标轴旋转所述第一旋转角度后在所述相机坐标系下的第二平面方程；根据所述第二点云数据计算所述2D激光雷达在所述竖直墙面上的扫描线在所述雷达坐标系下的第一直线方程；根据所述第二平面方程，计算第二平面方程对应的第二平面在所述2D激光雷达的扫描平面上的投影直线的投影直线方程；根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕所述雷达坐标系的Z轴旋转的第二旋转角度和所述相机坐标系相对所述雷达坐标系在X轴方向上的距离，完成标定，该深度相机与2D激光雷达标定方法可快速建立相机坐标系与雷达坐标系之间的转换关系，简单且易使用。
附图说明
图1为本申请提供的深度相机与2D激光雷达标定方法的流程图。
图2为本申请提供的深度相机与2D激光雷达标定的装置结构示意图。
图3为本申请提供的电子设备的结构示意图。
图4为差速机器人的实物示意图。
标号说明:1、获取模块；2、第一计算模块；3、第二计算模块；4、第三计算模块；5、第四计算模块；6、第五计算模块；301、处理器；302、存储器；303、通信总线。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
请参照图1，图1是本申请提供了一种深度相机与2D激光雷达标定方法的流程示意图，应用于差速机器人，图4是差速机器人的实物示意图，差速机器人上设置有2D激光雷达和深度相机，2D激光雷达和深度相机设置在差速机器人的正前方，2D激光雷达的雷达坐标系原点和深度相机的相机坐标系原点均设置在差速机器人的同一个竖直对称面上，2D激光雷达的扫描平面为雷达坐标系的XOY平面且平行于水平面，雷达坐标系的Y坐标轴垂直于竖直对称面，且相机坐标系的Y1坐标轴平行于水平面，深度相机与2D激光雷达标定方法包括步骤：
A1.获取由深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由2D激光雷达采集的竖直墙面在雷达坐标系下的第二点云数据；
A2.根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程；
A3.根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；
A4.根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程；
A5.根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；
A6.根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定。
具体地，根据深度相机采集的第一点云数据计算竖直墙面在相机坐标系下的第一平面方程，由于相机坐标系原点相对雷达坐标系在X轴方向上具有比较明显的位置偏差，即雷达坐标系的XOY平面与相机坐标系的X1O1Y1平面不是相互平行的，因此，根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程，由于该第二平面方程对应的第二平面与2D激光雷达的扫描平面垂直，可根据第二平面方程计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程，通过第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程，从而根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，即完成标定，本申请仅需将差速机器人放置于水平面上，使差速机器人的正前方面向竖直墙面，即可快速建立深度相机与2D激光雷达之间的转换关系，实现标定，简单且易使用。
在一些实施方式中，步骤A2包括：
A201.循环执行多次以下步骤：
随机选取第一点云数据中的三个点来计算第三平面方程；
计算第一点云数据中的各点到第三平面方程对应的第三平面的第一距离，并根据第一距离统计内点的数量，记为第一数量；内点是指第三平面内的点；
A202.选取最大的第一数量对应的第三平面方程作为第一平面方程。
具体地，通过上述方式可以获取比较准确的第一平面方程。
在一些实施方式中，计算第一点云数据中的各点到第三平面方程对应的第三平面的第一距离，并根据第一距离统计内点的数量，记为第一数量；内点是指第三平面内的点步骤包括：
计算第一点云数据中的各点到第三平面方程对应的第三平面的第一距离，若第一距离小于预设阈值，则该第一距离对应的点作为内点，并统计内点的数量，记为第一数量。
在一些实施例中，第一平面方程为：
；
式中，
、
、
、 为第一平面方程的系数，、、为相机坐标系下的坐标；
步骤A3包括：
根据以下公式计算第一旋转角度：
；
式中，为第一旋转角度。
具体地，根据第一平面方程可以知道第一平面方程的法向量为，由于雷达坐标系下的竖直墙面处于竖直状态，则可以将相机坐标系的X1O1Y1平面绕Y1坐标轴旋转，使相机坐标系下的竖直墙面也处于竖直状态（旋转之前的相机坐标系下的竖直墙面不是处于竖直状态），根据以下公式：
；
化简得：;
从而第一旋转角度的计算公式为：
；或者，。
在一些实施例中，根据以下公式计算第二平面方程：
;
式中，
、
、 为第二平面方程的系数。
具体地，由于
、 是竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程的系数，而
、
、 是竖直墙面在绕Y1坐标轴旋转第一旋转角度前在相机坐标系下的第一平面方程的系数，因此，竖直墙面绕Y1坐标轴旋转第一旋转角度后，相机坐标系下的原点到第二平面的距离不变，即：
根据第一旋转角度的计算公式以及，
即第二平面方程为：
++=0;
根据上述公式，即可得到第二平面方程的计算公式。
在一些实施例中，步骤A4包括：
根据第二点云数据，采用最小二乘法计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程。
具体地，通过采用最小二乘法，可简单快捷计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程，以提高标定效率，其中，第一直线方程具体为：，其中，、为第一直线方程的系数。
在一些实施例中，投影直线方程为：
。
具体地，由于第二平面与2D激光雷达的扫描平面垂直，因此，投影直线方程为上述公式。
在一些实施方式中，步骤A6包括：
根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕Z轴旋转的第二旋转角度；
根据投影直线方程和第二旋转角度，计算投影直线绕Z轴旋转第二旋转角度后的第三直线的第三直线方程；
根据第三直线方程和第一直线方程，计算扫描线相对第三直线沿X轴方向的截距，作为相机坐标系相对雷达坐标系在X轴方向上的距离。
具体地，由于第一直线方程为：，根据投影直线方程以及第一直线方程，可通过以下公式计算扫描线相对投影直线绕Z轴旋转的第二旋转角度：
；
其中，；
式中，是第二旋转角度。
因此，投影直线绕Z轴旋转第二旋转角度后的第三直线的第三直线方程为：;
则有;
且投影直线绕Z轴旋转后的第三直线到雷达坐标系的原点的距离不变，则有：
化简得到：；
由于第三直线与第一直线平行，则他们的斜率相同，即：
；
根据第三直线方程和第一直线方程，通过以下公式计算扫描线相对第三直线沿X轴方向的截距：,即将第三直线沿X轴方向平移d的距离可与第一直线重合，从而得到深度相机与2D激光雷达之间的转换关系，实现标定的目的。
由上可知，本申请提供的深度相机与2D激光雷达标定方法，获取由深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由2D激光雷达采集的竖直墙面在雷达坐标系下的第二点云数据；根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程；根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程；根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定，本申请仅需将差速机器人放置于水平面上，使差速机器人的正前方面向竖直墙面，即可快速建立深度相机与2D激光雷达之间的转换关系，实现标定，简单且易使用。
请参考图2，本申请提供了一种深度相机与2D激光雷达标定的装置，应用于差速机器人，差速机器人上设置有2D激光雷达和深度相机，2D激光雷达和深度相机设置在差速机器人的正前方，2D激光雷达的雷达坐标系原点和深度相机的相机坐标系原点均设置在差速机器人的同一个竖直对称面上，2D激光雷达的扫描平面为雷达坐标系的XOY平面且平行于水平面，雷达坐标系的Y坐标轴垂直于竖直对称面，且相机坐标系的Y1坐标轴平行于水平面，包括：
获取模块1，用于获取由深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由2D激光雷达采集的竖直墙面在雷达坐标系下的第二点云数据；
第一计算模块2，用于根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程；
第二计算模块3，用于根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；
第三计算模块4，用于根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程；
第四计算模块5，用于根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；
第五计算模块6，用于根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定。
具体地，根据深度相机采集的第一点云数据计算竖直墙面在相机坐标系下的第一平面方程，由于相机坐标系原点相对雷达坐标系在X轴方向上具有比较明显的位置偏差，即雷达坐标系的XOY平面与相机坐标系的X1O1Y1平面不是相互平行的，因此，根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程，由于该第二平面方程对应的第二平面与2D激光雷达的扫描平面垂直，可根据第二平面方程计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程，通过第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程，从而根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，即完成标定，本申请仅需将差速机器人放置于水平面上，使差速机器人的正前方面向竖直墙面，即可快速建立深度相机与2D激光雷达之间的转换关系，实现标定，简单且易使用。
在一些实施方式中，第一计算模块2在根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程的时候，具体执行：
A201.循环执行多次以下步骤：
随机选取第一点云数据中的三个点来计算第三平面方程；
计算第一点云数据中的各点到第三平面方程对应的第三平面的第一距离，并根据第一距离统计内点的数量，记为第一数量；内点是指第三平面内的点；
A202.选取最大的第一数量对应的第三平面方程作为第一平面方程。
具体地，通过上述方式可以获取比较准确的第一平面方程。
在一些实施方式中，计算第一点云数据中的各点到第三平面方程对应的第三平面的第一距离，并根据第一距离统计内点的数量，记为第一数量；内点是指第三平面内的点步骤包括：
计算第一点云数据中的各点到第三平面方程对应的第三平面的第一距离，若第一距离小于预设阈值，则该第一距离对应的点作为内点，并统计内点的数量，记为第一数量。
在一些实施例中，第一平面方程为：
；
式中，
、
、
、 为第一平面方程的系数，、、为相机坐标系下的坐标；
第二计算模块3在根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程的时候，具体执行：
根据以下公式计算第一旋转角度：
；
式中，为第一旋转角度。
具体地，根据第一平面方程可以知道第一平面方程的法向量为，由于雷达坐标系下的竖直墙面处于竖直状态，则可以将相机坐标系的X1O1Y1平面绕Y1坐标轴旋转，使相机坐标系下的竖直墙面也处于竖直状态（旋转之前的相机坐标系下的竖直墙面不是处于竖直状态），根据以下公式：
；
化简得：;
从而第一旋转角度的计算公式为：
；或者，。
在一些实施例中，根据以下公式计算第二平面方程：
;
式中，
、
、 为第二平面方程的系数。
具体地，由于
、 是竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程的系数，而
、
、 是竖直墙面在绕Y1坐标轴旋转第一旋转角度前在相机坐标系下的第一平面方程的系数，因此，竖直墙面绕Y1坐标轴旋转第一旋转角度后，相机坐标系下的原点到第二平面的距离不变，即：
根据第一旋转角度的计算公式以及，
即第二平面方程为：
++=0;
根据上述公式，即可得到第二平面方程的计算公式。
在一些实施例中，第三计算模块4在根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程的时候，具体执行：
根据第二点云数据，采用最小二乘法计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程。
具体地，通过采用最小二乘法，可简单快捷计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程，以提高标定效率，其中，第一直线方程具体为：。
在一些实施例中，投影直线方程为：
。
具体地，由于第二平面与2D激光雷达的扫描平面垂直，因此，投影直线方程为上述公式。
在一些实施方式中，第五计算模块6在根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定的时候，具体执行：
根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕Z轴旋转的第二旋转角度；
根据投影直线方程和第二旋转角度，计算投影直线绕Z轴旋转第二旋转角度后的第三直线的第三直线方程；
根据第三直线方程和第一直线方程，计算扫描线相对第三直线沿X轴方向的截距，作为雷达坐标系相对相机坐标系在X轴方向上的距离。
具体地，由于第一直线方程为：，根据投影直线方程以及第一直线方程，可通过以下公式计算扫描线相对投影直线绕Z轴旋转的第二旋转角度：
；
其中，；
式中，是第二旋转角度。
因此，投影直线绕Z轴旋转第二旋转角度后的第三直线的第三直线方程为：;
则有;
且投影直线绕Z轴旋转后的第三直线到雷达坐标系的原点的距离不变，则有：
化简得到：；
由于第三直线与第一直线平行，则他们的斜率相同，即：
；
根据第三直线方程和第一直线方程，通过以下公式计算扫描线相对第三直线沿X轴方向的截距：,即将第三直线沿X轴方向平移d的距离可与第一直线重合，从而得到深度相机与2D激光雷达之间的转换关系，实现标定的目的。
由上可知，本申请提供的深度相机与2D激光雷达标定的装置，获取由深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由2D激光雷达采集的竖直墙面在雷达坐标系下的第二点云数据；根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程；根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程；根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定，本申请仅需将差速机器人放置于水平面上，使差速机器人的正前方面向竖直墙面，即可快速建立深度相机与2D激光雷达之间的转换关系，实现标定，简单且易使用。
请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的深度相机与2D激光雷达标定方法，以实现以下功能：获取由深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由2D激光雷达采集的竖直墙面在雷达坐标系下的第二点云数据；根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程；根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程；根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定。
本申请实施例提供一种存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的深度相机与2D激光雷达标定方法，以实现以下功能：获取由深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由2D激光雷达采集的竖直墙面在雷达坐标系下的第二点云数据；根据第一点云数据计算竖直墙面在相机坐标系下的第一平面方程；根据第一平面方程计算雷达坐标系的XOY平面相对相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算竖直墙面绕Y1坐标轴旋转第一旋转角度后在相机坐标系下的第二平面方程；根据第二点云数据计算2D激光雷达在竖直墙面上的扫描线在雷达坐标系下的第一直线方程；根据第二平面方程，计算第二平面方程对应的第二平面在2D激光雷达的扫描平面上的投影直线的投影直线方程；根据投影直线方程和第一直线方程，计算扫描线相对投影直线绕雷达坐标系的Z轴旋转的第二旋转角度和相机坐标系相对雷达坐标系在X轴方向上的距离，完成标定。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（StaticRandom Access Memory, 简称SRAM），电可擦除可编程只读存储器（ElectricallyErasable Programmable Read-Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。
在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
另外，作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。
以上仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。
Claims (10)
1.一种深度相机与2D激光雷达标定方法，应用于差速机器人，所述差速机器人上设置有2D激光雷达和深度相机，所述2D激光雷达和所述深度相机设置在所述差速机器人的正前方，所述2D激光雷达的雷达坐标系原点和所述深度相机的相机坐标系原点均设置在所述差速机器人的同一个竖直对称面上，所述2D激光雷达的扫描平面为雷达坐标系的XOY平面且平行于水平面，所述雷达坐标系的Y坐标轴垂直于所述竖直对称面，且所述相机坐标系的Y1坐标轴平行于水平面，其特征在于，所述深度相机与2D激光雷达标定方法包括步骤：
A1.获取由所述深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由所述2D激光雷达采集的所述竖直墙面在所述雷达坐标系下的第二点云数据；
A2.根据所述第一点云数据计算所述竖直墙面在所述相机坐标系下的第一平面方程；
A3.根据所述第一平面方程计算所述雷达坐标系的XOY平面相对所述相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算所述竖直墙面绕Y1坐标轴旋转所述第一旋转角度后在所述相机坐标系下的第二平面方程；
A4.根据所述第二点云数据计算所述2D激光雷达在所述竖直墙面上的扫描线在所述雷达坐标系下的第一直线方程；
A5.根据所述第二平面方程，计算第二平面方程对应的第二平面在所述2D激光雷达的扫描平面上的投影直线的投影直线方程；
A6.根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕所述雷达坐标系的Z轴旋转的第二旋转角度和所述相机坐标系相对所述雷达坐标系在X轴方向上的距离，完成标定。
2.根据权利要求1所述的深度相机与2D激光雷达标定方法，其特征在于，步骤A2包括：
A201.循环执行多次以下步骤：
随机选取所述第一点云数据中的三个点来计算第三平面方程；
计算所述第一点云数据中的各点到所述第三平面方程对应的第三平面的第一距离，并根据所述第一距离统计内点的数量，记为第一数量；所述内点是指所述第三平面内的点；
A202.选取最大的所述第一数量对应的所述第三平面方程作为所述第一平面方程。
3.根据权利要求1所述的深度相机与2D激光雷达标定方法，其特征在于，所述第一平面方程为：
；
式中，、、、为所述第一平面方程的系数，、、为所述相机坐标系下的坐标；
步骤A3包括：
根据以下公式计算所述第一旋转角度：
；
式中，为所述第一旋转角度。
4.根据权利要求3所述的深度相机与2D激光雷达标定方法，其特征在于，根据以下公式计算所述第二平面方程：
；
式中，、、为所述第二平面方程的系数。
5.根据权利要求1所述的深度相机与2D激光雷达标定方法，其特征在于，步骤A4包括：
根据所述第二点云数据，采用最小二乘法计算所述2D激光雷达在所述竖直墙面上的扫描线在雷达坐标系下的第一直线方程。
6.根据权利要求4所述的深度相机与2D激光雷达标定方法，其特征在于，所述投影直线方程为：
。
7.根据权利要求5所述的深度相机与2D激光雷达标定方法，其特征在于，步骤A6包括：
根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕Z轴旋转的第二旋转角度；
根据所述投影直线方程和所述第二旋转角度，计算所述投影直线绕Z轴旋转第二旋转角度后的第三直线的第三直线方程；
根据所述第三直线方程和所述第一直线方程，计算所述扫描线相对所述第三直线沿X轴方向的截距，作为所述相机坐标系相对所述雷达坐标系在X轴方向上的距离。
8.一种深度相机与2D激光雷达标定的装置，应用于差速机器人，所述差速机器人上设置有2D激光雷达和深度相机，所述2D激光雷达和所述深度相机设置在所述差速机器人的正前方，所述2D激光雷达的雷达坐标系原点和所述深度相机的相机坐标系原点均设置在所述差速机器人的同一个竖直对称面上，所述2D激光雷达的扫描平面为雷达坐标系的XOY平面且平行于水平面，所述雷达坐标系的Y坐标轴垂直于所述竖直对称面，且所述相机坐标系的Y1坐标轴平行于水平面，其特征在于，包括：
获取模块，用于获取由所述深度相机采集的竖直墙面在相机坐标系下的第一点云数据，以及由所述2D激光雷达采集的所述竖直墙面在所述雷达坐标系下的第二点云数据；
第一计算模块，用于根据所述第一点云数据计算所述竖直墙面在所述相机坐标系下的第一平面方程；
第二计算模块，用于根据所述第一平面方程计算所述雷达坐标系的XOY平面相对所述相机坐标系的X1O1Y1平面绕Y1坐标轴旋转的第一旋转角度，并计算所述竖直墙面绕Y1坐标轴旋转所述第一旋转角度后在所述相机坐标系下的第二平面方程；
第三计算模块，用于根据所述第二点云数据计算所述2D激光雷达在所述竖直墙面上的扫描线在所述雷达坐标系下的第一直线方程；
第四计算模块，用于根据所述第二平面方程，计算第二平面方程对应的第二平面在所述2D激光雷达的扫描平面上的投影直线的投影直线方程；
第五计算模块，用于根据所述投影直线方程和所述第一直线方程，计算所述扫描线相对所述投影直线绕所述雷达坐标系的Z轴旋转的第二旋转角度和所述相机坐标系相对所述雷达坐标系在X轴方向上的距离，完成标定。
9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如权利要求1-7任一项所述深度相机与2D激光雷达标定方法中的步骤。
10.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1-7任一项所述深度相机与2D激光雷达标定方法中的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310177045.5A CN115856849B (zh) | 2023-02-28 | 2023-02-28 | 一种深度相机与2d激光雷达标定方法及相关设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310177045.5A CN115856849B (zh) | 2023-02-28 | 2023-02-28 | 一种深度相机与2d激光雷达标定方法及相关设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115856849A CN115856849A (zh) | 2023-03-28 |
| CN115856849B true CN115856849B (zh) | 2023-05-05 |
Family
ID=85659371
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310177045.5A Active CN115856849B (zh) | 2023-02-28 | 2023-02-28 | 一种深度相机与2d激光雷达标定方法及相关设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115856849B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103837869A (zh) * | 2014-02-26 | 2014-06-04 | 北京工业大学 | 基于向量关系的单线激光雷达和ccd相机标定方法 |
| CN111965624A (zh) * | 2020-08-06 | 2020-11-20 | 北京百度网讯科技有限公司 | 激光雷达与相机的标定方法、装置、设备和可读存储介质 |
| WO2020233443A1 (zh) * | 2019-05-21 | 2020-11-26 | 菜鸟智能物流控股有限公司 | 一种激光雷达与相机之间的标定方法和装置 |
| CN112017205A (zh) * | 2020-07-27 | 2020-12-01 | 清华大学 | 一种激光雷达和相机传感器空间位置自动标定方法及系统 |
| CN112180362A (zh) * | 2019-07-05 | 2021-01-05 | 北京地平线机器人技术研发有限公司 | 雷达与相机之间的转换位姿确定方法、装置以及电子设备 |
| CN112258590A (zh) * | 2020-12-08 | 2021-01-22 | 杭州迦智科技有限公司 | 一种基于激光的深度相机外参标定方法、设备及其存储介质 |
| CN112379352A (zh) * | 2020-11-04 | 2021-02-19 | 广州文远知行科技有限公司 | 一种激光雷达标定方法、装置、设备及存储介质 |
| CN113589261A (zh) * | 2021-07-16 | 2021-11-02 | 杭州环峻科技有限公司 | 一种全景环带相机与激光雷达联合标定的方法 |
| CN114488097A (zh) * | 2022-01-26 | 2022-05-13 | 广州小鹏自动驾驶科技有限公司 | 激光雷达的外参标定方法、计算机设备及计算机存储介质 |
| CN114782556A (zh) * | 2022-06-20 | 2022-07-22 | 季华实验室 | 相机与激光雷达的配准方法、系统及存储介质 |
| CN115661269A (zh) * | 2022-11-18 | 2023-01-31 | 深圳市智绘科技有限公司 | 相机与激光雷达的外参标定方法、装置及存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109212540A (zh) * | 2018-09-12 | 2019-01-15 | 百度在线网络技术（北京）有限公司 | 基于激光雷达系统的测距方法、装置及可读存储介质 |
-
2023
- 2023-02-28 CN CN202310177045.5A patent/CN115856849B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103837869A (zh) * | 2014-02-26 | 2014-06-04 | 北京工业大学 | 基于向量关系的单线激光雷达和ccd相机标定方法 |
| WO2020233443A1 (zh) * | 2019-05-21 | 2020-11-26 | 菜鸟智能物流控股有限公司 | 一种激光雷达与相机之间的标定方法和装置 |
| CN112180362A (zh) * | 2019-07-05 | 2021-01-05 | 北京地平线机器人技术研发有限公司 | 雷达与相机之间的转换位姿确定方法、装置以及电子设备 |
| CN112017205A (zh) * | 2020-07-27 | 2020-12-01 | 清华大学 | 一种激光雷达和相机传感器空间位置自动标定方法及系统 |
| CN111965624A (zh) * | 2020-08-06 | 2020-11-20 | 北京百度网讯科技有限公司 | 激光雷达与相机的标定方法、装置、设备和可读存储介质 |
| CN112379352A (zh) * | 2020-11-04 | 2021-02-19 | 广州文远知行科技有限公司 | 一种激光雷达标定方法、装置、设备及存储介质 |
| CN112258590A (zh) * | 2020-12-08 | 2021-01-22 | 杭州迦智科技有限公司 | 一种基于激光的深度相机外参标定方法、设备及其存储介质 |
| CN113589261A (zh) * | 2021-07-16 | 2021-11-02 | 杭州环峻科技有限公司 | 一种全景环带相机与激光雷达联合标定的方法 |
| CN114488097A (zh) * | 2022-01-26 | 2022-05-13 | 广州小鹏自动驾驶科技有限公司 | 激光雷达的外参标定方法、计算机设备及计算机存储介质 |
| CN114782556A (zh) * | 2022-06-20 | 2022-07-22 | 季华实验室 | 相机与激光雷达的配准方法、系统及存储介质 |
| CN115661269A (zh) * | 2022-11-18 | 2023-01-31 | 深圳市智绘科技有限公司 | 相机与激光雷达的外参标定方法、装置及存储介质 |
Non-Patent Citations (1)
| Title |
|---|
| 刘大学 ; 戴斌 ; 李政 ; 贺汉根 ; .一种单线激光雷达和可见光摄像机的标定方法.华中科技大学学报(自然科学版).2008,(第S1期),第68-71页. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115856849A (zh) | 2023-03-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9928595B2 (en) | Devices, systems, and methods for high-resolution multi-view camera calibration | |
| CN111487043B (zh) | 单目散斑结构光系统的散斑投射器标定参数确定方法 | |
| WO2020168685A1 (zh) | 一种三维扫描视点规划方法、装置及计算机可读存储介质 | |
| CN111220129B (zh) | 一种带旋转云台的聚焦测量方法及终端 | |
| SG189284A1 (en) | Rapid 3d modeling | |
| CN111123242B (zh) | 一种基于激光雷达和相机的联合标定方法及计算机可读存储介质 | |
| CN111127540B (zh) | 一种三维虚拟空间自动测距方法及系统 | |
| CN110332930B (zh) | 一种位置确定方法、装置及设备 | |
| CN108507462A (zh) | 一种全息干涉四轴测量设备旋转轴的标定方法 | |
| CN110095089B (zh) | 一种飞行器旋转角度的测量方法及系统 | |
| CN108182708B (zh) | 一种双目相机的标定方法、标定装置及终端设备 | |
| WO2022179094A1 (zh) | 车载激光雷达外参数联合标定方法、系统、介质及设备 | |
| CN111179351B (zh) | 一种参数标定方法及其装置、处理设备 | |
| CN110825079A (zh) | 一种地图构建方法及装置 | |
| CN114187589A (zh) | 一种目标检测方法、装置、设备和存储介质 | |
| CN114926538A (zh) | 单目激光散斑投影系统的外参数标定方法和装置 | |
| CN111627100A (zh) | 一种用于评价摄影测量精度的数值模拟方法 | |
| CN110766763A (zh) | 一种基于消失点运算的相机标定法 | |
| CN115856849B (zh) | 一种深度相机与2d激光雷达标定方法及相关设备 | |
| CN109859313B (zh) | 3d点云数据获取方法、装置、3d数据生成方法及系统 | |
| CN112150621B (zh) | 基于正射投影的鸟瞰图生成方法、系统及存储介质 | |
| CN113034615B (zh) | 一种用于多源数据融合的设备标定方法及相关装置 | |
| CN113421300B (zh) | 确定鱼眼相机图像中物体实际位置的方法及装置 | |
| CN110675445B (zh) | 一种视觉定位方法、装置及存储介质 | |
| CN114399553A (zh) | 基于相机姿态的虚拟视点生成方法及装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |