CN109816703B - 一种基于相机标定和icp算法的点云配准方法 - Google Patents

Abstract

本发明实施例公开了一种基于相机标定和ICP算法的点云配准方法，包括以下步骤：在深度相机和标定物上分别建立相机坐标系和世界坐标系，所述标定物为正反两面有相同棋盘格的标定板；采用多个深度相机同时拍摄标定物，通过改变所述标定物的位置或角度，拍摄多组图像，确定相机坐标系间转换关系；采用多个深度相机同时拍摄目标物体，获取不同视点下的点云，并对所述点云执行预处操作；在根据标定结果粗略配准点云之后，使用ICP算法精确配准点云；融合配准后的点云，使用移动最小二乘法滤波平滑点云，并添加RGB信息，输出三维彩色点云模型。通过本申请的方案，能够提升点云配准的效率。

Description

一种基于相机标定和ICP算法的点云配准方法

技术领域

本发明属于机器视觉技术领域，具体涉及多个深度相机的RGB和Depth标定与配准方法。

背景技术

近年来三维深度感知设备开始进入人们的眼球，高精度的三维深度感知设备作为一种新型的获取外界信息的媒介，有利于推动机器视觉的发展，有助于实现机器人理解外部世界，使其更加智能化，同时也推动了人机交互的发展。三维深度感知设备的应用离不开三维重建，即在计算机中对三维物体建立适当的数学模型。

点云配准是三维重建中的核心环节，多个深度相机的点云之间配准可以简化为两两配准问题。两两配准也就是两视角配准，通过一定的算法计算得到两个不同视角下点云之间的转换关系，一般用一个旋转矩阵和一个平移矩阵来表示这种转换关系。ICP算法由Besl和Mckay提出，是一种基于最小二乘法的优化算法，用于解决基于自由形态曲面的配准问题。该算法实现了点集到点集的精确配准，是点云配准的经典算法之一。但传统ICP算法对拟配准的两个点云的初始配准位置有要求，鲁棒性较差，容易陷入局部最优解。

发明内容

鉴于此，本发明提供了多个深度相机的RGB和深度信息(Depth)标定与配准方法，利用正反两面有相同棋盘格的标定板同时标定多台深度相机的RGB和Depth摄像头，使用标定结果粗略配准经预处理后的点云，再使用ICP算法精确配准点云，最后添加RGB信息，输出三维彩色点云模型。本发明可同时标定多个相机，并可以提高RGB和Depth之间配准的鲁棒性和精度。

本发明实施例提供了一种基于相机标定和ICP算法的点云配准方法，包括以下步骤：

步骤1、在深度相机和标定物上分别建立相机坐标系和世界坐标系，所述标定物为正反两面有相同棋盘格的标定板；

步骤2、采用多个深度相机同时拍摄标定物，通过改变所述标定物的位置或角度，拍摄多组图像，确定相机坐标系间转换关系；

步骤3、采用多个深度相机同时拍摄目标物体，获取不同视点下的点云，并对所述点云执行预处操作；

步骤4、在根据标定结果粗略配准点云之后，使用ICP算法精确配准点云；

步骤5、融合配准后的点云，使用移动最小二乘法滤波平滑点云，并添加RGB信息，输出三维彩色点云模型。

根据本发明实施例的一种具体实现方式，所述标定物为正反两面有相同棋盘格的标定板。

根据本发明实施例的一种具体实现方式，利用多相机标定结果粗略匹配参考点云和目标点；

根据刚体的旋转平移变换，利用相机坐标转换矩阵计算得到的旋转矩阵和平移向量，把参考点云的坐标转换到目标点云所在的相机坐标系下。

根据本发明实施例的一种具体实现方式，所述对所述点云执行预处操作，包括：

使用直通滤波器去除点云中无用的背景信息；

采用半径滤波方法去除离群的杂点以及体素滤波器进行降采样。

根据本发明实施例的一种具体实现方式，所述步骤4还包括：

采用kd-tree来加快点云的检索速度，在进行ICP配准之前，先构造模板点集的kd-tree结构；

当匹配点集需要搜索模板点集的对应点时，直接在kd-tree中搜索，提升对应点的搜索速度；

对于ICP算法中使对应点距离最小的等式，使用奇异值分解SVD来求解。

根据本发明实施例的一种具体实现方式，所述步骤5还包括：

对融合后的点云进行移动最小二乘法滤波，平滑点云。

采用本发明技术方案的有益效果将通过以下实施例的阐述而得到具体的体现。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例提供的整体流程示意图；

图2是本发明实施例提供的示例四个相机标定的示意图；

图3是本发明实施例提供的精确配准的流程图。

具体实施方式

下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

下面结合附图对本发明进行进一步的详细说明。

图1示出了本发明实施例的多个深度相机的RGB和Depth精确配准的整体流程。本实施例中所使用的深度相机个数为4个，但用于配准的深度相机的个数非限定4个，可以为2个或者更多个。

步骤1、在深度相机和标定物上分别建立相机坐标系和世界坐标系，其中标定物为正反两面有相同棋盘格的标定板。

本发明中多台相机的摆放需要保证相邻两个点云数据在相机的视角上具有重叠区域。在每一个相机上，以光心为原点，X轴和Y轴分别平行于相机像平面的两条垂直边，相机光轴为Z轴建立该相机的相机坐标系，获取的点云建立在其对应相机的相机坐标系下。

在标定版上建立两个世界坐标系，取棋盘格标定板正面左上方区域的一点作为第一个世界坐标系的原点，以相互垂直的网格线为该世界坐标系的X轴和Y轴。第二个世界坐标系的原点位于棋盘格标定板反面，相互垂直的网格线作为这个世界坐标系的X轴和Y轴。两个世界坐标系的原点，在第一个世界坐标系下有相同的x和y坐标，以两个世界坐标系之间的转换关系如下：

其中(X_w1，Y_w1，Z_w1)是建立在棋盘标定板正面的世界坐标系下的坐标，(X_w2，Y_w2，Z_W2)是建立在棋盘标定板反面的世界坐标系下的坐标，d是棋盘标定板的厚度，可以用游标卡尺测量得到。

步骤2、多个深度相机同时拍摄标定物，改变标定物的位置或角度，拍摄多组图像，确定相机坐标系间转换关系。

其中，进行标定前先关闭红外投射器和添加额外的红外辅助光源，避免投射器发出的散斑在红外图像中产生很多亮点，不利于棋盘角点的检查。

图2是四个相机标定的示意图，前后左右四个深度相机同时拍摄一块正反两面相同的棋盘标定板。前面相机和右边相机拍到的是棋盘标定板的正面，后面相机和左边相机拍到的是棋盘标定板的反面。

使用张正友标定法确定单个深度相机的RGB和Depth摄像头相机坐标系的转换关系，以及Depth摄像头相机坐标系与标定物上世界坐标系的转换关系。不同深度相机的Depth摄像头相机坐标系的转换关系如下：

其中R_f和T_f，R_r和T_r分别是前面，右边相机的相机机坐标系与建立在棋盘标定板正面的世界坐标系之间的旋转矩阵和平移矩阵，R_b和T_b，R_l和T_l分别是后面，左边相机的相机坐标系与建立在棋盘标定板反面的世界坐标系之间的旋转矩阵和平移矩阵，R_w和T_w是建立在棋盘标定板正反两面的两个世界坐标系之间的旋转矩阵和平移矩阵，d是棋盘标定板的厚度，可以用游标卡尺测量得到。通过张正友标定法可以求解出R_f，T_f，R_r，T_r，R_b，T_b，R_l，T_l。

步骤3、多个深度相机同时拍摄目标物体，获取不同视点下的点云，预处理获取点云，去除背景信息，去除离群杂点以及降采样。

其中，去除点云中无用的背景信息，是指采用直通滤波器对参考点云和目标点云进行滤波。直通滤波器根据用户设置的多个平面把空间分割成两个份，保留其中一个空间中的所有点，去除另外一个空间中的所有点。

采用半径滤波方法去除离群的杂点，根据用户设置的半径和阈值，遍历点云中的所有点，对于任何一个点，统计其半径为r的领域所包含的点的数量，当点的数量小于某个阈值时，则判定这个点是离群点，从点云中剔除这个点。

降采样是采用体素滤波器，设置边长为m的为体素，创建一个三维体素栅格，把三维空间分割成一个个体素。其中，边长m是根据数据密度、相机精度和实际场景中目标物体与相机的距离来设定。对于每个体素，它包含的点用一个点代替，这个点是这些点的重心，这样替换后得到的点云就是降采样后的点云。

步骤4、根据标定结果粗略配准点云，再使用ICP算法精确配准点云。

其中，经典ICP算法搜寻两个点集的对应点速度过慢，本发明中采用kd-tree来加快点云的检索速度，在进行ICP配准之前，先构造模板点集的kd-tree结构。当匹配点集需要搜索模板点集的对应点时，直接在kd-tree中搜索，以此提升对应点的搜索速度。对于ICP算法中使对应点距离最小的等式，本发明中使用奇异值分解SVD来求解。

ICP算法一次可以配准两个点云，对于多组带配准点云，采用两两配准的方法依次配准，下面以两组点云配准为例具体说明本发明的精确配准方法。

图3是本发明中精确配准的流程图，假定需要配准源点集

和目标点集

即求取点集P所在的坐标系到点集Q所在的坐标系的旋转矩阵R和平移矩阵T，按照以下的步骤进行精确配准：

根据步骤2多相机标定计算出源点集P的坐标系到目标点集Q的坐标系的旋转矩阵和平移矩阵的粗略估计值R₀和T₀，并得到点集P粗略配准结果：

P＝R₀P+T₀。

构造目标点集Q的kd-tree存储结构，记作Qtree。

定对应点间距离的阈值d_max，算法最大迭代次数n，停止迭代的最大误差值e_max，初始化R和T为单位阵，R＝T＝I。

对于点集P中的每一个点

在kd-ree结构Qtree中找到与之欧式距离最小的点

如果

和

的欧氏距离小于所设定的阈值dmax，则

和

组成一个对应点对

点集P中所有的对应点组成一个新的点集

点集Q中所有的对应点组成一个新的点集

是一个对应点对。

利用最小二乘法，建立误差函数如下式所示，运用奇异值分解法求解使得误差值最小旋转矩阵R′和平移矩阵T′。

对点集P进行旋转平移变换，如下式所示

P＝R′P+T′

更新旋转矩阵R和平移矩阵T的值

若误差值e大于e_max或者迭代次数小于n则重新计算新的点集P与Q的对应点，重复上述步骤，否则停止迭代，由下式求得最优的旋转矩阵R和平移矩阵T

步骤5、融合配准后的点云，使用移动最小二乘法滤波平滑点云，并添加RGB信息，输出三维彩色点云模型。

其中，步骤4中ICP算法对应点对中的点，在点云融合得到的新点云中只选择保留点对中的其中的一个点，对于不是对应点的点则全部保留。去除对应点后的点云数据作为点云融合的结果进行移动最小二乘法滤波，平滑点云。滤波处理后根据RGB摄像头和Depth摄像头的标定结果，给点云添加RGB信息，输出三维彩色点云模型。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (3)

1.一种基于相机标定和ICP算法的点云配准方法，包括以下步骤：

步骤1、在深度相机和标定板上分别建立相机坐标系和世界坐标系，所述标定板为正反两面有相同棋盘格的标定板；

所述深度相机包括按照顺时针方向依次设置的多个深度相机：前面相机、右边相机、后面相机和左边相机，其中，前面相机和右边相机拍到的是棋盘标定板的正面，后面相机和左边相机拍到的是棋盘标定板的反面；

前后左右四个深度相机的摆放需要保证相邻两个点云数据在相机的视角上具有重叠区域；

步骤2、采用前后左右四个深度相机同时拍摄一块正反两面相同的棋盘标定板，并通过改变所述标定板的位置或角度，拍摄多组图像，确定相机坐标系间转换关系；

步骤3、采用前后左右四个深度相机同时拍摄目标物体，获取不同视点下的点云，并对所述点云执行预处理操作；

步骤4、在根据标定结果粗略配准点云之后，使用ICP算法精确配准点云；

步骤5、融合配准后的点云，使用移动最小二乘法滤波平滑点云，并添加RGB信息，输出三维彩色点云模型；

其中，步骤1具体包括如下步骤：

在深度相机和标定板上分别建立相机坐标系和世界坐标系，其中标定板为正反两面有相同棋盘格的标定板；

在每一个相机上，以光心为原点，X轴和Y轴分别平行于相机像平面的两条垂直边，相机光轴为Z轴建立该相机的相机坐标系，获取的点云建立在其对应相机的相机坐标系下；

在标定版上建立两个世界坐标系，取棋盘格标定板正面左上方区域的一点作为第一个世界坐标系的原点，以相互垂直的网格线为该世界坐标系的X轴和Y轴；第二个世界坐标系的原点位于棋盘格标定板反面，相互垂直的网格线作为这个世界坐标系的X轴和Y轴；两个世界坐标系的原点，在第一个世界坐标系下有相同的x和y坐标，以两个世界坐标系之间的转换关系如下：

其中，(X_w1, Y_w1, Z_w1)是建立在棋盘标定板正面的世界坐标系下的坐标，(X_w2, Y_w2,Z_w2)是建立在棋盘标定板反面的世界坐标系下的坐标，d是棋盘标定板的厚度，用游标卡尺测量得到，R_w和T_w是建立在棋盘标定板正反两面的两个世界坐标系之间的旋转矩阵和平移矩阵；

其中，步骤2具体包括如下步骤：

前后左右四个深度相机同时拍摄标定板，改变标定板的位置或角度，拍摄多组图像，确定相机坐标系间转换关系；

其中，进行标定前先关闭红外投射器和添加额外的红外辅助光源，避免投射器发出的散斑在红外图像中产生很多亮点，不利于棋盘标定板角点的检查；

前后左右四个深度相机同时拍摄一块正反两面相同的棋盘标定板，具体包括：前面相机和右边相机拍到的是棋盘标定板的正面，后面相机和左边相机拍到的是棋盘标定板的反面；

使用张正友标定法确定单个深度相机的RGB和Depth摄像头相机坐标系的转换关系，以及Depth摄像头相机坐标系与标定板上世界坐标系的转换关系，不同深度相机的Depth摄像头相机坐标系的转换关系如下：

其中，R_f和T_f分别是前面相机的相机坐标系与建立在棋盘标定板正面的世界坐标系之间的旋转矩阵和平移矩阵，

R_r和T_r分别是右边相机的相机坐标系与建立在棋盘标定板正面的世界坐标系之间的旋转矩阵和平移矩阵，

R_b和T_b分别是后面相机的相机坐标系与建立在棋盘标定板反面的世界坐标系之间的旋转矩阵和平移矩阵；

R_l和T_l分别是左边相机的相机坐标系与建立在棋盘标定板反面的世界坐标系之间的旋转矩阵和平移矩阵；

通过张正友标定法求解出R_f，T_f，R_r，T_r，R_b，T_b，R_l，T_l。

2.根据权利要求1所述方法，其特征在于，所述步骤3中，对所述点云执行预处理操作，包括：

使用直通滤波器去除点云中背景信息；

采用半径滤波方法去除离群的杂点以及体素滤波器进行降采样。

3.根据权利要求1所述方法，其特征在于，所述步骤4还包括：

采用kd-tree来加快点云的检索速度，在进行ICP配准之前，先构造模板点集的kd-tree结构；

当匹配点集需要搜索模板点集的对应点时，直接在kd-tree中搜索，提升对应点的搜索速度；

对于ICP算法中使对应点距离最小的等式，使用奇异值分解SVD来求解。

Images