CN106446815B - 一种同时定位与地图构建方法 - Google Patents

Abstract

本发明公开了一种同时定位与地图构建方法，该方法能够可靠地处理强烈旋转和快速运动。该方法提出了一种基于关键帧的同时定位与地图构建算法框架，能够支持快速的局部地图扩张。在此框架下，提出一种新的基于多个单应矩阵的特征跟踪方法，该方法在强烈旋转和快速运动下高效而鲁棒。又提出了一种基于滑动窗口的相机方位优化框架，用模拟的或实际的IMU数据对连续帧之间增加运动约束。最后，提出了一种获得特定平面和场景真实尺度的方法，从而把虚拟物体以真实大小摆放于特定平面之上。

Description

一种同时定位与地图构建方法

技术领域

本发明涉及一种可应用于增强现实的单目摄像机跟踪方法，尤其涉及一种基于关键帧的鲁棒的摄像机跟踪方法。

背景技术

近年来，随着移动终端和可穿戴设备的流行，增强现实获得可空前的关注。逼真的增强现实需要精确地估计相机方位和实时地重建场景，这也是同时定位与地图构建(SLAM)技术所要解决的问题。

由于现实场景的复杂性，传统SLAM方法在实际应用中效果并不好。例如，在新用户用移动设备来拍场景并观看增强现实效果的时候，往往会随意地移动和旋转移动设备。因此，强烈旋转和快速运动非常常见，但是最先进的SLAM系统也难以处理这样的复杂情况。

良好的增强现实体验需要SLAM系统能够处理多种复杂的相机运动，同时还要让新手容易上手。并且，在遇到快速运动和严重的运动模糊时也要保证相机丢失的频率尽量小。甚至在遇到跟踪失败的情况，应该要能很快地重定位相机方位，从而避免长时间的等待。

视觉的SLAM可以分成两类：基于滤波的方法和基于关键帧的方法。MonoSLAM(A.J.Davison,I.D.Reid,N.D.Molton,and O.Stasse.MonoSLAM:Real-time singlecamera SLAM.IEEE Transactions on Pattern Analysis and Machine Intelligence,29(6):1052–1067,2007)是典型的基于滤波的方法。相机运动的参数和场景三维点位置合表示为一种高维概率状态。用扩展卡尔曼滤波器(EKF)来预测和更新每一帧的状态分布。MonoSLAM方法的最大缺点是时间复杂度为O(N3)，其中N为地标的数量，因此它只适用于几百个点的小地方。另外，EKF容易累积线性误差。为了突破上述限制，PTAM(G.Klein andD.W.Murray.Parallel tracking and mapping for small AR workspaces.In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality,pages 225–234,2007)采用一种新颖的基于关键帧的并行跟踪和地图构建框架。为了获得实时的性能，将跟踪从地图构建中独立出来；为了保证较高的精度，后台线程对所有关键帧和场景三维点做集束调整(BA)。正如(H.Strasdat,J.M.Montiel,and A.J.Davison.Visual SLAM:whyfilter？Image and Vision Computing,30(2):65–77,2012)中所总结的，基于关键帧集束调整的方法优于基于滤波器的方法，特别是在地标数量N很大的情况下。

近来，许多最先进的SLAM系统都采用基于关键帧的框架，比如RDSLAM(W.Tan,H.Liu,Z.Dong,G.Zhang,and H.Bao.Robust monocular SLAM in dynamicenvironments.In IEEE International Symposium on Mixed and Augmented Reality,pages 209–218,2013)和ORB-SLAM(R.Mur-Artal,J.Montiel,and J.D.Tardos.ORB-SLAM:aversatile and accurate monocular SLAM system.IEEE Transactions on Robotics,31(5):1147–1163,2015)。RDSLAM通过检测场景的外观和结构变化，提出了一种修改过的RANSAC方法来实现动态场景中的实时定位。ORB-SLAM采用ORB特征来做相机定位、地图构建、重定位和回路检测。同时通过优化方位图来闭合循环回路。与大多数视觉的SLAM系统不同的是，LSD-SLAM(J.Engel,T.Sch¨ops,and D.Cremers.LSD-SLAM:Large-scale directmonocular SLAM.In 13th European Conference on Computer Vision,Part II,pages834–849.Springer,2014)实时恢复半稠密深度图来代替稀疏的特征点。基于半稠密深度图，LSD-SLAM采用直接跟踪法实时定位相机方位，在缺乏特征的场景下具有更高的鲁棒性。SVO(C.Forster,M.Pizzoli,and D.Scaramuzza.SVO:Fast semi-direct monocularvisual odometry.In IEEE International Conference on Robotics and Automation,pages 15–22.IEEE,2014)也采用直接跟踪法，但是采用稀疏的特征点，能达到很高的运行效率。

传统的基于关键帧的方法有一个主要的缺点，就是对于强烈旋转和快速运动缺乏鲁棒性。首先，为了充分地约束三维点，要求相邻的关键帧之间要存在足够的视差，然而这在强烈旋转的情况下是无法满足的。其次，基线很大的关键帧间的特征匹配非常耗时，使得地图扩张的延迟在数帧以上，因此只能在后台操作，这也导致了在快速运动下容易跟踪丢失。

为了提高对大幅度旋转的跟踪的鲁棒性，ORB-SLAM在插入关键帧时放松了视差的要求。为了保证稀疏而又精确的全局地图，冗余的关键帧和不稳定的三维点在之后需要剔除。LSD-SLAM和SVO都提出采用滤波技术来获得鲁棒的深度估计。基于滤波的方法对于缓慢的旋转有一定的鲁棒性，因为这种情况下视差是逐渐从小变大的。对于纯旋转运动，任意深度值都能得到相似的跟踪结果。随着视差的缓慢增加，可以逐步更新深度，并且最终会收敛于一个良好的估计值。但如果相机快速运动，那么处理难度会大增。首先，这些方法还不能很好地解决地图扩张的延时问题。若后台线程的立体匹配和深度滤波无法跟上相机的快速运动，就会导致跟踪失败。其次，强烈旋转和快速运动使得鲁棒的立体匹配十分困难。由于搜索范围和图像块的畸变变的不可预测，使得PTAM和SVO采用的基于模板匹配的特征跟踪变的不可靠。ORB-SLAM和RDSLAM采用的不变性特征描述子对大的透视畸变和运动模糊也很敏感。ASIFT(J.-M.Morel and G.Yu.ASIFT:A new framework for fully affineinvariant image comparison.SIAM Journal on Imaging Sciences,2(2):438–469,2009)提出通过模拟不同的视角来处理该问题，但是巨大的计算代价不适用于实时的应用。

当然现在也有一些显式处理纯旋转问题的SLAM方法。Gauglitz等(S.Gauglitz,C.Sweeney, J.Ventura,M.Turk,and T.Hollerer.Live tracking and mapping fromboth general and rotationonly camera motion.In IEEE International Symposiumon Mixed and Augmented Reality,pages 13–22.IEEE,2012)提出在当前帧和最近关键帧之间切换六自由度的跟踪或者全景跟踪，从而得到全景地图和多个分散的三维子地图。Pirchheim等人(C.Pirchheim,D.Schmalstieg,and G.Reitmayr.Handling pure camerarotation in keyframe-based SLAM.In IEEE International Symposium on Mixed andAugmented Reality,pages 229–238,2013)和Herrera等人(C.Herrera,K.Kim,J.Kannala,K.Pulli,J.Heikkila,et al.DT-SLAM:Deferred triangulation for robust SLAM.InIEEE International Conference on 3D Vision,volume 1,pages 609–616,2014)提出了相似的想法，可以得到一个全局的三维地图。Pirchheim等人在进行六自由度跟踪时，将每个二维特征点的深度设置为无穷大。Herrera等人提出，对于二维特征，用最小化到极线的距离这个条件替代重投影误差。一旦检测到足够的视差，这两种方法都三角化出新的三维点。但是，纯旋转在实际中很少发生。即使是在用户尽量让相机绕着光学中心旋转的时候也很难避免相机的平移。全景跟踪会把这种平移错误地当成额外的旋转分量，从而导致漂移。其次，缺少三维约束容易导致尺度不一致。再者，这些方法都无法处理相机的快速运动。

近来，许多SLAM系统通过结合IMU数据来提升鲁棒性，称之为视觉-惯性融合的SLAM(VI-SLAM)。大部分的VI-SLAM方法都是基于滤波的。为了突破基于滤波方法的不可扩展性限制，MSCKF(A.I.Mourikis and S.I.Roumeliotis.A multi-state constraintkalman filter for vision-aided inertial navigation.In IEEE InternationalConference on Robotics and Automation,pages 3565–3572.IEEE,2007)将三维点和相机之间的二元关系约束转换为多个相机之间的多元约束。状态向量只包含相机运动，不包含三维点位置，从而达到对于特征点数量线性的计算复杂度。Li和Mourikis(M.Li andA.I.Mourikis.High-precision,consistent EKFbased visual-inertial odometry.TheInternational Journal of Robotics Research,32(6):690–711,2013)分析了MSCKF的系统可观测性，提出采用初次估计所得雅克比矩阵(G.P.Huang,A.I.Mourikis,andS.I.Roumeliotis.Analysis and improvement of the consistency of extendedkalman filter based SLAM.In IEEE International Conference on Robotics andAutomation,pages 473–479,2008)来固定线性化的点，从而提升了MSCKF的系统一致性。在(J.A.Hesch,D.G.Kottas,S.L.Bowman,and S.I.Roumeliotis.Consistency analysis andimprovement of vision-aided inertial navigation.IEEE Transactions onRobotics,30(1):158–176,2014)中通过显式地指定不可观测方向，从而避免了虚假的信息融合，减小了系统不一致性。另外也有一些基于非线性优化的VI-SLAM方法，将问题表达为因子图，并用最大后验(MAP)估计来求解。为达到实时的优化效率，可通过只维护一个局部地图，比如(T.-C.Dong-Si and A.I.Mourikis.Motion tracking with fixed-lagsmoothing:Algorithm and consistency analysis.In IEEE International Conferenceon Robotics and Automation,pages 5655–5662.IEEE,2011)和(S.Leutenegger,S.Lynen,M.Bosse,R.Siegwart,and P.Furgale.Keyframe-based visual-inertialodometry using nonlinear optimization.The International Journal of RoboticsResearch,34(3):314–334,2015)，也可以将采用增量式的优化方法，比如(M.Kaess,H.Johannsson,R.Roberts,V.Ila,J.J.Leonard,and F.Dellaert.iSAM2:Incrementalsmoothing and mapping using the bayes tree.International Journal of RoboticsResearch,31(2):216–235,2012)、(V.Indelman,S.Williams,M.Kaess,andF.Dellaert.Information fusion in navigation systems via factor graph basedincremental smoothing.Robotics and Autonomous Systems,61(8):721–738,2013)和(C.Forster,L.Carlone,F.Dellaert,and D.Scaramuzza.IMU preintegration onmanifold for efficient visual-inertial maximum-a-posteriori estimation.InRobotics:Science and Systems,2015)。这些方法大多是针对机器人导航，对IMU的质量要求较高。

在运动平滑的假设下，Lovegrove等人提出采用累积三次B样条来参数化相机轨迹。采用这种表示后，任意时刻的相机方位和相应的一阶、二阶导数都可插值得到，因此可以非常容易的融合IMU测量数据。用连续时间的表示还可以很好地补偿滚动快门现象。Kerl等人(C.Kerl,J.Stuckler,and D.Cremers.Dense continuous-time tracking andmapping with rolling shutter RGB-D cameras.In IEEE International Conferenceon Computer Vision,pages 2264–2272,2015)也采用累积三次B样条和直接跟踪法跟踪滚动快门的RGB-D相机，并创建出稠密的地图。然而，强烈旋转和快速运动违反了平滑运动的假设。另外，在快速运动时，运动模糊通常比滚动快门现象明显的多。Meilland等人提出了一种统一的方法来同时估计运动模糊和滚动快门形变。但是这个方法是针对RGB-D相机的，在单目相机时则需要已知三维模型。

发明内容

本发明的目的在于针对现有技术的不足，提供一种同时定位与地图构建方法，可靠地处理强烈旋转和快速运动，从而保证良好的增强现实体验。

本发明的目的是通过以下技术方案来实现的：

一种同时定位与地图构建方法,包括前台线程和后台线程，具体包括如下步骤：

1)前台线程实时地处理视频流，对任意的当前帧I_i，提取特征点(比如FAST角点)；

2)跟踪全局单应矩阵集合

3)使用全局单应矩阵和特定平面单应矩阵来跟踪三维点，得到相机姿态估计所需的3D-2D的对应关系集合：

4)根据跟踪到的特征点数，与设定阈值做比较，评估跟踪的质量，分为好、中和差三个质量类别；

4.1)当跟踪质量好的时候，进行局部地图的扩展和优化，然后再确定是否要选一个新的关键帧：

4.2)当跟踪质量中的时候，估计一个局部单应矩阵集合并重新匹配跟踪失败的特征；

4.3)当跟踪质量差的时候，触发重定位程序，一旦重定位成功，用重定位后的关键帧来做全局单应矩阵跟踪，随后再做一次特征跟踪；

所述的步骤4.1)中，当确定要选一个新的关键帧时，新选择的关键帧唤醒后台线程全局优化，添加新关键帧和新三角化的三维点用来扩展全局地图，并采用局部集束调整进行优化，之后，对已有三维平面进行扩展，将新增的三维点赋予已有平面，或从新增三维点中提取出新的三维平面，随后，通过匹配新的关键帧与已有关键帧来检测回路，最后，对全局地图全局集束调整。

优选的，所述的步骤2)具体为：

对于相邻两帧图像，先通过直接对齐上一帧图像I_i-1和当前帧图像I_i来估计上一帧到当前帧间的全局单应变换：其中和分别是I_i-1和I_i的小模糊图像，Ω是小图像域，上标h说明向量是齐次坐标，中的波浪号表示将单应矩阵H从源图像空间转化到小图像空间，||·||_δ表示Huber函数，计算方法如公式：

对每个关键帧F_k，已有到I_i-1的全局单应矩阵的初始解，利用帧F_k和I_i-1之间的特征匹配集合(记为M_k,i-1＝{(x_k,x_i-1)})来确定与I_i-1具有重叠的关键帧集合，记为K_i-1,选出|M_k,i-1|＞20且|M_k,i-1|最大的五个关键帧,对于K_i-1中的每个关键帧F_k，优化初始解 通过传递性得到关键帧F_k到当前帧I_i的全局单应变换

优选的，所述的步骤3)具体为：

对于约束充分的三维位置可靠的特征点，只需要预测出当前帧C_i的相机方位确定搜索位置，用全局单应矩阵来预测C_i，将帧F_k∈K_i-1的一个二维特征点记为x_k，相应的三维点记为X_k，通过公式得到三维到二维的对应关系(X_k,x_i)，所有的三维到二维的对应关系集合记为通过公式来预测相机位置(如果有真实的IMU数据，也可以直接通过IMU状态的传播来预测相机位置)，对于约束不充分、三维位置不可靠的特征点，首先直接把作为搜索位置，如果跟踪质量评定为中，则根据匹配的对应关系估计得到一个局部单应矩阵集合对于未匹配的特征，在搜索位置中逐一尝试，对于每一个搜索位置，其范围为以搜索位置为中心的r×r的正方形区域，对约束充分和不充分的点，r分别取10和30；

给定单应矩阵H_k→i，计算每个对齐后的图像块的四个顶点的变化，只在变化超过一定阈值时，通过公式将关键帧F_k中x_k附近的局部块对齐到当前帧I_i中，其中κ(y)是相对于块中心偏移y处的图像灰度，y∈(-W/2,W/2)×(-W/2,W/2)，其中W是图像块的尺寸，对搜索区域内提取的FAST角点计算零均值SSD的值，并在偏差小于一定阈值时选择最好的一个。

优选的，所述步骤4.1)中的进行局部地图的扩展和优化的方法具体为：

对于帧F_k的一个二维特征x_k，一旦在帧I_i中找到对应的特征x_i，就根据公式计算射线角，其中若α(i,k)≥δ_α，采用三角化算法来求解X，否则，为x_k指定关键帧F_k的平均深度d_k来初始化X，并根据公式来优化X；选择一个局部窗口，先固定相机方位，通过求解单独优化每个三维点位置X_i，其中是关键帧和滑动窗口中观测到三维点X_i的帧的帧号索引集合，再固定三维点位置，通过优化局部窗口内所有相机的方位。

优选的，所述的固定三维点位置，通过优化局部窗口内所有相机的方位具体为：

假设已有在局部坐标系下测量的线性加速度和旋转速度真实的线性加速度为真实的旋转速度为是惯性测量数据的高斯噪声，I是3×3的单位矩阵，b_a和b_ω分别是线性加速度和旋转速度随时间变化的偏移，将相机运动的状态扩展为其中v是全局坐标系下的线性速度，所有状态的连续时间运动方程为：其中[·]_×表示斜对称矩阵,和表示随机游走过，旋转分量的离散时间运动方程为：其中 是两个相邻关键帧i和i+1之间的时间间隔，是四元数乘法运算符，ε_ω是防止0除的一个小值，将的真实值用公式来近似，其中是一个3×1的误差向量，得到其中G_ω是与噪声n_ω有关的雅克比矩阵，进而得到定义旋转分量的代价函数和协方差为

定义其它分量的离散时间运动方程、代价函数和协方差为

其中将在滑动窗口中所有运动状态s₁…s_l的能量函数定义为：

其中l是滑动窗口的大小，是马氏距离的平方。

对于没有IMU传感器，设置用特征匹配来对齐连续帧图像求解一个最优角速度：

其中R_Δ(ω,t_Δ)是公式(18)的旋转矩阵，M_i,i+1是图像i和i+1之间的特征匹配集合，如果存在真实的IMU测量数据，把帧i和i+1之间的IMU测量数据集记为其中和t_ij分别是第j个线性加速度，旋转速度和时间戳，采用预积分技术来预先积分这些IMU测量数据，并用如下公式代替(18)和(24)中的分量：

其中和是在预积分时和的状态。

优选的，所述的后台线程全局优化的步骤为：

对于新关键帧F_k中的每个三维点，先用公式(8)计算与已有关键帧的最大射线角，如果max_iα(i,k)≥δ_α且三维坐标成功三角化，则把点标记为约束充分的三维点，随后用属于平面的新的约束充分的三维点X来扩展在帧F_k中可见的已有三维平面，对关键帧F_k中的二维特征进行狄洛尼三角化来定义邻域关系，当一个三维点同时满足如下三个条件时，则将该点添加到平面P的集合V_P中：1)X没被指定到任何平面，2)X与V_P中的另一个点连通，3)|n^TX+d|＜δ_P，其中d_k是关键帧F_k的平均深度，采用RANSAC算法来提取新的三维平面，每次迭代随机地采样三个连接的点来初始化一个三维内点的集合V_P，采用SVD恢复三维平面参数P，随后开始一个迭代的调整平面扩展和优化的内循环，每次内部迭代，检查与集合VP中的点连接的那些三维点X到面的距离，如果|n^TX+d|＜δ_P则把X添加到V_P中，通过对V_P中的所有点最小化公式来优化平面，并重复上述步骤迭代的扩展V_P，当|V_P|＜30时舍弃P和V_P，将提取的平面按照相关三维点的数量降序排列，排序后的内点集合记为从第一个开始，对于任一如果一个三维点也存在于之前的平面点集中，则将该点从中移除，在全局集束调整中通过添加点到面的约束，优化三维平面的参数

优选的，所述的同时定位与地图构建方法还可以包括一个需要将虚拟物体以真是尺寸摆放于真实场景的特定平面上的初始化步骤：

使用已知尺寸的平面标定物来初始化跟踪，将世界坐标系设置于平面标定物中心，沿平面标定物边缘均匀采用n个三维点X_i,i＝1,…,n，通过求解下面的优化方程来恢复初始帧的相机方位(R,t)：其中，δX_||和δX_⊥的方向分别为X_i处沿边缘切线、垂直向内的方向，其长度保证和投影至图像后与X_i的投影点距离为1个像素，采用由粗到细的策略扩大收敛范围，对图像构造4层金字塔，由最上层开始，优化结果作为下层优化的初值。

本发明的有益效果是：

1、本发明方法提供了一种新的同时定位与地图构建框架，将局部地图的扩展和优化提前至前台线程，提高对强烈旋转、快速运动的鲁棒性。

2、本发明方法提供了一种基于多个单应矩阵的特征跟踪的方法，既可用于跟踪约束充分、足以精确恢复三维位置的三维特征点，又可用于约束并不充分、三维位置并不可靠点的特征跟踪。该方法不仅能处理强烈旋转和快速运动，又能达到实时的计算效率，还能够实时运行于移动设备。

3、本发明方法提供了一种新的基于滑动窗口的局部地图优化的方法，通过合理模拟IMU测量值，来有效地优化相机方位，在相机快速运动和严重模糊的情况下显著地提高鲁棒性。这种优化的框架还可以扩展到结合实际IMU数据的情况，从而进一步提高鲁棒性和精度。

4、本发明方法提供了一种获得特定平面和场景真实尺度的方法，从而把虚拟物体以真实大小摆放于特定平面之上。该方法只需要用户对准一个已知标志物的尺寸即可初始化跟踪，无需事先捕获标志物纹理，因此使用更为便捷。

附图说明

图1是本文方法RKSLAM的系统框架图。需要注意的是，与大多数的把所有地图构建工作放在后台的基于关键帧的系统不同，本方法把局部地图扩张和优化放在前台，目的是为了处理强烈旋转和快速运动。

图2是全局单应矩阵估计的对照图。(a)当前图像。(b)需要匹配的所选关键帧。(c)通过求解公式(3)将(b)对齐到(a)。(d)通过求解公式(5)将(b)对齐到(a)。(e)(a)和(c)的组合。(f)(a)和(d)的组合。

图3是局部单应矩阵用于特征匹配的效果图。上：通过全局单应矩阵和特定平面单应矩阵获得的37对匹配。下：通过局部单应矩阵估计得到的153对匹配。

图4是通过拟合所找到的三维点提取得到的三维平面。

图5通过插入虚拟立方体的方式来对比SLAM方法在室内场景中的效果。(a)没有先验的运动约束的RKSLAM结果。(b)设置时RKSLAM的结果。(c)在估计旋转速度时RKSLAM的结果。(d)结合实际IMU数据的RKSLAM结果。(e)ORB-SLAM的结果。(f)PTAM的结果。(g)LSD-SLAM的结果。

具体实施方式

本发明公开了一种同时定位与地图构建方法，该方法能够可靠地处理强烈旋转和快速运动，从而保证了增强现实的良好体验。在该方法中，首先提出了一种新的基于多单应矩阵的特征跟踪方法，该方法在强烈旋转和快速运动下高效而鲁棒。基于此，又提出了一个实时的局部地图扩张策略，能够实时地三角化场景三维点。此外还提出了一种基于滑动窗口的相机方位优化框架，用模拟的或实际的IMU数据对连续帧之间增加先验的运动约束。

在详细介绍本方法之前，先引入本方法的数学公式的约定(同R.Hartley andA.Zisserman.Multiple view geometry in computer vision.Cambridge universitypress,2004)。标量用斜体字符(如x)表示，矩阵用黑体大写字母(如H)表示；列向量用黑体字母(如b)表示，其转置b┬表示相应的行向量。此外，点、向量以及矩阵的集合一般表示成斜体大写字母(如V)。对于向量x，用上标h表示相应的齐次坐标，如x^h。在本方法中，每个三维点i包含三维位置X_i和局部图像块χ_i。每一帧i保存的是它的图像I_i，相机方位C_i，二维特征点位置集合{x_ij}，及其相应的到三维点索引集合V_i。所选的关键帧集合表示成{F_k|k＝1…N_F}。相机方位参数化为一个四元数q_i和相机位置p_i。对于一个三维点X_j，其在第i幅图像中的二维投影用如下公式计算：x_ij＝π(K(R_i(X_j-p_i)))(1)，其中K是相机内参，默认为已知且恒定，R_i是q_i的旋转矩阵，π(·)是投影函数π([x,y,z]^T)＝[x/z,y/z]^T。每个平面i保存其参数P_i和属于它的三维点的索引，标记为定义平面参数为P_i＝[n_i ^T,d_i]^T，其中n_i表示其法向，d_i表示原点到平面的符号距离。对于平面上的三维点X，满足公式n_i ^TX+d_i＝0。系统维护三类单应矩阵，用于描述关键帧到当前帧的变换。这三类单应矩阵分别为：全局单应矩阵、局部单应矩阵和特定平面的单应矩阵。全局单应矩阵用于对其整幅图像。关键帧F_k到当前帧I_i的全局单应矩阵表示为所有关键帧的全局单应矩阵集合表示为局部单应矩阵用于对齐两个局部图像块。对于两帧(F_k,I_i)，可得到多个局部单应矩阵，表示成特定平面的单应矩阵也用于对齐一个特定的三维平面的图像区域。对于关键帧F_k中存在的三维平面P_j，其对应的从F_k到I_i的单应矩阵可以写成对于每个关键帧F_k，求出一个特定平面单应矩阵的集合，表示为

针对一组连续动态变化的视频序列(如图5)，使用本发明所提出的方法进行摄像机参数估计，拍摄的时候摄像机有强烈旋转和快速运动。

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

1.前台线程实时地处理视频流。对任意的当前帧I_i，首先提取FAST角点(E.Rostenand T.Drummond.Machine learning for highspeed corner detection.In 9thEuropean Conference on Computer Vision,Part I,pages 430–443.Springer,2006)，跟踪得到到I_i的全局单应矩阵集合

全局单应矩阵可以大致对齐整幅图像，通过直接对齐关键帧F_k和当前帧I_i来估计全局单应矩阵：其中和分别是F_k和I_i的小模糊图像(SBI)(G.Klein and D.W.Murray.Improving the agility of keyframe-basedSLAM.In 10th European Conference on Computer Vision,Part II,pages 802–815.Springer,2008)。Ω是SBI域，上标h说明向量是齐次坐标。中的波浪号表示将单应矩阵H从源图像空间转化到SBI空间。||·||_δ表示Huber函数，计算方法如公式：

假设两个相邻帧之间的基线很小，因此一个全局的单应矩阵可以大致表示两帧图像间的运动。通过对上一帧I_i-1和当前帧I_i求解公式(3)，可以得到通过传递性可以得到我们利用F_k和I_i-1之间的特征匹配集合(记为M_k,i-1＝{(x_k,x_i-1)})，首先用M_k,i-1来确定与I_i-1具有重叠区域的关键帧集合，记为K_i-1。我们挑选出|M_k,i-1|＞20的且|M_k,i-1|最大的五个关键帧。对于K_i-1中的每个关键帧F_k，利用M_k,i-1来防止累积误差和偏移，将公式(3)修正为：实验中取δ_I＝0.1,δ_x＝10。相对于公式(3)，公式(5)可以在基线较大的情况下插值多个平面运动。采用高斯牛顿法来求解公式(3)和(5)。图2是对比图，其中(a)是当前图像，(b)是所选关键帧。如图所示，如果直接求解公式(3)，从(c)和(e)可以看出全局单应矩阵的估计结果不好，对齐误差十分明显；相反，通过公式(5)求解得到的全局单应矩阵的估计效果好很多，如图(d)和(f)。

2.使用全局单应矩阵和特定平面单应矩阵来跟踪三维点，得到相机方位估计所需的3D-2D的对应关系集合。

对于约束充分的三维位置可靠的点，只需要预测当前帧C_i的相机方位来投影三维点并确定搜索位置。用全局单应矩阵来预测C_i。从上一帧I_i-1可以推出全局单应矩阵，而与之重叠的关键帧集合是K_i-1。为了不失一般性，将帧F_k∈K_i-1的任意二维特征记为x_k而相应的三维点记为X_k。通过公式可以得到三维到二维的对应关系(X_k,x_i)。所有的三维到二维的对应关系集合记为可以通过公式来预测相机位置。如果有真实的IMU数据，也可以直接通过IMU状态的传播来预测相机位置。

对于属于三维平面P_j的点，采用由公式(2)得到的平面单应矩阵来做图像块对齐，并用于估计相机方位。

3.采用论文(G.Klein and D.W.Murray.Parallel tracking and mapping forsmall AR workspaces.In 6th IEEE/ACM International Symposium on Mixed andAugmented Reality,pages225–234,2007)中的方法评估跟踪的质量，分为好、中(不好)和差(对于较大平移的快速运动，跟踪结果可能会不好甚至很差)。

3.1.当跟踪质量好的时候，直接采用跟踪结果来扩张和优化局部地图，然后再采用论文(G.Klein and D.W.Murray.Parallel tracking and mapping for small ARworkspaces.In 6th IEEE/ACM International Symposium on Mixed and AugmentedReality,pages 225–234,2007)和(G. Klein and D.W.Murray.Improving the agilityof keyframe-based SLAM.In 10th European Conference on Computer Vision,PartII,pages 802–815.Springer,2008)中相应的算法来确定是否要选一个新的关键帧。

对于帧F_k的一个二维特征x_k，一旦在帧I_i中找到对应的特征x_i，就将三维点X三角化。首先根据公式计算射线角，其中在存在足够视差且α(i,k)≥δ_α的理想情况下，采用(R.Hartley and A.Zisserman.Multiple viewgeometry in computer vision.Cambridge university press,2004)中的传统三角化算法来求解X。在实验中设置δ_α＝1^°。然而在大多情况下，第一次观测中视差往往是不够的。但是在另一方面，任意x_k的深度会在帧I_i中中得到相似的重投影结果，因此在这一步可以为x_k指定任意的深度。本方法中用关键帧F_k的平均深度d_k来为X初始化，也即并根据公式来优化X。

一旦存在足够的运动视差，深度误差会影响相机跟踪。在每个新的帧到来的时候最好对局部地图进行集束调整，但是这达不到实时的性能。因此可以将优化过程修改为每次仅优化三维点或相机方位，这两个步骤轮流进行。这个策略在实际应用中十分有效，并且能显著地减少计算量从而达到实时，甚至在移动设备上也可以实时。

在固定相机方位的前提下优化三维点比较简单。通过最小化公式可以单独优化每个三维点X_i，其中是关键帧和滑动窗口中观测到三维点X_i的帧的帧号索引集合。

用公式分别优化每个相机i的方位，其中V_i是在帧i中可见的三维点的点索引集合。但是，对于由快速的相机运动造成的严重模糊，任何特征跟踪方法都会失败。这些模糊的帧缺乏可靠相机方位估计的约束。鉴于快速相机运动一般只会偶尔出现而且不会持续很长时间，故可以利用相邻帧来约束模糊的帧。具体来说，我们借鉴VI-SLAM的想法，比如(A.I.Mourikis and S.I.Roumeliotis.A multi-state constraint kalman filter for vision-aided inertial navigation.In IEEEInternational Conference on Robotics and Automation,pages 3565–3572.IEEE,2007)、(J.A.Hesch,D.G.Kottas,S.L.Bowman,and S.I.Roumeliotis.Consistencyanalysis and improvement of vision-aided inertial navigation.IEEETransactions on Robotics,30(1):158–176,2014)和(S.Leutenegger,S.Lynen,M.Bosse,R.Siegwart,and P.Furgale.Keyframe-based visual-inertial odometry usingnonlinear optimization. The International Journal of Robotics Research,34(3):314–334,2015)。假设已有在局部坐标系下的IMU测量数据(包括线性加速度和旋转速度)，并且相机姿态和IMU的姿态相同。则相机运动的状态可以扩展为其中v是全局坐标系下的线性速度，b_a和b_ω分别是线性加速度和旋转速度随时间变化的偏移。则真实的线性加速度为 旋转速度为其中是惯性测量数据的高斯噪声，I是3×3的单位矩阵。在试验中，一般设置(如果提供了实际的IMU测量数据则设置)。连续时间的运动模型(A.B.Chatfield.Fundamentals of high accuracy inertial navigation.AIAA,1997)描述了状态的时间演化：其中[·]_×表示斜对称矩阵。b_a和分别是由高斯白噪声模型和表示的随机游走过程。在实验中设置为对于离散时间状态，采用(N.Trawny and S.I.Roumeliotis.Indirect kalman filter for 3Dattitude estimation.Technical Report 2005-002,University of Minnesota,Dept.ofComp.Sci.&Eng.,Mar.2005)提出的积分方法来传递四元数：其中 是两个相邻关键帧i和i+1之间的时间间隔，是四元数乘法运算符，ε_ω是防止0除的一个小值。在实验中，设置ε_ω＝0.00001rad/s。的真实值可以用公式来近似，其中是一个3×1的误差向量。将公式(14)代入公式(19)整理可得：

其中G_ω是与噪声n_ω有关的雅克比矩阵。将公式(19)代入公式(17)整理可得结合(20)和(21)，分别定义旋转分量的代价函数和协方差为

其他分量的推导相对简单。离散时间状态的传递公式如下：

其中，误差函数和相应的协方差为：

基于(22)和(25)定义的相邻帧之间的这些相关约束，可以将在滑动窗口中所有运动状态s₁…s_l的能量函数定义为：

其中l是滑动窗口的大小，是马氏距离的平方。

上述的导数假设已知惯性测量数据和但实际上可能没有IMU传感器。对于线性加速度，可以设为因为在AR应用中几乎不会出现突然的跳变。但对于旋转速度，由于用户经常要转头观看整个AR效果，所以不能设置为故用特征匹配来对齐连续的SBI从而求解一个最好的拟合：

其中R_Δ(ω,t_Δ)是公式(18)的旋转矩阵，M_i,i+1是图像i和i+1之间的特征匹配集合。δ_X控制特征匹配项的权重，在试验中设为10。采用高斯牛顿算法求解公式(27)。

上述的优化框架可以轻松地扩展到结合真实IMU数据的情况，主要的差别是两个相邻帧之间可能存在多个IMU测量数据，而且相机姿态和真实IMU传感器的姿态一般有所不同，不能简单地认为一致(可以通过事先标定来得到相机和真实IMU传感器的相对姿态)。把帧i和i+1之间的IMU测量数据集记为其中和t_ij分别是第j个线性加速度，旋转速度和时间戳。可以采用(T.Lupton and S.Sukkarieh.Visual-inertial-aided navigation for high-dynamic motion in built environmentswithout initial conditions.IEEE Transactions on Robotics,28(1):61–76,2012)和(C.Forster,L.Carlone,F.Dellaert,and D.Scaramuzza.IMU preintegration onmanifold for efficient visual-inertial maximum-a-posteriori estimation.InRobotics:Science and Systems,2015)中的预积分技术来预积分这些IMU测量数据，并用如下公式代替(18)和(24)中的分量：

其中和是在预积分时和的状态。在该状态下估计雅克比矩阵，并只须计算一次。这里假设已经排除了重力分量，故只包含运动产生的加速度。可以通过(N.Trawny andS.I.Roumeliotis.Indirect kalman filter for 3D attitude estimation.TechnicalReport 2005-002,University of Minnesota,Dept.of Comp.Sci.&Eng.,Mar.2005)中的传感器融合算法来处理从加速度计和陀螺仪获得的原始数据，从而得到所需数据。在实验中，使用的IMU测量数据来自iPhone6中滤波后的数据。

3.2.在结果中(不好)时，还是有一些匹配的特征，其中的大部分还是约束充分的点，所以可以估计一个局部单应矩阵集合并重新匹配跟踪失败的特征。

如果全局单应矩阵和特定三维平面单应矩阵的跟踪质量不好，可以进一步用得到的匹配来估计局部单应矩阵。本方法采用一种简单的多个单应矩阵的提取策略来估计一个局部单应矩阵集合。具体来说，对M_k,i采用RANSAC方法，根据满足单应变换的内点数量来估计出一个最好的局部单应矩阵，记为将满足的内点从M_k,i中剔除，然后对剩余匹配进行同样的操作得到重复上述步骤直到剩余的匹配数少于一定数量。

对于约束不充分、三维位置不可靠的特征点，首先直接把作为搜索位置。如果跟踪结果不好，则根据匹配的对应关系估计得到一个局部单应矩阵集合对于未匹配的特征，在搜索位置中逐一尝试。对于每一个搜索位置，其范围为以搜索位置为中心的r×r的正方形区域。对约束充分和不充分的点，r分别取10和30。

本方法中的多个单应矩阵的表示方法不仅可以预测得到好的起始点还能纠正透视畸变。给定单应矩阵H_k→i，可以通过公式将关键帧F_k中x_k附近的局部块对齐到当前帧I_i中。其中κ(y)是相对于块中心偏移y处的图像灰度。y∈(-W/2,W/2)×(-W/2,W/2)，其中W是图像块的尺寸，实验中设为8。对于属于三维平面P_j的点，采用由公式(2)得到的平面单应矩阵来做图像块对齐。对于其他点，与搜索范围的预测类似，首先尝试全局单应矩阵如果跟踪结果很差再对跟踪失败的特征逐一尝试集合中的局部单应矩阵。考虑到相邻帧之间很少会出现大幅度的透视变化，所以没必要对每一个输入的帧重新计算公式(7)。可以计算每个对齐后的图像块的四个顶点的变化，并且只在变化超过一定阈值才重新计算公式(7)。

给定一个精确的搜索范围和没有畸变的图像块，可以很容易地跟踪得到在当前帧的对应关系。与PTAM类似，对搜索区域内提取的FAST角点计算零均值SSD的值，并在偏差小于一定阈值时选择最好的一个。

本方法中，由于一般可以通过全局单应矩阵和特定平面单应矩阵来获得足够的特征匹配，因此很少需要求解局部单应矩阵。只有当在帧F_k和I_i之间存在很大视差，并且重叠区域存在多个不同的平面时，才需要对那些异常点额外地尝试每个局部单应矩阵。如图3所示，通过全局和特定平面的单应矩阵可以获得37对匹配，另外可以通过局部单应矩阵获得额外的153对匹配。

3.3.在结果很差时，触发论文(G.Klein and D.W.Murray.Parallel trackingand mapping for small AR workspaces.In 6th IEEE/ACM International Symposiumon Mixed and Augmented Reality,pages 225–234,2007)中介绍的重定位程序。一旦重定位成功，用重定位后的关键帧来做全局单应矩阵跟踪，随后再做一次特征跟踪。

4.如果选取了新的关键帧，则唤醒后台线程。

4.1.添加新关键帧和新三角化的三维点用来扩展全局地图，并采用局部集束调整进行优化。之后，对已有三维平面进行扩展，将新增的三维点赋予已有平面，或从新增三维点中提取出新的三维平面。

与论文(G.Klein and D.W.Murray.Parallel tracking and mapping for smallAR workspaces. In 6th IEEE/ACM International Symposium on Mixed and AugmentedReality,pages 225–234,2007)类似，本方法在后台用局部和全局的集束调整法来优化关键帧的地图和相机方位。对于新关键帧F_k中的每个三维点，先用公式(8)计算与已有关键帧的最大射线角。如果max_iα(i,k)≥δ_α且三维坐标成功三角化，则把点标记为约束充分的。随后用属于平面的新的约束充分的三维点X来扩展在帧F_k中可见的已有三维平面。为了确定一个三维点X是否属于平面P，只需要检查点到面的距离|n^TX+d|。但是满足平面方程只是必要条件而不是充分条件，因为平面方程没有包含平面边界信息。假设属于同一个平面的点在空间中互相接近，故在检查点到面距离时只检查那些点的相邻点在平面上的点。通过对关键帧F_k中的二维特征进行狄洛尼三角化来定义邻域关系。当一个三维点满足如下三个条件时，则将该点添加到平面P的集合V_P中：1)X没被指定到任何平面，2)X与V_P中的另一个点连通，3)|n^TX+d|＜δ_P。在实验中设置δ_P＝0.01d_k，其中d_k是关键帧F_k的平均深度。

采用RANSAC算法(M.A.Fischler and R.C.Bolles.Random sample consensus:Aparadigm for model fitting with applications to image analysis and automatedcartography.Commun.ACM,24(6):381–395,1981)来提取新的三维平面。在RANSAC的每次迭代，随机地采样三个连接的点来初始化一个三维内点的集合V_P。假设三维平面P是由V_P产生的。随后开始一个迭代地调整平面扩展和优化的内循环。在每次内部迭代，检查与集合V_P中的点连接的那些三维点X到面的距离，如果|n^TX+d|＜δ_P则把X添加到V_P中。之后通过对V_P中的所有点最小化公式来优化平面，并尝试进一步扩展V_P。当|V_P|＜30时舍弃P和V_P。为了避免将一个点标记到多个平面，在每次RANSAC操作后，将提取的平面按照相关三维点的数量降序排列。排序后的内点集合记为从第一个开始，对于任一如果一个三维点也存在于之前的平面点集中，则将该点从中移除。图4记为提取到的三维平面。

通过添加点到面的约束，可以在全局集束调整中优化三维平面的参数。本方法通过LM(Levenberg Marquart)算法最小化如下能量函数来联合优化关键帧的所有相机方位，三维点以及三维平面：

需要注意的是，本方法的其他函数也是用LM算法来求解的，除了(3)，(5)和(27)。

4.2.通过匹配新的关键帧与已有关键帧的特征来检测回路。

对于回路检测，采用和重定位相同的算法来寻找新关键帧与前面帧的特征匹配，随后通过全局集束调整来得到闭合循环回路。

4.3.对全局地图做一次集束调整。

在实验中，首先对比了本发明所提出的方法在四种不同设置下的效果(如图5)：1)没有先验的运动约束的情况；2)直接设置的情况；3)估计旋转速度时的情况；4)结合实际IMU数据的情况。结果表明，在1)和2)的情况下，我们的方法和其他方法的结果类似，都会在强烈旋转和快速运动时频繁地出现跟踪失败的结果，如图5(a)和(b)。在3)的情况下，我们的方法即使是在快速运动和严重模糊时也能够可靠地恢复出相机方位，如图5(c)。在结合实际IMU数据时，鲁棒性则进一步提升，如图5(d)。另外ORB-SLAM、PTAM以及LSD-SLAM的结果如图5(e)、(f)、(g)所示，由于强烈旋转、快速运动以及严重模糊等，出现了严重的漂移。随后，又在室外大场景下对比了本方法与其他方法的效果。本方法的结果再一次证明了本方法的优越性。在没有实际IMU数据时，本文方法的结果依然能够恢复得到可靠的相机方位；在有实际IMU数据时，则效果更可靠一些。

此外，我们还在移动终端上的一个应用中运用了本方法，并进行了测试。用户在使用该应用时，用手机拍摄场景，然后在房间里插入多个三维的虚拟家具模型来查看家具摆放的效果。单目同时定位与地图构建(SLAM)方法恢复的三维结构的实际大小我们无法得知，为了能够保证三维家具模型在插入时具有合适的尺寸，因此需要一种方法来精确估计。但是，根据手机提供的带有噪声的IMU数据可能难以及时地估计出精确的尺寸。采用已知大小的标定物会更为可靠。在我们的应用中需要用户用一张A4纸，在开始使用应用时将摄像头中心对准纸，我们的增强现实系统就会自动地用相机方位估计来自动检测纸的四条边。初始化之后，用户就可以在列表里面下载不同的三维模型并放到场景中。

Claims (6)

1.一种同时定位与地图构建方法,其特征在于包括如下步骤：

1)前台线程实时地处理视频流，对任意的当前帧I_i，提取特征点；

2)跟踪全局单应矩阵集合

3)使用全局单应矩阵和特定平面单应矩阵来跟踪三维点，得到相机姿态估计所需的3D-2D的对应关系集合：

对于约束充分的三维位置可靠的特征点，通过预测当前帧C_i的相机方位确定搜索位置，用全局单应矩阵预测C_i，将帧F_k∈K_i-1的一个二维特征点记为x_k，相应的三维点记为X_k，通过公式得到三维到二维的对应关系(X_k,x_i)，所有的三维到二维的对应关系集合记为通过求解预测相机位置，对于约束不充分、三维位置不可靠的特征点，首先直接把作为搜索位置，如果跟踪质量评定为中，则根据匹配的对应关系估计得到一个局部单应矩阵集合对于未匹配的特征，在搜索位置中逐一尝试，对于每一个搜索位置，其范围为以搜索位置为中心的r×r的正方形区域，对约束充分和不充分的点，r分别取10和30；

给定单应矩阵H_k→i，计算每个对齐后的图像块的四个顶点的变化，只在变化超过一定阈值时，通过公式将关键帧F_k中x_k附近的局部块对齐到当前帧I_i中，其中κ(y)是相对于块中心偏移y处的图像灰度，y∈(-W/2,W/2)×(-W/2,W/2)，其中W是图像块的尺寸，对搜索区域内提取的FAST角点计算零均值SSD的值，并在偏差小于一定阈值时选择最好的一个；

4)根据跟踪到的特征点数，评估跟踪的质量，分为好、中和差；

4.1)当跟踪质量好的时候，进行局部地图的扩展和优化，然后再确定是否要选一个新的关键帧：

4.2)当跟踪质量中的时候，估计一个局部单应矩阵集合并重新匹配跟踪失败的特征；

4.3)当跟踪质量差的时候，触发重定位程序，一旦重定位成功，用重定位后的关键帧来做全局单应矩阵跟踪，随后再做一次特征跟踪；

所述的步骤4.1)中，当确定要选一个新的关键帧时，新选择的关键帧唤醒后台线程全局优化，添加新关键帧和新三角化的三维点用来扩展全局地图，并采用局部集束调整进行优化，之后，对已有三维平面进行扩展，将新增的三维点赋予已有平面，或从新增三维点中提取出新的三维平面，随后，通过匹配新的关键帧与已有关键帧来检测回路，最后，对全局地图全局集束调整。

2.如权利要求1所述的一种同时定位与地图构建方法，其特征在于所述的步骤2)具体为：

对于相邻两帧图像，先通过直接对齐上一帧图像I_i-1和当前帧图像I_i来估计上一帧到当前帧间的全局单应变换：其中和分别是I_i-1和I_i的小模糊图像，Ω是小图像域，上标h说明向量是齐次坐标，中的波浪号表示将单应矩阵H从源图像空间转化到小图像空间，||·||_δ表示Huber函数，计算方法如公式：

对每个关键帧F_k，已有到I_i-1的全局单应矩阵的初始解，利用帧F_k和I_i-1之间的特征匹配集合(记为M_k,i-1＝{(x_k,x_i-1)})来确定与I_i-1具有重叠的关键帧集合，记为K_i-1,选出|M_k,i-1|＞20且|M_k,i-1|最大的五个关键帧,对于K_i-1中的每个关键帧F_k，优化初始解 通过传递性得到关键帧F_k到当前帧I_i的全局单应变换

3.如权利要求1所述的一种同时定位与地图构建方法，其特征在于所述步骤4.1)中的进行局部地图的扩展和优化的方法具体为：

对于帧F_k的一个二维特征x_k，一旦在帧I_i中找到对应的特征x_i，就根据公式计算射线角，其中若α(i,k)≥δ_α，采用三角化算法来求解三维位置X，否则，为x_k指定关键帧F_k的平均深度d_k来初始化X，并根据公式来优化X；选择一个局部窗口，先固定相机方位，通过求解单独优化每个三维点位置X_i，其中是关键帧和滑动窗口中观测到三维点X_i的帧的帧号索引集合，再固定三维点位置，通过优化局部窗口内所有相机的方位。

4.如权利要求3所述的一种同时定位与地图构建方法，其特征在于所述的固定三维点位置，通过优化局部窗口内所有相机的方位具体为：

假设已有在局部坐标系下测量的线性加速度和旋转速度真实的线性加速度为真实的旋转速度为是惯性测量数据的高斯噪声，I是3×3的单位矩阵，b_a和b_ω分别是线性加速度和旋转速度随时间变化的偏移，将相机运动的状态扩展为其中v是全局坐标系下的线性速度，所有状态的连续时间的运动方程为：其中[·]_×表示斜对称矩阵,和表示随机游走过，旋转分量的离散时间运动方程为：其中 是两个相邻关键帧i和i+1之间的时间间隔，是四元数乘法运算符，ε_ω是防止0除的一个小值，将的真实值用公式来近似，其中是一个3×1的误差向量，得到其中G_ω是与噪声n_ω有关的雅克比矩阵，进而得到定义旋转分量的代价函数和协方差为

定义其它分量的离散时间运动方程、代价函数和协方差为

其中将在滑动窗口中所有运动状态s₁…s_l的能量函数定义为：

其中l是滑动窗口的大小，是马氏距离的平方；

对于没有IMU传感器，设置用特征匹配来对齐连续帧图像求解一个最优角速度：

其中R_Δ(ω,t_Δ)是公式(18)的旋转矩阵，M_i,i+1是图像i和i+1之间的特征匹配集合，如果存在真实的IMU测量数据，把帧i和i+1之间的IMU测量数据集记为其中和t_ij分别是第j个线性加速度，旋转速度和时间戳，采用预积分技术来预先积分这些IMU测量数据，并用如下公式代替(18)和(24)中的分量：

其中和是在预积分时和的状态。

5.如权利要求3所述的一种同时定位与地图构建方法，其特征在于所述的后台线程全局优化的步骤为：

对于新关键帧F_k中的每个三维点，先用公式(8)计算与已有关键帧的最大射线角，如果max_iα(i,k)≥δ_α且三维坐标成功三角化，则把点标记为约束充分的三维点，随后用属于平面的新的约束充分的三维点X来扩展在帧F_k中可见的已有三维平面，对关键帧F_k中的二维特征进行狄洛尼三角化来定义邻域关系，当一个三维点同时满足如下三个条件时，则将该点添加到平面P的集合V_P中：1)X没被指定到任何平面，2)X与V_P中的另一个点连通，3)|n^TX+d|＜δ_P，其中d_k是关键帧F_k的平均深度，采用RANSAC算法来提取新的三维平面，每次迭代随机地采样三个连接的点来初始化一个三维内点的集合V_P，采用SVD恢复三维平面参数P，随后开始一个迭代的调整平面扩展和优化的内循环，每次内部迭代，检查与集合V_P中的点连接的那些三维点X到面的距离，如果|n^TX+d|＜δ_P则把X添加到V_P中，通过对V_P中的所有点最小化公式来优化平面，并重复上述步骤迭代的扩展V_P，当|V_P|＜30时舍弃P和V_P，将提取的平面按照相关三维点的数量降序排列，排序后的内点集合记为从第一个开始，对于任一如果一个三维点也存在于之前的平面点集中，则将该点从中移除，在全局集束调整中通过添加点到面的约束，优化三维平面的参数

6.如权利要求1所述的一种同时定位与地图构建方法,其特征在于还包括一个需要将虚拟物体以真是尺寸摆放于真实场景的特定平面上的初始化步骤：

使用已知尺寸的平面标定物来初始化跟踪，将世界坐标系设置于平面标定物中心，沿平面标定物边缘均匀采用n个三维点X_i,i＝1,…,n，通过求解下面的优化方程来恢复初始帧的相机方位(R,t)：其中，δX_||和δX_⊥的方向分别为X_i处沿边缘切线、垂直向内的方向，其长度保证和投影至图像后与X_i的投影点距离为1个像素，采用由粗到细的策略扩大收敛范围，对图像构造4层金字塔，由最上层开始，优化结果作为下层优化的初值。