[发明专利]基于语义平面的单目即时定位与致密语义地图构建方法

权利要求书

1.一种基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，所述方法包括以下步骤：

步骤1，当新一帧图像出现时，首先检测新一帧图像中的特征点，然后判断系统是否已初始化；若未初始化则进行初始化，建立最初两帧间的对应关系；若已初始化，则根据关键帧队列进行相机当前位置追踪；

步骤2，判断新一帧图像是否为关键帧；若不是关键帧，则根据新一帧图像对当前局部地图进行完善；若是关键帧，则将其输入到语义分割网络中获取该帧像素级语义标签；

步骤3，将新关键帧加入关键帧队列，并进行联合优化，包含光束平差以及语义大平面限制；光束平差利用特征点在不同关键帧中的对应关系，优化相机位姿和特征点位置参数；语义大平面限制利用语义平面的分布特性滤除外点，便于在后续优化中进一步提升定位精度；

步骤4，判断关键帧队列数量是否超出最大值；若未超出最大值，则跳回步骤1；若已超出最大值，挑选出旧关键帧中与新关键帧共视点个数最少的一帧关键帧进行边缘化；

步骤5，针对边缘化帧进行致密语义地图构建；结合语义分割结果以及特征点在图像中的位置以及颜色信息，进行语义小平面拟合，并计算其语义质心；该边缘化帧中静态语义类像素点根据其同语义类最近的语义质心对应的语义小平面进行3D位置估计，进而恢复出致密的语义地图；

构建致密语义地图的步骤如下：

步骤5-1，对每一类静态语义类特征点，根据距离公式dis＝dis_E+dis_I进行聚类，其中dis_E表示在图像坐标系中的欧式距离，dis_I表示灰度值差异；首先将点组合最近点对，之后将点对组合成点组，点组中的点不在同一条直线上且不重复；

步骤5-2，针对每个点组，估计该点组的平面模型，并计算该点组的语义质心，语义质心的位置为点组的几何中心，其灰度为点组中点的平均灰度；

步骤5-3，针对每一个静态语义类像素点，计算其到同一语义类语义质心的距离，选择最近语义质心对应的平面模型去估计该像素点的3D位置。

2.如权利要求1所述的基于语义平面的单目即时定位与致密语义地图构建方法，其特征在于，在所述步骤3中，联合优化的步骤如下：

步骤3-1，光束平差，利用新关键帧中特征与原关键帧队列关键帧中特征的对应关系，优化关键帧队列中关键帧对应的相机位姿以及特征点位置，最小化系统误差；

步骤3-2，对于处于某一平面上的点，其深度表示为其中[u v 1]是该点在图像坐标系中的齐次坐标；K是相机内参矩阵；[a₁ a₂ a₃]是所在平面的法向量；将新关键帧中的静态类特征点根据其语义标签分为水平分布语义类与垂直分布语义类；每一个水平分布语义类特征，其所在平面法向量简化为[0 a₂ 0]，其深度表示为即处于同一平面上的水平分布类特征的深度差异由v体现；每一个垂直分布语义类特征，其所在平面法向量简化为[a₁ 0 a₃]，其深度表示为即处于同一平面上的垂直分布类特征的深度差异可以由u体现；对于每一个水平分布语义类，根据其图像坐标v划分成若干个直方图；对于每一个垂直分布语义类，根据其图像坐标u划分成若干个直方图；假定每个直方图中的特征点大致符合一个大语义平面，根据特征点的深度利用置信区间(μ-θ₁δ，μ+θ₂δ)滤除与周围点不一致的外点，其中μ为该直方图中特征点深度的平均值，δ为深度的方差；

步骤3-3，针对每个直方图，将水平分布语义类以及垂直分布语义类的最大深度值作为动态语义类特征点深度阈值，深度大于该阈值的动态语义类特征点将被视为外点。