CN115855018A - 一种基于点线综合特征的改进同步定位与建图方法 - Google Patents

Abstract

本发明公开了一种基于点线综合特征的改进同步定位与建图方法，属于机器人定位与建图领域，所述方法是在传统视觉SLAM算法的基础上，增加了线特征作为补充特征，通过构建基于点线综合特征的后端约束方程实现定位与建图。首先，在SLAM前端，进行线特征和点特征的提取和匹配；在SLAM后端，改进了线特征重投影误差模型，构建了基于点线仿射不变量的点线综合特征残差约束，在此基础上构建了融合点线综合特征的后端优化函数，提升SLAM的精度和鲁棒性。所提出的弱纹理环境下基于点线综合特征的改进SLAM方法，可以提升室内弱纹理场景下机器人视觉SLAM的精度和可靠性，具有良好的工程应用价值。

Description

一种基于点线综合特征的改进同步定位与建图方法

技术领域

本发明属于机器人定位与建图领域，具体涉及一种基于点线综合特征的改进同步定位与建图方法。

背景技术

随着同步定位与建图(SLAM,Simultaneous Localization And Mapping)技术的发展和深入研究，单纯基于点特征的视觉SLAM的局限性慢慢表露出来，尤其是在缺少点特征的室内弱纹理环境下更为明显。而线特征作为在人工环境中出现最多的特征，非常适合作为点特征在弱纹理环境下的补充特征，并且线特征所包含的环境信息更加丰富，对于环境信息的结构化的表达效果更好。线特征的使用提高了在室内弱纹理场景下可靠的定位与建图能力，对提升弱纹理场景下机器人定位与建图的精度和鲁棒性具有重要意义。

发明内容

为了解决上述问题，本发明提出了一种基于点线综合特征的改进同步定位与建图方法，在传统视觉SLAM算法的基础上增加线特征作为补充特征，改进重投影误差模型，构建基于点线仿射不变量的点线综合特征残差约束，建立基于点线综合特征的后端优化方程，提升视觉SLAM算法在弱纹理场景下的精度和鲁棒性问题。

本发明的技术方案如下：

一种基于点线综合特征的改进同步定位与建图方法，包括以下步骤：

步骤1、在SLAM前端，提取图像的特征点得到点特征，然后进行点特征匹配；

步骤2、继续在SLAM前端，进行线特征的提取，构建线特征描述符，再将提取的线特征进行匹配；

步骤3、对点特征和线特征经过匹配之后，在SLAM后端，改进线特征的重投影模型，基于点线综合特征对相机的位姿进行估计；

步骤4、构建基于点线仿射不变性的点线综合特征约束，由得到的点线重投影约束构建基本的后端点线特征紧耦合约束方程，进而得到最优的相机位姿估计和特征坐标估计，最后生成地图。

进一步地，步骤1的具体过程如下：

步骤1.1、使用Harris算法进行角点的提取；

首先，构建一个沿着图像XY两方向移动的像素值变化函数E(x,y)：

其中，(x,y)表示像素点的位置，M为局部结构张量，ω为高斯函数，

I为图像的像素值随位置变化的函数，μ和ν分别为X、Y轴的像素位移量；

然后，定义R为每个角点的Harris响应函数，R表示为：

R＝det(M)-α[Tr(M)]² (3)

其中，det(M)为矩阵M的特征值，Tr(M)为矩阵M的迹，α为权重系数；

最后，根据求解的R值进行角点响应阈值判断，令Th_R表示角点检测阈值，R＞Th_R时区域判定为角点；

步骤1.2、采用光流法进行点特征的匹配；

首先进行图像金字塔的构建来满足图像的尺度不变性，随后根据光流法的灰度不变假设来追踪帧间的特征点灰度值，由于帧间位移量微小，并且同一特征点短时间的灰度值保持不变，所以在参考帧定位一个区域的灰度值，并以此区域在下一帧一定范围内寻找到与参考帧特征点附近区域灰度值相近的区域，最后进行点特征的追踪匹配。

进一步地，步骤2的具体过程如下：

步骤2.1、图像的线特征提取选择EDlines直线检测器；其过程描述如下：

步骤2.1.1、边缘提取；使用ED边缘检测器对灰度图像进行边缘特征提取；

步骤2.1.2、合并线段；将边缘提取得到的连续边缘像素组成的边缘线段分割成为一个或者多个线段，其基本思想为按照顺序遍历像素点，并使用最小二乘法将像素拟合为直线直至误差超过阈值，当误差超过阈值后，结束拟合并生成新的线段；

步骤2.1.3、剔除误检量；构造一个包含区域中所有点的矩形，然后将矩形中所有水平线方向角度与矩形方向角度偏差小于经验值τ的点称为对齐点；假设一个矩形内总的像素点数为n，对齐点数量为k；误检剔除采用Helm-holtz原理构建NFA，NFA模型如公式(4)所示：

公式中，i表示像素点的下标，其范围为从k到n，N代表图像像素值，p代表任意一个像素点为对齐点的概率；设置NFA的阈值为ξ，若NFA＞ξ，则保留直线段检测的结果；否则，丢弃该直线段的检测结果；

步骤2.2、构建线特征描述符，再将提取的线特征进行匹配；

线特征的匹配采用LBD描述子对线特征进行描述，并完成参考图像和查询图像之间的线特征匹配，线特征匹配包括三个步骤：

步骤2.2.1、查找候选匹配对；检查参考图像和查询图像的线矢量LineVec，比较它们的一元集合属性和局部外观相似性，若没有通过测试，则认为是不匹配的；

一元集合属性用LineVec的方向直方图衡量；计算参考图像和查询图像LineVec的方向直方图，分别用h_r、h_t表示，若||h_r-h_q||＜th_L，则认为是一组候选匹配，其中th_L为阈值，若||h_r-h_q||≥th_L，则需要判断局部外观相似性；

局部外观相似性用线描述符的距离度量；对于LineVec的每个直线，生成一个LBD描述符向量；当对参考图像和查询图像的两组LineVec做匹配时，计算参考图像和查询图像的所有描述符之间的距离，其中，距离的最小值为d_min，若d_min＜th_s，则被选为候选匹配，若d_min＞th_s，则认为线特征未匹配成功；其中，th_s为局部外观相似的容忍度；

步骤2.2.2、构建线段关系图；对于一组候选匹配，假设内部含k条曲线，构建一个一致性得分矩阵A来表示线段关系图，A的维数为k×k；设该组候选匹配内，l_i、l_j分别表示在参考图像和查询图像内的两条线段，矩阵A的元素A_ij计算为

其中，d_I、d_P、d_Θ为几何相似性，/>

为局部外观相似性；

步骤2.2.3、生成匹配结果；基于矩阵A，用指标向量x表示该簇，问题构建为如下问题：

x^*＝argmax(x^TAx) (5)

线特征的匹配从匹配簇的结果中搜索得到。

进一步地，步骤3的具体过程如下：

步骤3.1、基于点特征，利用传统视觉里程计的方法对相机位姿进行估计，其具体过程如下：

步骤3.1.1、采用对极约束恢复单目尺度和初始相机位姿；

步骤3.1.2、采用三角测量对于空间的二维坐标进行深度恢复，进而得到三维空间中的特征坐标；

步骤3.1.3、根据步骤3.1.2所求解的特征三维坐标以及对应的二维平面坐标，采用PNP算法进行帧间位姿的求解，由此获得连续的帧间相机位姿和关键的特征点坐标；

步骤3.1.4、在得到关键的特征点坐标后，根据ICP算法进行3D-3D匹配对的位姿求解，最终可得到相机坐标系相对于世界坐标系的位姿关系矩阵R和t；

步骤3.2、对线特征的重投影误差模型进行改进；改进的线特征重投影误差模型采用投影线段与像素平面线段中点的垂足距离和两线段的夹角作为优化变量；改进的线特征重投影方程和过程如下：

步骤3.2.1、对于空间中的某条线段L，用其首末两端的坐标(x₁,y₁,z₁)、(x₂,y₂,z₂)表示该线段，在普吕克坐标下L为6×1维的向量；

步骤3.2.2、将线段L投影到图像平面，得到图像上的投影线段L_c：

其中，K为线段的投影矩阵，Q为普吕克坐标系到相机坐标系的转换关系，f_x、f_y、c_x、c_y为相机内参，R_cw表示两个坐标系间的旋转矩阵，t_cw为平移向量，[]_×表示向量的反对称矩阵，L_c为3×1的向量，其三个分量分别用l₁,l₂,l₃表示；

步骤3.2.3、线特征重投影误差e_l计算：令l表示将L_c通过位姿参数投影到下一帧得到的线段，l′表示L_c通过帧间匹配得到的下一帧的线段；若(x_l,s,y_l,s)、(x_l,e,y_l,e)表示线段l的起点和终点，k_l、k_l′分别为l和l′的斜率，计算线段l′的中点(x_l′,M,y_l′,M)到投影线l的垂足距离d以及两条线之间的夹角θ作为重投影误差：

e_l＝[d,θ] (7)

步骤3.2.4、加入线特征，利用线特征约束优化对R和t的估计；以当前帧的位姿作为优化的状态变量，通过最小化点线特征的重投影残差函数进行优化，优化过程表示为：

其中，i、j代表点特征、线特征在相邻关键帧中的匹配对集合，e_pi为点特征的重投影误差，e_lj为线特征的重投影误差，ρ_p(·)、ρ_l(·)为Huber核函数，用于平衡e_pi、e_lj增长过快对优化过程的影响。

进一步地，步骤4的具体过程如下：

步骤4.1、基于点线特征的重投影误差约束构建步骤如下：

步骤4.1.1、对滑窗内帧的点特征和线特征进行跟踪，范围筛选为有共面的点特征和线特征，选出被观测到帧数最多的点特征和线特征之后，根据点特征强度选出点特征强度最强的点特征，根据线特征长度选出最长的两条线特征；

步骤4.1.2、在选取出的点特征和线特征所有共面帧上进行约束构建，根据仿射变换中面积比例不变的特性，约束的残差方程采用第i帧点特征和线特征投影到第j帧之后点和线特征所构成的三角形面积之比k_i，与在第j帧上根据光流追踪到的点线特征构建的三角形面积比例k_j之差构建，以此来构建帧内点线约束特征e_pl，方程如下：

i帧和j帧之间的变换矩阵T为：

T＝[R，t] (11)

其中，R表示旋转矩阵，t表示平移矩阵；

点特征坐标以及其变换为：

(u_j,v_j,1)^T＝R(u_i,v_i,1)^T+t (12)

其中，(u_i,v_i)表示第i帧点特征的位置，(u_j,v_j)表示第j帧点特征的位置；

线特征参数化形式和变换表示为：

其中，I_i表示第i帧线特征，I_j表示第j帧线特征，n_c,j表示第j帧图像中线特征I_j上一点与原点组成平面的法向量，d_c,j表示第j帧图像中线特征I_j的方向向量；

点线特征所构建三角形面积比例表示为：

其中，s₁、s₂为一张图中由点线特征构成的两个三角形；

基于公式(14)，k_i、k_j表示为：

其中，s_1,i、s_2,i表示第i帧图像中点线特征构成的两个三角形，s_1,j、s_2,j表示第j帧图像中点线特征构成的两个三角形；

点线综合约束特征方程为：

e_pl＝k_i-k_j (16)

代价方程C_fun表示为：

其中，ρ(·)为CauchyLoss函数，其形式为ρ(x)＝log(x+1)，

表示点线融合的观测协方差矩阵；

步骤4.2、融合点线综合特征构建后端紧耦合后端优化函数；紧耦合后端优化函数的构建描述为：

由点特征的重投影误差e_l和线特征的重投影误差e_p，构建基本的点线特征紧耦合约束方程C_p+l，即融合点线综合特征的紧耦合后端优化函数：

Γ(x)＝log(x+1) (19)

其中，

和/>

为点和线的重投影误差，h表示点特征个数的下标，j表示线特征个数的下标，p_k,h、L_k,j表示点、线的观测协方差矩阵，Γ(·)为CauchyLoss函数；通过Ceres优化器求解优化C_p+l，得到最优的相机位姿估计和特征坐标估计，相机位姿估计实现目标的定位，利用特征坐标估计生成地图。

本发明所带来的有益技术效果：

通过在SLAM前端采用线特征作为补充特征，在SLAM后端建立了基于点线综合特征的紧耦合优化函数，增强了视觉SLAM算法在弱纹理场景下的鲁棒性问题，提升了机器人在室内弱纹理场景下定位和建图的精度和可靠性。

附图说明

图1为本发明基于点线综合特征的改进同步定位与建图方法的流程图；

图2为本发明基于点线仿射不变性构建点线综合约束的原理图。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1所示，一种基于点线综合特征的改进同步定位与建图方法，包括以下步骤：

步骤1、在SLAM前端，提取图像的特征点得到点特征，然后进行点特征匹配。具体过程如下：

步骤1.1、使用常用的Harris算法进行角点的提取；

首先，构建一个沿着图像XY两方向移动的像素值变化函数E(x,y)：

其中，(x,y)表示像素点的位置，M为局部结构张量，ω为高斯函数，

I为图像的像素值随位置变化的函数，μ和ν分别为X、Y轴的像素位移量。

然后，定义R为每个角点的Harris响应函数，R可以表示为：

R＝det(M)-α[Tr(M)]² (3)

其中，det(M)为矩阵M的特征值，Tr(M)为矩阵M的迹，α为权重系数，一般取0.04～0.06。

最后，根据求解的R值进行角点响应阈值判断，令Th_R表示角点检测阈值，R＞Th_R时区域判定为角点。

步骤1.2、采用光流法进行点特征的匹配；

首先进行图像金字塔的构建来满足图像的尺度不变性，随后根据光流法的灰度不变假设来追踪帧间的特征点灰度值，由于帧间位移量微小，并且同一特征点短时间的灰度值保持不变，所以可在参考帧定位一个区域的灰度值，并以此区域在下一帧一定范围内寻找到与参考帧特征点附近区域灰度值相近的区域，最后进行特征点的追踪匹配。

步骤2、继续在SLAM前端，进行线特征的提取，构建线特征描述符，再将提取的线特征进行匹配。具体过程如下：

步骤2.1、图像的线特征提取选择EDlines直线检测器。其过程描述如下：

步骤2.1.1、边缘提取。使用Edge Drawing(ED,边缘检测器)对灰度图像进行边缘特征提取。

步骤2.1.2、合并线段。将边缘提取得到的连续边缘像素组成的边缘线段分割成为一个或者多个线段，其基本思想为按照顺序遍历像素点，并使用最小二乘法将像素拟合为直线直至误差超过一定阈值(1个像素误差)，当误差超过阈值后，结束拟合并生成新的线段。

步骤2.1.3、剔除误检量。构造一个包含区域中所有点的矩形，然后将矩形中所有水平线方向角度与矩形方向角度偏差小于τ(τ为经验值)的点称为对齐点。假设一个矩形内总的像素点数为n，对齐点数量为k。误检剔除采用Helm-holtz原理构建NFA(Number ofFalse Alarms)，NFA模型如公式(4)所示：

公式中，i表示像素点的下标，其范围为从k到n，N代表图像像素值，p代表任意一个像素点为对齐点的概率。设置一个NFA的阈值ξ，若NFA＞ξ，则保留直线段检测的结果；否则，丢弃该直线段的检测结果。

步骤2.2、构建线特征描述符，再将提取的线特征进行匹配；

线特征的匹配采用LBD(Line Band Discriptor)描述子对线特征进行描述，并完成参考图像和查询图像之间的线特征匹配，线特征的匹配包括三个步骤：

步骤2.2.1、查找候选匹配对。检查参考图像和查询图像的线矢量(LineVec)，比较它们的一元集合属性和局部外观相似性，若没有通过测试，则认为是不匹配的。

一元集合属性用LineVec的方向直方图衡量。计算参考图像和查询图像LineVec的方向直方图，分别用h_r、h_t表示，若||h_r-h_q||＜th_L，则认为是一组候选匹配，其中th_L为阈值，为经验值，一般选取0.4。若||h_r-h_q||≥th_L，则需要判断局部外观相似性。

局部外观相似性用线描述符的距离度量。对于LineVec的每个直线，生成一个LBD描述符向量。当对参考图像和查询图像的两组LineVec做匹配时，计算参考图像和查询图像的所有描述符之间的距离，其中，距离的最小值为d_min，若d_min＜th_s，则被选为候选匹配，若d_min＞th_s，则认为线特征未匹配成功。其中，th_s为局部外观相似的容忍度，为经验值，一般取th_s＝0.35。

步骤2.2.2、构建线段关系图。对于一组候选匹配(假设内部含k条曲线)，构建一个一致性得分矩阵A来表示线段关系图，A的维数为k×k。设该组候选匹配内，l_i、l_j分别表示在参考图像和查询图像内的两条线段，矩阵A的元素A_ij可以计算为

其中，d_I、d_P、d_Θ为几何相似性，/>

为局部外观相似性。

步骤2.2.3、生成匹配结果。基于矩阵A，线特征匹配问题就转换成了需要匹配簇的最优化问题，即用指标向量x表示该簇，问题构建为如下问题：

x^*＝argmax(x^TAx) (5)

线特征的匹配可以从匹配簇的结果中搜索得到。

步骤3、对点特征和线特征经过匹配之后，在SLAM后端，改进线特征的重投影模型，基于点线综合特征对相机的位姿进行估计。具体过程如下：

步骤3.1、基于点特征，利用传统视觉里程计的方法对相机位姿进行估计，其具体过程如下：

步骤3.1.1、采用对极约束恢复单目尺度和初始相机位姿；

步骤3.1.2、采用三角测量对于空间的二维坐标进行深度恢复，进而得到三维空间中的特征坐标；

步骤3.1.3、根据步骤3.1.2所求解的特征三维坐标以及对应的二维平面坐标，采用PNP(Perspective-n-Point)算法进行帧间位姿的求解，由此获得连续的帧间相机位姿和关键的特征点坐标。

步骤3.1.4、在得到关键的特征点坐标后，根据ICP(Iterative Closest Point)算法进行3D-3D匹配对的位姿求解，最终可得到相机坐标系相对于世界坐标系的位姿关系矩阵R和t。

步骤3.2、对线特征的重投影误差模型进行改进。

传统线特征重投影误差模型采用投影线段与像素平面线段端点的垂足距离作为优化变量，改进的线特征重投影误差模型采用投影线段与像素平面线段中点的垂足距离和两线段的夹角作为优化变量。改进的线特征重投影方程和过程如下：

步骤3.2.1、对于空间中的某条线段L，用其首末两端的坐标(x₁,y₁,z₁)、(x₂,y₂,z₂)表示该线段，在普吕克坐标下L为6×1维的向量。

步骤3.2.2、将线段L投影到图像平面，得到图像上的投影线段L_c：

其中，K为线段的投影矩阵，Q为普吕克坐标系到相机坐标系的转换关系，f_x、f_y、c_x、c_y为相机内参，R_cw表示两个坐标系间的旋转矩阵，t_cw为平移向量，[]_×表示向量的反对称矩阵，L_c为3×1的向量，其三个分量分别用l₁,l₂,l₃表示。

步骤3.2.3、线特征重投影误差e_l计算：令l表示将L_c通过位姿参数投影到下一帧得到的线段，l′表示L_c通过帧间匹配得到的下一帧的线段。若(x_l,s,y_l,s)、(x_l,e,y_l,e)表示线段l的起点和终点，k_l、k_l′分别为l和l′的斜率，计算线段l′的中点(x_l′,M,y_l′,M)到投影线l的垂足距离d以及两条线之间的夹角θ作为重投影误差：

e_l＝[d,θ] (7)

步骤3.2.4、加入线特征，利用线特征约束优化对R和t的估计。以当前帧的位姿作为优化的状态变量，通过最小化点线特征的重投影残差函数进行优化，优化过程可以表示为：

其中，i、j代表点特征、线特征在相邻关键帧中的匹配对集合，e_pi为点特征的重投影误差，e_lj为线特征的重投影误差，ρ_p(·)、ρ_l(·)为Huber核函数，用于平衡e_pi、e_lj增长过快对优化过程的影响。

步骤4、构建基于点线仿射不变性的点线综合特征约束，由得到的点线重投影约束构建基本的后端点线特征紧耦合约束方程。具体过程如下：

在SLAM的滑窗优化过程中，每帧图像帧之间的优化误差和噪声影响会不断累积进而造成滑窗优化方向产生偏移。为解决该问题，根据点线特征的仿射不变性在滑窗帧内构建短期的回环约束，即得到基于点线特征的重投影误差约束。

步骤4.1、基于点线特征的重投影误差约束构建步骤如下：

步骤4.1.1、对滑窗内帧的点特征和线特征进行跟踪，范围筛选为有共面(最少三个共面帧以上)的点特征和线特征，选出被观测到帧数最多的点特征和线特征之后，根据点特征强度选出点特征强度最强的点特征，根据线特征长度选出最长的两条线特征。

步骤4.1.2、在选取出的点特征和线特征所有共面帧上进行约束构建，根据仿射变换中面积比例不变的特性，约束的残差方程采用第i帧点特征和线特征投影到第j帧之后点和线特征所构成的三角形面积之比k_i，与在第j帧上根据光流追踪到的点线特征构建的三角形面积比例k_j之差构建，以此来构建帧内点线约束特征e_pl，图2为帧间点线特征约束，具体方程如下：

i帧和j帧之间的变换矩阵T为：

T＝[R，t] (11)

其中，R表示旋转矩阵，t表示平移矩阵。

点特征坐标以及其变换为：

(u_j,v_j,1)^T＝R(u_i,v_i,1)^T+t (12)

其中，(u_i,v_i)表示第i帧点特征的位置，(u_j,v_j)表示第j帧点特征的位置。

线特征参数化形式和变换表示为：

其中，I_i表示第i帧线特征，I_j表示第j帧线特征，n_c,j表示第j帧图像中线特征I_j上一点与原点组成平面的法向量，d_c,j表示第j帧图像中线特征I_j的方向向量。

点线特征所构建三角形面积比例可表示为：

其中，s₁、s₂为一张图中由点线特征构成的两个三角形。

基于公式(14)，k_i、k_j可以表示为：

其中，s_1,i、s_2,i表示第i帧图像中点线特征构成的两个三角形，s_1,j、s_2,j表示第j帧图像中点线特征构成的两个三角形。

点线综合约束特征方程为：

e_pl＝k_i-k_j (16)

代价方程(cost function)C_fun可以表示为：

其中，ρ(·)为CauchyLoss函数，其形式为ρ(x)＝log(x+1)，

表示点线融合的观测协方差矩阵。通过公式(16)的约束，能有效约束短期内视觉约束造成的累积误差，对于后端回环检测起到了有效的补充作用。

步骤4.2、融合点线综合特征构建后端紧耦合优化函数。

紧耦合后端优化函数的构建可以描述为：

由点特征的重投影误差e_l和线特征的重投影误差e_p，构建基本的点线特征紧耦合约束方程C_p+l，即融合点线综合特征的紧耦合后端优化函数：

Γ(x)＝log(x+1) (19)

其中，

和/>

为点和线的重投影误差，h表示点特征个数的下标，j表示点特征个数的下标，p_k,h、L_k,j表示点、线的观测协方差矩阵，Γ(·)为CauchyLoss函数。选择Ceres优化器求解优化函数C_p+l，可以得到最优的相机位姿估计和特征坐标估计，得到相机位姿估计即实现了目标的定位，利用特征坐标估计生成地图。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (5)

1.一种基于点线综合特征的改进同步定位与建图方法，其特征在于，包括以下步骤：

步骤1、在SLAM前端，提取图像的特征点得到点特征，然后进行点特征匹配；

步骤2、继续在SLAM前端，进行线特征的提取，构建线特征描述符，再将提取的线特征进行匹配；

步骤3、对点特征和线特征经过匹配之后，在SLAM后端，改进线特征的重投影模型，基于点线综合特征对相机的位姿进行估计；

步骤4、构建基于点线仿射不变性的点线综合特征约束，由得到的点线重投影约束构建基本的后端点线特征紧耦合约束方程，进而得到最优的相机位姿估计和特征坐标估计，相机位姿估计实现目标的定位，利用特征坐标估计生成地图。

2.根据权利要求1所述基于点线综合特征的改进同步定位与建图方法，其特征在于，所述步骤1的具体过程如下：

步骤1.1、使用Harris算法进行角点的提取；

首先，构建一个沿着图像XY两方向移动的像素值变化函数E(x,y)：

其中，(x,y)表示像素点的位置，M为局部结构张量，ω为高斯函数，

I为图像的像素值随位置变化的函数，μ和ν分别为X、Y轴的像素位移量；

然后，定义R为每个角点的Harris响应函数，R表示为：

R＝det(M)-α[Tr(M)]² (3)

其中，det(M)为矩阵M的特征值，Tr(M)为矩阵M的迹，α为权重系数；

最后，根据求解的R值进行角点响应阈值判断，令Th_R表示角点检测阈值，R＞Th_R时区域判定为角点；

步骤1.2、采用光流法进行点特征的匹配；

首先进行图像金字塔的构建来满足图像的尺度不变性，随后根据光流法的灰度不变假设来追踪帧间的特征点灰度值，由于帧间位移量微小，并且同一特征点短时间的灰度值保持不变，所以在参考帧定位一个区域的灰度值，并以此区域在下一帧一定范围内寻找到与参考帧特征点附近区域灰度值相近的区域，最后进行点特征的追踪匹配。

3.根据权利要求1所述基于点线综合特征的改进同步定位与建图方法，其特征在于，所述步骤2的具体过程如下：

步骤2.1、图像的线特征提取选择EDlines直线检测器；其过程描述如下：

步骤2.1.1、边缘提取；使用ED边缘检测器对灰度图像进行边缘特征提取；

步骤2.1.2、合并线段；将边缘提取得到的连续边缘像素组成的边缘线段分割成为一个或者多个线段，其基本思想为按照顺序遍历像素点，并使用最小二乘法将像素拟合为直线直至误差超过阈值，当误差超过阈值后，结束拟合并生成新的线段；

步骤2.1.3、剔除误检量；构造一个包含区域中所有点的矩形，然后将矩形中所有水平线方向角度与矩形方向角度偏差小于经验值τ的点称为对齐点；假设一个矩形内总的像素点数为n，对齐点数量为k；误检剔除采用Helm-holtz原理构建NFA，NFA模型如公式(4)所示：

公式中，i表示像素点的下标，其范围为从k到n，N代表图像像素值，p代表任意一个像素点为对齐点的概率；设置NFA的阈值为ξ，若NFA＞ξ，则保留直线段检测的结果；否则，丢弃该直线段的检测结果；

步骤2.2、构建线特征描述符，再将提取的线特征进行匹配；

线特征的匹配采用LBD描述子对线特征进行描述，并完成参考图像和查询图像之间的线特征匹配，线特征匹配包括三个步骤：

步骤2.2.1、查找候选匹配对；检查参考图像和查询图像的线矢量LineVec，比较它们的一元集合属性和局部外观相似性，若没有通过测试，则认为是不匹配的；

一元集合属性用LineVec的方向直方图衡量；计算参考图像和查询图像LineVec的方向直方图，分别用h_r、h_t表示，若||h_r-h_q||＜th_L，则认为是一组候选匹配，其中th_L为阈值，若||h_r-h_q||≥th_L，则需要判断局部外观相似性；

局部外观相似性用线描述符的距离度量；对于LineVec的每个直线，生成一个LBD描述符向量；当对参考图像和查询图像的两组LineVec做匹配时，计算参考图像和查询图像的所有描述符之间的距离，其中，距离的最小值为d_min，若d_min＜th_s，则被选为候选匹配，若d_min＞th_s，则认为线特征未匹配成功；其中，th_s为局部外观相似的容忍度；

步骤2.2.2、构建线段关系图；对于一组候选匹配，假设内部含k条曲线，构建一个一致性得分矩阵A来表示线段关系图，A的维数为k×k；设该组候选匹配内，l_i、l_j分别表示在参考图像和查询图像内的两条线段，矩阵A的元素A_ij计算为

其中，d_I、d_P、d_Θ为几何相似性，/>

为局部外观相似性；

步骤2.2.3、生成匹配结果；基于矩阵A，用指标向量x表示该簇，问题构建为如下问题：

x^*＝argmax(x^TAx) (5)

线特征的匹配从匹配簇的结果中搜索得到。

4.根据权利要求1所述基于点线综合特征的改进同步定位与建图方法，其特征在于，所述步骤3的具体过程如下：

步骤3.1、基于点特征，利用传统视觉里程计的方法对相机位姿进行估计，其具体过程如下：

步骤3.1.1、采用对极约束恢复单目尺度和初始相机位姿；

步骤3.1.2、采用三角测量对于空间的二维坐标进行深度恢复，进而得到三维空间中的特征坐标；

步骤3.1.3、根据步骤3.1.2所求解的特征三维坐标以及对应的二维平面坐标，采用PNP算法进行帧间位姿的求解，由此获得连续的帧间相机位姿和关键的特征点坐标；

步骤3.1.4、在得到关键的特征点坐标后，根据ICP算法进行3D-3D匹配对的位姿求解，最终可得到相机坐标系相对于世界坐标系的位姿关系矩阵R和t；

步骤3.2、对线特征的重投影误差模型进行改进；改进的线特征重投影误差模型采用投影线段与像素平面线段中点的垂足距离和两线段的夹角作为优化变量；改进的线特征重投影方程和过程如下：

步骤3.2.1、对于空间中的某条线段L，用其首末两端的坐标(x₁,y₁,z₁)、(x₂,y₂,z₂)表示该线段，在普吕克坐标下L为6×1维的向量；

步骤3.2.2、将线段L投影到图像平面，得到图像上的投影线段L_c：

其中，K为线段的投影矩阵，Q为普吕克坐标系到相机坐标系的转换关系，f_x、f_y、c_x、c_y为相机内参，R_cw表示两个坐标系间的旋转矩阵，t_cw为平移向量，[]_×表示向量的反对称矩阵，L_c为3×1的向量，其三个分量分别用l₁,l₂,l₃表示；

步骤3.2.3、线特征重投影误差e_l计算：令l表示将L_c通过位姿参数投影到下一帧得到的线段，l′表示L_c通过帧间匹配得到的下一帧的线段；若(x_l,s,y_l,s)、(x_l,e,y_l,e)表示线段l的起点和终点，k_l、k_l′分别为l和l′的斜率，计算线段l′的中点(x_l′,M,y_l′,M)到投影线l的垂足距离d以及两条线之间的夹角θ作为重投影误差：

e_l＝[d,θ] (7)

步骤3.2.4、加入线特征，利用线特征约束优化对R和t的估计；以当前帧的位姿作为优化的状态变量，通过最小化点线特征的重投影残差函数进行优化，优化过程表示为：

其中，i、j代表点特征、线特征在相邻关键帧中的匹配对集合，e_pi为点特征的重投影误差，e_lj为线特征的重投影误差，ρ_p(·)、ρ_l(·)为Huber核函数，用于平衡e_pi、e_lj增长过快对优化过程的影响。

5.根据权利要求1所述基于点线综合特征的改进同步定位与建图方法，其特征在于，所述步骤4的具体过程如下：

步骤4.1、基于点线特征的重投影误差约束构建步骤如下：

步骤4.1.1、对滑窗内帧的点特征和线特征进行跟踪，范围筛选为有共面的点特征和线特征，选出被观测到帧数最多的点特征和线特征之后，根据点特征强度选出点特征强度最强的点特征，根据线特征长度选出最长的两条线特征；

步骤4.1.2、在选取出的点特征和线特征所有共面帧上进行约束构建，根据仿射变换中面积比例不变的特性，约束的残差方程采用第i帧点特征和线特征投影到第j帧之后点和线特征所构成的三角形面积之比k_i，与在第j帧上根据光流追踪到的点线特征构建的三角形面积比例k_j之差构建，以此来构建帧内点线约束特征e_pl，方程如下：

i帧和j帧之间的变换矩阵T为：

T＝[R，t] (11)

其中，R表示旋转矩阵，t表示平移矩阵；

点特征坐标以及其变换为：

(u_j,v_j,1)^T＝R(u_i,v_i,1)^T+t (12)

其中，(u_i,v_i)表示第i帧点特征的位置，(u_j,v_j)表示第j帧点特征的位置；

线特征参数化形式和变换表示为：

其中，I_i表示第i帧线特征，I_j表示第j帧线特征，n_c,j表示第j帧图像中线特征I_j上一点与原点组成平面的法向量，d_c,j表示第j帧图像中线特征I_j的方向向量；

点线特征所构建三角形面积比例表示为：

其中，s₁、s₂为一张图中由点线特征构成的两个三角形；

基于公式(14)，k_i、k_j表示为：

其中，s_1,i、s_2,i表示第i帧图像中点线特征构成的两个三角形，s_1,j、s_2,j表示第j帧图像中点线特征构成的两个三角形；

点线综合约束特征方程为：

e_pl＝k_i-k_j (16)

代价方程C_fun表示为：

其中，ρ(·)为CauchyLoss函数，其形式为ρ(x)＝log(x+1)，

表示点线融合的观测协方差矩阵；

步骤4.2、融合点线综合特征构建后端紧耦合后端优化函数；紧耦合后端优化函数的构建描述为：

由点特征的重投影误差e_l和线特征的重投影误差e_p，构建基本的点线特征紧耦合约束方程C_p+l，即融合点线综合特征的紧耦合后端优化函数：

Γ(x)＝log(x+1) (19)

其中，

和/>

为点和线的重投影误差，h表示点特征个数的下标，j表示线特征个数的下标，p_k,h、L_k,j表示点、线的观测协方差矩阵，Γ(·)为CauchyLoss函数；通过Ceres优化器求解优化C_p+l，得到最优的相机位姿估计和特征坐标估计，由相机位姿估计实现目标的定位，利用特征坐标估计生成地图。/>

Images