CN111950370B - 动态环境下线拓展视觉里程计方法 - Google Patents

Abstract

本发明请求保护一种动态环境下线拓展视觉里程计方法。具体步骤为：首先在图像序列中提取点特征计算初始位姿矩阵，根据初始位姿矩阵得到预测当前帧图像。通过相邻两帧图像和预测前帧图像进行光流和残差值计算，对动态特征点进行剔除。根据剩余静态特征点的共线关系进行静态直线拓展，构建共线匹配矩阵。通过剩余的静态特征点和静态直线特征进行BA优化，对相机位姿进行调整。在动态环境中的实验结果表明：相较于其他方法，本方法能有效降低跟踪误差，提高视觉里程计精度和鲁棒性，减少静态直线识别提取时间和静态直线匹配时间，满足实时应用的需求。

Description

动态环境下线拓展视觉里程计方法

技术领域

本发明属于移动机器人自主导航领域，特别是动态环境下线拓展视觉里程计方法。

背景技术

视觉里程计是利用单个或多个视觉传感器，通过依靠图像序列相邻时刻图像之间的匹配关系来估计载体的位姿。视觉传感器因相比激光传感器获取的环境信息丰富、易得、成本低等优点成为机器人领域的一个重要研究方向和研究热点。

然而为简化定位和建图问题，目前大部分视觉里程计方法假设环境是静态的，当动态物体在图像中占比较小可当噪声处理，而当动态物体在图像中占比较大时，此方法不适用，限制了视觉里程计的使用。如在基于特征视觉里程计中，通常使用随机抽样一致性(RANSAC)算法减少噪声和错误匹配对位姿估计的影响，然而当图像中的动态特征点数量超过静态特征点时，通过RANSAC难以将静态特征点作为噪声剔除，对位姿估计带来误差,使视觉里程计性能下降。因此如何有效区分图像中的动静态特征，成为动态场景中视觉里程计的研究重点。

线特征在结构化环境中较为丰富、受环境影响小，且与特征点相比，包含更丰富的环境结构信息。因此线特征开始广泛应用于视觉里程计中。点特征和直线特征的结合对相机位姿进行估计，避免了视觉里程计在低纹理情况下特征点过少而导致的跟踪误差增大。但由于直线特征相较于特征点结构更高级，其描述符也更复杂，在匹配直线时其计算量相较于点匹配要大得多，对系统的实时性是一个挑战。同时，在动态环境下，不可避免要对动态直线特征进行识别剔除，使系统更难满足实时要求。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种提高视觉里程计在动态环境下的精度和鲁棒性，同时满足实时性要求的动态环境下线拓展视觉里程计方法。本发明的技术方案如下：

一种动态环境下线拓展视觉里程计方法，对动态物体上的动态特征点进行识别剔除，通过静态特征点的共线关系进行静态直线拓展，具体包括以下步骤：

S1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；

S2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；

S3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；

S4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；

S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。

进一步的，所述步骤S1获取机器人的帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，根据初始位姿矩阵得到预测当前帧图像，具体包括：

S11：在t时刻，机器人通过图像U_t提取快速特征点提取和描述(ORB)特征点，通过特征点匹配得到初始位姿估计θ＝(R,T)，R为旋转矩阵，T为平移矩阵；

S12：根据t时刻得到的初始位姿估计θ_t＝(R,T)和上一时刻相机观测的地图点X_t-1，得到t时刻相机观测的预测地图点

将t时刻相机观测到的预测地图点

投影到t时刻图像上得到t时刻预测图像

其中，Π代表把地图点投影到图像上操作；T表示位移矩阵，x、y表示在t时刻图像上特征点的坐标，z表示特征点对应的深度。

进一步的，所述步骤S2通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除，具体包括：

S13：根据t时刻图像U_t和上一时刻图像U_t-1计算光流，t时刻预测图像U_t和上一时刻图像U_t-1计算预测光流，公式如下：

表示t-1时刻到t时刻的光流

S14：根据光流，预测光流和深度值构建残差矩阵M，公式如下：

S15：得到残差值d＝∑M^TM，只有当特征点的残差值小于某一阈值ξ时，就可判定其为静态特征点，反之则为动态特征点。

进一步的，所述S3以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展，具体包括：

S21：关键帧插入时，使用直线检测算法(LSD)，计算每个像素的level-line angle以构成一个level-line场，然后level-line场分割成若干个连通的直线矩形区域，得到当前关键帧的直线线段图像；

S22：共视关键帧生成的地图点都将投影到当前关键帧直线线段图像上，同时对部分不符合要求的投影点进行剔除：

S23：地图点经过上述筛选的特征点投影在LSD直线线段图像后，系统将读取特征点投影在线段图像上的灰度值，如果为0，意味着这个点没有投影到直线特征的矩形连通区域内。如果不是，则该点投影到直线特征的矩形连通区域内；

S24：在得到LSD直线线段时，还得到每条直线线段的总体Level-Line角度，根据特征点的Level-line角度和投影在该直线线段连通区域的总体Level-Line角度差值进行比较，若该像素点的Level-line角度与该连通区域的总体Level-line角度差值小于某一阈值τ，则可表示该特征点在LSD直线线段上；

S25：将直线线段集合L_i中投影到线段矩形区域内地图点X_i小于2个的直线线段进行剔除，最后得到拓展的静态直线线段；

S26：搜索了所有静态特征点的共线的关系完成静态直线拓展后，如果在第i个关键帧上的直线线段集合L_i中检测到矩形区域内有两个或多个特征点的直线线段l_i,j，并且在第m个关键帧上的线段集合L_m中找到相应的特征点的直线l_m,n，则表示这两个直线线段是匹配，生成一个共线匹配矩阵来存储共线线段的匹配关系。

进一步的，所述步骤S22不符合要求的投影点进行剔除具体包括；1.投影在线段图像之外；2.当前关键帧视图射线v与地图点云平均视图方向n的夹角超过60度(v·n＜cos60°)；3.地图点到相机中心距离不在地图点云的尺度不变区间内；

进一步的，所述步骤S24LSD矩形连通区域的总体Level-Line角度如式所示：

sin(level-line_angle_j)、cos(level-line_angle_j)分别第j个像素处的Level-line角度的cos和sin值。

矩形连通区域内的某像素点的Level-Line方向跟该点的梯度方向垂直，图像中某像素点的梯度向量norm如式所示；

其中

和

分别表示在行方向和列方向的一阶偏导。

进一步的，所述S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差，具体包括：

S31：基于特征点的重投影误差，X_j∈R³为第j个地图点，在第i个关键帧上，这点投影在像平面上可表示为：

其中K是相机内参矩阵，θ_i＝(R_i,t_i)代表第i个关键帧的位姿，其点的重投影误差可定义为：

S32：基于线段的重投影误差，给定线段L，p,q∈R³为线段上的两个随机点，对直线L进行参数化，p^h，q^h为p，q在像平面投影的齐次坐标，由它们可得的归一化直线系数为：

其中p^h＝π(p,θ,K)，q^h＝π(q,θ,K)，π表示将地图点投影到像平面的操作，K是相机内参矩阵，θ＝(R,T)代表位姿，R为旋转矩阵，T为平移矩阵；

p_i,j,q_i,j∈R³为第i个关键帧检测到的第j个直线线段上的两个随机点，它们在像平面投影的其次坐标

为：

a_i,j,b_i,j代表第i个关键帧检测到的第j个直线线段的端点，线段的重投影误差为直线l_i,j两端点a_i,j,b_i,j的重投影误差

之和：

其中l_i,j表示第i个关键帧检测到的第j个直线线段。

S33：基于共线关系的重投影误差，X_n为属于直线线段l＝{X₁,X₂,…,X_m}上的地图点，p_l,q_l∈R³为直线l上的两个随机点，其共线误差定义为：

S34：光束平差法的代价函数C为：

其中N表示线段上的特征点个数，若N大于等于阈值ξ时，最终光束平差法的代价函数C可只计算线投影误差和共线投影误差；小于阈值ξ，则加上ORB-SLAM自带的点投影误差。ρ代表鲁棒的Huber函数，

代表点特征得信息矩阵，

分别代表线段两端点的信息矩阵，

代表共线误差的信息矩阵。

进一步的，所述S5通过静态特征点和静态直线进行BA优化对位姿进行调整，当得到光束平差法的代价函数C后，通过最小二乘法求解，对机器人位姿进行调整，提高机器人在动态环境下的鲁棒性和精度。

本发明的优点及有益效果如下：

本发明针对目前大部分视觉里程计方法假设环境是静态的，当动态物体在图像中占比较大时，跟踪误差增大，限制了视觉里程计的使用的问题，提出动态环境下线拓展视觉里程计方法。该方法采用了光流方法。根据机器人当前时刻获取的图像提取ORB特征点进行初始位姿估计，根据初始位姿和上一时刻相机观测到的地图点计算当前时刻预测图像预测，根据当前时刻图像和上一时刻图像光流、当前时刻预测图像和上一时刻图像预测光流和深度值构成残差矩阵，通过残差矩阵求得残差值，根据残差值对动态目标上的动态特征点进行剔除。同时，对动态特征点的剔除，意味着可得静态特征点的减少，当动态物体在图像中占比较大时，得到的静态特征点过少，会增大视觉里程计的跟踪误差。因此，本发明在得到静态特征点的基础上，通过LSD直线检测寻找静态特征点的共线关系进行静态直线拓展，同时根据直线线段匹配特征点大于2个的构建共线匹配矩阵。最后根据静态特征点和静态直线进行BA优化，即对基于特征点的重投影误差、基于直线线段的重投影误差和基于共线关系的重投影误差通过最小二乘法求解，对位姿进行调整。提高视觉里程计在动态环境下的鲁棒性和精度。因为通过特征点的共线关键进行静态直线拓展和匹配，相较于直接对动态直线线段进行识别剔除，和静态直线匹配，节省了大量时间，使视觉里程计基本满足实时性要求。

附图说明

图1是本发明提供优选实施例动态环境下线拓展视觉里程计实现框架；

图2为动态特征点识别剔除过程；

图3为不同关键帧下特征点的共线关系示意图；

图4为基于线段的重投影误差示意图；

图5为基于共线关系的重投影示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

本方法的主要步骤为：

S1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；

S2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；

S3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；

S4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；

S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。

步骤S1的具体实施过程为：

S11：t时刻，机器人通过图像U_t提取快速特征点提取和描述(ORB)特征点，通过特征点匹配得到初始位姿估计θ＝(R,T)，R为旋转矩阵，T为平移矩阵；

S12：根据t时刻得到的初始位姿估计θ_t＝(R,T)和上一时刻相机观测的地图点X_t-1，可得到t时刻相机观测的预测地图点

将t时刻相机观测到的预测地图点

投影到t时刻图像上得到t时刻预测图像

其中，Π代表把地图点投影到图像上操作。T表示位移矩阵，x、y表示在t时刻图像上特征点的坐标，z表示特征点对应的深度。

S13：根据t时刻图像U_t和上一时刻图像U_t-1计算光流，t时刻预测图像U_t和上一时刻图像U_t-1计算预测光流，公式如下：

S14：根据光流，预测光流和深度值构建残差矩阵M，公式如下：

S15：得到残差值d＝∑M^TM，只有当特征点的残差值小于某一阈值ξ时，就可判定其为静态特征点，反之则为动态特征点。

步骤S2的具体实施过程为：

S21：关键帧插入时，使用直线检测算法(LSD)，计算每个像素的level-line angle以构成一个level-line场，然后level-line场分割成若干个连通的直线矩形区域，得到当前关键帧的直线线段图像；

S22：共视关键帧生成的地图点都将投影到当前关键帧直线线段图像上，同时对以下情况的投影点进行剔除：1.投影在线段图像之外。2.当前关键帧视图射线v与地图点云平均视图方向n的夹角超过60度(v·n＜cos60°)。3.地图点到相机中心距离不在地图点云的尺度不变区间内；

S23：地图点经过上述筛选的特征点投影在LSD直线线段图像后，系统将读取特征点投影在线段图像上的灰度值，如果为0，意味着这个点没有投影到直线特征的矩形连通区域内。如果不是，则该点投影到直线特征的矩形连通区域内；

S24：在得到LSD直线线段时，还可以得到每条直线线段的总体Level-Line角度，根据特征点的Level-line角度和投影在该直线线段连通区域的总体Level-Line角度差值进行比较，若该像素点的Level-line角度与该连通区域的总体Level-line角度差值小于某一阈值τ，则可表示该特征点在LSD直线线段上。

其中LSD矩形连通区域的总体Level-Line角度如式所示。

其中sin(level-line_angl_je)、cos(level-line_angle_j)分别第j个像素处的Level-line角度的cos和sin值。

矩形连通区域内的某像素点的Level-Line方向跟该点的梯度方向垂直，图像中某像素点的梯度向量norm如式所示。

其中

和

分别表示在行方向和列方向的一阶偏导。

S25：将直线线段集合L_i中投影到线段矩形区域内地图点X_i小于2个的直线线段进行剔除，最后得到拓展的静态直线线段；

S26：搜索了所有静态特征点的共线的关系完成静态直线拓展后，如果在第i个关键帧上的直线线段集合L_i中检测到矩形区域内有两个或多个特征点的直线线段l_i,j，并且在第m个关键帧上的线段集合L_m中找到相应的特征点的直线l_m,n，则表示这两个直线线段是匹配，生成一个共线匹配矩阵来存储共线线段的匹配关系。

步骤S3的具体实施过程为：

S31：基于特征点的重投影误差，X_j∈R³为第j个地图点，在第i个关键帧上，这点投影在像平面上可表示为：

其中K是相机内参矩阵，θ_i＝(R_i,t_i)代表第i个关键帧的位姿，其点的重投影误差可定义为：

S32：基于线段的重投影误差，给定线段L，p,q∈R³为线段上的两个随机点，对直线L进行参数化，p^h，q^h为p，q在像平面投影的齐次坐标，由它们可得的归一化直线系数为：

其中p^h＝π(p,θ,K)，q^h＝π(q,θ,K)，π表示将地图点投影到像平面的操作，K是相机内参矩阵，θ＝(R,T)代表位姿，R为旋转矩阵，T为平移矩阵。

p_i,j,q_i,j∈R³为第i个关键帧检测到的第j个直线线段上的两个随机点，它们在像平面投影的其次坐标

为：

a_i,j,b_i,j代表第i个关键帧检测到的第j个直线线段的端点，线段的重投影误差为直线l_i,j两端点a_i,j,b_i,j的重投影误差

之和为：

S33：基于共线关系的重投影误差，X_n为属于直线线段l＝{X₁,X₂,…,X_m}上的地图点，p_l,q_l∈R³为直线l上的两个随机点，其共线误差定义为：

S34：光束平差法的代价函数C为：

其中N表示线段上的特征点个数，若N大于等于阈值ξ时，最终光束平差法的代价函数C可只计算线投影误差和共线投影误差；小于阈值ξ，则加上ORB-SLAM自带的点投影误差。ρ代表鲁棒的Huber函数，

代表点特征得信息矩阵，

分别代表线段两端点的信息矩阵，

代表共线误差的信息矩阵。

步骤S4的具体实施过程为：

当得到光束平差法的代价函数C后，通过最小二乘法求解，对机器人位姿进行调整，提高机器人在动态环境下的鲁棒性和精度。

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (8)

1.一种动态环境下线拓展视觉里程计方法，其特征在于，对动态物体上的动态特征点进行识别剔除，通过静态特征点的共线关系进行静态直线拓展，具体包括以下步骤：

S1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；

S2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；

S3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；

S4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；

S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。

2.根据权利要求1所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S1 t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像，具体包括：

S11：t时刻，机器人通过图像U_t提取快速特征点提取和描述ORB特征点，通过特征点匹配得到初始位姿估计θ＝(R,T)，R为旋转矩阵，T为平移矩阵；

S12：根据t时刻得到的初始位姿估计θ_t＝(R,T)和上一时刻相机观测的地图点X_t-1，得到t时刻相机观测的预测地图点

将t时刻相机观测到的预测地图点

投影到t时刻图像上得到t时刻预测图像

其中，Π代表把地图点投影到图像上操作；T表示位移矩阵，x、y表示在t时刻图像上特征点的坐标，z表示特征点对应的深度。

3.根据权利要求2所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S2通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除，具体包括：

S13：根据t时刻图像U_t和上一时刻图像U_t-1计算光流，t时刻预测图像U_t和上一时刻图像U_t-1计算预测光流，公式如下：

表示t-1时刻到t时刻的光流

S14：根据光流，预测光流和深度值构建残差矩阵M，公式如下：

S15：得到残差值d＝∑M^TM，只有当特征点的残差值小于某一阈值λ时，就可判定其为静态特征点，反之则为动态特征点。

4.根据权利要求3所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述S3以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展，具体包括：

S21：关键帧插入时，使用直线检测算法(LSD)，计算每个像素的level-line angle以构成一个level-line场，然后level-line场分割成若干个连通的直线矩形区域，得到当前关键帧的直线线段图像；

S22：共视关键帧生成的地图点都将投影到当前关键帧直线线段图像上，同时对部分不符合要求的投影点进行剔除：

S23：地图点经过上述筛选的特征点投影在LSD直线线段图像后，系统将读取特征点投影在线段图像上的灰度值，如果为0，意味着这个点没有投影到直线特征的矩形连通区域内；如果不是，则该点投影到直线特征的矩形连通区域内；

S24：在得到LSD直线线段时，还得到每条直线线段的总体Level-Line角度，根据特征点的Level-line角度和投影在该直线线段连通区域的总体Level-Line角度差值进行比较，若该像素点的Level-line角度与该连通区域的总体Level-line角度差值小于某一阈值τ，则可表示该特征点在LSD直线线段上；

S25：将直线线段集合L_i中投影到线段矩形区域内地图点X_i小于2个的直线线段进行剔除，最后得到拓展的静态直线线段；

S26：搜索了所有静态特征点的共线的关系完成静态直线拓展后，如果在第i个关键帧上的直线线段集合L_i中检测到矩形区域内有两个或多个特征点的直线线段l_i,j，并且在第m个关键帧上的线段集合L_m中找到相应的特征点的直线l_m,n，则表示这两个直线线段是匹配，生成一个共线匹配矩阵来存储共线线段的匹配关系。

5.根据权利要求4所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S22不符合要求的投影点进行剔除具体包括；1.投影在线段图像之外；2.当前关键帧视图射线v与地图点云平均视图方向n的夹角超过60度，v·n＜cos60°；3.地图点到相机中心距离不在地图点云的尺度不变区间内。

6.根据权利要求4所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S24 LSD矩形连通区域的总体Level-Line角度如式所示：

sin(level-line_angle_j)、cos(level-line_angle_j)分别第j个像素处的Level-line角度的cos和sin值；

矩形连通区域内的某像素点的Level-Line方向跟该点的梯度方向垂直，图像中某像素点的梯度向量norm如式所示；

其中

和

分别表示在行方向和列方向的一阶偏导；

7.根据权利要求6所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差，具体包括：

S31：基于特征点的重投影误差，X_j∈R³为第j个地图点，在第i个关键帧上，这点投影在像平面上可表示为：

其中K是相机内参矩阵，θ_i＝(R_i,t_i)代表第i个关键帧的位姿，其点的重投影误差可定义为：

S32：基于线段的重投影误差，给定线段L，p,q∈R³为线段上的两个随机点，对直线L进行参数化，p^h，q^h为p，q在像平面投影的齐次坐标，由它们可得的归一化直线系数为：

其中p^h＝π(p,θ,K)，q^h＝π(q,θ,K)，π表示将地图点投影到像平面的操作，K是相机内参矩阵，θ＝(R,T)代表位姿，R为旋转矩阵，T为平移矩阵；

p_i,j,q_i,j∈R³为第i个关键帧检测到的第j个直线线段上的两个随机点，它们在像平面投影的其次坐标

为：

a_i,j,b_i,j代表第i个关键帧检测到的第j个直线线段的端点，线段的重投影误差为直线l_i,j两端点a_i,j,b_i,j的重投影误差

之和：

其中l_i,j表示第i个关键帧检测到的第j个直线线段，

S33：基于共线关系的重投影误差，X_n为属于直线线段l＝{X₁,X₂,…,X_m}上的地图点，p_l,q_l∈R³为直线l上的两个随机点，其共线误差定义为：

S34：光束平差法的代价函数C为：

其中N表示线段上的特征点个数，若N大于等于阈值ξ时，最终光束平差法的代价函数C可只计算线投影误差和共线投影误差；小于阈值ξ，则加上ORB-SLAM自带的点投影误差；ρ代表鲁棒的Huber函数，

代表点特征得信息矩阵，

分别代表线段两端点的信息矩阵，

代表共线误差的信息矩阵。

8.根据权利要求7所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述S5通过静态特征点和静态直线进行BA优化对位姿进行调整，当得到光束平差法的代价函数C后，通过最小二乘法求解，对机器人位姿进行调整，提高机器人在动态环境下的鲁棒性和精度。

Images