CN103646391B - 一种针对动态变化场景的实时摄像机跟踪方法 - Google Patents

Abstract

本发明公开了一种针对动态变化场景的实时摄像机跟踪方法，该方法在不断动态变化的场景中也能进行稳定地跟踪和求解摄像机姿态；首先进行特征匹配及摄像机参数估计，然后进行场景的更新，最后，该方法在实际应用时实行前后台多线程协同运行，前台线程用于对每一帧进行特征点匹配和摄像机运动参数的估计，后台线程不断进行KD树、关键帧和三维点云的维护和更新，并联合优化关键帧的摄像机运动参数和三维点位置。在场景发生动态变化的情况下，本发明仍能实时地进行摄像机跟踪，本发明在跟踪准确性、稳定性以及运行效率等方面明显优于现有的摄像机跟踪方法。

Description

一种针对动态变化场景的实时摄像机跟踪方法

技术领域

本发明涉及一种实时摄像机跟踪方法，尤其涉及一种动态变化场景中的实时摄像机跟踪方法。

背景技术

摄像机跟踪技术是计算机视觉领域的重要技术之一，其在机器人导航和增强现实等领域中有着极其重要的应用。

摄像机跟踪技术的难点在于：1）大多数方法都是一帧一帧的对特征点进行跟踪，导致误差逐帧累积。如果某一帧的跟踪失败了，那么就将无法继续进行摄像机的跟踪，必须对相机重新定位。有一些方法提出使用随机分类器或者Harrwaveltes来解决摄像机的重定位问题，Chekhlov等人提出使用SIFT特征点来进行特征点的跟踪，但是却导致了计算复杂性增大。为了降低计算复杂性，有些方法简化了SIFT检测算法，但是随之而来的是丢失了SIFT特征点的一些不变性特征；2）目前现有的大多数摄像机跟踪算法都只能够处理静态场景，如果场景发生了动态变化，就会导致跟踪失败。但是，真实的自然场景往往不断变化，在很多的应用中需要进行处理的场景也都是动态变化的，因此现有的摄像机跟踪方法都很难对这些场景进行处理。3）在跟踪的过程中，现有的一些方法由于没有及时地对三维场景结构进行更新及优化，导致重建出的三维模型很快就无效，进而导致了摄像机跟踪的失败。

当下，有很多摄像机跟踪方法，本专利所属方法称为SLAM（SimultaneouslyLocalization and Mapping）。

SLAM是指实时地定位摄像机位置并获得摄像环境的空间地图的方法，它是实现制造自主移动机器人的关键算法之一。尽管该方法已经取得了巨大的进步，但是仍然存在着很多的挑战和困难。目前，现有算法能够处理静态、规模较小的场景，但是如何对动态的、规模较大的场景进行实时定位和地图构建仍然有待深究。

1.基于过滤器的SLAM

该方法的代表是MonoSLAM（Andrew J.Davison,Ian D.Reid,NicholasMolton,Olivier Stasse:MonoSLAM:Real-Time Single Camera SLAM.IEEE Trans.Pattern Anal.Mach.Intell.29（6）:1052-1067,2007）它是实时摄像机跟踪领域的先驱。该方法首先对一个大小已知的场景中的人工标记进行检测，从而初始化场景，然后逐帧进行类KLT特征点的检测。通过扩展卡尔曼滤波，场景中所有标记的位置以及摄像机参数紧密相关，并被约束到一个线性方程组中，每次输入一个新帧时，都同步更新方程组。尽管卡尔曼滤波是非常高效的，但是该方法的计算复杂度依旧很高（O（N2），N是场景中标记的数目），从而场景中标记的数目只能是几百个，并且如果不使用巧妙的特征点选取方法（比如ZongyingShi,Zhibin Liu,Xianliang Wu,WenliXu:Feature selection for reliable dataassociation in visual SLAM.Mach.Vis.Appl.24（4）:667-682,2013），就只能够处理一个很小的空间。Eade和Drummond（Ethan Eade,Tom Drummond:ScalableMonocular SLAM.CVPR（1）2006:469-476）提出的方法采用了FastSLAM-type粒子滤波（Michael Montemerlo,Sebastian Thrun,Daphne Koller,Ben Wegbreit:FastSLAM2.0:An Improved Particle Filtering Algorithm for SimultaneousLocalization and Mapping that Provably Converges.IJCAI2003:1151-1156）和自顶向下的搜索方法，从而能够实时的处理数百个标记。

2.基于运动推断结构（SfM）的SLAM

基于过滤器的SLAM方法边缘化当前帧以前帧的摄像机参数，并随时间收集概率分布信息，基于关键帧（即运动推动结构）的方法保持全局集束调整的优化方法，但是在计算时必须选择少量过去帧用来处理，通过比较（HaukeStrasdat,J.M.M.Montiel,Andrew J.Davison:Visual SLAM:Why filter?Image VisionComput.30（2）:65-77（2012）），基于关键帧的方法要优于基于过滤器的实时跟踪方法，本专利所保护的方法属于基于关键帧的方法。运动推断结构（SfM）技术可同时恢复每帧对应的相机运动及场景中稀疏的三维点位置，然而其中的核心算法——集束调整（Bundle Adjustment,BA），其计算复杂度非常大，从而严重的影响了处理的效率。近些年来，大量的优化算法都致力于对集束调整过程进行加速，而Klein和Murray独辟蹊径，提出一种并行跟踪和重建模型PTAM（Georg Klein,David W.Murray:Parallel Tracking and Mapping for Small ARWorkspaces.ISMAR2007:225-234）。该方法仍使用传统的集束调整算法，但将实时跟踪及集束调整分配到两个线程同时运行。前台线程对每一帧进行特征点的匹配以及摄像机参数的求解，后台线程不断的对场景的三维结构进行优化。

3.动态场景中的SLAM

为了处理动态变化的场景，需要在静态场景中判别出动态运动的物体。Hahnel（DirkRudolph Triebel,Wolfram Burgard,Sebastian Thrun:Mapbuilding with mobile robots in dynamic environments.ICRA2003:1557-1563）和Bibby（Charles Bibby,Ian D.Reid:Simultaneous Localisation and Mapping inDynamic Environments（SLAMIDE）with Reversible Data Associa.Robotics:Science and Systems2007）提出了使用期望最大化方法来检测场景中的动态物体，但是如果场景变化比较频繁，则场景中会出现很多不必要的三维特征点，增加了内存和计算的负担，同时也无法保证鲁棒性。Blaser（Gabriele Bleser,HaraldWuest,Didier Stricker:Online camera pose estimation in partially known anddynamic scenes.ISMAR2006:56-65）提出了一个基于SLAM的CAD模型，在该方法中，对于场景中的三维点，如果在超过半数以上的输入帧中都可以跟踪到该三维点，则保留该三维点，否则删除该三维点。该方法中的三维点都是分别通过三角化求得，因此得到的场景结构较之于基于SfM的PTAM方法准确性要差。Shimamura（J.Shimamura,M.Morimoto,and H.Koike.Robust vSLAMfordynamic scenes.In MVA,pages344347,2011.）基于PTAM提出了vSLAM方法，该方法在获得摄像机参数估计之后，对所有的无效点进行光流估计，并通过GMM方法对光流进行聚类，如果同一个类中的无效点的数目超过了一个阀值，那么将这个无效点从场景中删除。Blaser和Shimamura提出的方法都可以保证场景的紧凑和准确，但是如果存在较大的遮挡，标准的RANSAC方法很快就无法准确的估计摄像机参数，此外，vSLAM没有删除无效的关键帧，因此被已经存在的无效关键帧所遮挡的新物体的三维点就无法再添加到场景中。

发明内容

本发明的目的在于针对现有技术的不足，提供一种针对动态变化场景的实时摄像机跟踪方法。

本发明的目的是通过以下技术方案来实现的：一个针对动态变化场景的实时摄像机跟踪方法，它包括以下步骤：

（1）特征点匹配及摄像机参数估计，使用基于不变量的特征点来表达场景特征，每个场景点对应一个特征描述量，使用KD树组织所有场景特征，对每帧图像提取特征点并在KD树中搜索特征匹配，随后进行摄像机参数的估计；

（2）场景更新，使用关键帧及场景特征点对应的三维点云表达场景几何信息。通过添加关键帧和三维点扩充场景，若已有关键帧的对应场景发生变化，删除变化的关键帧和变化的三维点。

（3）前后台多线程协同运行，前台线程用于对每一帧进行特征点匹配和摄像机运动参数的估计，后台线程不断进行KD树、关键帧和三维点云的维护和更新，并联合优化关键帧的摄像机运动参数和三维点位置。

本发明的有益效果是：

1、本发明能够处理多种动态变化的场景，并且对于场景中光照发生变化以及存在无纹理区域的情况，仍然能够鲁棒地获得摄像机参数的估计；

2、本发明使用不变量特征点来表达场景，使用KD树组织所有场景特征，提取特征点并进行匹配，同时对KD树进行及时的更新和维护，从而确保了跟踪的准确性；

3、本发明实时的对关键帧以及三维场景进行更新，从而确保能够准确的重建三维场景以及摄像机跟踪；

4、本发明采用了多线程协同运行的方法，前台线程并行地对输入帧进行特征点的提取、匹配以及摄像机参数估计，后台线程并行地进行关键帧和三维场景的更新以及集束调整，提高了程序运行的速度；

5、最后，本发明在针对动态变化场景的摄像机参数估计的应用中，无论时间性能还是跟踪的准确性都要优于现有的方法。

附图说明

图1是对于动态场景中鲁棒的实时三维重建和摄像机跟踪方法的流程图；

图2是实施例中应用本发明的方法处理的视频图像的前后对照图；

图3是实施例中应用本发明的方法处理的视频图像的前后对照图；

图4是实施例中应用本发明的方法处理的视频图像的前后对照图；

图5是实施例中应用本发明的方法处理的视频图像的前后对照图。

具体实施方式

本发明针对动态变化场景的实时摄像机跟踪方法较之于现有的摄像机跟踪方法进行了如下改进：1）利用GPU技术进行特征描述量提取以及匹配，并且对KD树进行及时的更新，确保跟踪的准确性；2）实时地进行关键帧以及三维场景的更新，提高三维重建的准确性，进而提高跟踪的准确性；3）前后台多线程并行，加快了运行的速度。

本发明的实施步骤如下：

1.特征点匹配及摄像机参数估计。使用基于不变量的特征点（如SIFT、SURF等）来表达场景特征，每个场景点对应一个特征描述量，使用KD树组织所有场景特征，对每帧图像利用GPU提取SIFT特征点并在KD树中搜索特征匹配，随后进行摄像机参数的估计。具体步骤包括：

1.1.使用基于不变量的特征点（如SIFT：David G.Lowe:Distinctive ImageFeatures from Scale-Invariant Keypoints.International Journal of Computer Vision60（2）:91-110,2004；或SURF:Herbert Bay,Andreas Ess,TinneTuytelaars,Luc J.Van Gool:Speeded-Up Robust Features（SURF）.Computer Vision and ImageUnderstanding110（3）:346-359,2008）来表达场景特征，每个场景点对应一个特征描述量，使用KD树（Sunil Arya,David M.Mount:Approximate NearestNeighbor Queries in Fixed Dimensions.SODA1993:271-280）组织所有场景特征。构造两棵存储全局场景特征的KD树T₁、T₂，用于全局特征点描述量和图像特征点描述量间的匹配，T₁用于实时匹配，T₂用于动态更新。

1.2.根据方法（David Nistér:An Efficient Solution to the Five-Point RelativePose Problem.IEEE Trans.Pattern Anal.Mach.Intell.26（6）:756-777,2004）选定两帧初始关键帧，对场景结构、运动以及KD树等进行初始化。

1.3.对每帧图像利用GPU抽取SIFT特征点，利用SIFT特征描述量在T₁中搜索匹配。对于特征点x，假设T₁中它的最相邻的两个描述量是N₁(x)和N₂(x)，使用下面的公式作为匹配的置信度：

$c=\frac{\left|\right|p\left(x\right)-p\left(N_1\left(x\right)\right)\left|\right|}{\left|\right|p\left(x\right)-p\left(N_2\left(x\right)\right)\left|\right|};$

其中，p(x)表示特征点x的描述量。如果c<0.6,将x和N₁(x)作为一对匹配点，由此得到一系列场景特征点和当前帧图像特征点之间的匹配。

1.4.根据步骤1.3得到一系列场景特征点和当前帧图像特征点之间的匹配，通过最小化目标函数估计出当前帧摄像机运动参数（R表示旋转矩阵，t表示平移向量，π表示投影函数（作用是将三维空间点投影到二维空间，获得二维空间点位置，即π（X,Y,Z）=（x,y）,其中x=X/Z,y=Y/Z）），x_i表示特征点，K表示摄像机内置参数，X_i表示三维点），通过RANSAC算法（MartinA.Fischler,Robert C.Bolles:Random Sample Consensus:A Paradigm for ModelFitting with Applications to Image Analysis and Automated Cartography.Commun.ACM24（6）:381-395,1981）剔除误匹配，将正确的匹配定义为全局匹配集合G。

2.场景更新。使用关键帧及场景特征点对应的三维点云表达场景几何信息。通过添加关键帧和三维点扩充场景，若已有关键帧的对应场景发生变化，删除变化的关键帧和变化的三维点。

2.1.对于当前输入帧，经过步骤1后，如果可以成功地估计摄像机参数，并且全局匹配集合G中至多有m1（例如m1可以取80）个匹配点，则将当前帧作为潜在关键帧；

2.2.选取同当前帧公共特征点最多的五个关键帧，并在当前帧和该5个关键帧之间进行特征点匹配（对于当前帧中已经匹配的特征点不需要再进行匹配），为了加快匹配的速度，为选取的5帧关键帧分别建立一棵KD树T，并通过极线几何约束（Zhengyou Zhang,RachidDeriche,Olivier D.Faugeras,Quang-TuanLuong:A Robust Technique for Matching two Uncalibrated Images Through theRecovery of the Unknown Epipolar Geometry.Artif.Intell.78（1-2）:87-119,1995）除去无效的特征点以提高匹配的准确性。根据当前帧同5个关键帧之间的匹配点构成特征点轨迹，对于每一个特征点轨迹通过三角化（Triangulation）方法，即求解下述方程来估计该轨迹的三维点：

$X_i=\underset{X_i}{\min }\underset{j\&Element;\mathrm{\&phi;}\left(x_i\right)}{\mathrm{\&Sigma;}}{\left|\right|x_{\mathrm{ij}}-\mathrm{\&pi;}\left(K(R_j{X}_i+t_j)\right)\left|\right|}^2;$

其中，X_i表示三维点，φ(x_i)表示x_i出现的帧集合，x_ij表示特征点，R_j表示旋转矩阵，t_j表示平移向量，π表示投影函数，K表示摄像机内置参数。如果估计得到的三维点数量超过m2（一般设置在50到100之间，例如取100），则将当前帧作为新的关键帧加入到关键帧集合中，将得到的新的三维点X_i加入到场景中，并将当前帧中的特征点选作新的参考特征点加入到后台KD树T₂中，同时用T₂更新T₁。

2.3.若当前输入帧被选作新的关键帧，接下来通过比较当前帧和关键帧的Z轴方向，选取那些方向差异小于某个阀值的关键帧，随后进一步通过比较摄像机参数（旋转矩阵和平移向量）确定5个与当前关键帧最相邻的关键帧。

2.4.对于每个选择的关键帧，首先将其同当前关键帧进行颜色直方图比较，将颜色直方图的差异定义为R、G、B三个通道直方图欧式距离之和,如果差异大于1，表明图像中的一些区域很可能发生了改变，将关键帧中的特征点x对应的三维点X投影到当前帧图像中，将投影位置记为x′。用V（X）来表示三维点X的状态，若X是无效三维点，则V（X）=0，否则，令V（X）=1。接下来对每一个选择的关键帧，通过比较特征点x和x′来判定X是否是有效的三维点。

2.5.将从X到关键帧和当前帧摄像机中心的视角方向分别记为n_x和n_x ^'，如果（τ_n通常设为cos(30°)），则认为特征点x在当前帧中被遮挡了或者发生了较大的透视变形，令V（X）=1，否则，进行下一步。

2.6.比较x和x′之间的颜色差异

$D_c\left(X\right)=\underset{d}{\min }\underset{y\&Element;W\left(x\right)}{\mathrm{\&Sigma;}}|I_y-\left(I_{y^{\&prime;}+d}\right)|$

其中，W(x)表示以x为中心的局部窗口（例如W(x)可以取为高和宽都为11的窗口），y为W(x)中一个二维点，y'是根据X的深度以及摄像机参数估计得到的y在当前帧中的投影点，d是一个微小的平移向量，用来提高y'和y的匹配精度。通过对d进行局部搜索获得D_c(X)的最小值，此时如果D_c(X)小于某个阈值τ_c，继续维持V（X）的值为1，否则，表明x颜色发生了改变或者被遮挡了，进行下一步的比较。

2.7.查找在当前帧中同x′的距离不超过r₁（例如r₁可以取20）个像素的跟踪到的特征点，并标记为φ(x')，如果φ(x')为空，表明X被运动物体所遮挡了，保持V（X）的值不变；如果φ(x')不为空，并且对于φ(x')中任何一个特征点y，都有则将V（X）设为0（其中X是x'对应的三维点，X_y是y所对应的三维点，和Z_X分别表示X_y和X的深度值），表明X一定发生了变化；如果φ(x')不为空，且存在y，使得进行下一步的比较。

2.8.对φ(x')中每一个特征点y，将X和X_y分别投影到X首次出现的关键帧上，并将投影点分别记为x_p和y_p，如果存在至少一个特征点y，使得|x_p-y_p|<r2（例如r₂可以取20），则将V（X）设为0，表明X发生了变化（光照变化或者位置改变）；否则，保持V（X）为1。

2.9.对于每一个关键帧，如果该帧上的无效三维点和同其它关键帧所共有的特征点的数量超过了该帧像素点总数的90%，那么将该关键帧标记为无效关键帧。

3.前后台多线程协同运行。前台线程用于对每一帧进行特征点匹配和摄像机运动参数的估计，后台线程不断进行KD树、关键帧和三维点云的维护和更新，并联合优化关键帧的摄像机运动参数和三维点位置。

3.1.前台线程以流水线的方式对每一个输入帧并行处理，流水线包含4个处理单元，第一单元由摄像头获取图像并作图像滤波和畸变校正；第二单元抽取特征点；单元3进行特征点的匹配，以及及摄像机参数的求解；单元4根据跟踪结果进行增强现实的绘制。

3.2.后台线程包含两条流水线，第一流水线包含2个处理单元，第一单元判断输入帧是否为关键帧，如果为关键帧则与已有关键帧进行局部匹配，并添加新的场景点；第二单元检测和删除变化的关键帧和三维点；第二流水线同样也包含2个处理单元，第一单元用来进行选取局部关键帧集合，局部关键帧集合包含当前关键帧和与当前关键帧存在公共匹配点的已有关键帧，联合优化局部关键帧的摄像机运动参数及局部关键帧可见的三维点位置，其他关键帧的相机运动参数和及三维点位置保持不变，即局部集束调整（Local Bundle Adjustment）；第二单元用来联合优化所有关键帧的摄像机运动参数及所有三维点位置，即全局集束调整（Global Bundle Adjustment）。

实施例

针对一组连续动态变化的视频序列（如图2（a）、图2（a）、图4（a）、图5（a）），该视频序列中场景内桌面上的物体摆放方式以及位置发生了显著的变化，使用本专利所提出的方法进行摄像机参数估计。

如图1所示，实施步骤如下：

1.特征点匹配及摄像机参数估计。使用SIFT特征描述量表达场景特征，每个场景点对应一个特征描述量，使用KD树组织所有场景特征，对每帧图像利用GPU提取SIFT特征点并在KD树中搜索特征匹配，随后进行摄像机参数的估计，包括如下步骤：

1.1.使用SIFT特征描述量（David G.Lowe:Distinctive Image Features fromScale-Invariant Keypoints.International Journal of Computer Vision60（2）:91-110（2004））表达场景特征，每个场景点对应一个特征描述量，使用KD树（SunilArya,David M.Mount:Approximate Nearest Neighbor Queries in Fixed Dimensions.SODA1993:271-280）组织所有场景特征。构造两棵存储全局场景特征的KD树T₁、T₂，用于全局特征点描述量和图像特征点描述量间的匹配，T₁用于实时匹配，T₂用于动态更新。

1.2.根据方法（David Nistér:An Efficient Solution to the Five-Point RelativePose Problem.IEEE Trans.Pattern Anal.Mach.Intell.26（6）:756-777（2004））选定两帧初始关键帧，对场景结构、运动以及KD树等进行初始化。

1.3.对每帧图像利用GPU抽取SIFT特征点，利用SIFT特征描述量在T₁中搜索匹配。对于特征点x，假设T₁中它的最相邻的两个描述量是N₁(x)和N₂(x)，使用下面的公式作为匹配的置信度

$c=\frac{\left|\right|p\left(x\right)-p\left(N_1\left(x\right)\right)\left|\right|}{\left|\right|p\left(x\right)-p\left(N_2\left(x\right)\right)\left|\right|}$

其中，p(x)表示特征点x的描述量。如果c<0.6,将x和N₁(x)作为一对匹配点，由此得到一系列场景特征点和当前帧图像特征点之间的匹配。

1.4.根据步骤1.3.得到一系列场景特征点和当前帧图像特征点之间的匹配，通过最小化目标函数估计出当前帧摄像机运动参数（R表示旋转矩阵，t表示平移向量，π表示投影函数（作用是将三维空间点投影到二维空间，获得二维空间点位置，即π（X,Y,Z）=（x,y）,其中x=X/Z,y=Y/Z）），x_i表示特征点，K表示摄像机内置参数，X_i表示三维点），通过RANSAC算法（Martin A.Fischler,Robert C.Bolles:Random Sample Consensus:A Paradigm forModel Fitting with Applications to Image Analysis and Automated Cartography.Commun.ACM24（6）:381-395,1981）剔除误匹配，将正确的匹配定义为全局匹配集合G。

2.场景更新。使用关键帧及场景特征点对应的三维点云表达场景几何信息。通过添加关键帧和三维点扩充场景，若已有关键帧的对应场景发生变化，删除变化的关键帧和变化的三维点，包括如下步骤：

2.1.对于当前输入帧，经过步骤1后，如果可以成功地估计摄像机参数，并且全局匹配集合G中至多有80个匹配点，则将当前帧作为潜在关键帧；

2.2.选取同当前帧公共特征点最多的五个关键帧，并在当前帧和该5个关键帧之间进行特征点匹配（对于当前帧中已经匹配的特征点不需要再进行匹配），为了加快匹配的速度，为选取的5帧关键帧分别建立一棵KD树T，并通过极线几何约束（Zhengyou Zhang,RachidDeriche,Olivier D.Faugeras,Quang-TuanLuong:A Robust Technique for Matching two Uncalibrated Images Through theRecovery of the Unknown Epipolar Geometry.Artif.Intell.78（1-2）:87-119,1995）除去无效的特征点以提高匹配的准确性。根据当前帧同5个关键帧之间的匹配点构成特征点轨迹，对于每一个特征点轨迹通过三角化（Triangulation）方法，即求解方程来估计该轨迹的三维点：

$X_i=\underset{X_i}{\min }\underset{j\&Element;\mathrm{\&phi;}\left(x_i\right)}{\mathrm{\&Sigma;}}{\left|\right|x_{\mathrm{ij}}-\mathrm{\&pi;}\left(K(R_j{X}_i+t_j)\right)\left|\right|}^2;$

其中，X_i表示三维点，φ(x_i)表示x_i出现的帧集合，x_ij表示特征点，R_j表示旋转矩阵，t_j表示平移向量，π表示投影函数，K表示摄像机内置参数。如果估计得到的三维点数量超过100，则将当前帧作为新的关键帧加入到关键帧集合中，将得到的新的三维点X_i加入到场景中，并将当前帧中的特征点选作新的参考特征点加入到后台KD树T₂中，同时用T₂更新T₁。

2.3.若当前输入帧被选作新的关键帧，接下来通过比较当前帧和关键帧的Z轴方向，选取那些方向差异小于某个阀值的关键帧，随后进一步通过比较摄像机参数（旋转矩阵和平移向量）确定5个与当前关键帧最相邻的关键帧。

2.4.对于每个选择的关键帧，首先将其同当前关键帧进行颜色直方图比较，将颜色直方图的差异定义为R、G、B三个通道直方图欧式距离之和,如果差异大于1，表明图像中的一些区域很可能发生了改变，将关键帧中的特征点x对应的三维点X投影到当前帧图像中，将投影位置记为x′。用V（X）来表示三维点X的状态，若X是无效三维点，则V（X）=0，否则，令V（X）=1。接下来对每一个选择的关键帧，通过比较特征点x和x′来判定X是否是有效的三维点。

2.5.将从X到关键帧和当前帧摄像机中心的视角方向分别记为n_x和n_x ^'，如果则认为特征点x在当前帧中被遮挡了或者发生了较大的透视变形，令V（X）=1，否则，进行下一步。

2.6.比较x和x′之间的颜色差异

$D_c\left(X\right)=\underset{d}{\min }\underset{y\&Element;W\left(x\right)}{\mathrm{\&Sigma;}}|I_y-\left(I_{y^{\&prime;}+d}\right)|$

其中，W(x)表示以x为中心的局部窗口（例如W(x)可以取为高和宽都为11的窗口），y为W(x)中一个二维点，y'是根据X的深度以及摄像机参数估计得到的y在当前帧中的投影点，d是一个微小的平移向量，用来提高y'和y的匹配精度。通过对d进行局部搜索获得D_c(X)的最小值，此时如果D_c(X)小于某个阈值τ_c，继续维持V（X）的值为1，否则，表明x颜色发生了改变或者被遮挡了，进行下一步的比较。

2.7.查找在当前帧中同x′的距离不超过20个像素的跟踪到的特征点，并标记为φ(x')，如果φ(x')为空，表明X被运动物体所遮挡了，保持V（X）的值不变；如果φ(x')不为空，并且对于φ(x')中任何一个特征点y，都有则将V（X）设为0（其中X是x'对应的三维点，X_y是y所对应的三维点，和Z_X分别表示X_y和X的深度值），表明X一定发生了变化；如果φ(x')不为空，且存在y，使得进行下一步的比较。

2.8.对φ(x')中每一个特征点y，将X和X_y分别投影到X首次出现的关键帧上，并将投影点分别记为x_p和y_p，如果存在至少一个特征点y，使得|x_p-y_p|<20，则将V（X）设为0，表明X发生了变化（光照变化或者位置改变）；否则，保持V（X）为1。

2.9.对于每一个关键帧，如果该帧上的无效三维点和同其它关键帧所共有的特征点的数量超过了该帧像素点总数的90%，那么将该关键帧标记为无效关键帧。

3.前后台多线程协同运行。前台线程用于对每一帧进行特征点匹配和摄像机运动参数的估计，后台线程不断进行KD树、关键帧和三维点云的维护和更新，并联合优化关键帧的摄像机运动参数和三维点位置，包括如下步骤：

3.1.前台线程以流水线的方式对每一个输入帧并行处理，流水线包含4个处理单元，第一单元由摄像头获取图像并作图像滤波和畸变校正；第二单元抽取SIFT特征点；单元3进行特征点的匹配，以及及摄像机参数的求解；单元4根据跟踪结果进行增强现实的绘制。

3.2.后台线程包含两条流水线，第一流水线包含2个处理单元，第一单元判断输入帧是否为关键帧，如果为关键帧则与已有关键帧进行局部匹配，并添加新的场景点；第二单元检测和删除变化的关键帧和三维点；第二流水线同样也包含2个处理单元，第一单元用来进行选取局部关键帧集合，局部关键帧集合包含当前关键帧和与当前关键帧存在公共匹配点的已有关键帧，联合优化局部关键帧的摄像机运动参数及局部关键帧可见的三维点位置，其他关键帧的相机运动参数和及三维点位置保持不变，即局部集束调整（Local Bundle Adjustment）；第二单元用来联合优化所有关键帧的摄像机运动参数及所有三维点位置，即全局集束调整（Global Bundle Adjustment）。

对于该实施例，场景中人将桌子上的物体的摆放位置进行了更改，本专利方法获得的跟踪结果如图2（b）、图3（b）、图4（b）、图5（b）所示，其中每一幅跟踪结果图像都对应于该图中（a）相应的输入帧，图中的圆点表示跟踪到的特征点，图中插入的卡通小人物、小狗以及鱼，这些都是用来表明跟踪结果的准确性的，可以发现它们在跟踪结果图中的位置在跟踪过程中能够做到保持不变，这表示跟踪的结果非常准确。对于实时的单目摄像机跟踪来说，这是一个非常具有挑战的例子，通过实验，比较发现：PTAM方法在初始时可以做到正确地进行摄像机跟踪，但是随着跟踪的进行和场景的不断变化，很快后续帧就无法做到准确跟踪；相比之下，本专利的方法却可以非常鲁棒地估计出摄像机参数，获得准确的跟踪结果，并对三维场景进行正确的绘制。

Claims (1)

1.一种针对动态变化场景的实时摄像机跟踪方法，其特征在于，它包括以下步骤：

(1)特征点匹配及摄像机参数估计：使用基于不变量的特征点来表达场景特征，每个场景点对应一个特征描述量，使用KD树组织所有场景特征，对每帧图像提取特征点并在KD树中搜索特征匹配，随后进行摄像机参数的估计；

(2)场景更新：使用关键帧，以及场景特征点对应的三维点云表达场景几何信息；通过添加关键帧和三维点扩充场景，若已有关键帧的对应场景发生变化，删除变化的关键帧和变化的三维点；

(3)前后台多线程协同运行：前台线程用于对每一帧进行特征点匹配和摄像机运动参数的估计，后台线程不断进行KD树、关键帧和三维点云的维护和更新，并联合优化关键帧的摄像机运动参数和三维点位置；

所述步骤(1)通过以下子步骤来实现：

(1.1)使用基于不变量的特征点来表达场景特征，每个场景点对应一个特征描述量，使用KD树组织所有场景特征；构造两棵存储全局场景特征的KD树T₁、T₂，用于全局特征点描述量和图像特征点描述量间的匹配，T₁用于实时匹配，T₂用于动态更新；

(1.2)选定两帧初始关键帧，对场景结构、运动以及KD树进行初始化；

(1.3)对每帧图像抽取特征点，利用其特征描述量在T₁中搜索匹配；对于特征点x，假设T₁中它的描述量最接近的两个特征点是N₁(x)和N₂(x)，使用下面的公式作为匹配的置信度：

$c=\frac{\left|\right|p\left(x\right)-p\left(N_1\right(x\left)\right)\left|\right|}{\left|\right|p\left(x\right)-p\left(N_2\right(x\left)\right)\left|\right|}$

其中，p(x)表示特征点x的描述量，p(N₁(x))表示特征点N₁(x)的描述量，p(N₂(x))表示特征点N₂(x)的描述量；如果c<0.6，将x和N₁(x)作为一对匹配点，由此得到一系列场景特征点和当前帧图像特征点之间的匹配；

(1.4)根据步骤(1.3)得到一系列场景特征点和当前帧图像特征点之间的匹配，通过最小化目标函数估计出当前帧摄像机运动参数；其中，R表示旋转矩阵，t表示平移向量，π表示投影函数，其作用是将三维空间点投影到二维空间，获得二维空间点位置，即π(X，Y，Z)＝(x，y)，其中x＝X/Z，y＝Y/Z，x_i表示特征点，K表示摄像机内置参数，X_i表示三维点，通过RANSAC算法剔除误匹配，将正确的匹配定义为全局匹配集合G；

所述步骤(2)通过以下子步骤来实现：

(2.1)对于当前输入帧，经过步骤(1)后，如果成功地估计摄像机参数，并且全局匹配集合G中至多有m1个匹配点，则将当前帧作为潜在关键帧，m1为自然数；

(2.2)选取同当前帧公共特征点最多的5个关键帧，并在当前帧和该5个关键帧之间进行特征点匹配，对于当前帧中已经匹配的特征点不需要再进行匹配，为了加快匹配的速度，为选取的5个关键帧分别建立一棵KD树T，并通过极线几何约束除去无效的特征点以提高匹配的准确性；根据当前帧同5个关键帧之间的匹配点构成特征点轨迹，对于每一个特征点轨迹x_i，通过三角化方法，即求解下述方程来估计该轨迹的三维点：

$X_i=\underset{X_i}{min}\underset{j\&Element;\mathrm{\&phi;}\left(x_i\right)}{\mathrm{\&Sigma;}}\left|\right|x_{ij}-\mathrm{\&pi;}\left(K\right(R_j{X}_i+t_j\left)\right)|{|}^2;$

其中，X_i表示三维点，φ(x_i)表示x_i出现的帧集合，x_ij表示特征点，R_j表示旋转矩阵，t_j表示平移向量，π表示投影函数，K表示摄像机内置参数；如果估计得到的三维点数量超过m2，50≤m2≤100，则将当前帧作为新的关键帧加入到关键帧集合中，将得到的新的三维点X_i加入到场景中，并将当前帧中的特征点选作新的参考特征点加入到后台KD树T₂中，同时用T₂更新T₁；

(2.3)若当前输入帧被选作新的关键帧，接下来通过比较当前帧和关键帧的Z轴方向，选取那些方向差异小于某个阈值的关键帧，随后进一步通过比较摄像机参数确定5个与当前关键帧最相邻的关键帧，所述摄像机参数为旋转矩阵和平移向量；

(2.4)对于每个选择的关键帧，首先将其同当前关键帧进行颜色直方图比较，将颜色直方图的差异定义为R、G、B三个通道直方图欧式距离之和，如果差异大于1，表明图像中的一些区域很可能发生了改变，将关键帧中的特征点x对应的三维点X投影到当前帧图像中，将投影位置记为x′；用V(X)来表示三维点X的状态，若X是无效三维点，则V(X)＝0，否则，令V(X)＝1；接下来对每一个选择的关键帧，通过比较特征点x和x′来判定X是否是有效的三维点；

(2.5)将从X到关键帧和当前帧摄像机中心的视角方向分别记为n_x和n_x'，如果τ_n设为cos(30°)，则认为特征点x在当前帧中被遮挡了或者发生了较大的透视变形，令V(X)＝1，跳到步骤(2.9)，否则，进行下一步；

(2.6)比较x和x′之间的颜色差异：

$D_c\left(X\right)=\underset{d}{min}\underset{y\&Element;W\left(x\right)}{\mathrm{\&Sigma;}}|I_y-\left(I_{y^{\&prime;}+d}^{\&prime;}\right)|$

其中，W(x)表示以x为中心的局部窗口，y为W(x)中一个二维点，y'是根据X的深度以及摄像机参数估计得到的y在当前帧中的投影点，I_y为关键帧在y处的像素颜色，I'_y'+d为当前帧在y′+d处的像素颜色，d是一个微小的平移向量，用来提高y'和y的匹配精度；通过对d进行局部搜索获得D_c(X)的最小值，此时如果D_c(X)小于某个阈值τ_c，令V(X)＝1，跳到步骤(2.9)，否则，表明x颜色发生了改变或者被遮挡了，进行下一步的比较；

(2.7)查找在当前帧中同x′的距离不超过r₁个像素的跟踪到的特征点，并标记为φ(x')，r₁取20；如果φ(x')为空，表明X被运动物体所遮挡了，令V(X)＝1；如果φ(x')不为空，并且对于φ(x')中任何一个特征点y，都有则将V(X)设为0；其中，X是x'对应的三维点，X_y是y所对应的三维点，和Z_X分别表示X_y和X的深度值，表明X一定发生了变化；跳到步骤(2.9)；如果φ(x')不为空，且存在y，使得进行下一步的比较；

(2.8)对φ(x')中每一个特征点y，将X和X_y分别投影到X首次出现的关键帧上，并将投影点分别记为x_p和y_p，如果存在至少一个特征点y，使得|x_p-y_p|<r₂，r₂取20，则将V(X)设为0，表明X发生了变化，所述变化为光照变化或者位置改变；否则，令V(X)＝1；

(2.9)对于每一个关键帧，如果该帧上的无效三维点和该帧同其它关键帧所共有的特征点的数量超过了该帧像素点总数的90％，那么将该关键帧标记为无效关键帧；

所述步骤(3)通过以下子步骤来实现：

(3.1)前台线程以流水线的方式对每一个输入帧并行处理，流水线包含4个处理单元，第一单元由摄像头获取图像并作图像滤波和畸变校正；第二单元抽取特征点；第三单元进行特征点的匹配，以及摄像机参数的求解；第四单元根据跟踪结果进行增强现实的绘制；

(3.2)后台线程包含两条流水线，第一流水线包含2个处理单元，第一单元判断输入帧是否为关键帧，如果为关键帧则与已有关键帧进行局部匹配，并添加新的场景点；第二单元检测和删除变化的关键帧和三维点；第二流水线同样也包含2个处理单元，第一单元用来进行选取局部关键帧集合，局部关键帧集合包含当前关键帧和与当前关键帧存在公共匹配点的已有关键帧，联合优化局部关键帧的摄像机运动参数及局部关键帧可见的三维点位置，其他关键帧的相机运动参数及三维点位置保持不变，即局部集束调整；第二单元用来联合优化所有关键帧的摄像机运动参数及所有三维点位置，即全局集束调整。