融合中期语义信息的视觉里程计方法

1.本发明涉及基于视觉的智能车辆定位与导航领域，具体地说，涉及一种融合语义特征中期数据关联的智能车辆视觉里程计方法。


背景技术：

2.智能汽车是一类配置多种高精度传感器，涉及行驶环境感知、车辆导航定位、运动规划决策及车辆控制等多项技术，且可以在道路上自动安全行驶的移动终端。导航定位是汽车智能化的关键技术，其主要的作用在于估计车辆自身的姿态和位置，为驾驶任务中的路径规划及决策控制提供空间信息，是保证智能汽车实现无人驾驶的重要前提。里程计定位方法是依据传感器数据计算相邻两时刻间的位置变动以实现定位的重要方法。其中，摄像头是最常见，同时又成本低廉的传感器，使得视觉里程计得到了广泛而深入的研究。视觉里程计通过获取车载相机获取环境的图像序列，并根据图像间的多视图几何关系确定相邻时刻车辆位置姿态变换关系，从而得到车辆的位姿信息。现阶段视觉里程计的任务主要分为特征提取，数据关联，位姿估计三个部分。目前特征提取采用特征点法或采用直接法提取，采集到的特征是基于像素灰度关系得到的低级特征，随距离变化而变化，只能进行短期数据关联，如果位姿估计出现误差，则误差将逐渐积累，出现严重的位姿漂移现象。此外，低级特征在光照条件变化大、纹理特征缺失的环境中稳定性差，无法正确匹配建立可靠关联，导致帧间位姿估计误差增大，使得智能车辆的定位出现偏差。
3.而近年来人工智能技术快速发展，使计算机视觉中的很多关键问题都取得了革命性的进展，例如智能体对场景中的不同种类的物体之间的边界进行识别(语义分割)，这些技术使得智能体能够更加轻易地获取环境的语义特征，使得对环境的理解更加深刻。语义特征在形式上包括目标检测类别、语义分割像素级类别。语义分割像素级类别信息将场景类别精细到像素点，其和视觉里程计中的特征点具有相对一致的数据格式。该特征几乎不受距离影响，能长期保持稳定且在一定距离内都能被观测到，能间隔数帧建立数据关联。将当前帧与更早的关键帧建立起约束关系，从而减缓位姿估计的误差积累，改善里程计的轨迹漂移现象。此外，语义特征对光照条件变化、纹理信息依赖性小。因此，越来越多的研究者提出了将语义信息加入到视觉里程计的融合方法以提高定位精度。
4.研究人员提出了一种关键帧滑动窗口结构来建立语义误差边。该方法首先构建了语义误差边和orb特征重投影误差边，并构建了融合语义误差的视觉里程计，然后将距离当前关键帧最近的数帧关键帧加入滑动窗口，然后根据这些关键帧观测到的地图点构建与当前帧的特征关联。该方法的缺点在于滑动窗口中关键帧与当前关键帧之间的语义特征关联大多与基于orb特征的特征关联约束效果重复，使得系统定位精度性能提升不明显，无法建立合理有效的语义特征关联关系。
5.研究人员提出了将语义特征关联加入ba(bundle adjustment)优化的vso(visual semantic odometry)。该方法使用距离变换函数建模语义特征的似然观测模型，通过该观测模型建立了语义误差项，形成了融合语义的视觉里程计系统。该方法的缺点在于距离变
换函数计算前需要对语义分割图多次二值化操作，且距离变换函数本身需要计算全部像素点到所有类别之间的最近距离，计算效率低，似然观测模型的建模存在冗余。


技术实现要素：

6.本发明的目的在于解决现有技术中语义特征关联针对性弱和语义观测似然模型冗余的问题，提供一种融合中期语义信息的视觉里程计方法，该方法能够建立更高效的语义特征观测模型，形成更有针对性的语义误差项以提高融合语义的视觉里程计的位姿估计精度。
7.为了实现上述目的，本发明所采用的技术方案如下：
8.一种融合中期语义信息的视觉里程计方法，包括以下步骤：
9.s1.跟踪线程接收双目相机的双目图像流，并将单目图像传递给语义分割进程，语义分割进程对单目图像进行语义分割，并将语义分割图传递给跟踪线程；
10.s2.跟踪线程根据语义分割图构造当前帧并生成初始地图对双目相机位姿进行跟踪，将局部地图点与当前帧的特征点进行匹配，优化当前帧的位姿并确定关键帧，将关键帧信息及语义分割图传输至局部建图线程；
11.s3.局部建图线程根据关键帧的特征点更新地图点，并构建中期滑窗及更新中期滑窗的语义地图点，根据语义地图点建立中期约束并优化关键帧位姿；
12.s4.局部建图线程利用优化的位姿将地图点反向投影到语义分割图，更新语义地图点的语义信息，并根据语义地图点的质量信息管理语义地图点。
13.进一步地，步骤s1中，语义分割进程采用轻量级神经网络erfnet对单目图像进行语义分割，提取语义特征并将语义分割图传递给跟踪线程。
14.进一步地，步骤s2中，跟踪线程在图像金字塔中提取orb特征点，通过三角化初始化地图点，通过帧间的特征点匹配计算初始相机位姿，将局部地图点与当前帧中的特征点进行匹配，建立合适的数据关联，并运用最小化重投影误差优化当前帧位姿，根据当前帧的观测质量及系统的状态选择是否建立关键帧，若建立关键帧则将关键帧传递给局部建图线程。
15.进一步地，步骤s3中，构建中期滑窗及更新中期滑窗的语义地图点，根据语义地图点建立中期约束并优化关键帧位姿，具体为：
16.s31.将当前关键帧之前设定数量的关键帧作为管理地图点语义信息的滑窗容器，选取最开始的数帧作为中期滑窗，滑窗随时间不断更新；
17.s32.将中期滑窗中orb特征点三角化得到语义地图点；
18.s33.将语义地图点语义类别设计为多权重模式；
19.s34.将选取的语义地图点与当前关键帧建立语义观测，计算语义观测误差；
20.s35.将与当前关键帧有共视地图点的图像帧定义为局部关键帧，由局部关键帧中orb特征点三角化得到局部地图点；
21.s36.根据局部地图点与当前关键帧，计算重投影误差；
22.s37.将语义观测误差和重投影误差加入到局部ba优化的图优化模型中，优化关键帧的位姿及地图点。
23.进一步地，步骤s34中，将选取的语义地图点与当前关键帧建立语义观测，计算语
义观测误差，具体为：
24.s341.定义语义观测似然模型为
25.p(sk|zi＝c,xi,tk)；
26.式中，k为关键帧编号，c为一种语义类别，i为地图点编号，tk为车辆位姿，xi为地图点位置，zi为地图点类别，sk为当前帧的语义分割图；
27.s342.基于车辆位姿求出地图点在当前关键帧上投影的像素点坐标，u
i,k
代表地图点i的投影像素点坐标；
28.将观测似然对投影像素点坐标边缘化，根据贝叶斯公式表示为：
[0029][0030]
s343.当前帧的语义分割图sk独立于其他变量，投影像素点由地图点位置和车辆位姿决定，得到：
[0031][0032]
s344.假设语义分割图sk中每个类的先验概率相同，根据贝叶斯公式可将p(sk∣u
i,k
,zi＝c)表示为：
[0033]
p(sk|u
i,k
,zi＝c)
∝
p(u
i,k
,zi＝c|sk)＝p(u
i,k
|sk,zi＝c)p(zi＝c|sk)；
[0034]
s345.p(u
i,k
∣xi,tk)表示地图点重投影在当前关键帧的误差似然，在误差接受范围内该投影概率恒为1，得到：
[0035]
p(sk∣zi＝c,xi,tk)＝p(u
i,k
|sk,zi＝c)p(zi＝c|sk)；
[0036]
s346.p(ui,k|sk,zi＝c)表示给定地图点i为c类及第k帧语义分割图为sk时，地图点i投影得到的像素坐标为u
i,k
的概率，地图点的投影点离sk中的c类语义区域越近概率越大，采用剪枝优化的广度优先搜索近邻点算法，通过局部近邻距离表征该概率为：
[0037][0038]
式中，代表sk中c类像素点距离投影像素u
i,k
的最小距离，σ代表语义分割预测的像素偏移，α
k,c
是使得所有像素的概率和为1的归一因子；
[0039]
s347.p(zi＝c|sk)表示语义分割图sk中c类像素点出现的概率，使用给定语义类别下像素的高斯概率表示：
[0040][0041]
式中，αk是α
k,c
在不同c类别时的归一因子和；
[0042]
s348.得到语义观测误差为：
[0043][0044]
s349.指数概率在负对数下有较好的数学形式，将基于语义的位姿估计问题表示为：
[0045][0046]
其中，s代表系统中所有语义分割图，θ代表所有待优化地图点和车辆位姿，e
sem
(θ)代表所有的语义观测误差总和，得到
[0047][0048]
其中，π(tk,xi)代表位姿tk下地图点xi的投影像素点坐标，e
sem
(k,i)为第k帧位姿观测第i地图点形成的误差边，其与和语义分割可信度及近邻距离相关。
[0049]
进一步地，α
k,c
和αk的计算公式为：
[0050][0051]
进一步地，步骤s37中，将语义观测误差和重投影误差加入到局部ba优化的图优化模型中，优化关键帧的位姿及地图点，具体为：
[0052]
由于神经网络的预测存在一定的误差，地图点投影像素点可能会出现多个类，观测误差需要对多个类计算近邻距离，采用给定语义类别下像素的高斯概率表示地图点的类别权重，对语义观测误差进行带权求和的计算方法，考虑地图点类别权重的语义误差模型为：
[0053][0054]
其中，代表地图点i在c类别上的语义权重。
[0055]
进一步地，为减少语义特征误匹配的影响，系统剔除语义误差过大的误差边，并将优化分两阶段进行；
[0056]
第一阶段设置huber核函数限制过大的误差，求解图优化问题，然后根据跟踪线程运行状态选择是否进行第二阶段优化；
[0057]
第二阶段剔除语义误差较大的误差边，不再设置核函数，求解图优化问题。
[0058]
进一步地，步骤s4中，局部建图线程利用优化的位姿将地图点反向投影到语义分
割图，更新语义地图点的语义信息，具体为：
[0059]
利用关键帧优化后的位姿，将关键帧中的地图点反向投影到关键帧的语义分割图上；
[0060]
局部建图线程利用投影关系及语义分割图，更新关键帧中地图点的语义权重信息，计算公式为：
[0061][0062]
其中，α为归一因子，确保当局部建图线程中每增加一帧关键帧，地图点权重按照上式更新一次。
[0063]
进一步地，步骤s4中，根据语义地图点的质量信息管理语义地图点，具体为：
[0064]
如果地图点自生成以来不能被多帧图像连续观测到，则从语义地图点中剔除该地图点；
[0065]
如果地图点自生成以来经过多帧观测地图点语义权重依然分散，则从语义地图点中剔除该地图点。
[0066]
与现有技术相比，本发明具有如下优点和有益效果：
[0067]
1.本发明使用中期关键帧滑动窗口结构作为语义特征管理器，与之前的方法将采用距离当前关键帧最近的数帧关键帧建立特征关联相比，本发明可以建立更符合语义特征中期约束特点的语义特征关联，同等语义误差边数量的情况下估算出更精准的位姿。该方法选取的语义特征关联能够与orb特征的误差边形成优势互补，增强里程计的中期关联，可以有效减缓里程计位姿漂移现象，改善视觉里程计定位精度。
[0068]
2.本发明提出了一种基于局部近邻距离的语义特征观测似然模型，与之前的方法采用距离变换函数建模语义特征观测似然模型相比，本发明采用在地图投影点小范围区域内广度优先搜索部分近邻点的方法来计算局部近邻距离，可以省去建立语义误差项需要的图像二值化预处理操作，而且减少了语义特征观测似然模型的建模过程中没有意义的投影点距离计算，使语义特征观测似然模型更加简洁高效。此外，本发明只计算投影点最主要类别的近邻点简化了语义误差项的计算，而且保证了这种建模方法能够对位姿起到良好的约束作用，有效地提高了建立语义误差项的计算效率。
附图说明
[0069]
图1为本发明融合中期语义信息的视觉里程计方法的流程示意图。
[0070]
图2为本发明融合中期语义信息的视觉里程计方法的具体流程示意图。
[0071]
图3为本发明融合中期语义信息的视觉里程计方法的部分示意框图。
dtardos.bags of binary wordsfor fast place recognition in imagesequences.ieee transactions on robotics,2012,28(5):1188-1197.]进行加速，匹配完成后同样再与局部地图匹配，最终优化得到当前帧位姿。如果跟踪失败，则进行重定位。
[0084]
跟踪完成后，根据当前帧的观测质量及系统的状态选择是否将当前帧选作关键帧。如果生成关键帧，系统将数据存储在关键帧的数据结构中，传给局部建图线程。
[0085]
请参阅图2和图3，步骤s3中，构建中期滑窗及更新中期滑窗的语义地图点，根据语义地图点建立中期约束并优化关键帧位姿，具体为：
[0086]
s31.将当前关键帧之前设定数量的关键帧作为管理地图点语义信息的滑窗容器，选取最开始的数帧作为中期滑窗，滑窗随时间不断更新；
[0087]
s32.将中期滑窗中orb特征点三角化得到语义地图点；
[0088]
s33.将语义地图点语义类别设计为多权重模式；
[0089]
s34.将选取的语义地图点与当前关键帧建立语义观测，计算语义观测误差；
[0090]
s35.将与当前关键帧有共视地图点的图像帧定义为局部关键帧，由局部关键帧中orb特征点三角化得到局部地图点；
[0091]
s36.根据局部地图点与当前关键帧，计算重投影误差；
[0092]
s37.将语义观测误差和重投影误差加入到局部ba优化的图优化模型中，优化关键帧的位姿及地图点。
[0093]
步骤s34中，将选取的语义地图点与当前关键帧建立语义观测，计算语义观测误差，具体为：
[0094]
s341.定义语义观测似然模型为
[0095]
p(sk|zi＝c,xi,tk)；
[0096]
式中，k为关键帧编号，c为一种语义类别，i为地图点编号，tk为车辆位姿，xi为地图点位置，zi为地图点类别，sk为当前帧的语义分割图；
[0097]
s342.基于车辆位姿求出地图点在当前关键帧上投影的像素点坐标，u
i,k
代表地图点i的投影像素点坐标；
[0098]
将观测似然对投影像素点坐标边缘化，根据贝叶斯公式表示为：
[0099][0100]
s343.当前帧的语义分割图sk独立于其他变量，投影像素点由地图点位置和车辆位姿决定，得到：
[0101][0102]
s344.假设语义分割图sk中每个类的先验概率相同，根据贝叶斯公式可将p(sk∣u
i,k
,zi＝c)表示为：
[0103]
p(sk|u
i,k
,zi＝c)
∝
p(u
i,k
,zi＝c|sk)＝p(u
i,k
|sk,zi＝c)p(zi＝c|sk)；
[0104]
s345.p(u
i,k
∣xi,tk)表示地图点重投影在当前关键帧的误差似然，在误差接受范围内该投影概率恒为1，得到：
[0105]
p(sk∣zi＝c,xi,tk)＝p(u
i,k
|sk,zi＝c)p(zi＝c|sk)；
[0106]
s346.p(ui,k|sk,zi＝c)表示给定地图点i为c类及第k帧语义分割图为sk时，地图
点i投影得到的像素坐标为u
i,k
的概率，地图点的投影点离sk中的c类语义区域越近概率越大，采用剪枝优化的广度优先搜索近邻点算法，通过局部近邻距离表征该概率为：
[0107][0108]
式中，代表sk中c类像素点距离投影像素u
i,k
的最小距离，σ代表语义分割预测的像素偏移，α
k,c
是使得所有像素的概率和为1的归一因子；
[0109]
s347.p(zi＝c|sk)表示语义分割图sk中c类像素点出现的概率，使用给定语义类别下像素的高斯概率表示：
[0110][0111]
式中，αk是α
k,c
在不同c类别时的归一因子和；
[0112]
s348.得到语义观测误差为：
[0113][0114]
式中，代表sk中c类像素点距离投影像素u
i,k
的最小距离，σ代表语义分割预测的像素偏移，α
k,c
是使得所有像素的概率和为1的归一因子，αk是α
k,c
在不同c类别时的归一因子和；
[0115]
s349.指数概率在负对数下有较好的数学形式，将基于语义的位姿估计问题表示为：
[0116][0117]
其中，s代表系统中所有语义分割图，θ代表所有待优化地图点和车辆位姿，e
sem
(θ)代表所有的语义观测误差总和，得到
[0118][0119]
其中，π(tk,xi)代表位姿tk下地图点xi的投影像素点坐标，e
sem
(k,i)为第k帧位姿观测第i地图点形成的误差边，其与和语义分割可信度及近邻距离相关。
[0120]
α
k,c
和αk的计算公式为：
[0121][0122]
步骤s37中，将语义观测误差和重投影误差加入到局部ba优化的图优化模型中，优化关键帧的位姿及地图点，具体为：
[0123]
由于神经网络的预测存在一定的误差，地图点投影像素点可能会出现多个类，观测误差需要对多个类计算近邻距离，采用给定语义类别下像素的高斯概率表示地图点的类别权重，对语义观测误差进行带权求和的计算方法，考虑地图点类别权重的语义误差模型为：
[0124][0125]
其中，代表地图点i在c类别上的语义权重。
[0126]
为减少语义特征误匹配的影响，系统剔除语义误差过大的误差边，并将优化分两阶段进行；
[0127]
第一阶段设置huber核函数限制过大的误差，求解图优化问题，然后根据跟踪线程运行状态选择是否进行第二阶段优化；
[0128]
第二阶段剔除语义误差较大的误差边，不再设置核函数，求解图优化问题。
[0129]
局部建图线程获得关键帧之后，首先将新的关键帧加入局部地图中，从当前关键帧特征点的描述子计算其对应的词袋向量，遍历当前帧中的所有地图点，更新地图点的法向量、描述子等信息。如果当前地图点不是来自当前帧的观测(来自局部地图)，则为当前地图点添加观测，更新地图点的平均观测方向、观测距离范围和最佳描述子。如果当前地图点是双目相机跟踪过程中新生成的地图点，则将当前地图点放入近期新增地图点队列中，等待后续步骤的检验。更新地图点的法向量、描述子等信息后，将新的关键帧加入共视图中，更新有共视关系的关键帧的边，然后将关键帧插入地图中。
[0130]
新的关键帧加入局部地图后，遍历检查新添加的地图点，如果地图点能被观测到(有特征点匹配到)的帧数量与地图点能反向投影到普通帧图像范围的帧数量的比例小于25％，或从该地图点建立开始到现在已经过了不小于2个关键帧但观测到该点的帧数却不超过阈值，则认定该地图点质量不好并将其从地图中剔除。
[0131]
剔除质量不好的新增地图点后，在当前关键帧的共视关键帧中找到共视程度最高的相邻关键帧，然后在两关键帧之间搜索特征匹配对并用极线约束剔除误匹配，最终三角化生成新的地图点。
[0132]
三角化生成新的地图点后，获得当前关键帧在共视图中两级相邻关键帧，将当前帧的地图点分别投影到两级相邻关键帧并寻找匹配点，然后将两级相邻关键帧地图点分别投影到当前关键帧，寻找匹配点对应的地图点进行融合。最后更新当前帧地图点的描述子、深度、平均观测方向等属性，更新当前帧与其它帧的共视连接关系。
[0133]
根据本次进入局部建图线程的新生成关键帧的数量向前移动相应距离的关键帧滑动窗口。选取滑动窗口中最早的5帧关键帧作为中期滑窗关键帧，并依据新增中期滑窗关键帧和被移除的中期滑窗关键帧的地图点观测情况增减中期滑窗内的语义地图点观测次
数。如果语义地图点观测次数为零，将其从语义地图点中剔除，不断更新滑窗和语义地图。
[0134]
更新完中期滑窗和语义地图点后，将地图点的语义类别设计为多权重模式，对语义观测误差进行带权求和的计算方法：
[0135][0136]
在局部建图线程的融合中期语义约束位姿优化中，系统通过对局部地图点向对应的局部关键帧做重投影，计算出重投影误差，然后，通过对语义地图点向最新关键帧语义分割图做反向投影得到语义误差e
sem
(k,i)，最后将两种误差边加入到在局部ba优化的图优化模型中，进行位姿优化；为减少语义特征误匹配的影响，系统剔除语义误差过大的误差边，并将优化分两阶段进行：第一段优化设置huber核函数限制过大的误差，求解图优化问题，然后根据跟踪线程运行状态选择是否进行优化；第二段优化剔除图优化模型中语义误差较大的误差边，不再设置核函数，求解图优化问题。
[0137]
请参阅图2和图3，步骤s4中，局部建图线程利用优化的位姿将地图点反向投影到语义分割图，更新语义地图点的语义信息，具体为：
[0138]
利用关键帧优化后的位姿，将关键帧中的地图点反向投影到关键帧的语义分割图上；
[0139]
局部建图线程利用投影关系及语义分割图，更新关键帧中地图点的语义权重信息，计算公式为：
[0140][0141]
其中，α为归一因子，确保
[0142]
当局部建图线程中每增加一帧关键帧，地图点权重按照上式更新一次。然后如果地图点存在5帧以上且不能被连续4帧关键帧观测到，则从语义地图点中剔除该地图点；如果地图点存在5帧以上且至少能被连续4帧关键帧观测到但地图点语义权重依然分散(地图点最大类别权重依然小于0.75)，则从语义地图点中剔除该地图点。
[0143]
语义地图点是根据特征点三角化得到的，这些特征点三角化过程存在误差，需要设定一个筛选方法。语义地图点的语义信息是根据神经网络预测得到的，这个过程存在一定的误差，为了减少误差大的不可信的点，设定了一个筛选原则。
[0144]
根据语义地图点的质量信息管理语义地图点，具体为：
[0145]
第一个筛选原则：如果地图点存在5帧以上且不能被连续4帧关键帧观测到，则从语义地图点中剔除该地图点。
[0146]
第二个筛选原则：如果地图点存在5帧以上且至少能被连续4帧关键帧观测到但地图点语义权重依然分散(地图点最大类别权重依然小于0.75)，则从语义地图点中剔除该地
图点。
[0147]
为了进一步展示本发明的实施效果，本发明采用kitti数据集进行实验，验证本发明的定位精度。算法实验平台参数配置如下：软件环境ubuntu 16.04；处理器(cpu)为intel core i7-9750h，6核主频2.6ghz；内存16gb；显卡nvidia gtx 1660ti；并行计算框架为cuda10.1；深度学习框架为pytorch；第三方库为opencv 3.4.3，g2o等。系统中的误差权重λ和神经网络预测可信度σ在每组实验中根据经验选取。
[0148]
为了验证系统的定位精度，本发明的参照系统筛选最新关键帧之前的设定数量关键帧中地图点建立语义约束，被称为cvso。本发明选用相对位姿误差(relative pose error,rpe)和绝对轨迹误差(absolute trajectory error,ate)的均方根误差(rmse)作为评价标准。ape测量两条轨迹上位姿的绝对偏差值。实验结果如表1所示。
[0149]
表1 kitti数据集上的绝对轨迹误差(ate)
[0150][0151]
实验中除05序列以外其他序列对绝对轨迹的估计精度均有所提高，绝对轨迹误差平均下降11.37％，最大可达16.29％。
[0152]
rpe计算相同距离(时间)间隔内估计位姿变化量与真实变化量的偏差，以获取相对位姿误差，可用于评估里程计的位姿漂移现象。实验评估每100米距离的相对位姿平移误差，实验结果如表2所示。
[0153]
表2 kitti数据集上的相对位姿误差(rpe)
[0154][0155]
由于语义约束可以提供中期约束，以减少里程计位姿漂移，相较于cvso，lvso的rpe平均下降1.67％，最大可达4.76％，采用中期滑窗建立语义约束对位姿漂移现象有一定的改善作用。
[0156]
综上所述，本发明具有如下优点和有益效果：
[0157]
1.本发明使用中期关键帧滑动窗口结构作为语义特征管理器，与之前的方法将采用距离当前关键帧最近的数帧关键帧建立特征关联相比，本发明可以建立更符合语义特征中期约束特点的语义特征关联，同等语义误差边数量的情况下估算出更精准的位姿。该方法选取的语义特征关联能够与orb特征的误差边形成优势互补，增强里程计的中期关联，可以有效减缓里程计位姿漂移现象，改善视觉里程计定位精度。
[0158]
2.本发明提出了一种基于局部近邻距离的语义特征观测似然模型，与之前的方法采用距离变换函数建模语义特征观测似然模型相比，本发明采用在地图投影点小范围区域内广度优先搜索部分近邻点的方法来计算局部近邻距离，可以省去建立语义误差项需要的图像二值化预处理操作，而且减少了语义特征观测似然模型的建模过程中没有意义的投影点距离计算，使语义特征观测似然模型更加简洁高效。此外，本发明只计算投影点最主要类别的近邻点简化了语义误差项的计算，而且保证了这种建模方法能够对位姿起到良好的约束作用，有效地提高了建立语义误差项的计算效率。
[0159]
上述说明是针对本发明较佳可行实施例的详细说明，但实施例并非用以限定本发明的专利申请范围，凡本发明所揭示的技术精神下所完成的同等变化或修饰变更，均应属于本发明所涵盖专利范围。

技术特征：
1.一种融合中期语义信息的视觉里程计方法，其特征在于，包括以下步骤：s1.跟踪线程接收双目相机的双目图像流，并将单目图像传递给语义分割进程，语义分割进程对单目图像进行语义分割，并将语义分割图传递给跟踪线程；s2.跟踪线程根据语义分割图构造当前帧并生成初始地图对双目相机位姿进行跟踪，将局部地图点与当前帧的特征点进行匹配，优化当前帧的位姿并确定关键帧，将关键帧信息及语义分割图传输至局部建图线程；s3.局部建图线程根据关键帧的特征点更新地图点，并构建中期滑窗及更新中期滑窗的语义地图点，根据语义地图点建立中期约束并优化关键帧位姿；s4.局部建图线程利用优化的位姿将地图点反向投影到语义分割图，更新语义地图点的语义信息，并根据语义地图点的质量信息管理语义地图点。2.根据权利要求1所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s1中，语义分割进程采用轻量级神经网络erfnet对单目图像进行语义分割，提取语义特征并将语义分割图传递给跟踪线程。3.根据权利要求1所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s2中，跟踪线程在图像金字塔中提取orb特征点，通过三角化初始化地图点，通过帧间的特征点匹配计算初始相机位姿，将局部地图点与当前帧中的特征点进行匹配，建立合适的数据关联，并运用最小化重投影误差优化当前帧位姿，根据当前帧的观测质量及系统的状态选择是否建立关键帧，若建立关键帧则将关键帧传递给局部建图线程。4.根据权利要求1所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s3中，构建中期滑窗及更新中期滑窗的语义地图点，根据语义地图点建立中期约束并优化关键帧位姿，具体为：s31.将当前关键帧之前设定数量的关键帧作为管理地图点语义信息的滑窗容器，选取最开始的数帧作为中期滑窗，滑窗随时间不断更新；s32.将中期滑窗中orb特征点三角化得到语义地图点；s33.将语义地图点语义类别设计为多权重模式；s34.将选取的语义地图点与当前关键帧建立语义观测，计算语义观测误差；s35.将与当前关键帧有共视地图点的图像帧定义为局部关键帧，由局部关键帧中orb特征点三角化得到局部地图点；s36.根据局部地图点与当前关键帧，计算重投影误差；s37.将语义观测误差和重投影误差加入到局部ba优化的图优化模型中，优化关键帧的位姿及地图点。5.根据权利要求4所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s34中，将选取的语义地图点与当前关键帧建立语义观测，计算语义观测误差，具体为：s341.定义语义观测似然模型为p(s
k
|z
i
＝c,x
i
,t
k
)；式中，k为关键帧编号，c为一种语义类别，i为地图点编号，t
k
为车辆位姿，x
i
为地图点位置，z
i
为地图点类别，s
k
为当前帧的语义分割图；s342.基于车辆位姿求出地图点在当前关键帧上投影的像素点坐标，u
i,k
代表地图点i的投影像素点坐标；
将观测似然对投影像素点坐标边缘化，根据贝叶斯公式表示为：s343.当前帧的语义分割图s
k
独立于其他变量，投影像素点由地图点位置和车辆位姿决定，得到：s344.假设语义分割图s
k
中每个类的先验概率相同，根据贝叶斯公式可将p(s
k
∣u
i,k
,z
i
＝c)表示为：p(s
k
|u
i,k
,z
i
＝c)
∝
p(u
i,k
,z
i
＝c|s
k
)＝p(u
i,k
|s
k
,z
i
＝c)p(z
i
＝c|s
k
)；s345.p(u
i,k
∣x
i
,t
k
)表示地图点重投影在当前关键帧的误差似然，在误差接受范围内该投影概率恒为1，得到：p(s
k
∣z
i
＝c,x
i
,t
k
)＝p(u
i,k
|s
k
,z
i
＝c)p(z
i
＝c|s
k
)；s346.p(u
i,k
|s
k
,z
i
＝c)表示给定地图点i为c类及第k帧语义分割图为s
k
时，地图点i投影得到的像素坐标为u
i,k
的概率，地图点的投影点离s
k
中的c类语义区域越近概率越大，采用剪枝优化的广度优先搜索近邻点算法，通过局部近邻距离表征该概率为：式中，代表s
k
中c类像素点距离投影像素u
i,k
的最小距离，σ代表语义分割预测的像素偏移，α
k,c
是使得所有像素的概率和为1的归一因子；s347.p(z
i
＝c|s
k
)表示语义分割图s
k
中c类像素点出现的概率，使用给定语义类别下像素的高斯概率表示：式中，α
k
是α
k,c
在不同c类别时的归一因子和；s348.得到语义观测误差为：s349.指数概率在负对数下有较好的数学形式，将基于语义的位姿估计问题表示为：
其中，s代表系统中所有语义分割图，θ代表所有待优化地图点和车辆位姿，e
sem
(θ)代表所有的语义观测误差总和，得到其中，π(t
k
,x
i
)代表位姿t
k
下地图点x
i
的投影像素点坐标，e
sem
(k,i)为第k帧位姿观测第i地图点形成的误差边，其与和语义分割可信度及近邻距离相关。6.根据权利要求5所述的融合中期语义信息的视觉里程计方法，其特征在于，α
k,c
和α
k
的计算公式为：7.根据权利要求5所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s37中，将语义观测误差和重投影误差加入到局部ba优化的图优化模型中，优化关键帧的位姿及地图点，具体为：由于神经网络的预测存在一定的误差，地图点投影像素点可能会出现多个类，观测误差需要对多个类计算近邻距离，采用给定语义类别下像素的高斯概率表示地图点的类别权重，对语义观测误差进行带权求和的计算方法，考虑地图点类别权重的语义误差模型为：其中，代表地图点i在c类别上的语义权重。8.根据权利要求7所述的融合中期语义信息的视觉里程计方法，其特征在于，为减少语义特征误匹配的影响，系统剔除语义误差过大的误差边，并将优化分两阶段进行；第一阶段设置huber核函数限制过大的误差，求解图优化问题，然后根据跟踪线程运行状态选择是否进行第二阶段优化；第二阶段剔除语义误差较大的误差边，不再设置核函数，求解图优化问题。9.根据权利要求5所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s4中，局部建图线程利用优化的位姿将地图点反向投影到语义分割图，更新语义地图点的语义信息，具体为：利用关键帧优化后的位姿，将关键帧中的地图点反向投影到关键帧的语义分割图上；局部建图线程利用投影关系及语义分割图，更新关键帧中地图点的语义权重信息，计算公式为：
其中，α为归一因子，确保当局部建图线程中每增加一帧关键帧，地图点权重按照上式更新一次。10.根据权利要求1所述的融合中期语义信息的视觉里程计方法，其特征在于，步骤s4中，根据语义地图点的质量信息管理语义地图点，具体为：如果地图点自生成以来不能被多帧图像连续观测到，则从语义地图点中剔除该地图点；如果地图点自生成以来经过多帧观测地图点语义权重依然分散，则从语义地图点中剔除该地图点。

技术总结
本发明公开了一种融合中期语义信息的视觉里程计方法，包括以下步骤：S1.跟踪线程接收双目图像流，并将单目图像传递给语义分割进程，语义分割进程对单目图像进行语义分割，将语义分割图传递给跟踪线程；S2.跟踪线程根据语义分割图构造当前帧并生成初始地图，将局部地图点与当前帧的特征点进行匹配，优化当前帧的位姿并确定关键帧，将关键帧信息及语义分割图传输至局部建图线程；S3.局部建图线程根据关键帧的特征点更新地图点，并构建中期滑窗及更新中期滑窗的语义地图点，根据语义地图点建立中期约束并优化关键帧位姿；S4.局部建图线程利用优化的位姿将地图点反向投影到语义分割图，更新语义地图点的语义信息，并根据语义地图点的质量管理语义地图点。地图点的质量管理语义地图点。地图点的质量管理语义地图点。


技术研发人员：兰凤崇 姚司宇 陈吉清
受保护的技术使用者：华南理工大学
技术研发日：2022.11.18
技术公布日：2023/7/11