一种应用于多镜头组合式全景相机的SLAM方法

摘要

本发明提出一种应用于多镜头组合式全景相机的SLAM方法，很好地实现了车载多镜头组合式全景相机的自动定位与地图构建功能，达到0.1m级的高定位精度而无需借助昂贵的GPS/IMU组合导航系统。本发明方法先对多镜头组合设备进行相机检校，然后建立一个并行的三线程，其中位姿跟踪线程主要完成系统初始化，全景图像上的特征提取、匹配与投影，位姿求解优化以及关键帧的选取；稀疏地图构建线程主要负责根据初始位姿与匹配点建立稀疏地图点，对相机位姿与地图点进行局部优化，剔除错误的地图点与冗余的关键帧；闭环纠正线程主要负责探测相机的运动是否回到了之前经过的位置(即闭环)，加入闭环约束进行位姿的改正以及全局位姿及地图点优化(即全局光束法平差)。

说明书

技术领域

本发明涉及一种应用于多镜头组合式全景相机的SLAM方法，属于摄影测量与遥感，计算机视觉，机器人等领域。

背景技术

随着传感器、自动化和平台技术的发展，利用平台上安置的光学或距离传感器，同时实现自我定位与环境感知的智能系统成为摄影测量、计算机视觉和机器人学的新型研究方向；并在移动测图系统、无人驾驶汽车、火星和月球的深空探测、无人机侦察、室内导航等领域发挥着关键的作用。若以光学传感器为主要信息获取源，这种系统通常称为基于视觉的自动定位与地图构建(Simultaneous Localization And Mapping,SLAM)。主流的视觉SLAM系统有两种分类模式。一类按照所采用的传感器划分。包括单目SLAM(Mono-SLAM)、双目SLAM(Stereo-SLAM)和4D相机SLAM(RGBD-SLAM)。另一类按照所采用的方法划分，主流是基于点特征的SLAM(feature-based SLAM)和基于图像自身的SLAM(Direct SLAM)。成熟的、具代表性的SLAM系统如：基于特征点的ORB-SLAM，直接法的LSD-SLAM，将特征点法与直接法混用的SVO、SVO2.0以及RBG-D SLAM的代表作RTAB-MAP，等等。这些主流SLAM架构采用传统框幅式的相机或摄像机作为视觉信息获取装备。单目SLAM视场狭窄，尺度估计受累积误差的影响，较依赖于闭环条件。双目SLAM虽然克服了尺度漂移，但是视差狭窄依然没有改变。若局部成像区域信息较少(即提取其他特征较少)，或者出现较大的视角变化，则会引起跟踪频繁丢失。这也是SLAM技术尚未广泛应用于测绘行业的地面移动测图系统(mobilemapping system,MMS)的关键因素。大视场成像设备，如鱼眼镜头和全景镜头，理论上能够克服视场狭窄的问题。全景视觉成像具有360°全视角成像的优势，已经在测绘、机器人、计算机视觉等相关领域中逐步得到应用，如用于城市测图、视频监控、交通监督、虚拟现实、机器人导航、场景重建等。装载于移动测图系统或普通汽车上的全景成像装置通常只被用于街景收集，如谷歌和百度的街景图像，无法实现量测功能；基于全景视觉的几何定位和SLAM也有过很少量的研究，然而，目前尚缺少一套完整的、自动化的解决方案。

发明内容

本发明的目的就在于克服上述现有技术的不足而提供一种适用于多镜头组合式全景相机的SLAM系统，重点考虑将SLAM技术应用于多镜头组合设备所拍摄的全景影像序列，使其能够完成对全景相机在运动过程中拍摄图像时的定位与稀疏地图的构建。

实现本发明目的采用的技术方案是：事先对多镜头组合设备进行相机检校，然后建立一个三线程并行运行的系统，三线程分别负责全景图像位姿跟踪，稀疏地图构建以及闭环纠正。位姿跟踪线程主要完成系统初始化，全景图像上的特征提取、匹配与投影，位姿求解优化以及关键帧的选取；稀疏地图构建线程主要负责根据初始位姿与匹配点建立稀疏地图点，对相机位姿与地图点进行局部优化，剔除错误的地图点与冗余的关键帧；闭环纠正线程主要负责探测相机的运动是否回到了之前经过的位置(即闭环)，加入闭环约束进行位姿的改正以及全局位姿及地图点优化(即全局光束法平差)。

以上三个并行线程的具体实现步骤如下，

线程1具体包括如下步骤，

步骤1.1，采用三帧图像进行初始化，获取初始位姿及初始地图点，同时建立局部地图与全局地图；

步骤1.2，采用恒速模型跟踪下一帧地图点，即3D地图点到2D图像的匹配，匹配失败则执行步骤1.3；

步骤1.3，采用视觉词袋的方法进行3D地图点到2D图像的匹配，当匹配点数满足一定条件时执行步骤1.4；

步骤1.4，采用多视匹配获取当前帧位姿初值并用于匹配，当匹配点数满足一定条件时执行步骤1.5；

步骤1.5，当步骤1.2～步骤1.4中的匹配都失败时，进行重定位；

步骤1.6，步骤1.2～步骤1.5中任意一步成功即得到当前帧的位姿的初始值，并利用该值，将局部地图点全部投影至当前帧进行匹配，匹配完成后进行局部光束法平差，统计内点数，若大于内点数阈值a则判定为跟踪成功，若小于内点数阈值a判定为跟踪失败，等待系统传入下一帧，并直接执行步骤1.5；

步骤1.7，正确跟踪当前帧之后，判断当前帧是否作为关键帧；

线程2具体包括如下步骤；

步骤2.1，更新关键帧与地图点的连接关系，找出与其具有一定共视点数的关键帧作为共视关键帧；

步骤2.2，三角化地图点；

步骤2.3，将地图点插入步骤1.1中已构建的局部地图与全局地图；

步骤2.4，调用G2O的库建立优化图，并进行局部光束法平差；

步骤2.5，剔除误匹配点与冗余关键帧；

步骤2.6，再次执行步骤2.4；

步骤2.7，更新关键帧与地图点的连接关系，更新局部地图与全局地图；

线程3具体包括如下步骤，

步骤3.1，计算当前关键帧的BoW向量，用于表示当前帧的场景信息；

步骤3.2，选取与当前关键帧相连性最好的n1个关键帧，计算与他们的BoW相似性；

步骤3.3，选取步骤3.2中所计算的相似性的最小值minScore，设定判别阈值为n2*minScore；

步骤3.4，搜索与当前关键帧及其相连关键帧之外的关键帧，逐一计算BoW相似性，相似性大于阈值的设为第一候选关键帧；

步骤3.5，逐一计算当前帧与第一候选关键帧相连性最好的n3个关键帧的相似性，累加相似性计算累计得分AccumScore，将n4*AccumScore作为阈值；

步骤3.6，选取累计得分大于阈值的关键帧作为第二候选关键帧。

步骤3.7，连续性检验：将第二候选关键帧及其相连关键帧设为候选组，认为孤立的候选组具有很强的不确定性，因此检测候选组的连续性，若有n5个及以上候选组相连且与当前帧的相连关键帧同样构成候选关系，则认为此候选组通过连续性检验，否则删除该候选组；

步骤3.8，通过连续性检验的候选组中匹配点数最多的帧即视为闭环关键帧；

步骤3.9，依次与当前关键帧使用BoW搜索匹配特征点，若匹配点数小于n6则剔除此闭环关键帧，继续下一帧的匹配；匹配成功后，计算闭环帧到当前帧的相似变换群，即sim(3)变换，得到了闭环关键帧与当前帧的转换关系S，通过转化关系再次搜索匹配点并使用局部光束法平差优化S。

进一步的，所述相机检校采用一种适合于鱼眼相机的高精度模型完成，具体检校模型为，

其中，X_d＝(x_d>d)^T是鱼眼相机坐标，X_r＝(x_r>r)^T是鱼眼相机纠正后坐标，θ和分别是天顶角和水平角，k₁，k₂，A₁，A₂，A₃，C₁，C₂为多项式系数，f为镜头焦距，λ为比例系数。与为Kannala提出的通用相机检校模型的多项式；

检校具体步骤为，首先获取均匀分布在图像上的一系列纠正前与纠正后的点坐标；其次带入模型求解模型参数：k₁，k₂，A₁，A₂，A₃，C₁，C₂，λ以及与的多项式系数；最后通过模型纠正鱼眼图像上任意像素。

进一步的，步骤1.1中采用三帧图像进行初始化的具体实现方式如下，

确定第一帧为参考帧，提取ORB特征点，等待系统连续传入两帧，进行第1帧与第2帧，第2帧与第3帧的特征匹配，然后结合RANSAC剔除误匹配点，保留3帧共视点，计算1-2帧和2-3帧的本质矩阵E；对1-2、2-3、1-3帧组合分别进行三角化，则每个地图点得到三个值，若这三个值中有任意一个值为无穷大或三个值之间的两两差值大于0.08m，则剔除此地图点，否则认为此地图点三角化成功；若成功三角化的点数量大于正确匹配点数量的75％，则G2O进行优化；若优化后内点数量大于30个则视为初始化成功，否则判定为失败，等待系统新传入两帧图像进行初始化。

进一步的，步骤1.2的具体实现方式如下，

利用恒速模型估计当前相机位姿的初始值，将上一帧观察到的地图点投影到当前帧的全景球面上，然后，根据球面特征点坐标与鱼眼相机的外方位元素，判断该点所在的鱼眼相机编号，最后根据如下所示的模型：

x′＝mR_iK_i(x)+T_i

x′²+y′²+z′²＝r²

投射到鱼眼图像上设定搜索窗口进行特征匹配，上述模型中x′＝(x′ y′ z′)^T为特征点的球面坐标，[R_i|T_i]为第i个鱼眼相机相对于相机坐标系的位姿，K_i为第i个鱼眼相机的内参矩阵，r为人为设置的球面半径，同时计算地图点到全景球面的距离；

假定当前恒速模型的水平旋转角度绝对值为θ，预先人工设置地图点到相机中心距离的最大值d_max与最小值d_min，通过两点间的距离公式可计算出地图点到相机中心距离为d，则窗口大小W自适应地设为：

确定搜索窗口后，在窗口内搜索同名特征点，这里的匹配阈值设置为：最邻近次临近之比大于0.7且描述子距离小于70，对所有地图点进行逐个匹配，若匹配数大于20则认为匹配成功并结束当前帧的跟踪，传入下一帧进行跟踪，否则进入步骤1.3。

进一步的，步骤1.3的具体实现方式如下，

在当前帧与上一帧的特征点间进行匹配，若匹配点数小于10则进入步骤1.4，若匹配点数在10～25之间，则扩大搜索范围，匹配从当前帧直到上一个关键帧所观察到的所有地图特征点；若匹配点数大于等于25，设上一帧的位姿为当前帧的位姿，执行位姿优化，统计优化后的内点个数，若小于15，进入步骤1.4；否则判定为匹配成功，返回位姿，传入下一帧图像循环执行步骤1.2～步骤1.7。

进一步的，步骤1.4的具体实现方式如下，

使用当前帧与前两帧的所有特征点进行匹配，并提取三度重叠点，计算当前帧与前一帧的E矩阵，SVD分解后得到位姿初值，使用该初值将前一帧地图点投影到当前帧，并进行匹配，其中搜索窗口大小设置方式与1.2中的W一致。

进一步的，步骤1.7中当满足以下五项条件的任意一项时，则将此帧确定为关键帧，

①超过8帧未插入关键帧，则确定当前帧为关键帧；

②当前帧新插入的地图点大于其跟踪到的地图点的1.7倍；

③当前帧跟踪到的地图点数小于上一帧跟踪到的点数的85％且局部地图构建线程空闲；

④当前帧跟踪到的地图点数小于上一帧跟踪到的50％，中断局部建图并插入关键帧；

⑤局部地图构建的线程空闲。

进一步的，步骤2.1中根据全景相机的特殊情形，设定共视关键帧必须同时满足：(1)与当前关键帧的共视点数>＝50个；(2)与当前关键帧在图像序列中的间隔关键帧不超过40个；(3)其与当前关键帧的共视点在两帧所在金字塔层数差异>2的特征点数不超过总共视点数的85％。

进一步的，步骤3.9中若局部光束法平差优化后的匹配点内点数大于n7，则可认为此帧与当前帧位于相近的位置，设其为相连，根据相连关系与S将地图点投影到当前帧并与当前帧进行匹配，若匹配数量>n8则视为匹配成功，不再对之后的闭环候选帧进行计算；此时，调整当前帧及其相连关键帧的位姿与地图点，更新关键帧链接关系并传递调整位姿，构建Essential Graph进行全局位姿优化并调整相应地图点，最后执行全局光束法平差。

与现有技术相比，本发明有如下优点和有益效果：

1)使得当前的主流的基于特征点的SLAM算法能够应用于多镜头组合式的全景相机；

2)在全景图像上的SLAM系统相比于单目相机，双目或RGB-D相机的SLAM系统具有更好的视场角，从而具有更好的稳定性；

3)此系统能应用于高分辨率的移动测图系统中，在车载摄影测图系统中有望具有广泛的应用。

附图说明

下面结合附图和具体实施方式对本发明作进一步的说明。

图1为本发明实施例的流程图。

图2为本发明调用G2O优化库构建的优化图；其中(a)为位姿优化图；(b)构建的是局部图优化；(c)为Essential Graph；(d)为全图优化，即全局光束法平差；三角形代表位姿节点，圆点代表地图点节点，黑色与浅灰色为图的边，代表目标函数的误差项，其中黑色为位姿之间的误差项，浅灰色代表地图点与位姿之间的误差项。

图3为鱼眼相机拍摄的图像(上面一排)与其纠正后的图像(下面一排)。

图4为系统初始化中三帧同时匹配的结果。

图5为车载LADYBUG-5相机于天津彩虹花园小区连续拍摄的图像序列使用本发明进行定位与构图的结果总览。

图6为试验验证中总共6套数据的定位结果。

具体实施方式

本发明方法的具体作业流程如附图1所示，系统由三个并行线程构成，分别负责全景图像位姿跟踪，稀疏地图构建以及闭环纠正。位姿跟踪线程主要完成系统初始化，全景图像上的特征提取、匹配与投影，位姿求解优化以及关键帧的选取；稀疏地图构建线程主要负责根据初始位姿与匹配点建立稀疏地图点，对相机位姿与地图点进行局部优化，剔除错误的地图点与冗余的关键帧；闭环纠正线程主要负责探测相机的运动是否回到了之前经过的位置(即闭环)，加入闭环约束进行位姿的改正以及全局位姿及地图点优化(即全局光束法平差)。

位姿跟踪线程的具体流程如下：

线程1位姿跟踪，具体步骤为：

1.1：系统初始化，初始化的目的为启动系统并计算获取初始位姿，同时在系统中建立局部地图与全局地图。首先，鱼眼相机变形较大，匹配难度高，误匹配数量大，因此采用三帧而非两帧进行初始化。确定第一帧为参考帧，提取ORB特征点，等待系统连续传入两帧，进行第1帧与第2帧，第2帧与第3帧的特征匹配，然后结合RANSAC剔除误匹配点，保留3帧共视点，计算1-2帧和2-3帧的本质矩阵E，本质矩阵E的计算方法参考了Richard Hartley于2008年出版的计算机视觉教材《Multiple View Geometry in Computer Vision》，1-2、2-3、1-3帧组合分别进行三角化，则每个地图点会得到三个值，若这三个值中有任意一个值为无穷大或三个值之间的两两差值大于0.08m，则剔除此地图点，否则认为此地图点三角化成功。若成功三角化的点数量大于正确匹配点数量的75％，则调用由Rainer Kuemmerle所发布的开源优化库G2O进行优化(即摄影测量领域的光束法平差)。若优化后内点(匹配正确的点称为内点，匹配错误的点称为外点)数量大于30个则视为初始化成功，否则判定为失败，等待系统新传入两帧图像进行初始化。

1.2：采用恒速模型(即当前帧速度与上一帧速度保持不变)跟踪上一帧的地图点。首先，利用恒速模型估计当前相机位姿的初始值，将上一帧观察到的地图点投影到当前帧的全景球面上。然后，根据球面特征点坐标与鱼眼相机的外方位元素，判断该点所在的鱼眼相机编号。最后根据如下所示的模型：

x′＝mR_iK_i(x)+T_i

x′²+y′²+z′²＝r²

投射到鱼眼图像上设定搜索窗口进行特征匹配，上述模型中x′＝(x′ y′ z′)^T为特征点的球面坐标，[R_i|T_i]为第i个鱼眼相机相对于相机坐标系的位姿，K_i为第i个鱼眼相机的内参矩阵，r为人为设置的球面半径，同时计算地图点到全景球面的距离。假定当前恒速模型的水平旋转角度绝对值为θ，预先人工设置地图点到相机中心距离的最大值d_max与最小值d_min，通过两点间的距离公式可计算出地图点到相机中心距离为d，则窗口大小W自适应地设为：

确定搜索窗口后，在窗口内搜索同名特征点。这里的匹配阈值设置为：最邻近次临近之比大于0.7且描述子距离小于70。对所有地图点进行逐个匹配，若匹配数大于20则认为匹配成功并结束跟踪步骤功(即结束当前帧的跟踪，传入下一帧图像进行跟踪，即传入下一帧后循环1.2～1.7步骤)。否则进入步骤1.3。

1.3：在步骤1.2，即3D地图点到2D图像的匹配失败时执行。采用图像到图像匹配策略，方法是视觉词袋(Bag-of-words,BoW)技术。在当前帧与上一帧的特征点间进行匹配，若匹配点数小于10则进入步骤1.4。若匹配点数在10～25之间，则扩大搜索范围，匹配从当前帧直到上一个关键帧所观察到的所有地图特征点。若匹配点数大于等于25，设上一帧的位姿为当前帧的位姿，执行位姿优化(图2a)，统计优化后的内点个数。若小于15，进入步骤1.4；否则判定为匹配成功，返回位姿，传入下一帧循环1.2～1.7步骤。

1.4：采用多视匹配获取当前帧位姿初值并用于匹配。使用当前帧与前两帧的所有特征点进行匹配，并提取三度重叠点，计算当前帧与前一帧的E矩阵(E矩阵的计算参考Richard Hartley于2008年出版的计算机视觉教材《Multiple View Geometry inComputer Vision》)，SVD分解后得到位姿初值，使用该初值将前一帧地图点投影到当前帧，并进行匹配。搜索窗口大小设置方式与1.2中的W一致，也即下式：

假定当前恒速模型的水平旋转角度绝对值为θ，预先人工设置地图点到相机中心距离的最大值d_max与最小值d_min，通过两点间的距离公式可计算出地图点到相机中心距离为d，若匹配点数小于30则判定失败，进入步骤1.5。若匹配点数大于30，则执行位姿调用G2O开源优化库执行优化，剔除粗差后若匹配点数再次小于30，则采用第二步BoW的匹配点执行位姿调用G2O开源优化库执行优化。若内点数大于15则判定成功，返回位姿；否则进入1.5。

1.5：在以上匹配都失败时，进行重定位(Re-localization)。使用BoW搜索上一帧到上一个关键帧的所有匹配点，调用EPnP算法(此算法参考了Vincent Lepetit于2009年发表的论文:EPnP:An Accurate O(n)Solution to the PnP Problem)配合RANSA算法进行重定位。

1.6：以上步骤任意一步成功即可得到当前帧的位姿的初始值。利用该值，将局部地图点全部投影至当前帧进行匹配，匹配完成后进行局部光束法平差(图2b)。统计内点数，若大于20则判定为跟踪成功，若小于20判定为跟踪失败，等待系统传入下一帧，并直接执行步骤1.5。

1.7：选取关键帧：正确跟踪当前帧之后，判断当前帧是否作为关键帧。当满足以下五项条件的任意一项时，则将此帧确定为关键帧：

①超过8帧未插入关键帧，则确定当前帧为关键帧；

②当前帧新插入的地图点大于其跟踪到的地图点的1.7倍；

③当前帧跟踪到的地图点数小于上一帧跟踪到的点数的85％且局部地图构建线程空闲；

④当前帧跟踪到的地图点数小于上一帧跟踪到的50％，中断局部建图并插入关键帧；

⑤局部地图构建的线程空闲。

线程2局部地图构建

2.1：更新关键帧与地图点的连接关系，找出与其具有一定共视点数的关键帧作为共视关键帧。根据全景相机的特殊情形，设定共视关键帧必须同时满足：(1)与当前关键帧的共视点数>＝50个；(2)与当前关键帧在图像序列中的间隔关键帧不超过40个；(3)其与当前关键帧的共视点在两帧所在金字塔层数差异>2的特征点数不超过总共视点数的85％；

2.2：三角化地图点；

2.3：将地图点插入1.1中已构建的局部地图与全局地图；

2.4：调用G2O的库建立的优化图进行局部光束法平差；

2.5：剔除误匹配点与冗余关键帧，此步骤的算法参考Raul Mur-Artal于2016年发表的论文《ORB-SLAM2:An Open-Source SLAM System for Monocular,Stereo,and RGB-DCameras》；

2.6：再次执行2.4；

2.7：更新关键帧与地图点的连接关系，更新局部地图与全局地图。

线程3闭环探测与全图优化

3.1：计算当前关键帧的BoW向量，用于表示当前帧的场景信息；

3.2：选取与当前关键帧相连性最好的5个关键帧，计算与他们的BoW相似性；

3.3：选取3.2中所计算的相似性的最小值minScore，设定判别阈值为0.85*minScore；

3.4：搜索与当前关键帧及其相连关键帧之外的关键帧，逐一计算BoW相似性，相似性大于判别阈值的设为第一候选关键帧；

3.5：逐一计算当前帧与第一候选关键帧相连性最好的10个关键帧的相似性，累加相似性计算累计得分AccumScore，将0.75*AccumScore作为阈值；

3.6：选取累计得分大于阈值的关键帧作为第二候选关键帧。

3.7：连续性检验：将第二候选关键帧及其相连关键帧设为候选组，认为孤立的候选组具有很强的不确定性，因此检测候选组的连续性，若有3个及以上候选组相连且与当前帧的相连关键帧同样构成候选关系，则认为此候选组通过连续性检验。否则删除该候选组。

3.8：通过连续性检验的候选组中匹配点数最多的帧即视为闭环关键帧；

3.9：依次与当前关键帧使用BoW搜索匹配特征点，若匹配点数小于30则剔除此闭环关键帧，继续下一帧的匹配。匹配成功后，按照Horn于1998年提出的的使用四元数计算绝对位姿的方法计算闭环帧到当前帧的相似变换群，即sim(3)变换，此时便得到了闭环关键帧与当前帧的转换关系S，通过转化关系再次搜索匹配点并使用局部光束法平差优化S。若优化后的匹配点内点数大于20，则可认为此帧与当前帧位于相近的位置，设其为相连，根据相连关系与S将地图点投影到当前帧并与当前帧进行匹配，若匹配数量>50则视为匹配成功，不再对之后的闭环候选帧进行计算。此时，调整当前帧及其相连关键帧的位姿与地图点，更新关键帧链接关系并传递调整位姿，构建如附图2c所示的Essential Graph进行全局位姿优化并调整相应地图点，最后执行如附图2d所示的全图优化，即全局光束法平差。

试验验证

为验证本发明在全景影像序列上的同步定位于地图构建效果，选用了共6套数据集进行测试，其中两套数据集分别拍摄于日本东京附近的城区与郊区，另外四套数据集分别拍摄于中国天津市及周边各路段，包括城市主干道，城郊道路，小区内部道路，高速公路及隧道等场景。所有的定位结果均以与全景相机同步的车载GPS作为标准进行精度评估，定位精度可达分米级。

首先对所使用的LADYBUG多镜头组合式全景相机进行检校，LADYBUG拍摄鱼眼影像大小为1616pixels×1232pixels，像元分辨率为0.009mm，颜色分辨率为8bit量化的RGB色彩空间。鱼眼相机的焦距为3.3mm，投影中心相互距离约40mm；共有6个鱼眼相机，5个相机用于360°水平成像，1个相机指向天空。全景影像球面成像于距离球心20m处。拼接后的全景影像大小为4000×2000像素。相机检校中，我们采用了63个均匀分布的校准点，根据不同的检校模型，解算对应的模型参数，并评估了检校中误差。检校精度如表1所示，本文方法的精度为0.56像素，明显高于通用相机检校模型，并后继保证了优于一个像素的核线精度。图像检校效果如附图3所示，其中图3上面一排为鱼眼相机拍摄的图像，图3下面一排为纠正后的图像。

附图4所示为Ladybug数据集使用三帧同时匹配的结果，由于鱼眼相机变形较大，距离相机越近的地物变形越大，可以看出，特征点(图中圆圈标出的为特征点)基本分布于距相机相对较远的地物上。图中灰色方框中的电线杆在被4号鱼眼相机捕捉，可以看出极大的变形造成其上无任何匹配成功的特征点，除变形之外，距离4号相机的地物在不同帧上还具有较大的视差，扩大了匹配搜索范围，从而大大降低了匹配成功概率。纵然4号相机捕捉的图像正确的匹配点，但本发明使用的全景球模型使得只依靠其余相机捕捉到的特征依旧能完成跟踪定位。

附图5所示为使用车载LADYBUG-5相机于天津彩虹花园小区连续拍摄的图像序列，输入本发明进行定位于稀疏地图构建的结果总览，在图中可以清晰的看出车辆行驶的轨迹(连续黑色点形成的轨迹线)与周围地物的轮廓(黑色杂乱点)。

总共6套数据的位姿跟踪结果如附图6所示，可以看出，使用全景相机进行SLAM位姿跟踪的轨迹与GPS轨迹几乎完全重合，甚至，在一些GPS信号丢失如隧道内或周围有较高建筑物的场景(图6中方框内的路段)，本发明仍然能够通过全景图像所拍摄到的360度全方位场景进行较为准确的位姿恢复。位姿跟踪结果的精度如表2所示，可以看出，在GPS定位误差较小的两套数据集中，定位精度均达到了分米级，而天津数据集受GPS本身精度限制，定位精度达到了米级。

表1鱼眼相机检校精度的对比

表2共6套数据的跟踪结果

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。