CN106651926A - 一种基于区域配准的深度点云三维重建方法 - Google Patents

Abstract

本发明公开了一种基于区域配准的深度点云三维重建方法，首先输入彩色图像与深度图像，利用彩色图像的颜色相似性，对彩色图像进行区域划分，并利用深度信息对相邻空间相似区域进行合并；利用权值的迭代更新进行相邻时刻的区域预匹配，并对预处理后的区域对所对应的点云进行三维配准，最后将所有时刻的三维点云按照变换矩阵存储在栅格立方体中，使用移动立方体算法进行三维重建。实验结果表明，本发明能够实现立体模型的三维重建，并且重建的模型保留真实世界物体的特征信息，使重建的模型与现实物体之间具有一定的真实度与准确度，其成果可用于虚拟现实、三维打印等相关领域的研究与应用。

Description

一种基于区域配准的深度点云三维重建方法

技术领域

本发明涉及立体模型的三维重建技术，从彩色图像的颜色相似性出发，对彩色图像进行区域分割，将图像中的物体分割成具有局部相似性的多个区域，并利用深度信息对相邻区域进行相似度计算，将相邻的相似区域进行合并。在得到合并的区域后，利用权重信息的迭代更新进行相邻时刻的区域预匹配，并对预处理后的区域对所对应的点云进行三维配准得到变换矩阵，最后将所有时刻的三维点云按照变换矩阵存储在栅格立方体中，使用移动立方体算法进行三维表面重建。

背景技术

三维立体模型重建是计算机视觉、机器人系统与计算机图形学等领域中重要的研究课题。近些年来，由于Kinect等消费级深度采集设备的出现，该技术在虚拟现实、文物修复、医学成像、三维打印等领域中都有着广泛的应用。由于设备硬件上的限制，采集的深度数据往往会出现丢失现象。此外，设备的快速移动会导致配准技术的失败，产生与真实数据误差较大的三维模型。目前，一些优化技术可以检测到闭环，逐渐纠正设备位姿，并将误差分配到逐帧以减少累计误差带来的影响。但是，这些办法没有真正解决复杂环境下配准偏移的问题，当移动速度较快，或者物体特征信息不丰富时，如何获取较为精确的三维模型仍然是研究中亟待解决的关键问题。

发明内容

本发明的目的是提出一种基于区域配准的深度点云三维重建方法。

本发明的目的是这样实现的：一种基于区域配准的深度点云三维重建方法，其特征在于，对于采集的彩色图像与深度数据，采用以下步骤进行处理,即可以实现深度点云的三维重建。

步骤1：对彩色图像进行区域分割

为对彩色图像进行区域分割，首先将彩色图像所有像素点存储在图G中，然后使用基于图的贪心聚类分割方法将彩色图像过分割成多个区域。具体地，将彩色图像每个像素初始化为图G的顶点，像素之间的颜色差异初始化为顶点之间的边权值。首先计算所有像素之间的颜色差异，计为图G的边权值h，按照公式(1)计算：

其中，h_ij是像素点p_i与p_j之间的边权值，(r_i,g_i,b_i)与(r_j,g_j,b_j)分别是两个像素点的红绿蓝三个颜色值。为了进一步进行区域分割，首先将图G中每一个顶点当作一个区域，然后根据公式(2)对每两个区域进行聚类分析，如果两个区域满足公式(2)，那么将他们合并为一块新区域；

Dif(C_i，C_j)≤min(Int(C_i)+f/|C_i|,Int(C_j)+f/|C_j|) (2)

其中，C_i和C_j是待分割的两个区域，Dif(C_i，C_j)是连接两个区域C_i和C_j之间的最小的边权值，Int(C_i)和Int(C_j)分别为区域C_i和C_j包含的图所对应的最小生成树中最大的边权值。|C_i|和|C_j|分别是区域C_i和C_j中的像素点个数,f是分割的阈值参数，设为50。对所有像素点进行处理后，彩色图像被过分割成多个区域；

步骤2：对相似的过分割区域进行合并

为了对相似的过分割区域进行合并，首先计算所有区域的协方差矩阵，然后计算相邻区域的相似度，对相似度小于阈值的区域进行合并。具体地，首先根据公式(3)计算每个区域的协方差矩阵A(u(S))：

其中，c(S)是区域S的中心点的三维坐标，u(S)是区域S对应的三维点云。|u(S)|为u(S)的顶点数量。x是点云上u(S)任意一个点的三维坐标。对A(u(S))进行矩阵的特征分解，得到的三个特征值从大到小分别为w₁(S)、w₂(S)、w_n(S)，对应的三个特征向量分别为T₁(S)、T₂(S)、N(S)；

然后，对图像中所有相邻的区域进行相似度计算。具体地，对于图像中任意两个相邻区域S_i,S_j,按照公式(4)计算它们之间的相似度c_smooth。

其中||·||表示数量积计算，N(S_i)与N(S_j)是两个区域计算得到的最小的特征向量。如果两个区域的相似度c_smooth小于阈值常数c_thod，相邻区域S_i、S_j将合并成为新区域。其中c_thod设为0.02m。对相邻时刻所有区域都进行相似度计算和合并后，得到相邻时刻两组合并后的区域集合；

步骤3：匹配相邻时刻的两组区域，对所有匹配的区域所对应的点云进行三维配准

为了进行区域点云之间的三维配准，首先要进行相邻时刻的两组区域之间的区域配对，然后进一步对配对后的区域点云进行三维配准。具体地，从时刻t_k的区域集合中取一块区域S_m，再从下一时刻t_k+1的区域集合中取一块区域S_n，组成区域对(S_m,S_n)。然后按照公式(5)，计算S_m与S_n之间的空间位置关系r_mn；

其中，d_mn为两个点云区域之间所有最临近点对的平均欧氏距离，按照公式(6)计算。

其中，(x_m,y_m,z_m)是u(S_m)上的任意点的三维坐标，(x_n,y_n,z_n)是在u(S_n)上距离(x_m,y_m,z_m)欧式距离最小的点的三维坐标；

w_mn表示两个点云的重叠率，按照公式(7)进行计算。

其中，N_w是两个点云中欧式距离最小的点对距离小于d_thod的点对数，其中d_thod设为0.01m；

然后，对相邻两个时刻所有区域的点云进行权值初始化，具体地，区域S_m和S_n的权值e(S_m)，e(S_n)分别初始化为S_m和S_n对应点云u(S_m)和u(S_n)的顶点个数占该时刻所有点云的顶点个数合的比例。如果r_mn小于阈值r_thod，那么将该组点云区域对(S_m,S_n)加入预处理集合M中，并分别按照公式(8)更新权值e(S_m)，e(S_n),其中r_thod设为0.02；

对所有区域对的点云进行权值计算，迭代更新所有点云的权值，最终得到包含所有区域对的预处理集合M。

进一步对预处理的区域对进行对应点云的几何三维配准。首先从M中取出一对区域，对这两块区域对应的三维点云分别进行几何特征提取。具体地，对点云u(S_m)和u(S_n)中的每个点分别计算FPFH特征。其特征在于，对点云每个点周围距离小于R的点进行特征统计形成点特征直方图，其中特征统计包括对距离、角度的统计，R取值为0.5m。然后利用采样一致性初始配准算法计算物体的四阶变换矩阵，具体地，先在u(S_m)中寻找采样点，然后在另一个点云u(S_n)中查找与采样点FPFH特征相似的所有点，计算每个点对的四阶变换矩阵，选择其中误差最小的刚体变换矩阵T_kp作为最终变换矩阵。k表示第k时刻，p表示在预处理集合M中的第p对匹配区域。其中，采样一致性初始配准算法中的采样点之间的距离不应小于阈值d_min，设为0.1m。

步骤4：融合通过三维配准得到的所有区域的变换矩阵

对于同一个时刻k，将该时刻所有区域的变换矩阵T_kp融合成该时刻的完整变换矩阵T_k，假设时刻k总共有p_k组区域对，按照公式(9)计算融合后的变换矩阵T_k；

其中X为权值矩阵，设为(0.70.3)，T_kp是时刻k时的第p对区域点云配准得到的变换矩阵，w_kp是时刻k时的第p对区域点云的重叠率，按照公式(7)进行计算，d_kp是时刻k时的第p对区域点云的平均欧式距离，按照公式(8)进行计算；

步骤5：将不同时刻的深度点云进行点云融合与三维表面重建。

为了将不同时刻的深度点云进行点云融合与三维表面重建，首先使用截断符号距离函数(TSDF)进行点云的融合。具体地，将空间划分成128³个栅格立方体，将点云分布在栅格立方体中，然后计算每帧融合后每个栅格立方体的TSDF值；

为了计算每个栅格立方体中的截断符号距离函数，首先要计算每个栅格立方体中的符号距离函数sdf_kg(x)，k表示第k时刻，g表示第g个栅格立方体，sdf_kg(x)代表着点云上任意一点x的符号距离函数，按照公式(10)进行计算：

sdf_kg(x)＝dis(v_g,t_k)-D_k (10)

其中，t_k是相机光心在三维坐标系下的坐标，相机光心坐标设为(320,240)，v_g是栅格g在三维坐标系下的坐标，dis(v_g,t_k)表示栅格到光心的距离，D_k是图像中x对应的像素点所对应的深度值，由Kinect设备相机采集得到。如果sdf_kg(x)大于0，tsdf_kg(x)按照公式(11)计算，否则按照公式(12)计算：

tsdf_kg(x)＝min(1,sdf_kg(x)/m_truncation) (11)

tsdf_kg(x)＝min(-1,sdf_kg(x)/m_truncation) (12)

其中m_truncation为常量，取值为10。然后计算权重wt_kg(x)，初始值为1。W_kg(x)为迭代后的权重，计算公式如(13)所示；

W_kg(x)＝W_k-1,g(x)+wt_kg(x) (13)

最后记录迭代后的加权TSDF值，标记为TSDF_kg(x)，按照公式(14)计算。

将W_kg(x)和TSDF_kg(x)存储在对应的栅格g中，进行下个栅格g+1的计算。当对所有时刻的栅格都计算了截断符号距离函数后，所有点云已被存储在栅格立方体中。然后使用移动立方体算法抽取等值面，绘制三维表面，得到立体模型。

本发明利用彩色图像与深度图像，能得到具有一定真实度与准确度的三维模型，能够解决困难环境下的配准偏移问题。进一步实例证明了本发明的实验效果，适用于如TUMRGB-D库提供的数据集提供的深度与彩色图像等。

附图说明

图1为本发明实施例经过区域生成后的分割过程结果图；

图2为本发明实施例重建的三维模型结果图。

具体实施方式

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

实施例

本实施例采用TUM RGB-D库提供的large cabinet，cabinet,teddy数据集进行实验。在Windows 10操作系统的PC机上进行实验，硬件配置是2.6GHzCore^TM i5-3230MCPU、8GB RAM；实验平台是VS2010。

本发明包括以下具体步骤：

步骤1：对彩色图像进行区域分割

为对彩色图像进行区域分割，首先将彩色图像所有像素点存储在图G中，然后使用基于图的贪心聚类分割方法将彩色图像过分割成多个区域。具体地，将彩色图像每个像素初始化为图G的顶点，像素之间的颜色差异初始化为顶点之间的边权值。首先计算所有像素之间的颜色差异，计为图G的边权值h，按照公式(1)计算：

其中，h_ij是像素点p_i与p_j之间的边权值，(r_i,g_i,b_i)与(r_j,g_j,b_j)分别是两个像素点的红绿蓝三个颜色值。为了进一步进行区域分割，首先将图G中每一个顶点当作一个区域，然后根据公式(2)对每两个区域进行聚类分析，如果两个区域满足公式(2)，那么将他们合并为一块新区域。

Dif(C_i，C_j)≤min(Int(C_i)+f/|C_i|,Int(C_j)+f/|C_j|) (2)

其中，C_i和C_j是待分割的两个区域，Dif(C_i，C_j)是连接两个区域C_i和C_j之间的最小的边权值，Int(C_i)和Int(C_j)分别为区域C_i和C_j包含的图所对应的最小生成树中最大的边权值。|C_i|和｜C_j|分别是区域C_i和C_j中的像素点个数,f是分割的阈值参数，设为50。对所有像素点进行处理后，彩色图像被过分割成多个区域。

步骤2：对相似的过分割区域进行合并

为了对相似的过分割区域进行合并，首先计算所有区域的协方差矩阵，然后计算相邻区域的相似度，对相似度小于阈值的区域进行合并。具体地，首先根据公式(3)计算每个区域的协方差矩阵A(u(S))：

其中，c(S)是区域S的中心点的三维坐标，u(S)是区域S对应的三维点云。|u(S)|为u(S)的顶点数量。x是点云上u(S)任意一个点的三维坐标。对A(u(S))进行矩阵的特征分解，得到的三个特征值从大到小分别为w₁(S)、w₂(S)、w_n(S)，对应的三个特征向量分别为T₁(S)、T₂(S)、N(S)。

然后，对图像中所有相邻的区域进行相似度计算。具体地，对于图像中任意两个相邻区域S_i,S_j,按照公式(4)计算它们之间的相似度c_smooth。

其中||·||表示数量积计算，N(S_i)与N(S_j)是两个区域计算得到的最小的特征向量。如果两个区域的相似度c_smooth小于阈值常数c_thod，相邻区域S_i、S_j将合并成为新区域。其中c_thod设为0.02m。对相邻时刻所有区域都进行相似度计算和合并后，得到相邻时刻两组合并后的区域集合。

步骤3：匹配相邻时刻的两组区域，对所有匹配的区域所对应的点云进行三维配准

为了进行区域点云之间的三维配准，首先要进行相邻时刻的两组区域之间的区域配对，然后进一步对配对后的区域点云进行三维配准。具体地，从时刻t_k的区域集合中取一块区域S_m，再从下一时刻t_k+1的区域集合中取一块区域S_n，组成区域对(S_m,S_n)。然后按照公式(5)，计算S_m与S_n之间的空间位置关系r_mn。

其中，d_mn为两个点云区域之间所有最临近点对的平均欧氏距离，按照公式(6)计算。

其中，(x_m,y_m,z_m)是u(S_m)上的任意点的三维坐标，(x_n,y_n,z_n)是在u(S_n)上距离(x_m,y_m,z_m)欧式距离最小的点的三维坐标。

w_mn表示两个点云的重叠率，按照公式(7)进行计算。

其中，N_w是两个点云中欧式距离最小的点对距离小于d_thod的点对数，其中d_thod设为0.01m。

然后，对相邻两个时刻所有区域的点云进行权值初始化，具体地，区域S_m和S_n的权值e(S_m)，e(S_n)分别初始化为S_m和S_n对应点云u(S_m)和u(S_n)的顶点个数占该时刻所有点云的顶点个数合的比例。如果r_mn小于阈值r_thod，那么将该组点云区域对(S_m,S_n)加入预处理集合M中，并分别按照公式(8)更新权值e(S_m)，e(S_n),其中r_thod设为0.02。

对所有区域对的点云进行权值计算，迭代更新所有点云的权值，最终得到包含所有区域对的预处理集合M。

进一步对预处理的区域对进行对应点云的几何三维配准。首先从M中取出一对区域，对这两块区域对应的三维点云分别进行几何特征提取。具体地，对点云u(S_m)和u(S_n)中的每个点分别计算FPFH特征。其特征在于，对点云每个点周围距离小于R的点进行特征统计形成点特征直方图，其中特征统计包括对距离、角度的统计，R取值为0.5m。然后利用采样一致性初始配准算法计算物体的四阶变换矩阵，具体地，先在u(S_m)中寻找采样点，然后在另一个点云u(S_n)中查找与采样点FPFH特征相似的所有点，计算每个点对的四阶变换矩阵，选择其中误差最小的刚体变换矩阵T_kp作为最终变换矩阵。k表示第k时刻，p表示在预处理集合M中的第p对匹配区域。其中，采样一致性初始配准算法中的采样点之间的距离不应小于阈值d_min，设为0.1m。

步骤4：融合通过三维配准得到的所有区域的变换矩阵

对于同一个时刻k，将该时刻所有区域的变换矩阵T_kp融合成该时刻的完整变换矩阵T_k，假设时刻k总共有p_k组区域对，按照公式(9)计算融合后的变换矩阵T_k。

其中X为权值矩阵，设为(0.70.3)，T_kp是时刻k时的第p对区域点云配准得到的变换矩阵，w_kp是时刻k时的第p对区域点云的重叠率，按照公式(7)进行计算，d_kp是时刻k时的第p对区域点云的平均欧式距离，按照公式(8)进行计算。

步骤5：将不同时刻的深度点云进行点云融合与三维表面重建

为了将不同时刻的深度点云进行点云融合与三维表面重建，首先使用截断符号距离函数(TSDF)进行点云的融合。具体地，将空间划分成128³个栅格立方体，将点云分布在栅格立方体中，然后计算每帧融合后每个栅格立方体的TSDF值。

为了计算每个栅格立方体中的截断符号距离函数，首先要计算每个栅格立方体中的符号距离函数sdf_kg(x)，k表示第k时刻，g表示第g个栅格立方体，sdf_kg(x)代表着点云上任意一点x的符号距离函数，按照公式(10)进行计算：

sdf_kg(x)＝dis(v_g,t_k)-D_k (10)

其中，t_k是相机光心在三维坐标系下的坐标，相机光心坐标设为(320,240)，v_g是栅格g在三维坐标系下的坐标，dis(v_g,t_k)表示栅格到光心的距离，D_k是图像中x对应的像素点所对应的深度值，由Kinect设备相机采集得到。如果sdf_kg(x)大于0，tsdf_kg(x)按照公式(11)计算，否则按照公式(12)计算：

tsdf_kg(x)＝min(1,sdf_kg(x)/m_truncation) (11)

tsdf_kg(x)＝min(-1,sdf_kg(x)/m_truncation) (12)

其中m_truncation为常量，取值为10。然后计算权重wt_kg(x)，初始值为1。W_kg(x)为迭代后的权重，计算公式如(13)所示。

W_kg(x)＝W_k-1,g(x)+wt_kg(x) (13)

最后记录迭代后的加权TSDF值，标记为TSDF_kg(x)，按照公式(14)计算。

将W_kg(x)和TSDF_kg(x)存储在对应的栅格g中，进行下个栅格g+1的计算。当对所有时刻的栅格都计算了截断符号距离函数后，所有点云已被存储在栅格立方体中。然后使用移动立方体算法抽取等值面，绘制三维表面，得到立体模型。

本发明利用彩色图像与深度图像，能得到具有一定真实度与准确度的三维模型，能够解决困难环境下的配准偏移问题。实例进一步证明了本发明的实验效果，适用于如TUMRGB-D库提供的数据集提供的深度与彩色图像等。

图1是本发明实施例经过区域分割后的过程结果图。从图中的区域分割结果可以明显看出，利用本发明对彩色图像进行区域分割结果的合理性与有效性。

图2是本发明实施例重建的三维模型结果图，从图中的结果可以看出，利用本发明对物体的三维重建结果的真实度与准确度。

Claims (1)

1.一种基于区域配准的深度点云三维重建方法，其特征在于，该方法包括以下具体步骤：步骤1：对彩色图像进行区域分割

将彩色图像每个像素初始化为图G的顶点，像素之间的颜色差异初始化为顶点之间的边权值；计算所有像素之间的颜色差异，计为图G的边权值h，按照公式(1)计算：

$h_{ij}=\sqrt{{(r_i-r_j)}^2+{(g_i-g_j)}^2+{(b_i-b_j)}^2}---\left(1\right)$

其中，h_ij是像素点p_i与p_j之间的边权值，(r_i,g_i,b_i)与(r_j,g_j,b_j)分别是两个像素点的红绿蓝三个颜色值；为了进一步进行区域分割，先将图G中每一个顶点当作一个区域，然后根据公式(2)对每两个区域进行聚类分析，如果两个区域满足公式(2)，那么将他们合并为一块新区域；

Dif(C_i，C_j)≤min(Int(C_i)+f/|C_i|,Int(C_j)+f/|C_j|) (2)

其中，C_i和C_j是待分割的两个区域，Dif(C_i，C_j)是连接两个区域C_i和C_j之间的最小的边权值，Int(C_i)和Int(C_j)分别为区域C_i和C_j包含的图所对应的最小生成树中最大的边权值；|C_i|和|C_j|分别是区域C_i和C_j中的像素点个数,f是分割的阈值参数，设为50；对所有像素点进行处理后，彩色图像被过分割成多个区域；

步骤2：对相似的过分割区域进行合并

首先，根据公式(3)计算每个区域的协方差矩阵A(u(S))：

$A\left(u\right(S\left)\right)=\frac{1}{\left|u\left(S\right)\right|}{\underset{x\∈u(S}{\Σ}}_{)}(x-c(S\left)\right){(x-c(S\left)\right)}^T---\left(3\right)$

其中，c(S)是区域S的中心点的三维坐标，u(S)是区域S对应的三维点云；|u(S)|为u(S)的顶点数量；x是点云上u(S)任意一个点的三维坐标；对A(u(S))进行矩阵的特征分解，得到的三个特征值从大到小分别为w₁(S)、w₂(S)、w_n(S)，对应的三个特征向量分别为T₁(S)、T₂(S)、N(S)；

然后，对于图像中任意两个相邻区域S_i,S_j,按照公式(4)计算它们之间的相似度c_smooth：

$c_{smooth}=\frac{\max \left\{\right||N\left(S_i\right)\·\left(c\right(S_i)-c(S_j\left)\right)||,||N\left(S_j\right)\·\left(c\right(S_i)-c(S_j))\left|\right|\}}{\left|\right|N\left(S_i\right)\·N\left(S_j\right)\left|\right|}---\left(4\right)$

其中||·||表示数量积计算，N(S_i)与N(S_j)是两个区域计算得到的最小的特征向量；如果两个区域的相似度c_smooth小于阈值常数c_thod，相邻区域S_i、S_j将合并成为新区域；其中c_thod设为0.02m；对相邻时刻所有区域都进行相似度计算和合并后，得到相邻时刻两组合并后的区域集合；

步骤3：匹配相邻时刻的两组区域，对所有匹配的区域所对应的点云进行三维配准

从时刻t_k的区域集合中取一块区域S_m，再从下一时刻t_k+1的区域集合中取一块区域S_n，组成区域对(S_m,S_n)；然后按照公式(5)，计算S_m与S_n之间的空间位置关系r_mn；

$r_{mn}=w_{mn}{d}_{mn}\frac{\left|\right|N\left(S_m\right)\left|\right|\·\left|\right|N\left(S_n\right)\left|\right|}{\left|\right|N\left(S_m\right)\·N\left(S_n\right)\left|\right|}---\left(5\right)$

其中，d_mn为两个点云区域之间所有最临近点对的平均欧氏距离，按照公式(6)计算；

$d_{mn}=\frac{1}{\min \left\{\right|u\left(S_m\right)|,|u\left(S_n\right)\left|\right\}}\underset{(x_n,y_n,z_n)\∈u\left(S_n\right)}{\underset{(x_m,y_m,z_m)\∈u\left(S_m\right)}{\Σ}}\sqrt{{(x_m-x_n)}^2+{(y_m-y_n)}^2+{(z_m-z_n)}^2}---\left(6\right)$

其中，(x_m,y_m,z_m)是u(S_m)上的任意点的三维坐标，(x_n,y_n,z_n)是在u(S_n)上距离(x_m,y_m,z_m)欧式距离最小的点的三维坐标；

w_mn表示两个点云的重叠率，按照公式(7)进行计算；

$w_{mn}=\frac{N_w}{\min \left\{\right|u\left(S_m\right)|,|u\left(S_n\right)\left|\right\}}---\left(7\right)$

其中，N_w是两个点云中欧式距离最小的点对距离小于d_thod的点对数，其中d_thod设为0.01m；

然后，对相邻两个时刻所有区域的点云进行权值初始化，具体地，区域S_m和S_n的权值e(S_m)，e(S_n)分别初始化为S_m和S_n对应点云u(S_m)和u(S_n)的顶点个数占该时刻所有点云的顶点个数合的比例；如果r_mn小于阈值r_thod，那么将该组点云区域对(S_m,S_n)加入预处理集合M中，并分别按照公式(8)更新权值e(S_m)，e(S_n),其中r_thod设为0.02；

$\begin{array}{c}e\left(S_m\right)\←e\left(S_m\right)-\min \{e\left(S_m\right),e\left(S_n\right)\}\\ e\left(S_n\right)\←e\left(S_n\right)-\min \{e\left(S_m\right),e\left(S_n\right)\}\end{array}---\left(8\right)$

对所有区域对的点云进行权值计算，迭代更新所有点云的权值，最终得到包含所有区域对的预处理集合M；

进一步对预处理的区域对进行对应点云的几何三维配准；首先从M中取出一对区域，对这两块区域对应的三维点云分别进行几何特征提取；具体地，对点云u(S_m)和u(S_n)中的每个点分别计算FPFH特；对点云每个点周围距离小于R的点进行特征统计形成点特征直方图，其中特征统计包括对距离、角度的统计，R取值为0.5m；然后利用采样一致性初始配准算法计算物体的四阶变换矩阵，具体地，先在u(S_m)中寻找采样点，然后在另一个点云u(S_n)中查找与采样点FPFH特征相似的所有点，计算每个点对的四阶变换矩阵，选择其中误差最小的刚体变换矩阵T_kp作为最终变换矩阵；k表示第k时刻，p表示在预处理集合M中的第p对匹配区域；其中，采样一致性初始配准算法中的采样点之间的距离不应小于阈值d_min，设为0.1m；

步骤4：融合通过三维配准得到的所有区域的变换矩阵

对于同一个时刻k，将该时刻所有区域的变换矩阵T_kp融合成该时刻的完整变换矩阵T_k，假设时刻k总共有p_k组区域对，按照公式(9)计算融合后的变换矩阵T_k；

$T_k=X\·\left(\begin{array}{c}\frac{1}{\underset{p=1}{\overset{p_k}{\Σ}}{w}_{kp}}\underset{p=1}{\overset{p_k}{\Σ}}{w}_{kp}{T}_{kp}\\ \frac{1}{\underset{p=1}{\overset{p_k}{\Σ}}{d}_{kp}}\underset{p=1}{\overset{p_k}{\Σ}}{d}_{kp}{T}_{kp}\end{array}\right)---\left(9\right)$

其中X为权值矩阵，设为(0.70.3)，T_kp是时刻k时的第p对区域点云配准得到的变换矩阵，w_kp是时刻k时的第p对区域点云的重叠率，按照公式(7)进行计算，d_kp是时刻k时的第p对区域点云的平均欧式距离，按照公式(8)进行计算；

步骤5：将不同时刻的深度点云进行点云融合与三维表面重建

将空间划分成128³个栅格立方体，将点云分布在栅格立方体中，然后计算每帧融合后每个栅格立方体的TSDF值；

为了计算每个栅格立方体中的截断符号距离函数，首先要计算每个栅格立方体中的符号距离函数sdf_kg(x)，k表示第k时刻，g表示第g个栅格立方体，sdf_kg(x)代表着点云上任意一点x的符号距离函数，按照公式(10)进行计算：

sdf_kg(x)＝dis(v_g,t_k)-D_k (10)

其中，t_k是相机光心在三维坐标系下的坐标，相机光心坐标设为(320,240)，v_g是栅格g在三维坐标系下的坐标，dis(v_g,t_k)表示栅格到光心的距离，D_k是图像中x对应的像素点所对应的深度值，由Kinect设备相机采集得到；如果sdf_kg(x)大于0，tsdf_kg(x)按照公式(11)计算，否则按照公式(12)计算：

tsdf_kg(x)＝min(1,sdf_kg(x)/m_truncation) (11)

tsdf_kg(x)＝min(-1,sdf_kg(x)/m_truncation) (12)

其中m_truncation为常量，取值为10；然后计算权重wt_kg(x)，初始值为1；W_kg(x)为迭代后的权重，计算公式如(13)所示；

W_kg(x)＝W_k-1,g(x)+wt_kg(x) (13)

最后记录迭代后的加权TSDF值，标记为TSDF_kg(x)，按照公式(14)计算；

${\mathrm{TSDF}}_{kg}\left(x\right)=\frac{{\mathrm{TSDF}}_{k-1,g}\left(x\right)W_{k-1,g}\left(x\right)+{\mathrm{tsdf}}_{kg}\left(x\right)w_{kg}\left(x\right)}{W_{k-1,g}\left(x\right)+w_{kg}\left(x\right)}---\left(14\right)$

将W_kg(x)和TSDF_kg(x)存储在对应的栅格g中，进行下个栅格g+1的计算；当对所有时刻的栅格都计算了截断符号距离函数后，所有点云已被存储在栅格立方体中；然后使用移动立方体算法抽取等值面，绘制三维表面，得到立体模型。