CN115965686A

CN115965686A - 一种融合点线特征的半直接视觉定位方法

Info

Publication number: CN115965686A
Application number: CN202211522204.2A
Authority: CN
Inventors: 曹政才; 谢红玉; 胡小博; 王伟
Original assignee: Beijing University of Chemical Technology
Current assignee: Beijing University of Chemical Technology
Priority date: 2022-11-30
Filing date: 2022-11-30
Publication date: 2023-04-14

Abstract

一种融合点线特征的半直接视觉定位方法，属于三维视觉领域。利用单目相机连续获取图像，分别对每一个图像帧提取点特征和线特征，并基于特征描述子匹配当前帧和前一帧的特征，基于对极几何约束估计相邻帧间位姿变换并初始化三维地图；基于稀疏特征像素对齐方法进行当前帧的位姿估计，若位姿估计失败，则提取当前帧的点特征和线特征，最小化点线特征的重投影误差重新估计相机位姿；基于相机的运动约束提取关键帧，并对关键帧提取点特征和线特征，生成新的地图点和地图线，更新三维地图；进行回环检测，基于点线融合的BA进一步优化相机位姿和空间点位置，降低累积误差。本发明解决了单目视觉定位方法在低纹理场景下由于点特征不足导致的跟踪失败。

Description

一种融合点线特征的半直接视觉定位方法

技术领域

本发明属于三维视觉领域，涉及到一种基于单目视觉传感器，融合点线特征的半直接视觉定位方法。

背景技术

同时定位与地图构建(SLAM，Simultaneous Location And Mapping)是一种借助机器人搭载的传感器，在没有环境先验信息的情况下，在运动过程中建立环境模型，同时估计自身运动的技术。视觉SLAM技术以相机为主要传感器，从图像中估计相机的运动和环境的模型。视觉定位技术广泛应用于移动机器人、虚拟现实、自动驾驶等领域。

主流的视觉SLAM方法可以划分为特征点法和直接法。特征点法通常通过提取图像特征点，利用相机位姿和特征点位置的几何关系求解相机轨迹，如ORB-SLAM，然而特征的提取和匹配耗时较长，且在低纹理或运动模糊等条件下适应性不足。直接法无需计算图像特征点，依赖灰度不变性假设，通过图像像素的光度匹配，快速求解相机位姿和特征点的三维坐标，如DSO，LSD-SLAM，然而相机大基线运动或者光照变化等场景会导致基于直接法的相机位姿跟踪失败。

针对低纹理场景下特征点法适应性不足的问题，如图1所示，有些系统采用点和线融合的方式求解SLAM问题，如PL-SLAM方法，采用LSD方法在图像中提取直线段，在ORB-SLAM2的基础上增加特征线的处理，一定程度上提升了系统定位和建图的精度和低纹理环境下的鲁棒性。但点特征和线特征的提取需要占用大量的运行时间，使视觉定位的实时性受到挑战。本发明方法充分利用直接法和特征点法的优势，在普通图像帧位姿跟踪过程中应用基于点、线融合的直接法进行位姿估计，提高系统运行速度，若直接法失败则使用基于特征的方法重新估计相机位姿，在关键帧上提取点特征和线特征，利用基于点、线特征融合的重投影误差模型优化相机位姿和三维地图，从而保证系统定位和三维建图的精度。

发明内容

为有效提升视觉定位系统的精度和低纹理场景下的鲁棒性，本发明提出了一种融合点线特征的半直接视觉定位方法。该方法通过在非关键帧处采用基于稀疏特征像素对齐的方法进行相机的位姿估计，若直接法失败则使用基于特征的方法重新估计相机位姿，在关键帧处采用基于特征描述子匹配的方法和基于点、线特征融合的重投影误差模型进行相机位姿和三维地图的优化，实现了在低纹理场景下的实时鲁棒的相机轨迹跟踪。

本发明提出一种融合点线特征的半直接视觉定位方法，包括：

步骤1：系统初始化，该步骤利用单目相机连续获取图像帧，提取每个图像帧的点特征和线特征，并基于特征描述子方法匹配当前帧和前一帧的点特征和线特征，若匹配到足量的点、线特征，则基于对极几何约束估计当前帧和前一帧的位姿变换，初始化三维地图；

步骤1.1：通过相机连续获取图像，利用相机本身的畸变参数对图像进行去畸变处理，并转成灰度图；提取图像帧中的ORB点特征和LSD线特征，并分别计算点、线特征的BRIEF和LBD描述子；

步骤1.2：计算当前帧和前一帧图像点特征和线特征的描述子相似度，统计匹配到当前图像帧和前一图像帧点特征和线特征的数量，若匹配到点特征大于100个，并且匹配到线特征数量大于15个，以保证有足量的匹配特征能够维护系统的鲁棒性，则将当前帧和前一帧设为初始关键帧，用以进行视觉定位系统的初始化；

步骤1.3：基于对极几何约束计算两个初始关键帧之间的位姿变换，以第一个关键帧为坐标原点建立三维世界坐标系，利用三角化方法将两个初始关键帧的点特征和线特征映射到三维空间，初始化三维地图，最后使用基于点线特征融合的BA方法优化初始关键帧的位姿和三维地图点。其中，BA优化问题的求解器采用g2o库定义，迭代策略使用L-M算法，设置迭代次数为20次，以有效清除外点，减少误匹配。

步骤2：初始化完成后，基于稀疏特征像素对齐方法进行当前帧的位姿估计，若位姿估计失败，则提取当前图像帧的点特征和线特征，与三维地图点和地图线进行匹配，构建点、线特征的重投影误差模型估计相机位姿；

步骤2.1：利用前一帧上观测到的点和线特征逆投影到三维空间，然后将特征对应的三维坐标投影回当前帧的像素坐标系；

步骤2.2：通过稀疏特征像素对齐方法跟踪对点特征和线特征在当前帧的投影位置，构建光度误差模型，通过最小化光度误差估计相机位姿，设置迭代次数30次，将外点剔除，重复迭代剔除外点的操作4次，以有效清除外点。

步骤2.3：若位姿估计得到内点数量少于阈值，这里阈值设为50以保证优化的有效性，则判断直接法相机位姿估计失败，提取当前帧的点特征和线特征，并分别计算对应的描述子；

步骤2.4：若步骤2.3中内点数量少于阈值，则基于点、线特征描述子，对当前帧和三维地图中的地图点和地图线进行匹配，构建基于点特征和线特征融合的重投影误差模型，重新估计当前帧相机位姿；

步骤3：利用相机的运动约束和ORB-SLAM中关键帧判断约束，判断当前帧是否是关键帧，若判断为关键帧，则对当前帧提取点特征和线特征，生成新的地图点和地图线，并剔除冗余的关键帧以及重复的地图点和地图线；

步骤3.1：根据相机的运动约束判断当前帧是否是关键帧，若判断为关键帧，且当前帧尚未提取过特征，则提取当前帧的ORB点特征和LSD线特征，计算其对应的BRIEF和LBD描述子，以及ORB点特征的BOW向量，并将当前帧插入关键帧序列；

步骤3.2：基于点、线特征描述子，匹配当前帧提取到的点特征、线特征和三维地图中的地图点和地图线，基于三角化方法生成新的地图点和地图线；

步骤3.3：遍历三维地图范围内的所有关键帧，根据特征的共视冗余度剔除冗余的关键帧和重复的地图点、地图线。

步骤4：对关键帧基于ORB特征的词袋模型进行回环检测，优化关键帧位姿和三维地图，降低视觉定位累积误差。

步骤4.1：对当前帧和关键帧序列采用基于ORB特征点的词袋模型进行回环检测，提取与当前帧有共视区域的参考关键帧，并采用随机采样一致性算法计算当前帧和匹配到的参考关键帧之间的相似变换，进行当前关键帧和相邻关键帧的位姿修正；

步骤4.2：采用基于点、线特征融合的BA方法对所有的关键帧和三维地图点和地图线位置进行优化，降低视觉定位系统的累积误差，并输出关键帧的相机位姿。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1是本发明所涉及的低纹理场景；

图2是本发明的整体框架示意图；

图3是本发明点特征和线特征在初始关键帧上的匹配关系；

图4是本发明视觉定位的特征匹配与关键帧的运动轨迹；

图5是本发明在实际长走廊环境下的相机运动轨迹估计结果。

具体实施方式

以下给出一个或多个方面的主要概述以应对这些方面的基本理解。此概述不能将所有构想的方面进行详述，其唯一的目的是要以简化形式给出一个或多个方面的一些概念以为稍后给出的更加详细的描述之序。注意，以下结合附图和具体实施例描述的诸方面仅是示例性的，而不应被理解为对本发明的保护范围进行任何限制。

图2展示本发明的系统框架，一种融合点线特征的半直接视觉定位方法主要分四个阶段：系统初始化阶段，相机位姿估计阶段，地图更新阶段和回环检测阶段。

步骤1：视觉定位系统初始化阶段。利用单目相机连续获取图像帧，提取每个图像帧的点特征和线特征，并基于特征描述子方法匹配当前帧和前一帧的点特征和线特征，若匹配到足量的点、线特征，则基于对极几何约束估计当前帧和前一帧的位姿变换，初始化三维地图；

步骤1.1：通过相机连续获取RGB图像帧，利用相机的畸变参数纠正图像帧的畸变，并将RGB图转为灰度图；提取当前帧的点特征和线特征，并计算点、线特征对应的描述子。

其中，点特征的提取和匹配使用ORB特征点和BRIEF描述子。首先，利用去畸变后的灰度图，构建图像金字塔；其次，调用OpenCV函数提取FAST角点，并利用灰度质心法计算每个特征点的方向；再次，计算特征点的BRIEF描述子。

线特征的提取和匹配使用LSD线特征和LBD描述子。其中，线特征利用OpenCV库中的LSD算法进行提取，该算法通过将灰度梯度方向相近的像素点合并，生成线段支持域，再用最小外接矩形判断该支持域是否作为最终生成的线特征。特征描述子采用LBD线段描述子；

步骤1.2：计算当前帧和前一帧中点特征和线特征的描述子相似度，统计匹配到当前图像帧和前一图像帧点特征和线特征的数量，若匹配到点特征大于100个，并且匹配到线特征数量大于15个，保证有足量的匹配特征来维护系统的鲁棒性，则将当前帧和前一帧设为初始关键帧，用以进行视觉定位系统的初始化；

步骤1.3：基于初始的关键帧初始化三维地图，首先，基于对几何约束计算两个初始关键帧K₁,K₂之间的位姿变换T₁₂(从第一帧到第二帧的3×3旋转矩阵R₁₂和3×1平移向量t₁₂)；其次，以第一个关键帧为坐标原点建立三维世界坐标系，利用三角化方法将匹配到的点特征和线特征映射到三维世界坐标系得到三维坐标，构建初始的三维地图；最后，使用基于点特征和线特征融合的BA方法优化初始的相机位姿和三维地图。其中，BA优化问题的求解器采用g2o库定义，迭代策略使用L-M算法，设置迭代次数为20次，以有效清除外点，减少误匹配。

其中，图3是本发明点特征和线特征在初始关键帧K₁、K₂上的匹配关系。具体为：前一帧像素坐标系下提取到二维的点特征坐标x，线特征坐标l，其中线特征的两个端点对应像素坐标系坐标p,q，对应三维空间中线特征的两个端点坐标P,Q，W表示均匀采样在三维线特征上的空间点坐标，对应图像帧像素坐标系坐标w，相机的光心O₁、O₂之间的位姿变换用相机光心之间的旋转矩阵R₁₂和平移向量t₁₂表示。

对于前一帧和当前帧像素坐标系中的对应的p，p′等，满足对极几何约束：

p′＝₁₂p+t₁₂，基于两个关键帧中匹配的所有点特征和线特征端点的二维像素坐标，利用Ransac方法计算得到两个关键帧的位姿变换T₁₂(R₁₂，t₁₂)，以第一个关键帧的相机坐标系为基础，通过三角化方法计算出点特征和线特征的端点在三维地图中的位置。

最后使用基于点特征和线特征融合的BA方法优化初始位姿和三维地图点，其中，BA优化问题的求解器采用g2o库定义，迭代策略使用L-M算法，设置迭代次数为20次，以有效清除外点，减少误匹配。

步骤2：初始化完成后，基于稀疏特征像素对齐方法进行当前帧的位姿估计，若位姿估计失败，则提取当前图像帧的点特征和线特征，构建三维地图和当前帧中对应的点、线特征的重投影误差模型重新估计相机位姿；

步骤2.1：将前一帧上点特征和均匀采样在线特征上的点(相隔4个点采样一个)逆投影到三维地图坐标，然后将三维坐标点投影回当前帧的像素坐标系；

步骤2.2：基于稀疏特征像素对齐方法跟踪方法进行当前帧的位姿估计，构建光度误差模型，来估计当前帧的位姿，其中光度误差为步骤2.1中投影到当前帧的像素坐标系下点及其周围点形成的像素块的灰度值，和前一帧中观测到的点特征和线特征上分布的点及其周围点形成的像素块的灰度值的误差，通过最小化光度误差估计相机位姿，设置迭代次数30次，将外点剔除，重复迭代剔除外点的操作4次，以有效清除外点；

其中，点特征或线特征对应的点的光度误差的表示为：e_i＝I_c(u_i')-I_c-1(u_i)，其中I_c-1(u_i)表示c-1帧的第i个点及其周围点形成的像素块的灰度值，I_c(u_i')表示2.1中投影到当前帧的像素坐标系下点u_i'及其周围点形成的像素块的灰度值；

步骤2.3：若经过步骤2.2迭代优化后得到内点数量大于阈值，这里将阈值设为50，则判断位姿估计是可靠的，否则，判断直接法相机位姿估计失败，提取当前帧的ORB点特征和LSD线特征，并分别计算BRIEF和LBD描述子；

步骤2.4：若步骤2.3中内点数量小于阈值，基于特征描述子，对当前帧和三维地图中的地图点和地图线进行匹配，利用匹配的点特征和线特征，构建基于点、线特征的重投影误差模型，估计当前帧的相机位姿；

其中，设定归一化线系数为

其中P和Q分别代表线特征的两个端点对应的三维坐标，构造线特征的重投影误差

地图线两个端点的重投影误差E_pline＝L^Tπ(P,θ,K)，E_qline＝L^Tπ(Q,θ,K)，端点空间点坐标P映射到当前帧像素坐标p的映射模型为π(P,θ,K)，θ＝{R_cw,t_cw}表示相机的位姿(表示三维地图坐标系到当前帧相机坐标系转换关系的的3x3的旋转矩阵Rcw和平移向量tcw)，K表示相机内参。

任意点特征的重投影误差表示为三维地图点位置X_i映射到当前图像帧的像素坐标系坐标x_i的误差e_i＝x_i-π(X_i,θ,K)。

构建点、线特征融合的重投影误差模型，其重投影误差模型为：

其中e_point,k和e_line,j分别为当前帧匹配到第k个地图点和第j个地图线，np和nl为当前图像帧匹配到的地图点和地图线的总数，Ω_point,k和Ω_line,j表示点和线的观测协方差。

步骤3：基于相机的运动约束判断当前帧是否是关键帧，若判断为关键帧，则对当前帧提取点特征和线特征，生成新的地图点和地图线，并剔除冗余的关键帧以及地图点和地图线；

步骤3.1：根据相机的运动约束判断当前帧是否是关键帧，若判断为关键帧，且当前帧尚未提取过特征，则提取当前帧的ORB点特征和LSD线特征，计算其对应的描述子和ORB点特征的BOW向量，并将当前帧插入关键帧序列；

其中，判断关键帧的限定条件在满足ORB-SLAM的共视约束条件下，本发明增加运动约束的判断，若与前一个关键帧相比，阈值设为当前帧c与前一个关键帧r的旋转矩阵R_cr对应的旋转度数大于3°，对应的平移向量t_cr的绝对值大于0.06来降低运算负担，则增加判断当前帧为关键帧。

步骤3.2：根据描述子匹配当前帧提取到的点特征和线特征和三维地图点和地图线，基于三角化方法生成新的地图点和地图线；

其中新的地图点和地图线生成需要满足条件：3个及以上关键帧能够观测到该特征，获取与邻接关键帧的相机中心坐标，计算两个图像帧的相机光心与特征映射到三维空间的基线，基线长度满足大于0.01以保证系统运算效率；

步骤3.3：在与当前帧有共视点特征和线特征的关键帧上运行基于点、线特征融合BA，遍历三维地图范围内的所有关键帧，根据特征点的共视冗余度剔除冗余的关键帧，以及地图点和地图线。

其中，关键帧冗余满足判定条件：判断关键帧序列中存在某一关键帧，其所观测到的90％以上地图点或地图线能够被至少三个其他关键帧所观测到，则剔除该关键帧。

冗余地图点和地图线判定条件：遍历地图中的地图点和地图线，需要满足约束条件：1)能够在大于理论上观测到其对应特征的关键帧数量的25％的关键帧上被找到；2)如果从该地图点或地图线建立开始已经生成了超过一个关键帧，必须满足该特征可以被至少三个关键帧观测到。若不满足上述约束，则判断该地图点或地图线冗余。

步骤4：对关键帧采用基于ORB特征的词袋模型进行回环检测，使用点、线特征融合的BA方法进一步优化关键帧位姿和全局的三维地图，降低视觉定位累积误差。

步骤4.2：采用基于点、线特征融合的BA方法进行全局三维地图和关键帧位姿的优化，其中，优化问题的求解器采用g2o库定义，迭代策略使用L-M算法，设置迭代次数为20次，来降低视觉定位系统的累积误差，并输出关键帧的位姿。

全局BA优化基于点、线特征融合的重投影误差模型：

其中e_i,k和e_i,j分别为第i个关键帧匹配到第k个地图线和第j个地图点的误差，np和nl为所有关键帧匹配到的地图点和地图线的总数，Ω_i,k和Ω_i,j表示点和线的观测协方差。

图4是本发明视觉定位的特征匹配与关键帧的运动轨迹。

图5是本发明在实际长走廊环境下估计的相机运动轨迹结果。

为了使得文章的解释更简单化，已上述的图文描述为一系列步骤，但是应该理解并领会，这些方法不受操作的次序所限制，因为按照一个或多个步骤进行实施，一些动作可按不同的顺序发生，但本领域技术人员可以理解其动作发生的原理。

尽管已对本发明说明性的具体实施方式逐步进行了描述，以便于本技术领域的技术人员能够进行领会，但是本发明不仅限于具体实施方式的范围，本领域技术人员可以在权利要求的范围内做出各种变形或修改，只要各种变化只要在所附的权利要求限定和确定的本发明精神和范围内。

Claims

1.一种融合点线特征的半直接视觉定位方法，其特征在于，包括如下步骤：

步骤1：视觉定位系统初始化阶段；利用单目相机连续获取图像帧，提取每个图像帧的点特征和线特征，并基于特征描述子方法匹配当前帧和前一帧的点特征和线特征，若匹配到足量的点、线特征，则基于对极几何约束估计当前帧和前一帧的位姿变换，初始化三维地图；

步骤2：相机位姿估计阶段；初始化完成后，基于稀疏特征像素对齐方法进行当前帧的位姿估计；若位姿估计失败，则提取当前图像帧的点特征和线特征，构建三维地图和当前帧中对应的点、线特征的重投影误差模型，重新估计相机位姿；

步骤3：地图更新阶段；基于相机的运动约束判断当前帧是否是关键帧，若判断为关键帧，则对当前帧提取点特征和线特征，生成新的地图点和地图线，并剔除冗余的关键帧以及重复的地图点和地图线。

步骤4：回环检测阶段；对关键帧采用基于ORB特征的词袋模型进行回环检测，使用点、线特征融合的BA方法进一步优化全局的关键帧位姿和三维地图，降低视觉定位累积误差，并输出关键帧的位姿序列。

2.根据权利要求1所述的一种融合点线特征的半直接视觉定位方法，其特征在于，步骤1进一步包括如下步骤：

步骤1.1：通过相机连续获取图像，提取图像帧中的ORB点特征和LSD线特征，并分别计算点、线特征的BRIEF和LBD描述子；

步骤1.2：计算当前帧和前一帧中点特征和线特征的描述子相似度，统计当前图像帧匹配到的前一图像帧点特征和线特征的数量，若匹配到点特征大于100个，并且匹配到线特征数量大于15个，保证有足量的匹配特征来维护系统的鲁棒性，则将当前帧和前一帧设为初始关键帧，用以进行视觉定位系统的初始化；

步骤1.3：基于初始的关键帧初始化三维地图，首先，基于对几何约束计算两个初始关键帧K₁,K₂之间的位姿变换T₁₂即从第一帧到第二帧的3×3旋转矩阵R₁₂和3×1平移向量t₁₂；其次，以第一个关键帧为坐标原点建立三维世界坐标系，利用三角化方法将匹配到的点特征和线特征映射到三维世界坐标系得到其对应的三维坐标，构建初始的三维地图；最后，使用基于点特征和线特征融合的BA方法优化初始的相机位姿和三维地图；

其中，BA优化问题的求解器采用g2o库定义，迭代策略使用L-M算法，设置迭代次数为20次。

3.根据权利要求1所述的一种融合点线特征的半直接视觉定位方法，其特征在于，步骤2进一步包括如下步骤：

步骤2.1：将前一帧上观测到的点特征和均匀采样在线特征上的点，逆投影到三维空间，然后将三维坐标点投影到当前帧的像素坐标系；

步骤2.2：基于稀疏特征像素对齐方法进行当前帧的位姿估计，构建光度误差模型，来估计当前帧的位姿，其中光度误差为步骤2.1中投影到当前帧的像素坐标系的点及其周围点形成的像素块的灰度值，和前一帧中观测到的点及其周围点形成的像素块灰度值的误差，通过最小化光度误差估计相机位姿，设置迭代次数30次，将外点剔除，重复迭代剔除外点的操作4次，以有效清除外点；

步骤2.4：若步骤2.3中内点数量小于阈值，则基于点、线特征描述子，对当前帧和三维地图中的地图点和地图线进行匹配，构建基于点特征和线特征融合的重投影误差模型，重新估计当前帧相机位姿。

4.根据权利要求1所述的一种融合点线特征的半直接视觉定位方法，其特征在于，步骤3进一步包括如下步骤：

步骤3.1：根据相机的运动约束判断当前帧是否是关键帧，若判断为关键帧，且当前帧尚未提取过特征，则提取当前帧的ORB点特征和LSD线特征，计算其对应的BRIEF和LBD描述子，以及ORB点特征的BOW向量，并将当前帧插入关键帧序列；这里提取关键帧的运动约束满足ORB-SLAM的关键帧判断约束，并满足当前帧c与前一关键帧r位姿变换约束，阈值设为旋转矩阵R_cr对应的旋转度数大于3°，平移向量t_cr的绝对值大于0.06来降低运算负担；

步骤3.2：基于点、线特征的描述子，匹配当前帧提取到的特征和三维地图中的地图点和地图线，基于三角化方法生成新的地图点和地图线；

步骤3.3：遍历关键帧序列，根据特征的共视冗余度剔除冗余的关键帧和冗余的地图点和地图线，其中，如果关键帧序列中存在某一关键帧，其所观测到的90％以上地图点或地图线能够被至少三个其他关键帧所观测到，则剔除该冗余关键帧；判断地图点或地图线非冗余条件：能找到对应该地图点或地图线的特征的关键帧要大于理论上能观测到的关键帧数量的25％；如果从该地图点或地图线建立开始已经生成了超过一个关键帧，需要满足该特征可以被至少三个关键帧观测到，否则将该地图点或者地图线删除。

5.根据权利要求1所述的一种融合点、线特征的半直接视觉定位方法，其特征在于，步骤4进一步包括如下步骤：

步骤4.2：采用基于点、线特征融合的BA方法进行全局三维地图和关键帧位姿的优化，其中，优化问题的求解器采用g2o库定义，迭代策略使用L-M算法，设置迭代次数为20次，来降低视觉定位系统的累积误差，并输出关键帧的相机位姿。