本发明属于计算机视觉和机器人领域，涉及一种激光雷达和相机的外参标定系统和方法。
背景技术：
三维重建在计算机视觉以及机器人领域有着非常重要的作用，是虚拟现实，智能监控，机器人路径规划等应用的基础。激光雷达能够直接获取三维深度信息，图像能够获取纹理和色彩信息，二者相结合的系统能够获得更加真实的三维场景。激光雷达与相机之间的外参标定能够保证两种传感器之间的数据融合。
三维重建主要分为两类：基于相机图像进行的三维重建以及基于激光雷达的三维重建。基于图像的三维重建主要包括图像的获取、相机的标定、特征点提取、特征点匹配、表面重建等几个步骤。
基于激光雷达的重建算法，激光雷达能够直接获取深度信息，常用icp算法做三维点云的融合。
激光雷达和相机融合的系统能够达到实时表面重建，并且能够获得丰富的纹理信息。激光雷达和相机之间的外参能够保证系统坐标的统一，是多传感器信息融合系统中非常重要的部分。
技术实现要素：
本发明提供一种激光雷达和相机之间外参标定系统和方法。包括如下步骤：
步骤1：通过相机棋盘格获取相机姿态；
步骤2：通过激光雷达数据获取激光雷达姿态
步骤3：激光雷达与相机之间的外参估计
步骤4：优化外参估计
进一步地，所述步骤1进一步具体为：相机围绕棋盘格运动，保证棋盘格位于相机视野范围内。以两幅视图为例，取出第i帧和第j帧，假设i＜j。通过角点检测算法，可以获取每幅图像的棋盘格角点，然后利用相关匹配算法可以鲁棒地获得棋盘格角点的匹配关系。由于棋盘格角点位于同一平面上，对应关系约束于单应性矩阵h，通过分解单应性矩阵h，可以获取四组可能的相机姿态，但有效3d点必须位于相机前方，根据该约束条件，便可筛选出真正的相机姿态
其中，i，j分别表示第i帧和第j帧，i＜j。r表示旋转矩阵，t表示平移矩阵。
进一步地，所述步骤2进一步具体为：假设雷达和相机已经做好同步，提取第i帧和第j帧(与相机第i帧和第j帧时刻相同)雷达数据，根据loam系统提供的算法，获取这两帧雷达数据间的姿态。
首先，提取雷达数据的特征点，当前点的平滑度可以用下面公式评估：
其中，
然后，假设当前帧为第i帧，获得特征点集合pi，将第j帧点云保存到kdtree中，能够快速获取集合pi中特征点的最邻近点，把这些最邻近点作为pi的匹配点。这样，两帧雷达点云数据之间的关系可描述如下：
其中，
这里选取点到直线和点到面的距离作为icp算法里面的距离，迭代求解得到当前两帧雷达数据之间的姿态
进一步地，所述步骤3进一步具体为：通过步骤1，2，得到相同时刻第i帧和第j帧相机和雷达各自的相对姿态
可以得到：
其中，
将r和t分别代入，得到：
其中
其中r∈so(3)，由李代数到李群指数映射，可以得到：
其中exp表示李代数到李群的指数映射。由上面公式可以得到如下关系：
所以：
假设矩阵
所以：
将
所以：
这里已经得到一个初始的外参估计。
以上是两幅视图得到激光雷达与相机之间的外参。
进一步地，所述步骤4进一步具体为：对得到的外参做进一步的优化，通过两幅视图得到雷达和相机之间的外参，在进一步地，做相邻两两视图之间的雷达和相机之间的外参，得到若干组结果之后，取平均值。
根据本发明的另一方面，提供一种激光雷达和相机之间外参标定的系统和方法，系统包括如下模块：
数据采集模块，数据包括棋盘格图像序列和雷达点云数据，分别通过相机和激光雷达获取。
计算机处理模块，包括：a.图像棋盘格角点检测模块，b.两帧图像外参估计模块，c.激光雷达特征点检测模块，d.两帧雷达外参估计模块，e.激光雷达和相机之间外参估计模块，f.多帧数据求平均值模块。
a.图像棋盘格角点检测模块：对当前两帧图像，利用角点检测算法提取当前图像中的棋盘格角点。
b.两帧图像外参估计模块：对当前两帧图像，利用角点匹配关系获得当前两证图像之间的单应性矩阵h，通过对单应性矩阵h的分解，可以得到四组不同的外参估计，根据三维点必须落在当前相机前方的约束，找出正确的姿态估计。
c.激光雷达特征点检测模块：根据上述中的雷达数据平滑度估计公式，筛选出符合条件的特征点，通过kdtree去搜索下一帧最邻近空间点作为匹配点。
d.两帧雷达外参估计模块：根据上述雷达点运动关系，构建方程组，得到连续两帧雷达数据之间的外参姿态。
e.激光雷达和相机之间外参估计模块：通过上述公式，利用雷达外参和相机外参之间数学关系，得到激光雷达和相机之间的外参估计。
f.多帧数据求平均值模块：将上述模块运用到连续两帧数据之间获取多组激光雷达和相机之间外参估计，将旋转矩阵转换成旋转向量，求平均值。
本方法能够鲁棒地估计激光雷达和工业相机之间的外参。
附图说明
附图说明用于提供对本发明技术方案的进一步理解，并构成说明书的一部分，与本发明的实施一起用于解释本发明的技术方案，并不构成对本发明技术方案的限制。附图说明如下：
图1是本发明方法的流程图。图2是本发明系统的硬件组成图。
具体实施方式
如图1所示，本发明提供一种激光雷达和工业相机之间外参标定的系统和方法，包括如下步骤：
步骤1：图像棋盘格角点检测
利用角点检测算法，可以快速提取棋盘格角点。
步骤2：两帧图像外参估计
利用棋盘格角点匹配关系，得到两幅视图之间的单应性矩阵，通过对单应性矩阵的分解和真实空间点在相机前方的约束，得到两帧图像之间的姿态估计。
步骤3：激光雷达特征点检测
当前帧雷达数据，当前点的平滑度可以用下面公式评估：
其中，
步骤4：两帧雷达外参估计
当前帧为第i帧，获得特征点集合pi，将第j帧点云保存到kdtree中，能够快速获取集合pi中特征点的最邻近点，把这些最邻近点作为pi的匹配点。这样，两帧雷达点云数据之间的关系可描述如下：
这里选取点到直线和点到面的距离作为icp算法里面的距离，迭代求解得到当前两帧雷达数据之间的姿态
步骤5：激光雷达和相机之间外参估计
相同时刻第i帧和第j帧相机和雷达各自的相对姿态
可以得到：
将r和t分别代入，得到：
其中
由上面公式可以得到如下关系：
所以：
假设矩阵
所以：
将
所以：
这里已经得到一个初始的外参估计。
步骤6：多帧数据求平均值
将上述步骤运用到连续两帧数据之间获取多组激光雷达和相机之间外参估计，将旋转矩阵转换成旋转向量，求平均值。
如图2所示。本发明提供一种激光雷达和工业相机之间外参标定的方法，包括如下模块：
201数据采集模块，主要是通过激光雷达获得三维点云，通过相机获得棋盘格图像序列。
202计算机处理模块，包括：a.图像棋盘格角点检测模块，b.两帧图像外参估计模块，c.激光雷达特征点检测模块，d.两帧雷达外参估计模块，e.激光雷达和相机之间外参估计模块，f.多帧数据求平均值模块。
a.图像棋盘格角点检测模块：对当前两帧图像，利用角点检测算法提取当前图像中的棋盘格角点。
b.两帧图像外参估计模块：对当前两帧图像，利用棋盘格角点匹配关系，得到两幅视图之间的单应性矩阵，通过对单应性矩阵的分解和真实空间点在相机前方的约束，得到两帧图像之间的姿态估计。
c.激光雷达特征点检测模块：根据上述中的雷达数据平滑度估计公式，筛选出符合条件的特征点，通过kdtree去搜索下一帧最邻近空间点作为匹配点。
d.两帧雷达外参估计模块：根据上述雷达点运动关系，构建方程组，得到连续两帧雷达数据之间的外参姿态。
e.激光雷达和相机之间外参估计模块：通过上述公式，利用雷达外参和相机外参之间数学关系，得到激光雷达和相机之间的外参估计。
f.多帧数据求平均值模块：将上述模块运用到连续两帧数据之间获取多组激光雷达和相机之间外参估计，将旋转矩阵转换成旋转向量，求平均值。
本领域的技术人员应该明白，上述的本发明的系统结构和各个步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置组成的网络上，本发明不限制于任何特定的硬件和软件结合。
虽然本发明所示出和描述的实施方式如上，但是所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施的形式上以及细节上做任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。