多特征扩展信息滤波在RGB-D点云数据中的应用

常明，康志忠，李敏，李方舟

(1.中国地震局第一监测中心，天津 300180；2.中国地质大学(北京)土地科学与技术学院，北京 100081；3.天津城建大学 地质与测绘学院，天津 300384)

0 引言

最近几十年，三维激光扫描仪已经被广泛应用于室内、室外三维场景的构建[1]。由于其造价昂贵，扫描仪的使用受到了一定的限制。RGB-D相机是一款同时获取场景内深度影像和彩色影像的相机，能够实现对场景内彩色点云数据的生成。Kinect作为RGB-D相机的一种，通过对场景内点云数据的生成，构建三维模型，在实际生活中发挥着越来越重要的作用。

Kinect2.0可同时获取辨率为1 920像素×1 080像素的彩色影像和512像素×425像素的深度影像数据，通过相机间的相互标定，可以获取三维场景内彩色点云数据，并且通过对点云数据格网化的处理可以实现实时生成高密度场景模型[2-4]。对于范围较大的场景，单帧Kinect影像无法获取全部数据，需要进行多帧Kinect数据间的拼接。根据Kinect获取的数据特点，主要通过2种方式完成数据拼接：针对深度数据生成的点云数据，通过ICP[5]算法解算多帧点云数据之间的同名点，获得最优变换矩阵，完成三维点云数据的精确配准[6-8]；针对RGB-D相机的彩色影像数据，利用SIFT、SURF以及ORB[9-12]等方法对彩色影像中的像素同名点进行提取，完成彩色影像配准，依据深度相机与彩色相机的标定关系，实现对三维点云数据的配准[13-14]。Weber等[15]提出利用深度传感器获取的管道点云数据自动拼接算法，将所有输入的点云数据放在一个集合中，由相邻测站的同名点匹配结果构建全局坐标系统。单站坐标系的转换关系按照顺序自动转换为全局点云坐标系，进而实现全局坐标系的拼接。Xie[16]提出一种新算法实现对ICP算法中同名点权重的动态赋予，算法中还利用点云数据的结构对象信息以及稀疏SIFT算法获取的同名点的空间距离，实现对点云数据的离群消除，来实现点云数据的双站拼接。Santos等[17]使用RGB-D数据，提出了一种自适应的粗拼到精拼的室内拼接模型。利用深度测量的随机误差理论对三维点进行加权，并引入新的距离模型，进行具有的精确性和鲁棒性的由粗到细的拼接，并利用闭合检测的方法，最大限度地减少误差累积。关于室内场景的创建，New combe等[18]提出了利用RGB-D数据解决同时定位与测图(simultaneous localization and mapping，SLAM)，利用ICP算法模型解决了Kinect传感器的实时配准与地图显示；Endres等[19]提出了RGB-DSLAM系统模型，通过帧与帧之间的特征匹配完成粗配准，再利用ICP算法实现了帧间匹配。Murartal等[20]提出了ORBSLAM2系统，来解决单目、双目以及RGB-D的SLAM问题；Gao等[21]通过提出场景中的特征平面要素减弱误差的累积。

现阶段对Kinect生成的点云数据的拼接处理，大部分是基于ICP算法通过搜索最近点进行的。但Kinect点云数据单点的精度有限，随着测站数的增加，误差的累积会越来越大，影响最后拼接结果的精度，从而限制了Kinect点云数据的应用。

针对这个情况，本文通过对场景内点、面特征的提取构建了多特征扩展信息滤波模型，实现对Kinect点云数据的全局拼接，达到减小误差累积提升点云拼接质量的目的。

1 多特征扩展信息滤波

康志忠[22]提出了一种利用扩展卡尔曼滤波进行全局拼接的方法。扩展信息滤波是扩展卡尔曼滤波的对称形式。相比于卡尔曼滤波，信息滤波在实现的过程更加简单清晰[23]。

卡尔曼滤波在t时刻状态向量和状态方程表示为μt和Σt，而信息滤波的信息向量和信息矩阵则表示为：

(1)

式中：ξt表示t时刻的信息向量；Ωt表示t时刻的信息矩阵。

在利用信息滤波进行每个测站点的位置估计时，主要分为移动更新和观测更新。

1.1 系统信息空间和状态模型

1)系统信息空间。在对点云坐标系进行转换时可认为是刚性转换，其变换矩阵由平移矩阵T和旋转矩阵R构成。每一个测站位置都由6个自由度来表示，包括3个空间平移坐标x，y，z以及3个绕着X，Y，Z轴旋转的角度α，β，γ。

将多站坐标系统一到同一坐标系下实现全局拼接，每个站点的位姿用向量(x，y，z，α，β，γ)来表示，则第K时刻的系统状态为：

(2)

式中：n表示到目前为止已拼接的站数；Xi(k)表示第i站点云的位姿。

则信息矩阵

信息向量为：

(4)

式中：Σij(k)是一个分块矩阵，表示Xi(k)与Xj(k)之间的协方差。

2)系统状态模型。系统信息向量包括每个测站点的位置姿态。在观测过程中，每站的位置都是静态的，所以系统状态转换方程为：

Xt+1(k)=f(Xt(k))=Xt(k)

(5)

式中：状态转换函数f(·)单位阵I，所以可以忽略。

1.2 移动更新

当得到新一站的数据时，根据之前更新的系统状态，估计其粗略的同一坐标系下的位置与姿态，增加到系统状态向量中。首先把新的点云数据和已更新的相邻数据的原始数据(作为参考数据)进行粗略拼接。相邻数据当前状态估计即为：

Xt=[xt，yt，zt，αt，βt，γt]

把当前数据和参考数据的拼接结果记作为：

ΔXt=[Δx，Δy，Δz，Δα，Δβ，Δγ]

基于Xt和ΔXt来计算当前数据在全局坐标系下的坐标：

(6)

1.3 观测更新

观测更新主要是针对设备移动获取到的新数据，对场景数据的特征进行提取，构建相应的观测模型，对信息向量以及信息矩阵进行更新。通过获取点云数据的点特征和面特征信息，构建多特征观测模型，从而削减误差累积对全局拼接的影响，避免由于单点精度的有限，影响最后的点云数据拼接的质量。

1)点特征信息的获取。通过ICP算法，对相邻双帧Kinect点云数据中的近邻点进行提取，创建匹配特征点集，计算2帧点云数据的旋转、平移矩阵。为了获取最优的旋转、平移矩阵，需要不断的迭代上述过程。设当前时刻求解的旋转、平移矩阵为Rt、Tt，上一时刻求解的旋转、平移矩阵为Rt-1、Tt-1，则迭代终止条件为相邻两次迭代结果计算的旋转、平移矩阵差小于设定的阈值，即

(7)

本实验中Rlim、Tlim分别设置为[0.05，0.05，0.05]，[0.01，0.01，0.01]，此时得到的最近点作为点特征信息。相邻两帧数据的原始影像、深度点云数据及点特征提取结果如图1所示。

图1 相邻测帧点特征提取结果

2)面特征信息的获取。针对场景内的点云数据，利用区域面生长法[24]对其进行分类、拟合，获取主要平面的点云数据及其在各自坐标系下的空间平面方程a*x+by+c*z+d=0。以图1中2帧图像的深度点云数据为例，对场景内主要面特征进行提取。面特征的提取结果如图2所示，为了区别不同的平面，面特征提取结果采用不同的灰度值进行显示。由于Kinect获取数据时相邻帧场景相似度很高，可认为面特征提取结果中相似空间平面方程所代表的2个平面为场景内的同一平面，以其的单位法向量作为面特征信息，构建多特征观测模型。

图2 相邻两帧面特征的提取结果

3)多特征观测模型。常用Kinect点云数据的观测模型是利用点特征进行构建的，但是由于单点的精度有限，存在一定的误差。鉴于此本文提出利用点特征信息与面特征信息构建多特征观测模型。

(1)点特征观测模型。利用ICP算法获取的邻近点作为点特征信息，构建点特征观测模型。假设相邻两站的点特征集，在第i站的坐标系下表示为(xL1，yL1，zL1)，在第j站的坐标系下表示为(xL2，yL2，zL2)，则点特征信息的观测模型表示如下：

(8)

式中：h1(·)是系统点特征信息观测函数；v(k)表示传感器测量和坐标转换的不确定性，假设为高斯分布，表示为白噪声向量N(0，S)。

(2)面特征观测模型。对相邻两测站中同一平面的法向量进行提取，并进行单位化，构建面特征观测模型。假设同一个平面在第i站坐标系的空间方程为：

ai*x+biy+ci*z+di=0

(9)

该平面在第i站下的法向量为(ai，bi，ci)；该平面在第j站坐标系下的空间方程为：

aj*x+bjy+cj*z+dj=0

(10)

该平面在第j站下的法向量为(aj，bj，cj)。则面特征观测模型表示为：

(11)

式中：h2(·)是系统面特征信息观测函数；v(k)表示传感器测量和坐标转换的不确定性，假设为高斯分布，表示为白噪声向量N(0，S)。

(3)多特征观测模型。将点特征信息与面特征信息当做观测值，构建多特征观测模型。则观测模型为：

(12)

式中：Z1表示点特征的观测模型；Z2表示面特征的观测模型。由于2种特征信息的精度不同，所以对2种信息赋予不同的权值，来提高精度。

(13)

式中：λ1为点特征信息的权值；λ2为面特征信息的权值；Q1表示点特征信息的协方差矩阵；Q2表示面特征信息的协方差矩阵。

4)状态增广。当获取新一站的数据n时，根据系统状态模型获得该站的位置姿态Xn，然后增加到系统状态向量中，则系统状态向量和系统解方程矩阵分别增广为：

(14)

则系统信息向量及信息矩阵分布为：

(15)

(16)

5)状态更新。利用多特征观测模型的观测值，对预测信息矩阵及信息向量更新，按式(16)计算：

(17)

2 实验分析

本文对2个实验场景进行实验，场景一为室内房屋闭合场景，场景二为走廊不闭合场景。针对Kinect获取的数据，分别利用ICP算法和多特征扩展信息滤波算法进行处理。

ICP算法对场景内点云数据的最近点进行获取，构建点集，实现旋转平移矩阵的求解。并对场景内整体的点云数据进行旋转平移，逐步将所有帧数据统一到全局坐标系下实现场景内的全局拼接；MEIF根据场景内提取的点、面特征信息构建多特征扩展信息滤波模型实现全局拼接。通过比较2种算法在全局坐标系下配准点残差的标准差(rootmean square error，RMSE)来验证本文模型的正确性。

2.1 场景一全局拼接结果比较

针对场景一，利用Kinect相机共获取45帧数据，分别用2种方法进行全局拼接，生成的数据如下图3所示。为了能够直观、定性地比较2种方法全局拼接的误差累积情况，在生成的场景中选择A、B、C 3个位置点云数据的拼接结果进行放大，比较拼接误差。

图3 场景一ICP与MEIF全局拼接结果比较

其中A处2种方法拼接结果的放大图中，不存在明显的误差累积，点云数据拼接结果较好；在B处ICP算法拼接结果放大图中，存在一定的误差累积，导致点云数据拼接结果存在明显分层现象；在C处放大图中，ICP算法的误差累积比较大，点云数据拼接结果错位明显；而MEIF算法在全局拼接过程中误差累积较小，在B、C 2处点云数据拼接结果的放大图中，不存在明显的分层、错位现象。

为了定量地分析2种算法的全局拼接精度，计算全局坐标系下配准点残差的标准差，对2种算法的拼接精度进行分析。计算结果绘制标准差过程线如图4所示。ICP算法和MEIF算法针对场景一数据全局坐标系下配准点残差的标准差期望分别是0.227 m和0.038 m，MEIF算法相比于ICP算法能够有效削弱多站拼接中误差的累积。

图4 拼接结果标准差

2.2 场景二全局拼接结果比较

相对于场景一，场景二为不闭合的走廊。RGB-D相机在进行室内点云场景拼接的应用中，不闭合的场景可以说是这个研究方向的一大难点。Kinect作为RGB-D相机的一种，本文利用其对走廊数据进行采集，共获取73帧数据。利用ICP算法和MEIF算法分别进行全局拼接，结果如图5所示。

图5 场景二 全局拼接结果

从图5可以直观看出，利用ICP算法进行全局拼接的结果存在一定的误差，走廊存在着明显的弯曲变形；利用MEIF算法进行全局拼接的结果相对于ICP算法质量有了一定的提高，拼接结果中不存在明显的扭曲现象。为了能够定量地比较2种方法针对不闭合场景的拼接精度，统计全局坐标系下配准点残差的标准差，生成图6。

其中ICP算法全局拼接结果的标准差的期望为2.64 m；利用MEIF算法实现全局拼接的标准差的期望为0.039 m。

图6 拼接结果标准差

3 结束语

利用ICP算法处理Kinect点云数据，只有单独2站的情况下，精度可以满足需求。但是当需要对多站的点云数据进行全局拼接时，随着测站数的增加，误差累积也越来越严重，影响最终的拼接结果。通过对实验场景一、二全局拼接结果定性、定量地分析，相比于ICP算法，利用本文提出的多特征扩展信息滤波模型，能够在全局拼接的过程中，有效地减小误差累积的影响，提高全局拼接的精度，对RGB-D相机在室内三维点云场景的应用有一定的参考价值。