CN103413352A - 基于rgbd多传感器融合的场景三维重建方法 - Google Patents

Abstract

本发明公开了一种基于RGBD多传感器融合的场景三维重建方法，用于解决现有场景三维重建方法重建速度低的技术问题。技术方案是首先采用SURF算法代替SIFT算法进行特征匹配，其次，根据观察中心的方法改进了子地图的划分标准，使得经多层图抽象后的图像金字塔更好的反映环境的拓扑结构，更有助于提高构建大场景三维地图的速度。同时，在地图生成部分，提出了使用经典的基于空间体的多帧融合方法来减少冗余点，提高了重建速度。经测试，本发明方法的重建速度比背景技术提高了近两倍。

Description

基于RGBD多传感器融合的场景三维重建方法

技术领域

本发明涉及一种场景三维重建方法，特别涉及一种基于RGBD多传感器融合的场景三维重建方法。

背景技术

在自主移动机器人领域，对未知环境的探索与建立一直以来都是一个研究热点。现有的三维场景重建方法主要有：基于红外激光传感器和基于视觉传感器两大类。

文献“RGB-D mapping: Using depth cameras for dense 3D modeling of indoorenvironments，12th ISER. 2010, 20: p22-25”公开了一种基于RGB-D传感器的TORO图优化算法。该方法采用SIFT算法进行特征匹配，得到相对于第一帧的当前帧位姿的初始估计，进而使用ICP算法进行点云匹配以优化初始估计，当检测到loop closure时，将输入的所有帧添加到pose图中进行全局优化，从而得到高精度的地图。但是，在时间代价方面，特征匹配是采用计算速度一般的SIFT算法，不能很好地满足实时三维重建对实时速度的要求，因此，在特征匹配时应该采用速度更高的算法，以提高实时性。在地图优化方面，由于移动设备的轨迹仅是以一个位置为中心来构建地图的，导致空缺区域的产生，因此不能建立完整的地图；同时，在输出3D点云图时，仅将各个帧的点云数据简单相加，导致冗余点的增加，不仅降低了运算速度，而且为后续的再处理带来了极大干扰。

发明内容

为了克服现有场景三维重建方法重建速度低的不足，本发明提供一种基于RGBD多传感器融合的场景三维重建方法。该方法首先采用SURF算法代替SIFT算法进行特征匹配，其次，根据观察中心的方法改进了子地图的划分标准，使得经多层图抽象后的图像金字塔更好的反映环境的拓扑结构，更有助于提高构建大场景三维地图的速度。同时，在地图生成部分，提出了使用经典的基于空间体的多帧融合方法来减少冗余点，不仅可以提高重建速度，而且为后续操作提供了更为准确的数据。

本发明解决其技术问题所采用的技术方案是：一种基于RGBD多传感器融合的场景三维重建方法，其特点是包括以下步骤：

步骤一、采用SURF算法进行特征提取与匹配。特征的提取利用计算近似的Hessian矩阵的行列式的极值来确定特征点的位置。输入一幅图像I，对图上每一个像素点x=（x，y），利用公式（1）计算出其在x处尺度为的Hessian矩阵H(X，σ)。

$H(x,\mathrm{\σ})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

其中，L_xx(x,σ)是Gaussian二阶偏导数

在x处对图像I的卷积。继而，计算每个H的行列式：

det(H_approx)=D_xxD_yy-(0.9D_xy)²   (2)

其中，D_xx是盒子模版与图像的卷积，用D_xx近似代替L_xx。接着，判断行列式（2）的符号。若符号为负，则不是局部极值点；若符号为正，则该点归类为极值点。

接着，构建图像的尺度空间，SURF通过不同尺寸盒子作为过滤模版和积分图像求取Hessian矩阵行列式的响应图像，然后，在响应图像上采用3D非极大值抑制，求取各个不同尺度的斑点。使用3×3×3的模版在三维空间内进行非极大值抑制，根据预设的Hessian阈值H比较，当h大于H，且比邻近的26个点的相应值都大时，被选为特征点，然后采用三维线性插值使其更精确。

以特征点为中心，以6s为半径的圆内，对图像进行Haar小波响应运算，设计一个以方向为中心，张角为PI/3d的扇形滑动窗口，以步长为0.2弧度，转动滑动窗口，并对窗口内所有点的Haar响应值进行累加，主方向为最大的Haar响应累加值对应的方向。

进行特征点的匹配。

1）在计算H矩阵的行列式时，同时计算出矩阵的迹。按亮度不同，将特征点分为两种：①特征点及其周围小邻域的亮度大于背景区域，H矩阵的迹为正；②特征点及其周围小邻域的亮度小于背景区域，H矩阵的迹为负。先对两个特征点的H矩阵的迹进行比较：同号，说明两个特征点具有相同对比度；异号，说明对比度不同，则放弃步骤2）的相似性度量；

2）在步骤1）的基础上，对两个特征点描述子的相似性度量，采用欧式距离进行计算：

${\mathrm{Dis}}_{\mathrm{ij}}={\left[\begin{array}{c}\underset{k=0}{\overset{k=n}{\mathrm{\∑}}}{(X_{\mathrm{ik}}-X_{\mathrm{jk}})}^2\end{array}\right]}^{1/2}---\left(3\right)$

其中，X_ik表示待配准图中第i个特征描述子的第k个元素，X_jk表示参考图中第j个特征描述子的第k个元素，n表示特征向量的维数。

对于待配准图上的特征点，计算其到参考图像上所有特征点的欧式距离，得到一个距离集合。通过对距离集合进行比较运算得到小欧式距离和次欧式距离。设定阈值，当最小欧式距离与次欧式距离的比值小于该阈值时，认为特征点与对应最小欧式距离的特征点是匹配的，否则，没有点与该特征点相匹配。

步骤二、针对位姿估计和优化的三维重建中点云数据配准问题，先采用RANSAC配准算法进行粗匹配，并结合深度信息进行初始位姿估计，将计算出结果作为ICP配准算法的初始值，再使用深度信息进行ICP算法对初始位姿的优化实现由粗到细的配准。

1）利用RANSAC算法划分出局外点和局内点，剔除匹配异常点，将得到的所有局内点随机建立点云集合，先进行粗匹配，设得到的两个点集分别是S和T，从源数据集S中随机抽取不在一条直线上的三个点｛S_a，S_b，S_c｝，在目标点集T中，搜索对应的三个点｛T_a，T_b，T_c｝,利用三对控制点进行欧式变换矩阵的估计，计算出H_c。然后计算在估计变换关系H_c误差阈值δ下，两个点集之间的一致程度，并利用对应点的深度信息得到初始位姿的估计；

2）以步骤1）的结果作为ICP算法的输入，避免单调收敛到局部最小值，根据最小化目标函数估计配准参数 $H_k=\left[\begin{array}{cc}{R}_k& T_k\\ 0& 1\end{array}\right],$ 并计算估计误差d_k；使用变换矩阵H_k对数据集P_k更新，P_k+1=H_k(P₀)＝R_kP_O+T_k;重复迭代，当两次计算的估计误差变化小于阈值τ时，即‖d_k-d_k+1‖〈τ时，停止；

步骤三、得到经优化的初始位姿后，将其作为节点，与其相邻的帧间的位姿关系作为边添加到图中，进而采用Hog-man图优化算法进行地图的全局优化，得到高精度的三维地图。

将相机观察中心的距离值作为标准将原始的图划分为多个子图，以子图中的一个节点来表示该子图，得到原始图一层的抽象，对得到的图再依次进行抽象，就得到一个多层图结构，提取出原始图中的拓扑结构。

$\mathrm{IP}=P{\left[\begin{array}{cccc}0& d& 0& 1\end{array}\right]}^T---\left(4\right)$

其中，O为摄像机中心，IP为兴趣中心点，d为IP到O的距离。

当有新节点加入到图中时，首先是加入到原始图中，然后查看是否改变子图划分，如果有改变就需要更新高层图，同时对最顶层进行优化，只有当顶层的拓扑结构发生很大改变的时候才由顶层图向底层图反向传递，更新底层图。

在对多帧点云数据进行融合时，首先建立一个包含所有帧的空间体，然后依次对每一帧进行处理，修改空间像素点的值。最后空间体中的零值像素即为最后的曲面上的点，这样得到的曲面是具有最小二乘性质的曲面。采用权值函数进行距离值叠加，最后的叠加方程如公式(5)、公式(6)。

$D_{i+1}\left(x\right)=\frac{W_1\left(x\right)D_1\left(x\right)+W_{i-1}\left(x\right)D_{i-1}\left(x\right)}{W_1\left(x\right)+W_{i-1}\left(x\right)}---\left(5\right)$

W_i+1(x)=W_i(x)+W_i-1(x)    (6)

$W\left(x\right)=\frac{{x-x}^{*}}{\mathrm{Err}\left(x^{*}\right)}---\left(7\right)$

Err(dist)=dist²×0.0035             (8)

根据Kinect传感器的误差方程(8)权值函数的选取公式(7)。其中，x *为单帧曲面上点的到相机中心的距离，最后得到整个场景的三维重建效果图。

本发明的有益效果是：该方法首先采用SURF算法代替SIFT算法进行特征匹配，其次，根据观察中心的方法改进了子地图的划分标准，使得经多层图抽象后的图像金字塔更好的反映环境的拓扑结构，更有助于提高构建大场景三维地图的速度。同时，在地图生成部分，提出了使用经典的基于空间体的多帧融合方法来减少冗余点，提高了重建速度。经测试，本发明方法的重建速度比背景技术提高了近两倍。

下面结合具体实施方式对本发明作详细说明。

具体实施方式

本发明基于RGBD多传感器融合的场景三维重建方法具体步骤如下：

1、特征提取与匹配。

采用SURF算法进行特征提取与匹配。特征的提取利用计算近似的Hessian矩阵的行列式的极值来确定特征点的位置。输入一幅图像I,对图上每一个像素点x=（x，y），利用公式（1）计算出其在x处尺度为σ的Hessian矩阵H（X,σ)。

$H(x,\mathrm{\σ})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

其中，L_xx(x，σ)是Gaussian二阶偏导数

在x处对图像I的卷积。继而，计算每个H的行列式：

det(H_approx)=D_xxD_yy-(0.9D_xy)²           (2)

其中，D_xx是盒子模版与图像的卷积，用D_xx近以代替L_xx。接着，判断行列式（2）的符号。若符号为负，则不是局部极值点；若符号为正，则该点归类为极值点。

接着，构建图像的尺度空间，即图像在不同解析度下的表示。与SIFT建立金字塔的原理相反，SURF通过不同尺寸盒子作为过滤模版和积分图像求取Hessian矩阵行列式的响应图像，然后，在响应图像上采用3D非极大值抑制，求取各个不同尺度的斑点。使用3×3×3的模版在三维空间内进行非极大值抑制，根据预设的Hessian阈值H比较，当h大于H，且比邻近的26个点的相应值都大时，被选为特征点，然后采用三维线性插值使其更精确。

为了保证特征矢量具有旋转不变性，需要给每一个特征点分配一个主方向。以特征点为中心，以6s为半径的圆内，对图像进行Haar小波响应运算，设计一个以方向为中心，张角为PI/3d的扇形滑动窗口，以步长为0.2弧度，转动滑动窗口，并对窗口内所有点的Haar响应值进行累加，主方向为最大的Haar响应累加值对应的方向。

进行完特征点提取后进行特征点的匹配，有如下步骤：

1）在计算H矩阵的行列式时，同时计算出矩阵的迹。由于按亮度不同，将特征点分为两种：①特征点及其周围小邻域的亮度大于背景区域，H矩阵的迹为正；②特征点及其周围小邻域的亮度小于背景区域，H矩阵的迹为负。依此特性，先对两个特征点的H矩阵的迹进行比较：同号，说明两个特征点具有相同对比度；异号，说明对比度不同，则放弃2）的相似性度量；

2）在1）的基础上，对两个特征点描述子的相似性度量，采用欧式距离进行计算：

${\mathrm{Dis}}_{\mathrm{ij}}={\left[\begin{array}{c}\underset{k=0}{\overset{k=n}{\mathrm{\∑}}}{(X_{\mathrm{ik}}-X_{\mathrm{jk}})}^2\end{array}\right]}^{1/2}---\left(3\right)$

其中，X_ik;从表示待配准图中第i个特征描述子的第k个元素，X_jk表示参考图中第j个特征描述子的第k个元素，n表示特征向量的维数。

对于待配准图上的特征点，计算其到参考图像上所有特征点的欧式距离，得到一个距离集合。通过对距离集合进行比较运算得到小欧式距离和次欧式距离。设定阈值，一般为0.8，当最小欧式距离和次欧式距离的比值小于该阈值时，认为特征点与对应最小欧式距离的特征点是匹配的，否则，没有点与该特征点相匹配。阈值越小，匹配越稳定，但极值点越少。

2、位姿估计与优化。

针对位姿估计和优化的三维重建中点云数据配准问题，为了防止ICP算法收敛到局部最小点，产生错误的配准结果，则先采用RANSAC配准算法进行粗匹配，并结合深度信息进行初始位姿估计，将计算出结果作为ICP配准算法的初始值，再次使用深度信息进行ICP算法对初始位姿的优化实现由粗到细的配准策略，并减少迭代次数，达到令人满意的收敛结果。

具体步骤如下：

1）利用RANSAC算法划分出“局外点”和“局内点”，剔除匹配异常点，将得到的所有局内点随机建立点云集合，先进行粗匹配，设得到的两个点集分别是S和T，从源数据集S中随机抽取不在一条直线上的三个点｛S_a，S_b，S_c｝，在目标点集T中，搜索对应的三个点｛T_a，T_b，T_c｝,利用三对控制点进行欧式变换矩阵的估计，计算出H_c。然后计算在估计变换关系H_c误差阈值δ下，两个点集之间的一致程度，并利用对应点的深度信息得到初始位姿的估计；

2）以1）计算出的结果作为ICP算法的输入，避免单调收敛到局部最小值，根据最小化目标函数估计配准参数 $H_k=\left[\begin{array}{cc}{R}_k& T_k\\ 0& 1\end{array}\right],$ 并计算估计误差d_k;使用变换矩阵H_k对数据集P_k更新，P_k+1＝H_k(P₀)＝R_kP₀+T_k;重复迭代，当两次计算的估计误差变化小于阈值τ时，即‖d_k-d_k+1‖＜τ时，停止；

3、Hog-man地图优化。

得到经优化的初始位姿后，将其作为节点，与其相邻的帧间的位姿关系作为边添加到图中，进而采用Hog-man图优化算法进行地图的全局优化，得到高精度的三维地图。

由于当想要建立一块区域完整的信息时，会在这块区域周围采集较多的点属于一种包围式的建立。因此，核心思想是根据相机观察中心的距离值（如式(5)）作为标准将原始的图划分为多个子图，以子图中的一个节点来表示该子图，从而得到原始图一层的抽象，对得到的图再依次进行抽象，就得到一个多层图结构，提取出原始图中的拓扑结构。

$\mathrm{IP}=P{\left[\begin{array}{cccc}0& d& 0& 1\end{array}\right]}^T---\left(4\right)$

其中，设O为摄像机中心，IP为兴趣中心点，d为IP到O的距离，这里取2m。假设当前相机的位姿用一个4×4的矩阵P表示，则IP的坐标为使用观察中心代替相机中心作为子图分割标准，更好地反应建立完整地图时的节点分布特点。

当有新节点加入到图中时，首先是加入到原始图中，然后查看是否改变子图划分，如果有改变就需要更新高层图，同时对最顶层进行优化，只有当顶层的拓扑结构发生很大改变的时候才由顶层图向底层图反向传递，更新底层图。这样就保证了实时的优化。

根据得到的优化结果进行简单叠加得到为了去除直接叠加所有帧带来的冗余点，采用的基于空间体的点云融合方法。其主要思想是在对多帧点云数据进行融合时，首先建立一个包含所有帧的空间体，空间体以一定的分辨率划分为空间像素点，每个点上将记录到环境中物体的距离，然后依次对每一帧进行处理，修改空间像素点的值。最后空间体中的零值像素即为最后的曲面上的点，这样得到的曲面是具有最小二乘性质的曲面。同时为了反映采集到的每帧数据的一些特点，在进行距离值叠加的时候，采用权值函数，最后的叠加方程如式(5)、公式(6)。

$D_{i+1}\left(x\right)=\frac{W_1\left(x\right)D_1\left(x\right)+W_{i-1}\left(x\right)D_{i-1}\left(x\right)}{W_1\left(x\right)+W_{i-1}\left(x\right)}---\left(5\right)$

W_i+1(x)＝Ｗ_i(x)+W_i-1(x)   (６)

$W\left(x\right)=\frac{{x-x}^{*}}{\mathrm{Err}\left(x^{*}\right)}---\left(7\right)$

Err(dist)＝dist²×0.0035   (8)

实验中使用kinet摄像头作为RGBD摄像头，因此，根据Kinect传感器的误差方程(8)权值函数的选取如公式(7)。其中x*为单帧曲面上点的到相机中心的距离，最后得到3D点云图，即整个场景的三维重建效果图。

Claims (1)

1.一种基于RGBD多传感器融合的场景三维重建方法，其特征在于包括以下步骤：

步骤一、采用SURF算法进行特征提取与匹配；特征的提取利用计算近似的Hessian矩阵的行列式的极值来确定特征点的位置；输入一幅图像I，对图上每一个像素点x=（x，y），利用公式（1）计算出其在x处尺度为σ的Hessian矩阵H(X,σ);

$H(x,\mathrm{\σ})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

其中，L_xx(x，σ)是Gaussian二阶偏导数

在x处对图像I的卷积；继而，计算每个H的行列式：

det(H_approx)=D_xxD_yy-(0.9D_xy)²   (2)

其中，D_xx是盒子模版与图像的卷积，用D_xx近似代替L_xx;接着，判断行列式（2）的符号；若符号为负，则不是局部极值点；若符号为正，则该点归类为极值点；

接着，构建图像的尺度空间，SURF通过不同尺寸盒子作为过滤模版和积分图像求取Hessian矩阵行列式的响应图像，然后，在响应图像上采用3D非极大值抑制，求取各个不同尺度的斑点；使用3×3×3的模版在三维空间内进行非极大值抑制，根据预设的Hessian阈值H比较，当h大于H，且比邻近的26个点的相应值都大时，被选为特征点，然后采用三维线性插值使其更精确；

以特征点为中心，以6s为半径的圆内，对图像进行Haar小波响应运算，设计一个以方向为中心，张角为PI/3d的扇形滑动窗口，以步长为0.2弧度，转动滑动窗口，并对窗口内所有点的Haar响应值进行累加，主方向为最大的Haar响应累加值对应的方向；

进行特征点的匹配；

1）在计算H矩阵的行列式时，同时计算出矩阵的迹；按亮度不同，将特征点分为两种：①特征点及其周围小邻域的亮度大于背景区域，H矩阵的迹为正；②特征点及其周围小邻域的亮度小于背景区域，H矩阵的迹为负；先对两个特征点的H矩阵的迹进行比较：同号，说明两个特征点具有相同对比度；异号，说明对比度不同，则放弃步骤2）的相似性度量；

2）在步骤1）的基础上，对两个特征点描述子的相似性度量，采用欧式距离进行计算：

${\mathrm{Dis}}_{\mathrm{ij}}={\left[\begin{array}{c}\underset{k=0}{\overset{k=n}{\mathrm{\∑}}}{(X_{\mathrm{ik}}-X_{\mathrm{jk}})}^2\end{array}\right]}^{1/2}---\left(3\right)$

其中，X_ik表示待配准图中第i个特征描述子的第k个元素，X_jk表示参考图中第j个特征描述子的第k个元素，n表示特征向量的维数；

对于待配准图上的特征点，计算其到参考图像上所有特征点的欧式距离，得到一个距离集合；通过对距离集合进行比较运算得到小欧式距离和次欧式距离；设定阈值，当最小欧式距离与次欧式距离的比值小于该阈值时，认为特征点与对应最小欧式距离的特征点是匹配的，否则，没有点与该特征点相匹配；

步骤二、针对位姿估计和优化的三维重建中点云数据配准问题，先采用RANSAC配准算法进行粗匹配，并结合深度信息进行初始位姿估计，将计算出结果作为ICP配准算法的初始值，再使用深度信息进行ICP算法对初始位姿的优化实现由粗到细的配准；

1）利用RANSAC算法划分出局外点和局内点，剔除匹配异常点，将得到的所有局内点随机建立点云集合，先进行粗匹配，设得到的两个点集分别是S和T，从源数据集S中随机抽取不在一条直线上的三个点｛S_a，S_b，S_c｝，在目标点集T中，搜索对应的三个点｛T_a，T_b，T_c｝,利用三对控制点进行欧式变换矩阵的估计，计算出H_c；然后计算在估计变换关系H_c误差阈值δ下，两个点集之间的一致程度，并利用对应点的深度信息得到初始位姿的估计；

2）以步骤1）的结果作为ICP算法的输入，避免单调收敛到局部最小值，根据最小化目标函数估计配准参数 $H_k=\left[\begin{array}{cc}{R}_k& T_k\\ 0& 1\end{array}\right],$ 并计算估计误差d_k;使用变换矩阵H_k对数据集P_k更新，P_k+1＝H_k(P₀)＝Ｒ_kP₀+T_k;重复迭代，当两次计算的估计误差变化小于阈值τ时，即‖d_k-d_k+1‖＜τ时，停止；

步骤三、得到经优化的初始位姿后，将其作为节点，与其相邻的帧间的位姿关系作为边添加到图中，进而采用Hog-man图优化算法进行地图的全局优化，得到高精度的三维地图；

将相机观察中心的距离值作为标准将原始的图划分为多个子图，以子图中的一个节点来表示该子图，得到原始图一层的抽象，对得到的图再依次进行抽象，就得到一个多层图结构，提取出原始图中的拓扑结构；

$\mathrm{IP}=P{\left[\begin{array}{cccc}0& d& 0& 1\end{array}\right]}^T---\left(4\right)$

其中，O为摄像机中心，IP为兴趣中心点，d为IP到O的距离；

当有新节点加入到图中时，首先是加入到原始图中，然后查看是否改变子图划分，如果有改变就需要更新高层图，同时对最顶层进行优化，只有当顶层的拓扑结构发生很大改变的时候才由顶层图向底层图反向传递，更新底层图；

在对多帧点云数据进行融合时，首先建立一个包含所有帧的空间体，然后依次对每一帧进行处理，修改空间像素点的值；最后空间体中的零值像素即为最后的曲面上的点，这样得到的曲面是具有最小二乘性质的曲面；采用权值函数进行距离值叠加，最后的叠加方程如公式(5)、公式(6)；

$D_{i+1}\left(x\right)=\frac{W_1\left(x\right)D_1\left(x\right)+W_{i-1}\left(x\right)D_{i-1}\left(x\right)}{W_1\left(x\right)+W_{i-1}\left(x\right)}---\left(5\right)$

W_i+1(x)=W_i(x)+W_i-1(x)   (6)

$W\left(x\right)=\frac{{x-x}^{*}}{\mathrm{Err}\left(x^{*}\right)}---\left(7\right)$

E_rr(dist)-dist²×0.0035   (8)

根据Kinect传感器的误差方程(8)权值函数的选取公式(7)；其中，x*为单帧曲面上点的到相机中心的距离，最后得到整个场景的三维重建效果图。