CN104463953B - 基于惯性测量单元与rgb‑d传感器的三维重建方法 - Google Patents

Abstract

本发明公开了一种基于惯性测量单元与RGB‑D传感器的三维重建方法，用于解决现有基于RGB‑D传感器的三维重建方法实时性差的技术问题。技术方案是首先采用SURF算法进行关键点的提取，接下来计算各个关键点处法向量，然后计算其三维快速点特征直方图FPFH描述子；其次通过惯性测量单元获得RGB‑D传感器的粗略运动估计，将该粗略估计与由三维点云计算而来的运动估计通过算法处理得到较为精确的运动估计，然后将其作为ICP算法的初始值进行迭代计算；最终当检测到Loop Closure时，通过ELCH算法快速对全局构图进行优化。由于利用SURF算法寻找关键点，并采用三维快速点特征直方图FPFH特征作为关键点的特征描述子，提升了三维重构算法在特征提取时的速度；实时性强。

Description

基于惯性测量单元与RGB-D传感器的三维重建方法

技术领域

本发明涉及一种基于RGB-D传感器的三维重建方法，特别涉及一种基于惯性测量单元与RGB-D传感器的三维重建方法。

背景技术

文献“Henry P,Krainin M,Herbst E,et al.RGB-D mapping:Using depthcameras for dense 3D modeling of indoor environments[C]//In the 12thInternational Symposium on Experimental Robotics(ISER.2010.”公开了一种基于RGB-D传感器的三维重建方法。该方法利用SIFT(Scale-invariant feature transform)特征通过RANSAC(RANdom SAmple Consensus)算法得到一个粗略的运动估计，然后利用ICP(Iterative Closest Point)算法进行点云匹配来改善初始估计，在检测到环闭合(loopclosure)后，将所有的点云集通过TORO(Tree-based netwORk Optimizer)优化算法进行全局优化。文献所述方法由于SIFT特征的选择以及RANSAC算法的速度的原因，不能够满足三维构图的实时性；对于粗略估计方面，若粗略运动估计的结果误差较大，ICP算法将不能得到一个满意的效果；对于全局地图优化方面，通常是由特征点提取和数据融合(Frontend)与优化特征点和位姿(Backend)两部分相互迭代直到收敛完成的，效率不高。

发明内容

为了克服现有基于RGB-D传感器的三维重建方法实时性差的不足，本发明提供一种基于惯性测量单元与RGB-D传感器的三维重建方法。该方法首先采用SURF算法进行关键点的提取，然后计算关键点三维快速点特征直方图FPFH描述子；其次通过惯性测量单元获得RGB-D传感器的粗略运动估计，将该粗略估计与由三维点云计算而来的运动估计通过算法处理得到较为精确的运动估计，然后将其作为ICP算法的初始值进行迭代计算；最终当检测到Loop Closure时，通过ELCH算法快速对全局构图进行优化。由于利用SURF算法寻找关键点，并采用三维快速点特征直方图FPFH特征作为关键点的特征描述子，提升了三维重构算法在特征提取时的速度；在粗匹配的过程中，由于采用惯性测量单元对传感器位姿的估计与三维特征点云对传感器位姿的估计结合的方法，相对于背景技术RANSAC算法，减少了搜索的次数，提高了效率；由于为ICP算法提供了一个较为准确的初始值，防止ICP算法局部收敛，提升了匹配的精度；最终的地图优化部分，采用ELCH算法避免了通常所用地图优化算法所必须的迭代的过程，提升了算法的速度。

本发明解决其技术问题所采用的技术方案是：一种基于惯性测量单元与RGB-D传感器的三维重建方法，其特点是包括以下步骤：

步骤一、采用SURF算法进行关键点的提取，接下来采用基于局部表面拟合的方法计算各关键点处法向量，然后计算关键点的三维快速点特征直方图FPFH描述子

其中，P_q表示查询点，P_k表示其邻近点，权重ω_k表示查询点P_q和其邻近点P_k之间的距离。SPFH简化的点特征直方图的计算是依据这个点和它的K邻域点之间的一个元组得到的。

步骤二、通过三轴陀螺仪和三轴加速度计获取一个粗略变换矩阵。首先获得三轴陀螺仪和三轴加速度计所测量RGB-D传感器的XYZ三轴向的位移变化x、y和z以及三轴向的夹角变化α、β和γ。求得旋转矩阵R＝R_x R_y R_z，其中R_x、R_y以及R_z分别为传感器绕X轴、Y轴和Z轴的旋转矩阵

定义一个4阶的变换矩阵T如下：

其次通过kd-tree完成待配准点云集关键点与参考点云集关键点快速点特征直方图FPFH描述子的特征匹配。进行M次随机选点对

其中，s为最少要选取的点对的个数，ε为局内点的百分比，p是所需的匹配成功概率。通过每次随机选取的点对完成一个变换估计，接着通过扩展卡尔曼滤波算法将此变换与通过惯性测量单元所得到的变换T进行数据融合，得到一个融合后的变换T_EKF。使用T_EKF对参考点云集中的关键点进行坐标变换，并计算每一对匹配关键对之间的欧式距离

其中，P为点云集中的点，Dis为点对间的距离。将距离Dis小于一定阈值的点对视为局内点对，剩余的点对视为局外点对，选取局内点对数最多的一次进行后续计算，通过局内点对进行N次估计，粗略估计变换T_c。

最终将获得的粗略估计变换T_c与通过惯性测量单元所得到的变换T作用于其中一点云集，计算与另一点云集间的最小均方根，取使其中值较小的变换作为新的变换T_c。

步骤三、将T_c作为ICP算法的初始值，避免算法局部收敛。将粗略变换T_c作用于参考点云集关键点获得一个新的位置的参考关键点云集，其次利用ICP算法精确估计该参考关键点到待匹配关键点的坐标变换，方法如下：

首先进行错误匹配点对的去除，然后通过剩余关键点估计坐标变换T_i。对以上几步进行迭代直到满足任意以下条件时停止：

1)迭代到达一定的次数N。

2)本次计算得到的精确坐标变换T_i与上次迭代得到的坐标变换T_i ^*之间的变化小于某一阈值。

3)其均方根误差E(a)小于某一阈值

式中，m_j和d_i分别为不同点云集中的点。

最终得到精确坐标变换估计T＝T_iT_c，利用该坐标变换完成对两个三维点云集的拼接。

步骤四、当检测到Loop Closure时，通过ELCH算法快速对全局构图进行优化。

计算相邻两节点v_i与v_j间边的协方差c_i,j，通过ICP算法计算对Loop Closure检测到的两个节点进行处理得到一个坐标变换ΔX。然后通过如下方法将权重在每一个节点对之间进行分配：

其中，w_s与w_e为环路开始与结束时的两个节点v_s与v_e的权重，w_i为环路间节点的权重。d(v_l,v_k)表示从节点v_l到v_k所经过边的协方差c_i,j的求和

最终通过以上权重分配完成对全局地图的初步优化后通过LUM算法求出最优解。

本发明的有益效果是：该方法首先采用SURF算法进行关键点的提取，接下来计算各个关键点处法向量，然后计算其三维快速点特征直方图FPFH描述子；其次通过惯性测量单元获得RGB-D传感器的粗略运动估计，将该粗略估计与由三维点云计算而来的运动估计通过算法处理得到较为精确的运动估计，然后将其作为ICP算法的初始值进行迭代计算；最终当检测到Loop Closure时，通过ELCH算法快速对全局构图进行优化。由于利用SURF算法寻找关键点，并采用三维快速点特征直方图FPFH特征作为关键点的特征描述子，提升了三维重构算法在特征提取时的速度；在粗匹配的过程中，由于采用惯性测量单元对传感器位姿的估计与三维特征点云对传感器位姿的估计结合的方法，相对于背景技术RANSAC算法，减少了搜索的次数，提高了效率；由于为ICP算法提供了一个较为准确的初始值，防止ICP算法局部收敛，提升了匹配的精度；最终的地图优化部分，采用ELCH算法避免了通常所用地图优化算法所必须的迭代的过程，提升了算法的速度。

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

具体实施方式

本发明基于惯性测量单元与RGB-D传感器的三维重建方法具体步骤如下：

1、关键点提取算法通过SURF算法完成。

利用计算近似的Hessian矩阵的行列式的极值来确定特征点的位置。某函数f(x,y)的Hessian矩阵如下：

对每一个像素点求出一个Hessian矩阵。在SURF算法中，用图像像素l(x,y)代替函数值f(x,y)，选用二阶标准高斯函数作为滤波器，通过特定核间的卷积计算二阶偏导数，从而计算出H矩阵：

式中，L_xx(x,σ)是图像在点x处与高斯函数二阶偏导的卷积，L_xy(x,σ)和L_yy(x,σ)的含义与之类似，σ值即包含了尺度信息。因此能够得到一个原图像的变换图像，其每个像素就是原图每个像素的Hessian矩阵行列式的近似值，Hessian矩阵行列式近似值计算方法如下(通过方框滤波近似代替二阶高斯滤波)：

det(H_approx)＝D_xxD_yy-(0.9D_xy)² (3)

式中，D_xx，D_yy和D_xy分别为x方向、y方向和xy方向方框滤波掩模同图像卷积后的值。得到近似Hessian矩阵行列式图之后，构造图像的尺度空间，SURF算法中图片大小一直保持不变，尺度空间中的不同层所得到的待检测图片是改变高斯模糊尺寸的大小(方框滤波掩模大小)得到的，同时同一层中的图片用到的高斯模糊尺寸也不同。

在得到图像的尺度空间之后，将经过Hessian矩阵处理过的每个像素点与其三维领域的26个点进行大小比较，如果是这26个点中的最大值或者最小值，则保留下来作为初步的特征点。最后将保留下来的初步特征点通过三维线性差值法得到亚像素级的特征点，同时去除掉那些值小于一定阈值的点。

2、计算关键点特征描述子。

经过提取关键点之后，对每一个关键点计算其三维的特征描述子。三维特征描述子选择快速点特征直方图FPFH(Fast Point Feature Histograms)描述子。快速点特征直方图FPFH描述子计算方法如下：

其中，P_q表示查询点P_k表示其邻近点，权重ω_k表示查询点P_q和其邻近点P_k之间的距离。SPFH(Simple Point Feature Histograms)简化的点特征直方图的计算是依据这个点和它的K邻域点之间的一个元组(点对间的三个角度关系)得到的。

3、快速粗匹配。

首先获得三轴陀螺仪和三轴加速度计所测量RGB-D传感器的XYZ三轴向的位移变化x，y和z以及三轴向的夹角变化α，β和γ。求得旋转矩阵R＝R_x R_y R_z，其中R_x、R_y以及R_z分别为传感器绕X轴、Y轴和Z轴的旋转矩阵

定义一个4阶的变换矩阵T如下：

其次，通过kd-tree完成待配准点云集关键点与参考点云集关键点快速点特征直方图FPFH描述子的特征匹配。进行M次随机选点对(每次选3个点对)

其中，s为最少要选取的点对的个数，ε为局内点的百分比，p是所需的匹配成功概率。通过每次随机选取的点对完成一个变换估计，接着通过扩展卡尔曼滤波算法将此变换与通过惯性测量单元所得到的变换T进行数据融合，得到一个融合后的变换T_EKF。使用T_EKF对参考点云集中的关键点进行坐标变换，计算每一对匹配关键对之间的欧式距离

其中，P为点云集中的点，Dis为点对间的距离。将距离Dis小于一定阈值的点对视为局内点对(inliers)，剩余的点对视为局外点对(outliers)，选取局内点对数最多的一次进行后续计算，通过局内点对进行N次估计(N远小于点对)粗略估计变换T_c，提高了传统RANSAC算法的效率。

最终将获得的粗略估计变换T_c与通过惯性测量单元所得到的变换T作用于其中一点云集，计算与另一点云集间的最小均方根，取使其中值较小的变换作为新的变换T_c。

4、ICP算法。

ICP算法具体如下：将两组点云数据分别表示如下：和在三维欧几里得空间中变换T共有6个变换参数需要计算，其中包括旋转角度φ,θ和ψ与平移参数t_x,t_y和t_z。将这6个参数用一个参数向量a表示为：

a＝[φ,θ,ψ,t_x,t_y,t_z] (9)

定义变换：

T(a；x)＝Rx+[t_x,t_y,t_z]^T (10)

匹配误差是通过误差方程确定的：

ε²(|x|)＝||x||² (11)

ICP算法通过两个步骤的迭代以拼接点云集M与D，给定一个初始的拼接参数a₀，通过算法的迭代得到一系列的估计拼接参数a_k，并逐渐减小误差E(a)：

迭代步骤一和步骤二分别如下：

步骤一，通过下述方程计算相关点对

可以得到，通过变换a_k后，φ(i)是M中距离D中点i最近的点。

步骤二，通过下述方程更新变换矩阵a

通过步骤一中得到的点对去寻找更优的变换矩阵a_k+1。可以看出步骤一与步骤二两步都减少了误差，能够保证算法局部收敛。

本发明方法通过快速粗匹配算法得到a₀，作为ICP算法的初始值以防止算法局部收敛，同时加快其收敛的速度。而且在ICP算法步骤一与步骤二之间通过距离约束以及RANSAC算法对点对进行筛选。详细步骤如下：

1)对步骤一种所有的配对点对计算其欧氏距离，将距离大于一定阈值的点对进行去除。

2)将剩余的点对通过RANSAC(选择较少采样次数，设为50)算法排除局外点对(outliers)，将局内点对作用于ICP算法步骤二。

4、全局地图优化。

当检测到Loop Closure时，选取ELCH(explicit loop closing heuristic)算法对全局地图的优化。Loop Closure检测通过比较每一步传感器的位置是否在之前出现过(在某一个阈值之内)，若出现过则设置为True，若没有则为False。

ELCH算法首先通过ICP算法对Loop Closure检测到的两个点云集进行处理得到一个坐标变换ΔX。然后通过如下方法将权重在每一个点云集对之间进行分配：

其中，w_s与w_e为环路开始与结束时的两个节点v_s与v_e的权重，w_i为环路间节点的权重。d(v_l,v_k)表示从节点v_l到v_k所经过边的协方差c_i,j的求和

最终通过以上权重分配完成对全局地图的初步优化(不一定最优)后通过LUM(Luand Milios)算法求出最优解。

Claims (1)

1.一种基于惯性测量单元与RGB-D传感器的三维重建方法，其特征在于包括以下步骤：

步骤一、采用SURF算法进行关键点的提取，接下来采用基于局部表面拟合的方法计算各关键点处法向量，然后计算关键点的三维快速点特征直方图FPFH描述子

$FPFH\left(P_q\right)=SPFH\left(P_q\right)+\frac{1}{k}\underset{i=1}{\overset{k}{\Σ}}\frac{1}{{\mathrm{\ω}}_k}\·SPFH\left(P_k\right)---\left(1\right)$

其中，P_q表示查询点，P_k表示其邻近点，权重ω_k表示查询点P_q和其邻近点P_k之间的距离；SPFH简化的点特征直方图的计算是依据查询点和它的K邻域点之间的一个元组得到的；

步骤二、通过三轴陀螺仪和三轴加速度计获取一个粗略变换矩阵；首先获得三轴陀螺仪和三轴加速度计所测量RGB-D传感器的XYZ三轴向的位移变化x、y和z以及三轴向的夹角变化α、β和γ；求得旋转矩阵R＝R_x R_y R_z，其中R_x、R_y以及R_z分别为传感器绕X轴、Y轴和Z轴的旋转矩阵

$\begin{array}{ccc}{R}_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]& R_y=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]& R_z=\left[\begin{array}{ccc}\cos \mathrm{\γ}& -\sin & 0\\ \sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]\end{array}---\left(2\right)$

定义一个4阶的变换矩阵T如下：

$T=\left[\begin{array}{cccc}& & & x\\ & R& & y\\ & & & z\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

其次通过kd-tree完成待配准点云集关键点与参考点云集关键点快速点特征直方图FPFH描述子的特征匹配；进行M次随机选点对

$M=\frac{log(1-p)}{log(1-{(1-\mathrm{\ϵ})}^s)}---\left(4\right)$

其中，s为最少要选取点对的个数，ε为局内点的百分比，p是所需的匹配成功概率；通过每次随机选取的点对完成一个变换估计，接着通过扩展卡尔曼滤波算法将此变换估计与通过惯性测量单元所得到的变换矩阵T进行数据融合，得到一个融合后的变换T_EKF；使用T_EKF对参考点云集中的关键点进行坐标变换，并计算每一对匹配关键点对之间的欧式距离

${\mathrm{Dis}}_{ij}={\[\underset{k=0}{\overset{k=n}{\Σ}}{(P_{ik}-P_{jk})}^2\]}^{1/2}---\left(5\right)$

其中，P为点云集中的点，Dis为点对间的距离；将距离Dis小于阈值t₁的匹配关键点对视为局内点对，剩余的点对视为局外点对，选取局内点对数最多的一次进行后续计算，通过局内点对进行N次估计，粗略估计变换T_c；

最终将获得的粗略估计变换T_c与通过惯性测量单元所得到的变换矩阵T作用于其中一点云集，计算该一点云集与另一点云集间的最小均方根，取使其中值较小的变换T_cm；

步骤三、将T_cm作为ICP算法的初始值，避免算法局部收敛；将粗略估计变换T_c作用于参考点云集关键点获得一个新的位置的参考关键点云集，其次利用ICP算法精确估计该参考关键点到待匹配关键点的坐标变换，方法如下：

首先进行错误匹配点对的去除，然后通过剩余关键点估计坐标变换T_i；采用ICP算法进行迭代直到满足任意以下条件时停止：

1)迭代到达一定的次数N；

2)本次计算得到的精确坐标变换与上次迭代得到的坐标变换T_i ^*之间的变化小于阈值t₂；

3)其均方根误差E(a)小于阈值t₃；

$E\left(a\right)=\underset{i=1}{\overset{N_d}{\Σ}}{\mathrm{\ω}}_i\underset{j}{\min }\left|\right|m_j-T(a_k;d_i)|{|}^2---\left(6\right)$

式中，m_j和d_i分别为不同点云集中的点；N_d表示一组点云集中点的个数，a_k表示估计拼接参数，其包括旋转角度φ,θ和ψ与平移参数t_x,t_y和t_z，a_k＝[φ,θ,ψ,t_x,t_y,t_z]_k，

最终得到精确坐标变换估计T_f＝T_iT_cm，利用该坐标变换完成对两个三维点云集的拼接；

步骤四、当检测到Loop Closure时，通过ELCH算法快速对全局地图进行优化；

计算相邻两节点v_i与v_j间边的协方差c_i,j，通过ICP算法计算对Loop Closure检测到的两个节点进行处理得到一个坐标变换ΔX；然后通过如下方法将权重在每一个节点对之间进行分配：

$W_i=w_s+\frac{d(v_s,v_i)}{d(v_s,v_e)}(w_e-w_s)---\left(7\right)$

其中，w_s与w_e为环路开始与结束时的两个节点v_s与v_e的权重，W_i为环路间节点的权重；d(v_l,v_k)表示从节点v_l到v_k所经过边的协方差c_i,j的求和

$d(v_l,v_k):=\underset{v_l{\mathrm{tov}}_k}{\underset{edge\{i,j\}\∈Path}{\Σ}}{c}_{i,j}---\left(8\right)$

最终通过以上分配完成对全局地图的初步优化后通过LUM算法求出最优解。