CN108090958A - 一种机器人同步定位和地图构建方法和*** - Google Patents

Abstract

本发明公开了一种基于线段特征和线段消隐点的机器人同步定位和地图构建方法，应用于机器人在未知环境中的定位和地图构建。该方法包括从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；基于交叉点进行闭环检测和全局的地图优化。本发明还公开了一种同步定位和地图构建***。本发明可以用于机器人在未知环境中的定位和地图构建，特别适用于结构化或者半结构化的场景中，例如室内环境、室外有建筑的环境等。

Description

一种机器人同步定位和地图构建方法和***

技术领域

本发明涉及智能机器人的导航技术领域，更具体的涉及同步定位与地图构建(simultaneous localization and mapping,SLAM)技术，涉及一种基于线段特征和线段消隐点建立地图，并根据所建立的地图进行定位的方法和***。

背景技术

在各类自主移动机器人***中，同步定位和地图构建(SLAM)***起着重要的作用。在不需要对环境进行改造的方式中，目前的SLAM主要有基于激光雷达的方案和基于视觉传感器的方案。所述的视觉传感器通常包括单目摄像头、双目摄像头、多个摄像头的组合等。

基于激光雷达的SLAM方案相对比较成熟，其具有精度高、容易实现、不易受光照等外界光源干扰等优点。但是，性能好的激光雷达的成本较高；而低成本的激光雷达通常使用电机进行360度扫描。激光雷达的SLAM方案需要电机按照一定速率旋转，一般使用皮带及滑环的方式，这种低成本的激光雷达具有能耗高、噪声大、使用寿命有限等问题。近年来，随着计算机视觉技术的日渐成熟，开始有机器人产品使用基于视觉传感器的SLAM方案。基于视觉传感器的SLAM方案具有价格低、功耗低等优点，但是算法复杂，实现难度较大。

在机器人的SLAM***中，地图的表示方式起着非常重要的作用。基于激光雷达的SLAM方案通常使用栅格地图；基于视觉传感器的SLAM方案通常使用基于点特征的特征地图。

除了点特征以外，线段特征也是一类比较重要的特征，但是在SLAM领域，关于线段特征的研究比较少。然而，在室内环境或者室外有建筑物的环境中，存在丰富的线段特征，如果能够充分利用这些线段特征构建地图，那么地图将拥有更强的几何表达能力，从而更能表现环境的几何结构。此外，线段特征对光照变化比较鲁棒，并且可以建立半稠密的环境地图，还可以方便地提取平面结构。因此，本申请的发明人认为：在线段特征丰富或者点特征比较稀少的环境下，基于线段特征的SLAM方法具有优势。

发明内容

本申请针对基于视觉传感器的同步定位和地图构建，提出一种基于线段特征和消隐点的同步定位和地图构建方法和***，以实现在室内环境和室外有建筑环境下的定位和地图构建。

本申请公开了一种同步定位和地图构建SLAM方法，应用于机器人在未知环境中的定位和地图构建，包括：

A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；

B、在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

C、基于交叉点进行闭环检测和全局的地图优化。

较佳的，所述A中从图像中提取线段特征、交叉点和消隐点包括：

A1、采集当前图像，从当前图像中提取线段特征；

A2、提取线段的交叉点；

A3、提取交叉点的描述子；

A4、根据描述子建立交叉点的初始匹配，利用对极几何和非刚性匹配消除错误的匹配；

A5、在线段特征的基础上提取消隐点。

较佳的，所述A2包括：

如果两条线段满足以下三个条件，则判定所述两条线段构成一个候选的交叉点：

条件1：两条线段的长度均大于一设定阈值；

条件2：交叉点到两条线段的距离均小于一设定阈值；

条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。

较佳的，所述A3包括：

定义交叉点的方向和尺度，其中，将交叉点的方向定义为构成该交叉点的两条线段的中分线，并采用至少两个尺度的策略；

将交叉点视为特征点，在交叉点周围提取描述子。

较佳的，所述A4中根据描述子建立交叉点的初始匹配包括：

对于双目图像和非双目图像采用不同的策略进行匹配，其中：

对于双目摄像头获取的双目图像，根据所述双目图像中的一幅图像中给定的交叉点，在另一幅图像的极线附近寻找匹配的交叉点；如果在极线附近存在匹配的交叉点，则两个交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则判定所述两个交叉点构成一对匹配；

对于非双目图像，根据所述非双目图像中的一幅图像中给定的交叉点，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点，并计算最小距离和次小距离的比值，如果所述比值小于设定的阈值，则判定所述一幅图像中给定的交叉点与所述另一幅图像中距离最小的交叉点构成一对匹配。

较佳的，所述B包括：

在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：

线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；

消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；

如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点。

较佳的，所述误差函数为：

其中，ρ(·)是鲁棒的柯西函数；

e_l是一个四维向量，表示线段的重投影误差；

e_v是一个三维向量，表示消隐点的重投影误差；

和分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵；

i是图像的索引；

k是线段的索引；

j是属于同一组平行线的所有直线的索引。

较佳的，所述C包括：

C1、根据当前图像的交叉点描述子，构建所述描述子的词袋特征，并利用当前图像的词袋特征，从关键帧中挑选相似度大于设定阈值的帧作为候选关键帧，然后将当前图像与候选关键帧之间进行几何一致性验证；如果一候选关键帧通过了几何一致性验证，则认为检测到闭环；

C2、当检测到闭环后，对关键帧建立消除线段的重投影误差和消隐点的重投影误差的误差函数，进行全局地图的优化。

本申请还公开了一种同步定位和地图构建SLAM***，应用于机器人在未知环境中的定位和地图构建，包括：

运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；

局部地图更新模块，用于在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。

本发明提供的基于线段特征和消隐点的同步定位和地图构建方法和***，可以用于机器人在未知环境中的定位和地图构建，特别适用于结构化或者半结构化的场景中，例如室内环境、室外有建筑的环境等。

附图说明

图1为本发明一较佳实施例中SLAM***的示意图；

图2为本发明一较佳实施例中线段交叉点提取规则的示意图；

图3为本发明一较佳实施例中交叉点描述子提取方法的示意图；

图4为本发明一较佳实施例中线段提取与匹配流程的示意图；

图5为本发明一较佳实施例中线段三维重建方法的示意图；

图6为本发明一较佳实施例中线段重投影误差的示意图；

图7为本发明一较佳实施例中消隐点重投影误差的示意图；

图8为本发明一较佳实施例中SLAM实验结果的示意图。

具体实施方式

为使本申请的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本申请作进一步详细说明。

本发明提出一种基于线段特征和线段消隐点的同步定位和地图构建方法。本发明提及的同步定位和地图构建(simultaneous localization and mapping)，简称SLAM，是指：机器人从未知环境的未知地点出发，在运动过程中通过不同视角下多次观测到的地图特征(比如，点特征、线段特征等)对自身进行定位，再根据自身位置增量式地构建地图，从而达到同步定位和地图构建的目的。本发明中所述的消隐点(vanishing point)，也称作消失点、灭点，指的是一组平行线在图像平面上的交汇点。

本发明主要用于自主移动机器人在室内环境以及室外有建筑物的环境中进行定位和地图构建，使用的传感器包括视觉传感器，可选地，可以使用辅助定位传感器。在本发明较佳实施例中，采用的视觉传感器为双目摄像头。需要说明的是，本发明同样适用于其他的视觉传感器，如多目摄像头。本发明也可以增加辅助定位传感器，包括码盘和惯性测量单元(inertial measurement unit,IMU)。

本发明提出的同步定位和地图构建方法，应用于机器人在未知环境中的定位和地图构建，主要包括以下步骤：

A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；

B、在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

C、基于交叉点进行闭环检测和全局的地图优化。

本发明方法可以用软件实现，该软件***的模块划分如图1所示，主要包括三个部分：运动估计模块、局部地图更新模块、闭环优化模块，其中：

运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；

局部地图更新模块，用于在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。

作为一个实施例，本发明将这三个部分各用一个线程来实现。其中：

运动估计部分主要包括：(1)提取图像的线段特征、交叉点和消隐点；(2)根据交叉点建立线段的匹配；(3)进行位姿估计，选取新关键帧。

局部地图更新部分包括：在地图中***关键帧、计算新出现线段的三维坐标、局部集束调整、剔除野点观测。局部集束调整的目标是最小化一个误差函数，使得相机位姿的估计更加准确。该误差函数包含两类误差：线段的重投影误差(reprojection error)和消隐点的重投影误差。

闭环优化部分包括：基于交叉点的闭环检测、全局相机位姿优化。

接下来分别介绍这三个部分。

一、运动估计部分

1.1提取线段特征、交叉点和消隐点的方法

线段的特征提取可以采用现有的线段特征提取方法，如线段特征提取器(LSD,line segment detector)方法、边缘画线(EDLine,Edge Drawing Line)方法、道格拉斯-普克(Douglas-Peucker)算法等。作为一个实施例，本发明采用道格拉斯-普克算法提取线段特征。

得到线段特征之后，我们提取候选的线段交叉点(junction)。本发明定义线段交叉点如下：

如果两条线段满足以下三个条件，则判定这两条线段可以构成一个候选的线段交叉点(junction)：

条件1：两条线段的长度均大于一设定阈值。

条件2：交叉点到两条线段的距离均小于一设定阈值。

条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。值得一提的是，两条线段可以不直接相交，只要这两条线段的延长线相交即可。

图2为上述三个条件的图示。其中，图2中的(a)满足以上三个条件，因而构成一个交叉点；图2中的(b)不满足条件1，不构成交叉点；图2中的(c)不满足条件2，不构成交叉点；图2中的(d)不满足条件3，不构成交叉点。

消隐点的提取可以采用已有的算法，本发明并不依赖于某个特定的消隐点提取算法。作为一个实施例，本发明采用基于随机采样一致性(RANdom SAmple Consensus,RANSAC)算法的消隐点提取方法。

1.2根据交叉点建立线段的匹配

这部分包括：提取交叉点的描述子、进行交叉点的初始匹配、消除交叉点的错误匹配、根据交叉点的匹配建立线段匹配。接下来详细介绍这些过程。

线段交叉点(junction)的描述子提取方法如图3所示。图3的中部示出两条共面的线段，这两条共面的线段被两个不同视角的摄像头分别观测到，如图像1和图像2所示，这两条共面的线段形成了交叉点。图中，x_i和x′_i分别是图像1和图像2中交叉点的图像坐标，θ_i和是图像1中构成交叉点的两条线段的角度，θ′_i和是图像2中构成交叉点的两条线段的角度。

首先，定义交叉点的方向和尺度。交叉点的方向为构成它的两条线段的中分线，即：图像1中交叉点的方向为图像2中交叉点的方向为交叉点的尺度采用多尺度的策略，在交叉点定义多个尺度，本实施例采用3个尺度。例如：图像1中交叉点x_i周围的虚线矩形框为一个尺度的举例。

其次，在交叉点周围提取描述子。给定交叉点的位置、方向和尺度，可以将交叉点当作常规的特征点，因此可以采用特征点描述子提取方法在交叉点周围提取描述子。作为一个较佳实施例，本发明采用带有旋转的BRIEF(Binary Robust Independent ElementaryFeatures，二值的鲁棒独立基本特征)描述子。

提取了交叉点的描述子之后，就可以建立描述子之间的初始匹配。本实施例采用汉明距离作为BRIEF描述子之间的距离度量。对于双目图像和非双目图像采用不同的策略进行匹配：

(1)对于双目摄像头获取的双目图像，图像通过校正之后，所有的极线都是平行于图像水平方向的。给定左目图像中的某个交叉点，我们在右目图像的极线附近寻找匹配的交叉点；如果在极线附近存在交叉点，则计算左目图像与右目图像的交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则认为左目图像的交叉点与右目图像的交叉点构成一对匹配。

(2)对于非双目图像，两幅图像之间的相对位姿未知，因此极线也是未知的。给定一幅图像中的交叉点，首先，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点；其次，计算最小距离和次小距离的比值；最后，如果该比值小于设定的阈值，则认为左目图像的交叉点和右图距离最小的交叉点构成一对匹配。

由于描述子只表达了交叉点周围的局部信息，因此交叉点的匹配不可避免地含有错误匹配(亦称作野点、离群点)，需要进一步消除交叉点的错误匹配。本实施例采用随机采样一致性方法来消除野值点。如果一对匹配的交叉点是三维空间中的两条共面直线投影得到的，那么它们满足对极几何。将对极几何中的五点法结合到随机采样一致性算法中，可以消除错误匹配。优选地，还可以结合非刚性匹配做进一步的筛选，进一步消除错误匹配。本实施例采用向量场一致性算法作为非刚性匹配算法。

当找到两幅图像中的交叉点匹配后，根据构成交叉点的两条线段的逆时针角度介于0和180度之间这一准则，可以方便地建立两条线段的匹配。具体地，假设第一幅图像中线段1到线段2的角度介于0和180度之间，第二幅图像中的线段3到线段4的角度介于0和180度之间，那么线段1和线段3构成一个匹配、线段2和线段4构成一个匹配，从而可以建立两条线段的匹配。线段匹配的整体流程如图4。

1.3进行位姿估计，选取新关键帧

对于每一幅新获取的双目图像，通过上述的线段提取和匹配方法可以得到线段之间的匹配，通过消隐点检测算法可以检测左目和右目图像中的所有消隐点。所有检测到的线段和消隐点将作为观测量，用于局部地图的更新。

根据计算机视觉的理论，只要在两个不同时刻的双目图像中寻找到3条匹配线段，就可以根据三焦张量进行运动估计。将该求解方法和随机采样一致性(RANSAC)框架结合起来，可以鲁棒地进行位姿估计。

在视觉SLAM领域，当摄像机的位移或旋转超过阈值时，通常把当前图像的主要信息保存下来，作为关键帧。本发明也采用了提取关键帧的策略。当SLAM算法初始化成功后，把当前图像作为关键帧。在此之后，如果当前图像与最后时刻的关键帧之间的相对旋转角度大于设定的阈值、或者相对平移的距离大于设定的距离，那么把当前图像选作关键帧。

二、局部地图更新

这部分包括：在地图中***关键帧、计算新出现线段的三维坐标、局部集束调整、剔除野点观测。

在运动估计部分，会判断当前图像是否需要加入到关键帧。随着关键帧的加入，***维护一个位姿图。其中，位姿图的节点代表关键帧，位姿图的边代表关键帧之间的相对位姿。当一个新的关键帧***到位姿图中时，首先计算该关键帧中线段的三维坐标，如图5。图中，双目图像的线段构成了一个匹配。

首先，计算关键帧中新出现的线段的三维坐标，计算过程如下：

(1)根据双目图像之间的极线是水平的这一准则，可以找到两条线段之间的对应端点。如图5所示，左目图像中的端点(u₁,v₁)对应右目图像中的端点(u′₁,v′₁)，左目图像中的端点(u₂,v₂)对应右目图像中的端点(u′₂,v′₂)。

(2)根据两个对应的端点、相机的内参数和双目相机的基线距离b，计算两个对应端点的三维坐标。此处相机的内参数包括相机的主点(principle point)和焦距。

(3)根据两个对应端点的三维坐标计算线段端点的三维坐标。将端点(u₁,v₁)对应的三维坐标记做P₁，端点(u₂,v₂)对应的三维坐标记做P₂，则端点三维坐标的计算公式为：

其次，将当前的关键帧以及最新的N个关键帧作为活跃的关键帧，使用集束调整来优化相机姿态以及活跃关键帧中出现的线段特征的三维坐标。

本发明使用基于线段特征的集束调整来优化相机的姿态和线段的三维坐标。优化的过程利用三维线段在图像平面上的两种观测：图像平面上的线段、平行线的消隐点。具体地，定义如下的代价函数：

该代价函数同时考虑了线段和消隐点的观测。

其中，ρ(·)是鲁棒的柯西函数；

e_l是一个四维向量，表示线段的重投影误差，具体的定义在下文阐述；

e_v是一个三维向量，表示消隐点的重投影误差，具体的定义在下文阐述；

和分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵(协方差矩阵的逆)；本实施例将这两个信息矩阵设为单位矩阵；

i是图像的索引；

在求和的第一项中，k是线段的索引；

在求和的第二项中，j是属于同一组平行线的所有直线的索引。

接下来定义线段和消隐点的重投影误差。

对于线段特征，重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和，如图6所示。投影线段是将线段的三维坐标按照相机位姿投影到图像平面得到。图中，d₁和d₂分别是线段投影的两个端点到检测线段的距离。本实施例采用双目相机，对于左目和右目图像可以分别求取线段的重投影误差，因此线段的重投影误差是三维向量。

对于消隐点特征，我们把消隐点映射到单位球面上。消隐点的重投影误差定义为投影过来的消隐点坐标和实际检测的消隐点坐标之间的距离，如图7所示。图中L₁和L₂是一组平行线，它们在图像平面I上的投影线段分别为l₁和l₂，消隐点为v。如果投影线段l₂有噪声，变成了l′₂，那么消隐点变成了v_n。消隐点的重投影误差定义为v和v_n在图像平面上的坐标差异。本实施例中，我们可以把v和v_n进一步投影到单位球面上，图中v在单位球上的投影点是v_s，然后用v和v_n在球面上的坐标差异作为重投影误差。

最后，当姿态修正完毕，剔除不好的线段匹配和消隐点。具体地，如果线段的重投影误差超过阈值，则剔除该线段特征。类似地，如果消隐点的观测位置和投影位置超过阈值，则剔除该消隐点。

三、闭环优化

闭环优化部分包括：基于交叉点进行闭环检测，进行全局相机位姿优化。

我们将前文所述的交叉点作为特征点，利用交叉点的描述子进行闭环检测。本发明采用词袋(bag-of-words)模型，用一个离线聚类得到的词汇树(vocabulary tree)模型来对特征点进行组织。闭环检测部分将当前的图像与之前的t个关键帧进行比较。当前图像作为查询图像，关键帧集合中的各关键帧与查询图像进行比较，排除掉相似度小于指定阈值的图像。若构成一个候选的闭环，需要连续3帧得到的闭环候选是相同的。得到这些候选的闭环后，进一步进行几何验证。几何验证采用五点法和随机采样一致性(RANSAC)框架。如果随机采样一致性算法保留的正确匹配数量大于指定的阈值，认为检测到了一个闭环。

当检测到闭环之后，进行全局地图的优化。全局地图优化时，误差函数的定义与局部地图更新中定义的误差函数基本相同，区别在于将闭环检测所涉及关键帧的线段参与到优化中。

本发明的典型实验结果如图8所示。

以上所述仅为本申请的较佳实施例而已，并不用以限制本申请，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。

Claims (9)

1.一种同步定位和地图构建SLAM方法，应用于机器人在未知环境中的定位和地图构建，其特征在于，包括：

A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；

B、在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

C、基于交叉点进行闭环检测和全局的地图优化。

2.根据权利要求1所述的方法，其特征在于，所述A中从图像中提取线段特征、交叉点和消隐点包括：

A1、采集当前图像，从当前图像中提取线段特征；

A2、提取线段的交叉点；

A3、提取交叉点的描述子；

A4、根据描述子建立交叉点的初始匹配，利用对极几何和非刚性匹配消除错误的匹配；

A5、在线段特征的基础上提取消隐点。

3.根据权利要求2所述的方法，其特征在于，所述A2包括：

如果两条线段满足以下三个条件，则判定所述两条线段构成一个候选的交叉点：

条件1：两条线段的长度均大于一设定阈值；

条件2：交叉点到两条线段的距离均小于一设定阈值；

条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。

4.根据权利要求2所述的方法，其特征在于，所述A3包括：

定义交叉点的方向和尺度，其中，将交叉点的方向定义为构成该交叉点的两条线段的中分线，并采用至少两个尺度的策略；

将交叉点视为特征点，在交叉点周围提取描述子。

5.根据权利要求2所述的方法，其特征在于，所述A4中根据描述子建立交叉点的初始匹配包括：

对于双目图像和非双目图像采用不同的策略进行匹配，其中：

对于双目摄像头获取的双目图像，根据所述双目图像中的一幅图像中给定的交叉点，在另一幅图像的极线附近寻找匹配的交叉点；如果在极线附近存在匹配的交叉点，则两个交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则判定所述两个交叉点构成一对匹配；

对于非双目图像，根据所述非双目图像中的一幅图像中给定的交叉点，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点，并计算最小距离和次小距离的比值，如果所述比值小于设定的阈值，则判定所述一幅图像中给定的交叉点与所述另一幅图像中距离最小的交叉点构成一对匹配。

6.根据权利要求1所述的方法，其特征在于，所述B包括：

在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：

线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；

消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；

如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点。

7.根据权利要求6所述的方法，其特征在于，所述误差函数为：

<mrow> <mi>C</mi> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </munder> <mi>&amp;rho;</mi> <mrow> <mo>(</mo> <msubsup> <mi>e</mi> <mi>l</mi> <mi>T</mi> </msubsup> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>)</mo> <msubsup> <mi>&amp;Sigma;</mi> <mi>l</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>e</mi> <mi>l</mi> </msub> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>+</mo> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </munder> <mi>&amp;rho;</mi> <mrow> <mo>(</mo> <msubsup> <mi>e</mi> <mi>v</mi> <mi>T</mi> </msubsup> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>)</mo> <msubsup> <mi>&amp;Sigma;</mi> <mi>v</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>e</mi> <mi>v</mi> </msub> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>)</mo> <mo>)</mo> </mrow> </mrow>

其中，ρ(·)是鲁棒的柯西函数；

e_l是一个四维向量，表示线段的重投影误差；

e_v是一个三维向量，表示消隐点的重投影误差；

和分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵；

i是图像的索引；

k是线段的索引；

j是属于同一组平行线的所有直线的索引。

8.根据权利要求1所述的方法，其特征在于，所述C包括：

C1、根据当前图像的交叉点描述子，构建所述描述子的词袋特征，并利用当前图像的词袋特征，从关键帧中挑选相似度大于设定阈值的帧作为候选关键帧，然后将当前图像与候选关键帧之间进行几何一致性验证；如果一候选关键帧通过了几何一致性验证，则认为检测到闭环；

C2、当检测到闭环后，对关键帧建立消除线段的重投影误差和消隐点的重投影误差的误差函数，进行全局地图的优化。

9.一种同步定位和地图构建SLAM***，应用于机器人在未知环境中的定位和地图构建，其特征在于，包括：

运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；

局部地图更新模块，用于在地图中***关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。