一种基于FAST和FREAK特征匹配算法的RGB‑D和SLAM场景重建方法

技术领域

本发明属于计算机图形学领域，具体地说是一种基于FAST和FREAK特征匹配算法的RGB-D和SLAM场景重建方法。

背景技术

三维重建技术涉及计算机图形学、传感技术、机器视觉、逆向工程、虚拟现实、机器人导航等领域，它的目的在于更加真实和客观地展示物体模型或者室内场景的空间形状及位置，因此，该技术是目前国内外众多学者研究的热点内容。

Marr提出的视觉计算理论将视觉划分为三个阶段。第一阶段，三维重建的初级阶段，从简单的二维图像信息的获取开始，包括深度图像、边缘图、彩色纹理图像等基本几何形态或者特征要素等。通过灰度值表征了采样空间中物体表面的位置和形状信息，在这个阶段所获取的数据都是二维坐标上的。而进入第二阶段，这些二维信息需要转换到三维信息空间坐标系中。实际上，第二阶段并非真正意义的三维显示，我们称为2.5维坐标系下的信息还原。该阶段中所包含的空间信息是不完全的，并且是在以观察者为原点的坐标系中的三维形状和位置，是从观察值或者摄像头的位置出发对重建物体进行描述。这样物体背面的全部信息无法正常地获得。

可见，前两个阶段只是“视觉的低层次的处理”。仅依靠在第二阶段获得的数据并不能真实、全面地展示一个场景的全部空间信息。所以对三维重建提出了更高的要求，进入了第三阶段。首先。坐标的原点应该是基于观察物体本身或者是客观世界中的固定位置；其次，应该从多个角度观察并获取场景中物体的三维数据信息，并将其转换至同一坐标系中。

三维重建是连接摄像机与真实环境的一个重要桥梁，在虚拟现实与人机交互中有着重要额作用。作为图像处理和模式识别在工程应用上的重要分支，三维重建在3D打印、机器人导航、文物修复、3D视频、医学CT图像等领域都有着广泛的应用。

本发明的研究意义在于通过个人消费级别的深度摄像头来获取室内三维场景的相关数据，采用OpenCV、G2O和PCL等工具包，使消费者在家里就可以完成小范围室内场景的重建。

如果要实现场景的三维重建，一个完整的RGB-D SLAM方法主要包括图像匹配、回环检测以及图优化三大步骤，流程如3所示。(由于Kinect标定参数通过标定程序即可获得，所以在重建过程中，被当作已知参数，在框架中没有添加标定过程)。该框架又可分为两大部分：SLAM前端和SLAM后端。SLAM前端包括图像匹配和回环检测两步，完成场景重建；SLAM后端对重建场景点云数据进行全局图优化。下面我们详细介绍这两大部分。

1.SLAM前端

下面我们首先来介绍一下SLAM前端(SLAM front-end)：图像匹配主要解决连续图像帧间的匹配及相对Kinect摄像机的姿态估计问题，而回环检测则主要根据图像匹配判断摄像机是否先前来过该区域，这两部分密切相关。SLAM前端主要解决数据关联的问题，图像匹配考虑局部数据关系，而回环检测则处理全局数据关系。或者说，图像匹配与回环检测都是根据图像帧数据建立图节点间的约束，即完成场景图的重建。

2.SLAM后端

由于图像噪声以及匹配误差的存在，通过图像帧配准得到的位姿图通常不一致。反而，图中的边与边之间常常存在“冲突”。从理论上分析，若用T_i来表示图像帧间的相对变换矩阵并且T₀,T₁,T₂…T_n构成一个回环的话，理论上讲，必然存在T₀T₁T₂…T_n＝I其中I为单位矩阵。但通过图像帧配准得到的相对变换矩阵通常不满足单位阵的理论约束。在采用图结构来表示摄像机位姿及其约束中，摄像机的位姿可看作随机变量，位姿间的约束则是与随机变量相关的观测，那么图优化结果则对应于位姿的最大似然估计。与图像匹配和回环检测不同，图优化部分不直接处理图像帧数据，而只是对SLAM前端构造的图进行优化。所以我们将图优化称为SLAM后端(SLAM>

发明内容

本发明的目的在于提供一种基于FAST和FREAK特征匹配算法的RGB-D和SLAM场景重建方法。

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

(1)首先对Kinect进行标定；

(2)对彩色图像进行FAST特征点提取，采用FREAK特征描述子进行图像匹配，然后再对特征点对采用RANSAC算法剔除掉外点，保留内点；

(3)通过图像匹配获得的内点对当前帧与最近关键帧进行运动估计和获取变换矩阵，只有当运动距离在(D_min，D_max)之间，我们才将当前帧作为是关键帧。

(4)根据获得的关键帧序列构建出姿态图，并且用图优化算法对姿态图进行全局误差一致性优化。

(5)通过(4)，得到的关键帧序列的变换关系是误差一致的，然后对关键帧的彩色图和深度图像进行融合，获得点云数据。再通过变换矩阵进行点云拼接，最终获得整个场景的点云数据。

本发明的有益效果在于：现有的室内场景重建采用的特征匹配算法大多是SIFT、SURF、ORB和KAZE等。SIFT、SURF和KAZE等算法的鲁棒性较好，但是实时性较差。ORB算法实时性较好，鲁棒性较差。在实时性和鲁棒性上很难两者兼顾。以前的RGB-D SLAM方法通过特征匹配计算变换矩阵，直接点云帧数据进行拼接。这样做的弊端是加大了计算量，并且容易造成无匹配现象。本发明采用对关键帧进行筛选，仅对关键帧进行点云的拼接，这样极大地减少了点云的拼接次数。本发明还采用了基于图优化的回环检测算法，通过构建姿态图，对其进行全局优化，极大降低了误差的累积。

附图说明

图1是本发明方法的基本流程图；

图2是本发明方法中Kinect的标定流程图；

图3是待FAST和FREAK特征匹配的原始图像；

图4是FAST和FREAK特征匹配结果图和RANSAC后的结果图；

图5是SIFT、SURF、ORB、FREAK和KAZE匹配时间对比图；

图6是SIFT、SURF、ORB、FREAK和KAZE匹配成功率对比图；

图7是待拼接的一帧彩色图像和深度图像；

图8是待拼接的另一帧彩色图像和深度图像；

图9是两帧图像的点云拼接图；

图10是RGB-D SLAM场景重建方法对场景的重建结果图；

图11是RGB-D SLAM场景重建方法对场景的重建结果图。

具体实施方式

下面结合附图对本发明做进一步描述。

传统的RGB-D SLAM采用的是SIFT、SURF和ORB等特征匹配算法，在实时性方面，SIFT和SURF等算法表现不理想，ORB和FREAK算法是为了在移动设备上运行提出的算法，所以实时性较好；在鲁棒性方面，SIFT的综合表现较好，SURF在光照变化时表现较差，其他条件下表现比较稳定，ORB算法的综合表现一般，FREAK算法在各种环境下表现出较好的稳定性。综合方法实时性和鲁棒性两方面的考虑，提出了一种基于FAST和FREAK的特征匹配算法的RGB-D SLAM场景重建方法。

(1)首先对Kinect设备进行标定。本发明采用OpenCV库中的标定函数。通过这些函数，可以比较容易地完成摄像机标定，获得彩色摄像机和红外摄像机的内参数和外参数。Kinect标定流程如附图2所示。

本发明采用cvFindChessboardCorners()计算棋盘图像上的角点位置，函数原型如下：

int cvFindChessboardCorners(const void*image,CvSize pattern_size,CvPoint2D32f*corners,int*corner_count＝NULL,int flags＝CV_CALIB_CB_ADAPTIVE_THRESH)；

函数输入参数有：输入的棋盘图像、标定板每行和每列的角点数目，检测到的角点、输出角点的个数以及使用的操作标志。

我们采用函数cvFindCornerSubPix()对角点进行细化，函数原型为：

void cvFindCornerSubPix(const CvArr*image,CvPoint2D32f*corners,intcount,CvSize win,CvSize zero_zone,CvTermCriteria criteria)；

这个函数的输入参数有：棋盘标定板图像、提取的角点的初始坐标和数目、搜索窗口和死区的一半尺寸、校正角点的迭代的终止条件。

角点的精细化过程使用了迭代算法，所以需要确定一个迭代的终止条件。在本系统中，设定迭代过程的终止条件为：

cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1)

即迭代终止条件为：迭代次数最高达到30次，或者迭代精度达到0.1个像素。

下面将得到的精确的角点位置代入函数cvStereoCalibrate()，函数的原型为：

cvStereoCalibrate(const CvMat*object_points,const CvMat*image_points1,

const CvMat*image_points2,const CvMat*npoints,

CvMat*camera_matrix1,CvMat*dist_coeffs1,

CvMat*camera_matrix2,CvMat*dist_coeffs2,

CvSize image_size,CvMat*R,CvMat*T,

CvMat*E CV_DEFAULT(0),CvMat*F CV_DEFAULT(0),

CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria(

CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,1e-6)),

int flags CV_DEFAULT(CV_CALIB_FIX_INTRINSIC))；

该函数将要做大量分解工作，所以运行的时间较长，返回值为摄像机的内参数和外参数矩阵。

(2)图像特征匹配算法。首先本发明研究了SIFT、SURF、ORB、KAZE和FREAK等特征匹配算法。选取的三组实验图像如附图3，图像分辨率为640*480。特征匹配算法的执行效率手硬件设备的约束较大，本发明采用的硬件配置：处理器为Intel Core i5-2300 2.8GHz四核，内存8GB，显卡NvidiaQuadro 4000(2GB)。实验结果如附图4所示，统计算法执行时间如表1所示。

表1特征提取匹配算法执行时间统计表

将表1特征匹配算法执行时间统计表绘制成柱状图，如附图5，按照由长到短顺序为KAZE>SIFT>SURF>ORB>FREAK，KAZE耗时最长，根本无法达到实时性的要求。ORB和FREAK执行时间属于同数量级的，FREAK稍快一点，都能符合实时性的要求，适于应用到场景重建中。

在RGB-D SLAM场景重建方法中鲁棒性与实时性同样重要，如果出现特征无匹配，那么容易导致场景重建的不连续，最终导致场景重建的失败。SIFT、SURF、ORB、FREAK、KAZE五种算法的匹配成功率统计如表2。

表2特征提取匹配算法匹配成功率统计表

从表2中可以看出这几种算法的平均匹配成功率，虽然各算法区分度不大，但仍然能够看出算法的优劣。将表2的数据采用柱状图来显示，更加形象，如图6所示。匹配成功率最高的是KAZE，其次是FREAK，接下来依次为SURF和SIFT，但是相差不大，最低的是ORB。

尽管KAZE算法的匹配成功率高，稳定，但是算法耗时太长，无法满足实时性的要求。ORB算法虽然检测和匹配效率高，但是匹配成功率较低。所以综合算法执行效率和匹配成功率两个方面，FREAK算法是最适合场景重建的。

本发明采用FAST和FREAK算法对图像进行匹配。FAST特征提取运行效率较高，通过检测候选点邻域一周像素的灰度值，若候选点邻近域内有足够的像素点和候选点的灰度值差别较大，我们则规定该候选点为特征点。

$>N={\Sigma }_{x\forall \left(circle\right(p)}|I\left(x\right)-I\left(p\right)|>{ϵ}_d$>

其中，I(x)为圆周上任意一点的灰度，I(p)为圆心的灰度，ε_d为灰度值之差的阈值，如果N大于给定阈值，一般为周围圆圈点的四分之三，则认为p是一个特征点。

FAST特征点方向规定为特征点与圆形邻域重心的夹角：

$>\theta =\arctan \left(\frac{m_{01}}{m_{10}}\right)=\arctan \left(\frac{\underset{x,y}{\Sigma }yI(x,y)}{\underset{x,y}{\Sigma }xI(x,y)}\right)$>

FREAK描述子是利用了视网膜的拓扑结构原理，由0和1组成的二进制串。用M表示：

$>M=\underset{0\le \alpha \le N}{\Sigma }{2}^{\alpha }T\left(P_{\alpha }\right)$>

$>T\left(P_{\alpha }\right)=\left(\begin{array}{cc}1,& \left(I\right(P_{\alpha }^{r1})-I(P_{\alpha }^{r2})>0)\\ 0,& else\end{array}\right)$>

式中P_α代表感受域对，N代表采样点对的个数，T(P_α)是0和1的测试，和分别代表P_α的上一位和下一位的灰度均值，是采样点经过高斯平滑后的强度值。

采用FAST和FREAK特征匹配算法获得的特征点对存在误匹配，所以本发明采用RANSAC算法对其进行筛选，剔除掉外点，保留内点。

(3)Kinect设备采集点云数据的速度能达到30fps，数据量是相当大，因为帧与帧之间的距离很近，如果对每一帧数据都进行匹配，将会消耗大量的时间和内存空间，是完全没有必要的。所以我们在这里引用了“关键帧”，只需要把检测到的关键帧拼接到重建场景中，这样不仅降低了帧与帧间匹配次数，还避免了重建场景的频繁更新，从而节省时间和空间。

关键帧满足条件：

1.与上一关键帧能特征匹配成功。

2.与上一关键帧之间的距离在设定范围(D_min，D_max)之内，该设定距离范围为经验值。

(4)构建姿态图并进行基于图优化的回环检测。在姿态图中每个节点代表摄像机的一个姿态，每条边代表了一个转换关系。

本发明采用Rainer等人开发的G2O库，使得图优化较为方便。要想构建一个姿态图并进行图优化，只需要以下几步：

1.定义一个SparseOptimizer，它一般使用Gauss-Newton或LM算法；

2.库中定义的边、节点变量，与节点相关的操作的头文件都以vertex_开头，而与边相关的操作的头文件则以edge_开头，该节点和边的类型都可以直接来自于Eigen::Isometry；

3.通过帧间匹配程序，计算出变换矩阵；

4.将计算出的关键帧作为节点，关键帧之间的变换矩阵作为节点间的边，添加到optimizer中，同时设定节点的估计值与边的约束；

5.不断做帧间检测来维护姿态图；

6.在程序结束时调用optimizer.optimize(steps)进行优化，优化完成后读取每个关键帧节点的估计值。

回环检测的任务是判断当前帧是否在帧序列中出现过。在回环检测中，通过保存过去关键帧，然后判断当前帧是否与过去关键帧匹配，如果当前帧与某一过去关键帧相似度较高，可以计算该两帧之间的转换和添加新约束。加入新的约束可以有效地减少误差的累积。

RGB-D SLAM场景重建方法回环检测流程伪代码如下：

1.初始化关键帧序列H，并将第一帧h₀放入H。

2.对于当前帧h_i，计算H中上一帧h_i-1与h_i的运动，并估算该运动的大小d。有以下几种可能性：

a.若d>D_max，则表明上一帧与当前帧之间的距离较大，容易导致计算错误。为防止该现象，选择丢弃当前帧；

b.如果出现无匹配现象，说明图像的质量不高和检测到的inliers点对太少，以至于无法匹配，丢弃；

c.若d<D_min，则表明与上一关键帧运动距离较近，同样丢弃该帧；

d.其余情况均是理想的关键帧，特征匹配成功，运动估计正确，同时又与上一关键帧h_i-1有一定距离，并且该距离较为合适，则把h_i作为新的关键帧，接下来进入回环检测。

3.近距离回环：匹配h_i与关键帧序列H末尾的k个关键帧，如果匹配成功，就在图G里增加一条边。

4.均匀回环：在从帧首开始的n-k个关键帧中均匀等间距选取(n-k)/k个关键帧，与h_i进行匹配，如果能匹配，则再在图G里增加一条边。

5.将h_i放入H末尾，并进行优化再与当前重建场景拼接。若有新的数据，则返回步骤2；若无，则程序停止。

(5)对关键帧的彩色图和深度图像进行融合，获得关键帧的点云数据。再通过变换矩阵进行点云拼接，最终获得整个场景的点云数据。

要想获得点云数据，我们首先要用Kinect设备的RGB摄像头和红外CMOS摄像机分别读取RGB图像和Depth图像。RGB图像含有颜色信息，Depth图像含有距离信息，所以我们定义数据结构来保存两者的信息。数据结构定义如下：

struct PointXYZRGB{

float x；float y；float z；float rgb[3]；

}；

点云数据融合的过程：将Depth图像经过一定的坐标变换与RGB图像对齐，然后计算出空间点的X、Y坐标，最后将三维点云数据(X，Y，Z，R，G，B)通过PCL开源库显示。

获得点云数据后，我们需要知道前后帧之间的变换矩阵。接下来我们通过OpenCV中的solvePnPRansac()函数求解变换矩阵(即相机的旋转矩阵和平移矩阵)，实现对Kinect设备的摄像机姿态估计。下面是solvePnPRansac()函数原型：

void solvePnPRansac(InputArray objectPoints,InputArray imagePoints,InputArray cameraMatrix,InputArray distCoeffs,OutputArray rvec,OutputArraytvec,bool useExtrinsicGuess＝false,intiterationsCount＝100,floatreprojectionError＝8.0,int minInliersCount＝100,OutputArray inliers＝noArray(),int flags＝ITERATIVE)

PCL库中含有对点云拼接的方法，函数为transformPointCloud(input,output,T)。通过该方法不断进行两两拼接，即可得到完整重建场景。

图10是对数据集1的场景重建图，重建过程中，关键帧筛选和重建时间情况如表3所示。

表3数据集1重建中关键帧和重建时间情况

图11是对数据集2的场景重建图，表4是关键帧筛选和重建时间情况表

表4数据集2重建中关键帧和重建时间情况