具体实施方式

为了使本领域技术人员更好地理解本发明，下面结合附图和具体实施方式对本发明的示例性实施例作进一步详细说明。

图1是示出根据本发明的示例性实施例的基于地图重建的SLAM系统100的框图。

参照图1，基于地图重建的SLAM系统100(以下简称为“SLAM系统100”)包括图像获取单元110、初始位姿估计单元120、地图构建单元130、位姿确定单元140以及存储单元150。

所述图像获取单元110可获取SLAM系统100当前所在周围环境的彩色图像及深度图像，作为待处理的当前帧的彩色图像和深度图像。此外，下文所提及的“先前帧”是指已经过根据本发明的示例性实施例的SLAM系统100处理和并在存储单元150中存储了相关信息的帧。所述图像获取单元110可包括单目相机和深度相机来分别获得所述彩色图像及所述深度图像，然而，应该理解，本申请不限于此，还可使用其他任何能够获得深度图像和彩色图像的相机或相机组合(例如，双目相机)来实现图像获取单元110。所述彩色图像可以是RGB图像，或者也可以是其他类型的能够提供像素灰度信息的彩色图像(例如，灰度图像)。

初始位姿估计单元120可基于所述彩色图像和深度图像，估计SLAM系统100的初始位姿。在本发明的示例性实施例中，所述初始位姿可以是一种相对位姿，稍后将结合图3来对此进行详细解释。

图2是示出根据本发明的示例性实施例的基于地图重建的SLAM系统100的处理流程的示意图。

参照图2，可知SLAM系统100的主要处理流程包括：惯性导航数据预积分，特征跟踪，新特征提取，特征再识别，全局光束平差和局部光束平差等处理。

在惯性导航数据预积分处理中，可根据采集的惯性导航数据进行相对姿态预估，该处理的输入是惯性导航数据，输出是相对姿态；在特征跟踪处理中，可对初始提取特征进行跟踪，该处理的输入为初始提取特征，输出为下一帧跟踪到的特征；在新特征提取处理中，可在图像获取单元110获取的图像中提取特征，该处理的输入为彩色图像，输出是特征；在特征再识别处理中，可寻找曾经提取的特征，该处理的输入为原始特征和之前帧的位姿结果，输出为特征再识别处理后的特征，具体实现可参照后面图6解释；在全局光束平差处理中，可进行全局非线性优化，该处理的输入是特征匹配集合，输出是位姿；在局部光束平差处理中，可进行局部非线性优化，该处理的输入是特征匹配集合，输出是位姿。

图3是示出根据本发明的示例性实施例的基于地图重建的SLAM系统100中的初始位姿估计单元120的框图。

参照图3，初始位姿估计单元120可包括：特征提取模块121，被配置为在图像获取单元110获取的彩色图像中提取特征；特征匹配模块122，被配置为根据所述特征进行特征匹配；以及估计模块123，被配置为使用匹配的特征来估计SLAM系统100的初始位姿。

在本发明的示例性实施例中，在特征提取模块121完成特征提取之后，特征匹配模块122可根据深度图像的深度信息获得提取的各个特征在当前帧的相机坐标系下的三维坐标，然后根据特征的局部信息描述找出其在另一帧上的匹配特征，并获得匹配特征在所述另一帧的相机坐标系下的三维坐标。在本发明的示例性实施例中，所述另一帧可以是先前帧之中的特定帧，稍后将对此进行更详细的描述。特征提取模块121和特征匹配模块122获得的这些特征相关信息可作为针对当前帧的相关信息而被存储在存储单元150中以在之后的处理中使用。

仅作为示例，特征提取模块121可在当前帧的彩色图像中提取点特征，并且特征匹配模块122可根据所述点特征进行点匹配。更具体地讲，特征匹配模块122可在先前帧之中的特定帧中寻找与特征提取模块121提取的点特征匹配的点特征。之后，估计模块123可使用匹配的点特征来估计SLAM系统100的初始位姿。使用点特征进行特征匹配的方法对于本领域技术人员而言是已知的，因此为了简明，在此将不再进行详细描述。

此外，当SLAM系统100所处的的环境纹理较为缺乏时(例如，处于玻璃房间、颜色较为统一的房间等)，能够获得的匹配的点特征的数量较少，这会导致SLAM系统100的误差较大或直接失效。因此，优选地，在本发明的示例性实施例中，当特征匹配模块122匹配的点特征的数量少于第一预设阈值(例如，20个)时，所述特征提取模块121还可在当前帧的彩色图像中进一步提取线段特征，并且特征匹配模块122还可根据所述线段特征进行线段匹配，即，在所述特定帧中寻找与特征提取模块121提取的线段特征匹配的线段特征。此时，估计模块123可使用匹配的点特征和线段特征两者来估计SLAM系统100的初始位姿。

在本发明的示例性实施例中，为了利用提取的线段特征进行特征匹配，可对线段设置特征描述符。此时，由于环境纹理匮乏，线段特征周围的局部灰度信息利用效率较低，因此在本发明的示例性实施例中，可根据线段之间的相对形状信息对线段特征进行描述。具体来说，可将彩色图像中提取出的任意一条线段用作参考线段，然后可根据彩色图像中剩余的所有线段相对于此参考线段的相对位置和相对角度来建立特征描述符。使用线段的中点坐标来表示其位置。例如，对于任一线段i，其描述符的矢量形式v_i(k)可表示为：

v_i(k)＝#{q≠p_i:(q-p_i)∈bin(k)} (1)

其中，bin(k)表示彩色图像中的多个区域之中的第k个区域，k为区域标号，p_i为线段i的中点坐标，q表示除线段i之外的其他线段的中点坐标。

图5示出线段特征描述符矢量的生成示意图。在图5中，{v₁,v₂,…,v_n}分别指示彩色图像的如图5所示的4×4的块区域中的第1个区域、第2个区域、…、以及第n个区域中的线段特征描述符矢量。

特征匹配模块122可根据线段特征周围的几何结构信息进行线段匹配。具体来说，特征匹配模块122可对当前帧和所述特定帧的线段特征描述符矢量两两计算欧式距离，从而找到最佳线段匹配。这里，为了保证匹配的正确率，在本发明的示例性实施例中，可设定最佳线段匹配必须是两个线段均为各自在对应图像中的最佳匹配。

在本发明的示例性实施例中，由于当前帧的深度图像可提供深度信息，因此估计模块123可基于迭代最近点(ICP)方法，使用匹配的点特征和/或线段特征对当前帧与所述特定帧之间的位姿变化进行估计，从而获得SLAM系统100的初始位姿。也就是说，针对当前帧所获得的SLAM系统100的初始位姿是相对于所述特定帧的相对位姿。此外，应该理解，本申请不限于此，还可使用其他各种合适的方法来估计SLAM系统100的初始位姿。

此外，在本发明的示例性实施例中，当SLAM系统100首次进入特定环境时，估计模块123可直接将此时SLAM系统100的初始位姿设置为零矢量。

此外，在本发明的示例性实施例中，当匹配的点特征的数量非常少时，例如，当在所述特定帧中与特征提取模块121提取的点特征匹配的点特征的数量少于第二预设阈值(例如，5个)时，通过点特征匹配所估计的相对位姿不再可靠，此时，所述估计模块123可将SLAM系统100的初始位姿确定为与所述特定帧中SLAM系统100的相对位姿保持相同。

此外，在本发明的示例性实施例中，所述特定帧可以是当前帧的上一帧或附近的若干帧。然而，如果仅使用与当前帧附近的若干帧的关系来建立针对当前帧的约束，则当SLAM系统100运行较快或者旋转较大时，能与当前帧建立联系的帧数就会变少，可能影响SLAM系统100的准确性。因此，优选地，根据本发明的示例性实施例的SLAM系统100还可包括全图跟踪模块(未示出)。所述全图跟踪模块(未示出)可在先前帧(例如，先前的关键帧)之中找出与当前帧具有共视点(例如，共视特征点)的共视关键帧(即，在当前帧与这些共视关键帧中都能够看到该共视点，存在与该共视点对应的投影点)，这些共视关键帧可被用于进一步建立针对当前帧的约束。在这种情况下，所述特定帧还可包括这些共视关键帧。也就是说，特征匹配模块122还可在所述共视关键帧中寻找与特征提取模块121提取的特征(包括点特征及线段特征)匹配的特征，从而估计模块123可进一步使用这些匹配的特征估计初始位姿。以下将结合图6、图7和图8对全图跟踪模块进行详细描述。

图6示出特征再识别操作的示意图。图7示出全图跟踪模块的关键帧筛选示意图。图8示出全图跟踪模块的全图跟踪流程示意图。

参照图6，左上角图像提取了一个点特征，标号为6993，从帧号1035的图像一直跟踪到帧号1042的图像，然后在帧号1043图像中，这个特征跟踪丢失，这是因为在这幅图像中，有一个梯子挡住了这个特征。但是在帧号1723图像中，这个特征被重新识别出。提取出该特征的帧(即，帧号1035的帧)和重新识别出此特征的帧(即，帧号1723的帧)之间角度为67.8度，平移距离大概为3米左右。帧号1035至1042的帧与帧号1723的帧具有共同的共视特征点6993，因此，帧号1035至1042的帧也可被用于建立针对帧号1723的帧的约束。由此可提高帧帧之间产生约束的特征数量并且为当前帧找到更广泛而稳定的约束(即，找出更多能够建立针对当前帧的约束的帧)，进而可提高SLAM系统100定位的精度。

可选择地，由于已使用当前帧的上一帧或附近的帧来进行初始位姿估计，再使用时间距当前帧较近的共视关键帧来进行初始位姿估计意义不大，因此，在本发明的示例性实施例中，优选地，初始位姿估计单元120可仅选择使用时间距当前帧超过预设时间阈值的共视关键帧进行初始位姿估计。稍后参照图7对此进行进一步描述。

在选择和当前帧进行匹配的关键帧时，由于计算负担原因，需进行关键帧筛选，这里可使用时间和空间结合的策略对关键帧进行筛选。

对于时间上的筛选，仅作为示例，参照图7，时间上距离当前帧较近的关键帧，如图中K_t,K_t+1,K_t+2等帧，由于和当前帧距离较近，则它们之间本就存在许多的共视点，因此再增加少数共视点对最终精度影响不大，因此可设定一个时间阈值，在距离当前帧时间较为久远的关键帧中去寻找匹配特征，提高计算效率。另外，从空间上看，也需要进行一系列的筛选操作，例如，对关键帧上的点进行特征过滤、基线较大特征匹配和特征分级等操作。当含有所有对空间时间条件敏感的点(即，经过空间和时间条件筛选出的点)的关键帧都经过处理以后，则关键帧筛选完成。

对点进行空间上的筛选的处理可如下：

首先，可过滤掉不稳定的点。每个地图点的逆深度都在全图光束平差中进行迭代优化，因此可计算每个地图点的逆深度在一段时期内的变化，如果该变化过大或者其逆深度小于零，则认为这个点是不稳定的，并去掉此点。

另外，还可去掉处于图像边缘的点。这里，通过相机姿态的初步估计(例如，通过使用初始位姿)，通常地图点的投射点会投射在当前帧上对应特征点的周围，如果所述投射点与对应特征点之间的计算的空间距离太大，则可过滤掉该地图点。

之后，可计算所述投射点与对应特征点之间的描述距离(即，描述符)，然后当该描述距离小于某一阈值时，可执行特征再识别操作。

由于对空间时间条件敏感的点，往往存在和当前帧之间对应点基线过宽的情况，容易产生误匹配，因此在本发明的示例性实施例中可使用下面结合图8描述的处理来确定共视关键帧。

仅作为示例，如图8所示，C₁,C₂,C₃均为关键帧，C₄为当前帧。其中，C₁,C₂,C₃与C₄距离较远，传统方法中是不考虑C₁,C₂,C₃与C₄之间的约束的。但在本发明的示例性实施例中，以特征点P₁(这里，特征点P₁是地图点)为例，在关键帧C₁中可看到该特征点P₁，全图跟踪模块(未示出)可通过如下操作来确定关键帧C₁是否是当前帧C₄的共视关键帧：

(1)将特征点P₁根据初始位姿估计单元120估计的初始相对位姿关系投射到当前帧C₄，标记为特征点p_i；

(2)计算当前帧C₄中该特征点p_i附近的特征点q_i与特征点p_i的空间坐标以及局部灰度差值d(p_i,q_i)；

(3)找出当前帧C₄中与特征点p_i的局部灰度差值小于设定阈值的所有特征点的集合其中，k＝1,2,3,…,m，m为找出的特征点的数量；

(4)基于以下公式(2)和(3)来对特征点p_i的特征描述符D(p_i)和特征点的特征描述符分别进行比较。这里，如果关键帧C₁和当前帧C₄的相对旋转ΔR和相对平移ΔT均小于给定阈值(例如，相对旋转阈值T_ΔR＝45度，相对平移阈值T_ΔT＝2米)，则直接比较特征点p_i的ORB描述符D(p_i)和特征点的ORB描述符的汉明距离，否则，可如公式(3)所示先将特征点的描述重新投射(warp)到当前帧上，然后比较ORB描述符的汉明距离；

(5)将上述集合中的ORB描述符之间的汉明距离最小的点确定为与特征点P₁匹配的特征点，并且此时可确定关键帧C₁与当前帧C₄具有共视特征点P₁。

其中，

在以上描述中，使用汉明距离来指示特征点之间的距离，但这仅是示例，还可使用其他各种距离表示方法来确定特征点之间的距离。

在本发明的示例性实施例中，当初始位姿估计单元120使用多个帧(例如，除了使用当前帧的上一帧之外还使用了上述共视关键帧)进行位姿估计而获得了多个初始位姿时，初始位姿估计单元120可将获得的多个初始位姿的统计值(例如，平均值、中值等等)确定为SLAM系统100的初始位姿。

接下来，返回参照图1，地图构建单元130可基于图像获取单元110获取的深度图像和彩色图像构建三维地图。

图4示出根据本发明的示例性实施例的基于地图重建的SLAM系统100中的地图构建单元130的结构。

如图4所示，根据本发明的示例性实施例的，地图构建单元130可包括地图重建模块131、地图存储模块132和地图更新模块133。

地图重建模块131可在首次进入特定环境时基于所述特定环境的第一深度图像和第一彩色图像来重建初始的三维地图，之后地图更新模块133可在再次进入所述特定环境时基于所述特定环境的第二深度图像和第二彩色图像来更新所述三维地图

更具体地讲，当SLAM系统100首次进入特定环境时，可由地图重建模块131基于SLAM系统100首次进入该特定环境时所获取的深度图像和彩色图像来重建三维地图，确定重建的三维地图上的每个地图点的三维坐标(即，在世界坐标系下的三维坐标)及其空间准确度。

仅作为示例，地图重建模块131可基于深度图像中所包括的深度信息以及彩色图像中所包括的灰度信息，使用三维语义重建来重建三维地图。在地图重建模块131使用三维语义重建来重建三维地图的过程中，除了确定三维地图上的每个地图点的三维坐标以外，还可确定每个地图点的空间准确度。所述空间准确度可指示计算出的地图点的重投影误差，并且关于空间准确度的计算在下面被详细描述。此外，应该理解，以上列举的三维语义重建方法仅是实现上述三维地图重建处理的方式之一，本申请不限于此，还可使用本领域已知的任何其它合适的三维地图重建方法来重建三维地图并确定每个地图点的空间准确度。

在完成三维地图重建之后，地图重建模块131可将带有空间准确度的的三维地图存储在地图存储模块132中。这里，由于SLAM系统100首次进入特定环境时并未获得空间时间置信度，因此可将此时的三维地图上的地图点的空间时间置信度设置为空并进行存储。

之后，当SLAM系统100再次进入该特定环境时，可无需再次重建三维地图，而是由地图更新模块133基于SLAM系统100再次进入该特定环境时所获取的深度图像和彩色图像计算每个地图点的三维坐标及其空间时间置信度，根据计算出的每个地图点的三维坐标及其空间时间置信度来更新地图存储模块132中所存储的三维地图上的对应地图点的三维坐标及其空间时间置信度。更新后的三维地图可作为地图构建单元130构建的三维地图进行后续使用。

这里，对于三维地图上的特定地图点，其空间时间置信度通常会随着时间的流逝而下降，并且其下降速度通常与该特定地图点的三维语义类型相关。在本发明的示例性实施例中，可如以下公式(4)来确定所述特定地图点的空间时间置信度C_c：

其中，t₁表示SLAM系统100的当前系统时间，t₀表示SLAM系统100的初始系统时间。ω1和ω2分别表示空间权重和时间权重。表示进行三维语义重建时该特定地图点的重投影误差，并且可由以下公式(5)来计算：

在公式(5)中，为与该特定地图点对应的投影点的二维坐标，ε表示SLAM系统100在世界坐标系下的位姿，这里可基于初始位姿估计单元120所估计出的初始位姿来获得，X表示该特定地图点在世界坐标系下的三维坐标，π是用于将输入变量转换为二维坐标的函数。

由此，可如以下公式(6)和(7)来分别实现该特定地图点的三维坐标及其空间准确度的更新：

C_p′＝0.5×(C_p+C_c)......(7)

其中，X_p和C_p分别为地图存储模块132中所存储的该特定地图点的三维坐标及其空间时间置信度，X_c为地图更新模块133计算出的该特定地图点的当前三维坐标，X_p′和C_p′分别为该特定地图点的更新后的三维坐标及其空间时间置信度。P_r表示初始位姿估计单元120所估计出的SLAM系统100的初始位姿。

在计算出上述X_p′和C_p′之后，可使用计算出X_p′和C_p′来分别替换地图存储模块132中所存储的X_p和C_p，由此实现地图存储模块132中存储的三维地图的更新。

在图4的图示中，地图存储模块132被示出为单独的模块，然而，应该理解，仅作为示例，地图存储模块132也可与地图更新模块133集成为一个模块。

通过地图更新模块133的上述更新处理，每当SLAM系统100进入相同的环境时，可仅对存储的三维地图进行更新而无需每次重建三维地图，由此降低了获得三维地图所需的计算量，并且还使得地图越来越精确，从而能够在后续的三维地图的使用过程中，固定三维地图点，而仅优化SLAM系统100的位姿。

接下来，返回参照图1，根据本发明的示例性实施例的位姿确定单元140可基于估计出的SLAM系统100的初始位姿以及构建的三维地图来确定SLAM系统100的最终位姿。在本发明的示例性实施例中，位姿确定单元140可基于所述初始位姿以及三维地图，使用预设帧集合来确定SLAM系统100的最终位姿。所确定的最终位姿是在世界坐标系下的位姿，并且所确定的最终位姿可被存储在存储单元150中。

在本发明的示例性实施例中，可使用全局光束平差方法来确定所述最终位姿。具体地讲，位姿确定单元140可基于下面的公式(8)和(9)来进行全局光束平差以确定SLAM系统100的最终位姿：

ψ^*＝argmin∑_i∈K∑_j∈P||e^ij|| …… (8)

e^ij＝x^ij-π(ε^iw,X^wj) …… (9)

在上述公式(8)和(9)中，K是预设帧集合并且包括当前帧，例如，可以是包括关键帧和当前帧的集合。P表示特征提取模块121提取的特征点的集合。e^ij表示所述特征点的集合P中的第j个特征点在所述集合K中的第i个帧上的重投影误差，并且当不存在所述第j个特征点在所述第i个帧上的投影时，可将e^ij设置为0。x^ij为所述第j个特征点在所述第i个帧上的投影点的二维坐标。ε^iw表示针对所述第i个帧，SLAM系统100在世界坐标系下的位姿。X^wj表示所述第j个特征点在世界坐标系下的三维坐标，并且可基于地图构建单元130所提供的三维地图来确定。

基于公式(8)求最优解，可获得SLAM系统100的与集合K中的各帧相应的位姿(即，SLAM系统100在获得所述各帧时的位姿)。此时，不仅可确定SLAM系统100的最终位姿，还可用所获得的SLAM系统100的针对其他帧的位姿来更新先前确定的对应位姿，由此能够不断提高SLAM系统100的定位精确度。

此外，在对上述公式(8)求最优解的过程中，初始位姿估计单元120所确定的初始位姿可作为ε^iw的参考(例如，ε^iw的取值可初始设置为所述初始位姿，并且在之后的计算中基于该初始位姿进行调整)，以便加快运算速度。

在本发明的示例性实施例中，关键帧可以是存储单元150中存储的多个帧之中的按照预设规则选择的帧，例如，例如，按照预设间隔选择的第1帧、第5帧、第9帧、……，或者甚至可以是存储的全部帧。

在上面基于公式(8)和(9)进行全局光束平差以确定SLAM系统100的最终位姿时，例如，对于当前帧的第j个特征点，可将所有以第j个特征点的对应地图特征点为共视特征点的共视帧(即，在这些共视帧上都能够看到第j个特征点的对应地图特征点)作为与该第j个特征点相关的一个特征跟踪集合(即，所述预设帧集合)来整体参与最终位姿的计算(例如，在本发明中，可使用前述全图跟踪模块(未示出)来找出这样的共视帧)，由此保持全局一致性。然而，这样耗时较长，无法满足SLAM系统100的实时性，甚至有时不得不牺牲SLAM系统100的精度来保证实时性。

可选择地，当进行全局光束平差时，位姿确定单元140可设置所述预设帧集合包括对与当前帧相关的特征跟踪集合进行划分所获得的多个跟踪子集。更具体地，位姿确定单元140可将与当前帧的各特征相关的各特征跟踪集合分别划分为一个或更多个特征跟踪子集，并通过基于各个特征跟踪子集进行全局光束平差来确定SLAM系统100的最终位姿。也就是说，所述预设帧集合可包括对与当前帧的各特征相关的各特征跟踪集合分别划分所获得的一个或更多个特征跟踪子集。下面参照图9对此进行描述。

图9示出特征跟踪集合的划分示例示意图。

参照图9，仅作为示例，假设C₁至C₄为与当前帧的第j个特征点相关的共视帧，K为与当前帧的第j个特征点相关的共视关键帧。也就是说，在共视帧C₁至C₄以及共视关键帧K中均可看到所述第j个特征点的对应地图特征点，T_j为所述第j个特征点的特征跟踪集合，并由共视关键帧K以及共视帧C₁至C₄构成。

在本发明的示例性实施例中，参照图9，位姿估计单元140可将T_j划分为子集T_j1、T_j2、T_j3和T_j4，其中，T_j1由共视关键帧K以及共视帧C₁至C₃构成，T_j2由共视关键帧K以及共视帧C₂至C₄构成，T_j3由共视关键帧K以及共视帧C₁、C₃和C₄构成，并且T_j4由共视关键帧K以及共视帧C₁、C₂和C₄构成。

这样，当使用如公式(8)的全局光束平差方法来计算SLAM系统100的最终位姿时，公式(8)中与第j个特征点相关的子项中所涉及的帧集合并非是与T_j，而是子集T_j1、T_j2、T_j3和T_j4，通过基于这4个更短的子集来计算所述相应的子项，可有效降低计算负担。

此外，在本发明的示例性实施例中，可基于与每个特征相应的特征跟踪集合中的帧的数量来确实是否对该特征跟踪集合进行划分，例如，可仅在所述特征跟踪集合中的帧的数量超过预设阈值时将该特征跟踪集合划分为预设大小的若干个特征跟踪子集，使得每个特征跟踪子集中的帧的数量少于或等于所述预设阈值并且每个特征跟踪子集均包括共视关键帧，并且所有特征跟踪子集中的帧涵盖了特征跟踪集合中的所有帧。

此外，在SLAM系统100的应用中，对于较长的运动轨迹，会累积相当数量的关键帧在后端进行优化，因此容易导致SLAM系统100无法高效的工作。因此，在本发明的另一示例性实施例中，位姿确定单元140可针对当前帧设置预定大小的时域窗口，并使用处于时域窗口内的帧(例如，处于时域窗口内的关键帧，可包括共视关键帧，也可包括非共视关键帧)来确定SLAM系统100的最终位姿。也就是说，前述预设帧集合也可包括通过对针对当前帧设置预定大小的时域窗口所确定的处于所述时域窗口内的关键帧。图10示出基于时域窗口的全局光束平差示意图。如图10中所示，针对当前帧的时域窗口中即可包括共视关键帧，也可包括非共视关键帧。

可选择地，位姿确定单元140还可针对当前帧设置预定大小的时域窗口，并使用所述时域窗口对前述处理中产生的特征跟踪子集之中的帧进行筛选，去除处于时域窗口以外的帧，然后基于经过该筛选处理的特征跟踪子集来进行全局光束平差，从而确定SLAM系统100的最终位姿，由此能够进一步降低运算负担，提高图像处理效率。

然而，当SLAM系统100的运动轨迹存在回环时，仅使用时域窗口内的关键帧而不是全部关键帧进行处理可能会导致回环无法闭合，如图11的(a)所示。在本发明的示例性实施例中，可利用闭环检测使回环闭合，而且由此产生的位姿差值可将轨迹依次传递回去，保证运动轨迹平滑，其过程如图11的(a)至图11(d)所示。图11所示的闭环误差消除处理对本领域技术人员来说是已知的，因此为了简明，在此将不再进行更加详细的描述。

此外，以上以点特征为示例描述了位姿确定单元140确定SLAM系统100的最终位姿的详细处理，然而，当初始位姿估计单元120还提取了线段特征时，还可进一步考虑线段特征来进行全局光束平差。

具体来说，根据本发明的示例性实施例的SLAM系统还可包括点线优化组合模块(未示出)，该点线优化组合模块(未示出)可基于特征匹配模块132的线段特征的匹配结果来建立三维共线约束，使得位姿确定单元140还可根据所述三维共线约束来确定最终位姿。仅作为示例，还可在前述全局光束平差方法中添加并使用与所述三维共线约束对应的线段约束子项。这里，三维共线约束表示第一帧的线段上的点在第二帧的对应线段上。更具体地讲，根据线段描述符矢量获取两帧之间的线段匹配，对任一线段匹配，两条匹配的线段在相机坐标系下的三维坐标均可得，则可知，其中一条线段的端点(空间三维坐标)经过帧帧位姿变换到另一系相机坐标系下时，一定在此线段所匹配的线段之上

换句话说，在如公式(8)计算SLAM系统100的最终位姿时，公式右侧的子项所涉及的重投影误差不仅包括点特征的重投影误差，还包括线段特征的重投影误差。这里，计算线段特征的重投影误差与公式(9)中示出的计算点特征的重投影误差的方式类似并且对于本领域技术人员来说是已知的，因此在这里不再赘述。

此外，以上以全局光束平差方法进行了描述，但本发明不限于此，还可使用其他能够确定装置位姿的方法。

图12是示出根据本发明的示例性实施例的基于地图重建的SLAM方法的流程图。

参照图12，在所述方法的步骤1210，可使用图像获取单元110来获取SLAM系统100的周围环境的彩色图像及深度图像。

然后，在步骤1220，可使用初始位姿估计单元120基于步骤1210获得的彩色图像和深度图像，估计SLAM系统100的初始位姿。

更具体地，在步骤1220，初始位姿估计单元120可在所述彩色图像中提取点特征，根据所述点特征进行点匹配，然后使用匹配的点特征来估计SLAM系统100的初始位姿。

当匹配的点特征的数量少于第一预设阈值时，初始位姿估计单元120还可在所述彩色图像中提取线段特征，并根据所述线段特征进行线段匹配。此时，初始位姿估计单元120可使用匹配的点特征和线段特征来估计的SLAM系统100的初始位姿。

此外，当匹配的点特征的数量少于第二预设阈值(这里，第二预设阈值小于第一预设阈值)时，初始位姿估计单元120可将SLAM系统100的初始位姿直接确定为与所述特定帧中SLAM系统100的相对位姿保持相同。

这里，所述特定帧可以是当前帧的前一帧或附近的若干帧。

可选择地，尽管图12的流程图中没有示出，但所述方法还可包括确定共视关键帧的操作。在本发明的示例性实施例中，可使用前面描述的全图跟踪单元(未示出)在先前帧(例如，先前关键帧)中确定与当前帧具有共视点的共视关键帧。由此，步骤1220中初始位姿估计单元120还可进一步使用这些共视关键帧来估计初始位姿，从而能够增强SLAM系统100的准确性。

然后，在步骤1230，可使用地图构建单元130来基于所述深度图像和彩色图像，构建三维地图。

地图构建单元130可在首次进入特定环境时基于所述特定环境的第一深度图像和第一彩色图像来重建初始的三维地图，之后在再次进入所述特定环境时基于所述特定环境的第二深度图像和第二彩色图像来更新所述三维地图。

更具体地讲，当SLAM系统100首次进入特定环境时，地图构建单元130可基于首次进入所述特定环境时由图像获取单元110所获取的深度图像和彩色图像来重建三维地图，确定重建的三维地图上的每个地图点的三维坐标及其空间准确度，并存储带有空间准确度的三维地图上的每个地图点的三维坐标及其空间时间置信度。这里，由于SLAM系统100首次进入特定环境时并未获得空间时间置信度，因此可将此时的三维地图上的地图点的空间时间置信度设置为空并进行存储。

之后，当SLAM系统100再次进入该特定环境时，地图构建单元130可基于再次进入该特定环境时由图像获取单元110所获取的深度图像和彩色图像计算每个地图点的三维坐标及其空间时间置信度，根据计算出的每个地图点的三维坐标及其空间时间置信度来更新所存储的三维地图上的对应地图点的三维坐标及其空间时间置信度。更新后的三维地图可作为地图构建单元130构建的三维地图进行后续使用。

在步骤1240，可使用位姿确定单元140来基于步骤1220估计出的SLAM系统100的初始位姿以及在步骤1230构建的三维地图，确定SLAM系统100的最终位姿。

这里，如前所述，位姿确定单元140可基于所述初始位姿以及三维地图，使用预设帧集合来确定最终位姿。所述预设帧集合可包括对与当前帧相关的跟踪集合进行划分所获得的多个跟踪子集。可选择地，位姿确定单元140也可针对当前帧设置预定大小的时域窗口，并基于初始位姿以及三维地图，利用处于所述时域窗口内的关键帧来确定最终位姿。

优选地，当在步骤1220执行了线段特征的提取和匹配之后，所述方法还可使用点线优化组合单元(未示出)来基于线段特征的匹配结果建立三维共线约束，使得位姿确定单元140可进行一步利用所述三维共线约束来确定所述最终位姿。由此可进一步提高SLAM系统100的准确性

最后，在步骤S1250，可由存储单元150存储包括SLAM系统100的最终位姿的当前帧相关信息。

以上已结合图1至图11详细描述了图12的各个步骤中由根据本发明的示例性实施例的SLAM系统100的各个组成部件所执行的操作，因此为了简明，在此将不再进行重复描述。

本发明的示例性实施例的SLAM系统100及其方法能够确保建图的实时性，降低计算量并还保证地图准确性。此外，应该理解，本说明书中记载的示例性实施例仅是为了便于理解而示出的示例，本申请不限于此，在不脱离本发明的构思的精神和范围的情况下，本发明的任何等同形式、替换形式以及修改可被视为落入本发明的范围之内。

本发明示例性实施例也可实现为计算机可读记录介质上的计算机可读代码。计算机可读记录介质是可存储其后可由计算机系统读出的数据的任意数据存储装置。计算机可读记录介质的示例包括：只读存储器(ROM)、随机存取存储器(RAM)、CD-ROM、磁带、软盘、光数据存储装置和载波(诸如经有线或无线传输路径通过互联网的数据传输)。计算机可读记录介质也可分布于连接网络的计算机系统，从而计算机可读代码以分布式存储和执行。此外，完成本发明的功能程序、代码和代码段可容易地被与本发明相关的领域的普通程序员在本发明的范围之内解释。

尽管已经参照其示例性实施例具体显示和描述了本发明，但是本领域的技术人员应该理解，在不脱离权利要求所限定的本发明的精神和范围的情况下，可以对其进行形式和细节上的各种改变。