CN114862949A - 一种基于点线面特征的结构化场景视觉slam方法 - Google Patents

Abstract

本发明公开的一种基于点线面特征的结构化场景视觉SLAM方法，首先输入彩色图像及其对应的深度图像，在图像中提取点线面特征并进行特征匹配；然后根据平面法向量检测曼哈顿世界坐标系，若存在曼哈顿世界坐标系且在曼哈顿世界地图中出现则求出相机姿态并跟踪点线面特征估计位移，否则跟踪点线面特征估计位姿；然后对当前帧进行关键帧判断，若为关键帧则插入到局部地图；接着维护地图信息并对当前关键帧、相邻关键帧和三维特征进行联合优化；最后进行回环检测，若检测到闭环帧则闭合回环并进行全局优化。本发明是一种高精度、鲁棒性强的视觉SLAM方法，解决了低纹理的结构化场景下仅基于点特征的视觉SLAM精度下降甚至系统失效的问题。

Description

一种基于点线面特征的结构化场景视觉SLAM方法

技术领域

本发明属于机器人同时定位与地图构建技术领域，具体设计一种基于点线面特征的结构化场景视觉SLAM方法。

背景技术

近年来智能车辆定位系统在城市交通中的应用越来越广泛，传统定位方法如基于GNSS技术的室外定位系统目前已经非常成熟。然而，在GNSS信号受到遮蔽的室内环境，高性能的实时定位仍然是一个难点。虽然基于无线信号的室内定位系统得到了长足发展，如基于蓝牙、WiFi、UWB以及RFID等，但由于其设备部署成本高昂，而且在室内容易受到遮挡以及多路径效应的干扰，难以有效用于室内定位与建图。

针对上述问题，同时定位与建图方法(SLAM)被提出。SLAM是一种在未知环境中，进行自身定位并同时建立地图的技术。主要应用在移动机器人、自动驾驶、虚拟现实、增强现实等方面。针对室内环境已经有成熟的基于激光的SLAM系统如cartographer、hector map、gmapping等系统提出。但由于激光雷达造价仍然较为昂贵，因此人们普遍期望采用价格低廉的视觉传感器，比如单目相机，双目相机，深度相机等，系统将图像作为输入，输出相机轨迹以及重建的三维地图，如DSO利用摄像机等实现高精度的室内定位。然而，现有的基于视觉的SLAM系统大量依赖对图像中的低层次的点特征的提取，如目前先进的基于点特征的ORB-SLAM2(ORB-SLAM2：An Open Source SLAM System for Monocular Stereo and RGB-DCameras)，该SLAM方法仅基于点特征点实现位姿估计，虽然在纹理丰富的环境下能够实现高精度定位，但在纹理特征缺乏、光照变化比较大的人造室内环境，点特征难以提取且不稳定，该方法位姿估计精度会变差甚至会跟踪失败。在该类人造室内环境中富含墙面、地面、天花板等规则的几何结构，以线、面为代表的结构特征相比于点特征，更易获取，且不易受光照变化的影响，因此如何在纹理特征缺乏的室内几何结构化场景利用视觉信息找到适用于SLAM定位的结构特征显得尤为重要。通过利用室内几何结构元素，本发明提出了一种基于点线面特征的结构化场景视觉SLAM方法，解决在纹理缺乏、光照变化的人造结构化室内场景中位姿估计精度低、鲁棒性差的问题。

发明内容

针对现有技术存在的上述问题，本发明的目的是提供一种基于点线面特征的结构化场景视觉SLAM方法，以解决目前的视觉SLAM技术在纹理缺乏、光照变化的人造结构化室内场景中位姿估计精度低、鲁棒性差的问题。

本发明至少通过如下技术方案之一实现。

一种基于点线面特征的结构化场景视觉SLAM方法，包括以下步骤：

S1、输入彩色图像，根据彩色图像提取点特征和线特征并进行特征匹配；

S2、输入深度图像，转换成点云序列结构，然后提取图像平面，接着对提取的图像平面与地图面进行匹配；

S3、检测图像中是否存在曼哈顿世界坐标系，若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系，则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移，否则跟踪点特征、线特征和平面特征去优化相机位姿；

S4、判断当前帧是否为关键帧，若为关键帧，则加入到局部地图中，并更新地图中的三维点、三维线和曼哈顿世界地图，对当前关键帧和相邻的关键帧进行联合优化，优化相机的位姿和三维点和三维线特征，并剔除部分外点和冗余关键帧；

S5、对关键帧进行回环检测，若检测到回环则进行闭合回环并全局优化，减少累积误差。

进一步地，所述步骤S1具体为：输入彩色图像，首先采用ORB算法进行点特征的提取，并根据描述子进行点特征匹配，再通过PROSAC方法剔除点特征间的误匹配；之后采用EDLine算法对线特征提取，根据距离、角度信息和描述子作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配。

进一步地，所述步骤S2具体为：首先对深度有效的像素点求出其对应的三维点，形成有组织的点云序列结构，接着使用层次聚类算法使小平面块融合为更大的平面，并对融合的平面进行分割优化，最后对分割好的平面求出平面的三维中心点P_C＝[X_C Y_C Z_C]^T和平面的单位法向量n＝[n_x n_y n_z]^T，其中X_C、Y_C、Z_C为中心点P_C对应的三维坐标值，n_x、n_y、n_z为法向量n对应的值；则相机光心到平面的距离

该平面的面特征表示为π＝[n^T d]^T；接着对图像中的平面投影到世界坐标系上并根据法线夹角大小和世界坐标系原点到平面距离的差值大小对投影平面与地图面进行匹配。

进一步地，所述步骤S3具体为：

S31、对步骤S2所提取的平面的法向量进行遍历，寻找是否存在三个互相正交的法向量组合，若是只存在两个互相正交的法向量组合，其另外一维法向量通过两个相互正交的法向量叉积获得；

S32、若步骤S31所寻找的法向量组合存在，则在曼哈顿世界地图中查询历史关键帧中是否观测到与这组正交法向量一致的互相正交的平面，若存在则构成一个曼哈顿世界坐标系，取对应平面点数总和最多的法向量组合所构成的曼哈顿世界坐标系，并求出该曼哈顿世界坐标系到该帧相机坐标系的旋转矩阵为：

其中n₁、n₂、n₃表示找到的相互正交的法向量，c_i表示当前帧的id，m_k表示曼哈顿世界坐标系的id；对

作SVD分解，得到正交化后曼哈顿世界坐标系到相机坐标系的旋转矩阵

则相机坐标系到世界坐标系的旋转矩阵为：

其中c_j表示所找到的历史关键帧的id，

为第c_j帧时曼哈顿世界坐标系m_k到相机坐标系的旋转矩阵，

为第c_j帧时世界坐标系到相机坐标系的旋转矩阵；

然后跟踪点特征、线特征和平面特征去优化相机位移t，其位移的误差模型e_t为：

其中e_p、e_l、e_π分别表示点特征、线特征、平面特征的重投影误差，具体形式分别为：

e_p＝p-(KR_cwP_w+t_cw)

e_l＝l^T(KR_cwP_L+t_cw)

其中K为相机内参，R_cw为世界坐标系到相机坐标系的旋转矩阵，t_cw为世界坐标系到相机坐标系的位移，T_cw为世界坐标系到相机坐标系的位姿变换矩阵；p为该帧识别到的点特征的像素坐标，P_w为该点特征对应的三维点；l为该帧识别到的图像中的线特征，L为该线特征对应的三维线；P_L为三维线的三维端点；

为面特征参数表达形式，π＝[n_x n_x n_x d]^T，n_x、n_y、n_z为平面π的法向量对应的值，d为相机光心到平面π的距离；π_c为该帧检测到的平面，π_w为该平面对应的地图面；其中Λ_p、Λ_l、Λ_π分别表示该点特征、线特征和平面特征的信息矩阵，ρ_p、ρ_l、ρ_π分别为点特征、线特征和平面特征的Huber鲁棒核函数，Φ_p(p)、Φ_l(l)、Φ_π(π)分别表示点特征、线特征和平面特征的置信度系数，分别为：

其中n_p、n_l、n_π分别表示对应的点特征、线特征、平面被观测的次数，t_p、t_l、t_π分别为点特征、线特征和平面特征的权重系数，level_i代表该点特征所在ORB金字塔的层数；α为权重系数，α∈[0.5,1]；θ_i表示第i帧相机视线与地图线的夹角，

S33、假如不存在步骤S31所求的曼哈顿世界坐标系或曼哈顿世界地图中的历史关键帧没有观测到对应的法向量组合，则跟踪点特征、线特征和平面特征去优化相机位姿R,t，相机位姿误差模型e_R,t为：

进一步地，所述步骤S4具体为：

S41、根据点特征和线特征的匹配对数、是否检测到新的平面、特征跟踪情况以及局部地图内的关键帧情况来判断是否设该帧为关键帧，若为关键帧则加入局部地图，否则返回步骤S1；

S42、对于新插入的关键帧，更新共视图和生成树，加入新关键帧节点；根据观测一致性对从被创建开始未被连续可靠观测到的点特征、线特征和平面特征进行剔除；对新关键帧没有匹配的点特征和线特征根据深度信息反投影生成新的地图点和地图线并插入到地图中；根据图像中的平面垂直关系记录垂直平面组合与当前关键帧的关联，并更新曼哈顿世界地图；

S43、完整地图更新之后，对当前关键帧和与之关联的关键帧进行联合优化，并在优化中剔除外点以最大程度优化相机位姿；优化对象为相关关键帧的相机位姿R,t以及三维特征参数P,L，优化过程中采用的重投影误差e为：

其中e_p、e_l分别表示点特征和线特征的重投影误差，Φ_p(p)、Φ_l(l)分别表示点特征和线特征的置信度系数，Λ_p、Λ_l分别表示该点特征和线特征的信息矩阵，ρ_p、ρ_l分别为点特征和线特征的Huber鲁棒核函数；

S44、将关键帧之间特征的重合度大于90％的关键帧剔除。

进一步地，判断关键帧的条件具体为：

(1)插入关键帧之后已处理多于10帧图像，且当前局部建图线程处于空闲状态，则判断为关键帧；

(2)上一次插入关键帧之后已处理超过20帧图像，则判断为关键帧；

(3)当前图像上匹配的点特征、线特征和平面特征总和不少于20，否则不能作为关键帧；

(4)当前图像上跟踪的特征相比最近关键帧跟踪的特征少于90％，则判断为关键帧；

(5)图像中提取到新平面，则判断为关键帧。

进一步地，所述步骤S5具体为：基于点线特征词典模型，首先根据当前关键帧所有点线特征对应的描述子，在词典K叉树中找到所述的单词种类，计算每种单词对应的权重，得到当前关键帧的单词向量；计算当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度，获得帧与帧之间的相似度并根据相似度获得当前关键帧的回环帧；构建回环内所有关键帧的位姿图，进行全局位姿图优化，减少累积误差。

进一步地，预选相似度为：

其中v_c为当前关键帧的单词向量，v_o为其他关键帧的单词向量，其中单词向量v的具体形式为：

v＝{(w₁,η₁),(w₂,η₂),…,(w_i,η_i)}

其中w_i为视觉词典中的第i个单词，η_i为w_i的单词权重，其中单词权重η_i计算方式为：

其中n_i为图像中属于单词w_i的特征数量，n为图像中的点线特征总数量，N为关键帧数据库中所有特征数量，N_i为数据库中属于单词w_i的点线特征总数量。

进一步地，所述步骤S1具体为：输入彩色图像，首先采用ORB算法进行点特征的提取，并根据描述子进行点特征匹配，再通过使用RANSAC方法剔除点特征间的误匹配；之后采用EDLine算法对线特征提取，根据距离、角度信息和描述子作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配。

进一步地，所述步骤S1具体为：输入彩色图像，首先使用LSD算法提取线特征，并根据描述子进行点特征匹配，再通过使用RANSAC方法剔除点特征间的误匹配；之后采用EDLine算法对线特征提取，根据距离、角度信息和描述子作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配。

与现有技术相比，本发明具有如下显著性有益效果：

(1)本发明改善了仅基于点特征法的视觉SLAM系统定位精度低、鲁棒性差等难题，能够在低纹理、光照变化大的室内人工结构化场景下达到满意的定位跟踪效果；

(2)本发明通过利用室内场景中的结构化特征，基于曼哈顿世界假设，通过提取曼哈顿世界坐标系求出绝对的曼哈顿世界坐标系到相机坐标系的旋转矩阵，进而直接获得世界坐标系到相机坐标系的旋转矩阵，从而极大减小帧间累积漂移误差，提高跟踪效果和精度。

附图说明

图1为实施例一种基于点线面特征的结构化场景视觉SLAM方法的流程图；

图2为本发明在TUM fr3_str_notext_far数据序列中估计的轨迹对比图；

图3为本发明在TUM fr3_str_notext_far数据序列中估计的xyz位移对比图；

图4为本发明在TUM fr3_str_notext_far数据序列中估计的三轴欧拉角姿态对比图；

图5为本发明在TUM fr3_str_notext_far数据序列中重建的稀疏三维特征图；

图6为本发明在TUM fr3_str_notext_far数据序列中重建的平面网格图。

具体实施方式

下面结合具体的实施例对本发明技术方案做进一步详细、完整地说明。

如图1所示，本发明提供的一种基于点线面特征的结构化场景视觉SLAM方法，包括如下步骤：

步骤S1、输入彩色图像，首先采用ORB算法对点特征提取和根据描述子去匹配，然后通过PROSAC方法剔除点特征间误匹配，其中在本实施例中除了可以用PROSAC方法剔除点特征间的误匹配，也可以使用RANSAC方法剔除点特征间的误匹配；接着采用EDLine算法对线特征提取，根据端点距离、角度和描述子信息作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配；

步骤S2、输入深度图像，首先对深度有效的像素点求出其对应的三维点，形成有组织的点云序列结构；然后根据所求点云序列使用层次聚类算法快速提取平面；接着根据平面参数对图像中的平面和地图面进行匹配；其具体步骤为：

步骤S21、输入深度图像，对深度有效的像素点求出其对应的三维点，形成有组织的点云序列结构，方便接下来算法处理；

步骤S22、初始化图模型，将图像中的点云均匀分成10×10大小的方块，每一块相当于图模型中的一个节点，当节点的均方误差高于设定阈值或节点含有深度缺失的数据或节点包含的深度不连续的点或节点在两个平面的边界时就把该节点和连接的边从图模型中删除；

步骤S23、使用层次聚类算法，首先建立一个最小堆的数据结构，使得能够更有效找到有最小均方误差的节点进行融合；然后再次计算融合后的平面拟合均方误差，找到获得均方误差最小的对应两个节点；若均方误差超过一个预先设定的阈值，一个平面的分割节点就找到并从图中提出，否则就把这两个节点融合重新加到构建的图中，对建立的最小堆进行更新；重复前两个步骤直到图中的节点全部被取出；

步骤S24、对平面进一步分割优化，对边缘处产生的锯齿状分割采用腐蚀边界区域进行优化；对未使用的数据点采用就近分类到该点周围的平面；对过度分割的面在特别小的图中再次进行层次聚类；

步骤S25、对分割好的平面求出平面的中心点P_C＝[X_C Y_C Z_C]^T和其单位法向量n＝[n_x n_y n_z]^T，其中X_C,Y_C,Z_C为中心点P_C对应的三维坐标值，n_x,n_yn_z为法向量n对应的值；则相机光心到平面的距离

则该平面的面特征表示为π＝[n^T d]^T；

步骤S26、将图像中的平面投影到世界坐标系上并根据法线夹角大小和世界坐标系原点到平面距离的差值大小对投影平面与地图面进行匹配；

步骤S3、检测图像中是否存在曼哈顿世界坐标系，若存在且曼哈顿世界地图中的历史关键帧观测到，则根据所提取得到的曼哈顿世界坐标系求相机旋转矩阵并跟踪点特征、线特征和平面特征优化相机位移，否则跟踪点特征、线特征和平面特征优化相机位姿，具体步骤为：

步骤S31、对步骤S24所提取的平面其对应的法向量进行遍历寻找是否存在3个互相正交的法向量组合，或是否存在2个互相正交的法向量组合，其另外一维法向量通过2个相互正交的法向量叉积获得；

步骤S32、假如存在则在曼哈顿世界地图中查询历史关键帧中是否存在与这组正交法向量一致的3个互相正交的平面组合，若存在则构成一个曼哈顿世界坐标系，取对应平面点数总和最多的法向量组合所构成的曼哈顿世界坐标系，并求出该曼哈顿世界坐标系到该帧相机坐标系的旋转矩阵为：

其中n₁,n₂,n₃代表找到的相互正交的法向量，c_i表示当前帧的id，m_k表示曼哈顿世界坐标系的id；因为传感器噪声的存在会导致n₁,n₂,n₃不完全正交或

不为正交矩阵，需要对

作SVD分解，得到正交化后曼哈顿世界坐标系到相机坐标系的旋转矩阵：

则相机坐标系到世界坐标系的旋转矩阵为：

其中c_j表示所找到的历史关键帧的id，

为第c_j帧时曼哈顿世界坐标系m_k到相机坐标系的旋转矩阵，

为第c_j帧时世界坐标系到相机坐标系的旋转矩阵；

然后跟踪点特征、线特征和平面特征优化相机位移t，其位移的误差模型e_t为：

其中e_p,e_l,e_π分别表示点特征、线特征和平面特征的重投影误差，具体形式分别为：

e_p＝p-(KR_cwP_w+t_cw)

e_l＝l^T(KR_cwP_L+t_cw)

其中K为相机内参，R_cw为世界坐标系到相机坐标系的旋转矩阵，t_cw为世界坐标系到相机坐标系的位移，T_cw为世界坐标系到相机坐标系的位姿变换矩阵；p为该帧识别到的点特征的像素坐标，P_w为该点特征对应的三维点；l为该帧识别到的线特征，L为该线特征对应的三维线；P_L为该三维线的三维端点；

为本发明采取的平面特征参数表达形式，π＝[n_x n_x n_x d]^T，n_x、n_y、n_z为平面π的法向量对应的值，d为相机光心到平面π的距离；π_c为该帧检测到的平面，π_w为该平面对应的地图面；其中Λ_p,Λ_l,Λ_π分别表示点特征、线特征和平面特征的信息矩阵，ρ_p、ρ_l、ρ_π分别为点特征、线特征和平面特征的Huber鲁棒核函数，Φ_p(p),Φ_l(l),Φ_π(π)分别表示点特征、线特征和平面特征的置信度，分别为：

其中n_p,n_l,n_π分别表示对应的点特征、线特征和平面特征被观测的次数，t_p,t_l,t_π为点特征、线特征和平面特征的权重系数，level_i代表该点特征所在ORB金字塔的层数；α为权重系数，α∈[0.5,1]；θ_i表示第i帧相机视线与地图线的夹角，

其中在本实施例平面特征采用的是球坐标参数表示形式

π＝[n_x n_x n_x d]^T，n_x、n_y、n_z为平面π的法向量对应的值，d为相机光心到平面π的距离；此外平面特征也可以采用单位四元数参数表示形式或者是最近点参数表示形式表示平面特征。

步骤S33、假如不存在S31所求的曼哈顿世界坐标系或曼哈顿世界地图中的历史关键帧没有观测到对应的法向量组合，则跟踪点特征、线特征和平面特征优化相机位姿R,t，相机位姿误差模型e_R,t为：

其中e_p、e_l、e_π分别表示点特征、线特征和平面特征的重投影误差，Φ_p(p)、Φ_l(l)、Φ_π(π)分别表示点特征、线特征和平面特征的置信度系数，Λ_p、Λ_l、Λ_π分别表示该点特征、线特征和平面特征的信息矩阵，ρ_p、ρ_l、ρ_π分别为点特征、线特征和平面特征的Huber鲁棒核函数；

步骤S4、判断当前帧是否为关键帧，若为关键帧，则加入到局部地图中，并更新地图中的三维点、三维线和曼哈顿世界地图，对当前关键帧和相邻的关键帧进行联合优化，优化相机的位姿和三维点和三维线特征，并剔除部分外点和冗余关键帧，具体步骤为：

步骤S41、判断该帧是否为关键帧，若为关键帧，则将其加入到局部地图，否则返回步骤S1，继续处理下一帧图像，其判断的条件为：(1)插入关键帧之后已处理多于10帧图像，且当前局部建图线程处于空闲状态，则判断为关键帧；(2)上一次插入关键帧之后已处理超过20帧图像，则判断为关键帧；(3)当前图像上匹配的点特征、线特征和平面特征总和不少于20，否则不能作为关键帧；(4)当前图像上跟踪的特征相比最近关键帧跟踪的特征少于90％，则判断为关键帧；(5)图像中提取到新平面，则判断为关键帧；

步骤S42、对于新插入的关键帧，首先更新共视图和生成树，加入新关键帧节点；然后剔除局部地图中的三维特征被观测次数少于三次的点特征和线特征；对新关键帧没有匹配的点线特征根据深度信息反投影生成新的地图点和地图线并插入到地图中；最后根据图像中的平面垂直关系记录垂直平面组合与当前关键帧的关联，并更新曼哈顿世界地图；

步骤S43、完整地图更新之后，对当前关键帧和与之关联的关键帧进行联合优化，主要优化对象为相关关键帧的相机位姿以及三维特征参数，这里平面的参数不作为优化对象；在迭代优化10次后将重投影误差过大的外点移除，再继续迭代优化，重复4次，以尽可能地清除外点，最大程度优化相机位姿R,t和三维特征信息P,L，这里不优化平面的参数；优化过程中采用的重投影误差e如下式所示：

其中e_p、e_l分别表示点特征、线特征的重投影误差，Φ_p(p)、Φ_l(l)分别表示点特征和线特征的置信度系数，Λ_p、Λ_l分别表示该点特征和线特征的信息矩阵，ρ_p、ρ_l分别为点特征和线特征的Huber鲁棒核函数；

步骤S44、将关键帧之间特征的重合度大于90％的关键帧剔除，以精简共视图；

步骤S5、对关键帧进行回环检测以及全局优化，其具体步骤为：

步骤S51、基于点线特征词典模型，首先根据当前关键帧所有点特征和线特征对应的描述子，在词典K叉树中找到所述的单词种类，计算每种单词对应的权重，得到当前关键帧的单词向量；

步骤S52、计算当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度s，选取相似度最高且超过设定阈值的关键帧作为当前关键帧的回环帧，其中当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度为：

其中v_c为当前关键帧的单词向量，v_o为其他关键帧的单词向量，其中单词向量v的具体形式为：

v＝{(w₁,η₁),(w₂,η₂),…,(w_k,η_k)}

其中w_i为视觉词典中的第i个单词，η_i为w_i的单词权重，其中单词权重η_i计算方式为：

其中n_i为图像中属于单词w_i的特征数量，n为图像中的点线特征总数量，N为关键帧数据库中所有特征数量，N_i为数据库中属于单词w_i的点线特征总数量。

步骤S53、构建回环内所有关键帧的位姿图，位姿图的顶点为每一个关键帧的位姿，边为关键帧之间的共视点特征或共视线特征，实现全局位姿图优化。

步骤S6、结果输出：位姿保存，路径绘制和地图的保存与同步实时显示，如图2为本发明在TUM fr3_str_notext_far数据序列中估计的轨迹对比图，图3的a、b、c为本发明在TUM fr3_str_notext_far数据序列中估计的xyz轴位移对比图，图4的a、b、c为本发明在TUMfr3_str_notext_far数据序列中估计的xyz轴欧拉角姿态对比图；如图5为本发明在TUMfr3_str_notext_far数据序列中重建的稀疏三维特征图，图6为本发明在TUM fr3_str_notext_far数据序列中重建的平面网格图。

实施例2

一种基于点线面特征的结构化场景视觉SLAM方法，包括以下步骤：

S1、输入彩色图像，根据彩色图像提取点特征和线特征并进行特征匹配；其中在本实施例中采用EDLine算法提取线特征，也可以使用LSD算法提取线特征，并且对LSD算法提取的割裂线段的合并方法同样是根据端点距离、角度和描述子信息作为筛选标准；

S2、输入深度图像，转换成点云序列结构，然后提取图像平面，接着对提取的图像平面与地图面进行匹配；在本实施例中关于平面匹配除了根据法线夹角大小和世界坐标系原点到平面距离的差值大小作为判断平面匹配标准，也可以采用法线夹角大小与两平面间是否存在碰撞区域作为判断平面匹配标准；

S3、检测图像中是否存在曼哈顿世界坐标系，若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系，则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移，否则跟踪点特征、线特征和平面特征去优化相机位姿；

S4、判断当前帧是否为关键帧，若为关键帧，则加入到局部地图中，并更新地图中的三维点、三维线和曼哈顿世界地图，对当前关键帧和相邻的关键帧进行联合优化，优化相机的位姿和三维点和三维线特征，并剔除部分外点和冗余关键帧；

S5、对关键帧进行回环检测，若检测到回环则进行闭合回环并全局优化，减少累积误差。

实施例3

一种基于点线面特征的结构化场景视觉SLAM方法，包括以下步骤：

S1、输入彩色图像，根据彩色图像提取点特征和线特征并进行特征匹配；其中在本实施例中使用LSD算法提取线特征，并且对LSD算法提取的割裂线段的合并方法同样是根据端点距离、角度和描述子信息作为筛选标准；

S2、输入深度图像，转换成点云序列结构，然后提取图像平面，接着对提取的图像平面与地图面进行匹配；在本实施例中关于平面匹配除了根据法线夹角大小和世界坐标系原点到平面距离的差值大小作为判断平面匹配标准，也可以采用法线夹角大小与两平面间是否存在碰撞区域作为判断平面匹配标准；

S3、检测图像中是否存在曼哈顿世界坐标系，若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系，则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移，否则跟踪点特征、线特征和平面特征去优化相机位姿；

S4、判断当前帧是否为关键帧，若为关键帧，则加入到局部地图中，并更新地图中的三维点、三维线和曼哈顿世界地图，对当前关键帧和相邻的关键帧进行联合优化，优化相机的位姿和三维点和三维线特征，并剔除部分外点和冗余关键帧；

S5、对关键帧进行回环检测，若检测到回环则进行闭合回环并全局优化，减少累积误差。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (10)

1.一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于，包括以下步骤：

S1、输入彩色图像，根据彩色图像提取点特征和线特征并进行特征匹配；

S2、输入深度图像，转换成点云序列结构，然后提取图像平面，接着对提取的图像平面与地图面进行匹配；

S3、检测图像中是否存在曼哈顿世界坐标系，若存在且在曼哈顿世界地图中的历史关键帧观测到该坐标系，则根据所提取到的曼哈顿世界坐标系估计相机姿态并跟踪点特征、线特征和平面特征去优化相机位移，否则跟踪点特征、线特征和平面特征去优化相机位姿；

S4、判断当前帧是否为关键帧，若为关键帧，则加入到局部地图中，并更新地图中的三维点、三维线和曼哈顿世界地图，对当前关键帧和相邻的关键帧进行联合优化，优化相机的位姿和三维点和三维线特征，并剔除部分外点和冗余关键帧；

S5、对关键帧进行回环检测，若检测到回环则进行闭合回环并全局优化，减少累积误差。

2.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S1具体为：输入彩色图像，首先采用ORB算法进行点特征的提取，并根据描述子进行点特征匹配，再通过PROSAC方法剔除点特征间的误匹配；之后采用EDLine算法对线特征提取，根据距离、角度信息和描述子作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配。

3.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S2具体为：首先对深度有效的像素点求出其对应的三维点，形成有组织的点云序列结构，接着使用层次聚类算法使小平面块融合为更大的平面，并对融合的平面进行分割优化，最后对分割好的平面求出平面的三维中心点P_C＝[X_C Y_C Z_C]^T和平面的单位法向量n＝[n_x n_y n_z]^T，其中X_C、Y_C、Z_C为中心点P_C对应的三维坐标值，n_x、n_y、n_z为法向量n对应的值；则相机光心到平面的距离

该平面的面特征表示为π＝[n^T d]^T；接着对图像中的平面投影到世界坐标系上并根据法线夹角大小和世界坐标系原点到平面距离的差值大小对投影平面与地图面进行匹配。

4.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S3具体为：

S31、对步骤S2所提取的平面的法向量进行遍历，寻找是否存在三个互相正交的法向量组合，若是只存在两个互相正交的法向量组合，其另外一维法向量通过两个相互正交的法向量叉积获得；

S32、若步骤S31所寻找的法向量组合存在，则在曼哈顿世界地图中查询历史关键帧中是否观测到与这组正交法向量一致的互相正交的平面，若存在则构成一个曼哈顿世界坐标系，取对应平面点数总和最多的法向量组合所构成的曼哈顿世界坐标系，并求出该曼哈顿世界坐标系到该帧相机坐标系的旋转矩阵为：

其中n₁、n₂、n₃表示找到的相互正交的法向量，c_i表示当前帧的id，m_k表示曼哈顿世界坐标系的id；对

作SVD分解，得到正交化后曼哈顿世界坐标系到相机坐标系的旋转矩阵

则相机坐标系到世界坐标系的旋转矩阵为：

其中c_j表示所找到的历史关键帧的id，

为第c_j帧时曼哈顿世界坐标系m_k到相机坐标系的旋转矩阵，

为第c_j帧时世界坐标系到相机坐标系的旋转矩阵；

然后跟踪点特征、线特征和平面特征去优化相机位移t，其位移的误差模型e_t为：

其中e_p、e_l、e_π分别表示点特征、线特征、平面特征的重投影误差，具体形式分别为：

e_p＝p-(KR_cwP_w+t_cw)

e_l＝l^T(KR_cwP_L+t_cw)

其中K为相机内参，R_cw为世界坐标系到相机坐标系的旋转矩阵，t_cw为世界坐标系到相机坐标系的位移，T_cw为世界坐标系到相机坐标系的位姿变换矩阵；p为该帧识别到的点特征的像素坐标，P_w为该点特征对应的三维点；l为该帧识别到的图像中的线特征，L为该线特征对应的三维线；P_L为三维线的三维端点；

为平面特征参数表达形式，π＝[n_x n_x n_x d]^T，n_x、n_y、n_z为平面π的法向量对应的值，d为相机光心到平面π的距离；π_c为该帧检测到的平面，π_w为该平面对应的地图面；其中Λ_p、Λ_l、Λ_π分别表示该点特征、线特征和平面特征的信息矩阵，ρ_p、ρ_l、ρ_π分别为点特征、线特征和平面特征的Huber鲁棒核函数，Φ_p(p)、Φ_l(l)、Φ_π(π)分别表示点特征、线特征和平面特征的置信度系数，分别为：

其中n_p、n_l、n_π分别表示对应的点特征、线特征、平面被观测的次数，t_p、t_l、t_π分别为点特征、线特征和平面特征的权重系数，level_i代表该点特征所在ORB金字塔的层数；α为权重系数，α∈[0.5,1]；θ_i表示第i帧相机视线与地图线的夹角，

S33、假如不存在步骤S31所求的曼哈顿世界坐标系或曼哈顿世界地图中的历史关键帧没有观测到对应的法向量组合，则跟踪点特征、线特征和平面特征去优化相机位姿R,t，相机位姿误差模型e_R,t为：

5.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S4具体为：

S41、根据点特征和线特征的匹配对数、是否检测到新的平面、特征跟踪情况以及局部地图内的关键帧情况来判断是否设该帧为关键帧，若为关键帧则加入局部地图，否则返回步骤S1；

S42、对于新插入的关键帧，更新共视图和生成树，加入新关键帧节点；根据观测一致性对从被创建开始未被连续可靠观测到的点特征、线特征和平面特征进行剔除；对新关键帧没有匹配的点特征和线特征根据深度信息反投影生成新的地图点和地图线并插入到地图中；根据图像中的平面垂直关系记录垂直平面组合与当前关键帧的关联，并更新曼哈顿世界地图；

S43、完整地图更新之后，对当前关键帧和与之关联的关键帧进行联合优化，并在优化中剔除外点以最大程度优化相机位姿；优化对象为相关关键帧的相机位姿R,t以及三维特征参数P,L，优化过程中采用的重投影误差e为：

其中e_p、e_l分别表示点特征和线特征的重投影误差，Φ_p(p)、Φ_l(l)分别表示点特征和线特征的置信度系数，Λ_p、Λ_l分别表示该点特征和线特征的信息矩阵，ρ_p、ρ_l分别为点特征和线特征的Huber鲁棒核函数；

S44、将关键帧之间特征的重合度大于90％的关键帧剔除。

6.根据权利要求5所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：判断关键帧的条件具体为：

(1)插入关键帧之后已处理多于10帧图像，且当前局部建图线程处于空闲状态，则判断为关键帧；

(2)上一次插入关键帧之后已处理超过20帧图像，则判断为关键帧；

(3)当前图像上匹配的点特征、线特征和平面特征总和不少于20，否则不能作为关键帧；

(4)当前图像上跟踪的特征相比最近关键帧跟踪的特征少于90％，则判断为关键帧；

(5)图像中提取到新平面，则判断为关键帧。

7.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S5具体为：基于点线特征词典模型，首先根据当前关键帧所有点线特征对应的描述子，在词典K叉树中找到所述的单词种类，计算每种单词对应的权重，得到当前关键帧的单词向量；计算当前关键帧的单词向量与其他关键帧单词向量之间的预选相似度，获得帧与帧之间的相似度并根据相似度获得当前关键帧的回环帧；构建回环内所有关键帧的位姿图，进行全局位姿图优化，减少累积误差。

8.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：预选相似度为：

其中v_c为当前关键帧的单词向量，v_o为其他关键帧的单词向量，其中单词向量v的具体形式为：

v＝{(w₁,η₁),(w₂,η₂),…,(w_i,η_i)}

其中w_i为视觉词典中的第i个单词，η_i为w_i的单词权重，其中单词权重η_i计算方式为：

其中n_i为图像中属于单词w_i的特征数量，n为图像中的点线特征总数量，N为关键帧数据库中所有特征数量，N_i为数据库中属于单词w_i的点线特征总数量。

9.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S1具体为：输入彩色图像，首先采用ORB算法进行点特征的提取，并根据描述子进行点特征匹配，再通过使用RANSAC方法剔除点特征间的误匹配；之后采用EDLine算法对线特征提取，根据距离、角度信息和描述子作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配。

10.根据权利要求1所述的一种基于点线面特征的结构化场景视觉SLAM方法，其特征在于：所述步骤S1具体为：输入彩色图像，首先使用LSD算法提取线特征，并根据描述子进行点特征匹配，再通过使用RANSAC方法剔除点特征间的误匹配；之后采用EDLine算法对线特征提取，根据距离、角度信息和描述子作为筛选标准合并割裂线段，并根据LBD描述子对线特征进行匹配。

Images