【基于视觉语义特征的室外SLAM建图算法研究】

首先什么是语义SLAM，我们知道传统的视觉SLAM是基于关键点、描述子等低级图像特征来进行定位和建图的，那么对于一些更高层次的带有明确意义的语义信息，比如现实世界中真实的物体，将它作为特征来进行优化的SLAM就称为语义SLAM，它更接近于人类的感知，因为人类对地图的观测不是看一堆点云的点，或者位姿的四元数，人类看的地图，就是这里有地铁站，那前里有红绿灯。

现如今语义SLAM的工作主要分为semantic mapping和real semantic slam，语义建图中一部分工作就是将2D图像的语义分割结果，即带标签的像素，映射到3D点云中，这种大多数是采用RGBD相机的稠密或半稠密的室内方案，在室外的应用是西交有一篇大型户外环境的路标slam，在kitti数据集上建了一个带有gps地标数据和语义标签的地图。还有一部分工作是以对象为单位构建地图，这种大多数就是在一个稀疏特征点的地图上融入单个目标三维重建的结果，最终得到一个带有目标对象的地图，比如cmu的cubeslam，伦敦大学学院的dsp-slam，它们是将车作为一个物体加到地图上。此外还有秦通的avp-slam，是直接将停车线减速带这种路面物体作为特征构建地图，它并没有把这种语义特征加到优化中去优化位姿，就是单纯的用语义特征建图，其他两个都是将语义特征加到了优化中，属于真正意义上的语义slam。

真正的语义SLAM就是要将语义信息当作一种约束融合到优化中去优化地图的成员，比如相机的位姿和地图点或者语义物体，有一部工作就是将语义物体加到BA公式里，然后联合物体，相机和地图点去量化重投影误差，比如宾夕法尼亚大学的一篇论文语义slam的概率数据关联Probabilistic Data Association for Semantic SLAM和cmu的cubeslam，还有一部分工作是从语义信息中推断出动态区域，因为在室外建图，必然有动态物体，传统SLAM方法几乎都是假设当前场景是静态的，当面对含有运动物体的场景时，运动物体就会对相机位姿估计产生巨大的偏差，所以就有人从语义信息中推断出动态区域，然后剔除它，比如清华的DS-SLAM，但是这是在室内，室外有篇印度帝国理工的鸟瞰图单目多体slam就是解决动态车辆定位的，最后就是因为语义信息提供的物体级别的描述，拥有季节（光线）不变性，所以有一部分工作就是用语义特征做定位的，就比如avp-slam有一个实验就是用orb和avp建的图去算一定时间后重定位的召回率，orb一天之后到25%了，而avp在一个月以后还能保持80%。

下面首先介绍ICRA2017的Best Paper，是美国宾夕法尼亚大学做的，这篇是语义slam的开山之作，本文第一次将几何，语义，IMU统一到一个优化框架中，用EM算法(期望最大化算法)求解，实现了一个更高定位精度的slam系统。他的核心目标是用语义信息辅助获得路标和摄像头姿态，也就是探索语义信息在数学模型中对姿态估计造成的影响。
在语义SLAM中，假如空间中有一个静态的物体M（真正意义上的landmark），相机在运动过程中连续观测到了很多的物体Z，那么如何知道那些观测是对应到静态物体M，在传统SLAM中，可以通过匹配或者跟踪的方法来确定某些观测属于同一个几何结构，但是对于物体级别的特征而言，并没有很好的方法来确定这件事情。

整个SLAM问题的概率表达式如下，也就是带着关联关系的最大似然问题，传统的SLAM方法将整个SLAM过程拆分为了两个部分：根据初值路标L，相机位姿X和观测Z求解数据之间关联关系D，然后根据初值和关联关系求解优化变量，所有的变量中，除去待优化的变量，观测可以通过几何结构提取方法和目标检测方法获得，只有关联关系是无法显式获得的，也就是是关联关系是一个隐变量，于是作者想到用EM方法进行求解整个带隐变量的优化问题，他引入期望测量的似然模型，最后通过一系列变换得到最后这个公式，他将这种硬性的数据关联，即观测就是和某个路标对应，转成了考虑每一个观测和每一个路标之间都有可能对应，这个对应关系的可信度用一个权重因子w来表示，EM算法的E步骤就是求解数据关联的概率分布，也就是这个权重因子。

M步骤是做一个位姿图优化，即把所有的语义factor、几何factor和IMU factor联合起来一起构成一个要优化的目标函数，在这个分布基础上最大化期望测量似然，其中语义factor的误差函数的几何意义是最小化投影点到检测框中心的距离，即最小化高斯分布均值和检测框中心点位置之间的差，几何factor是将所有地图上的特征点投影到当前图像中计算距离差，imu factor是计算两个连续关键帧之间的相对姿态差(位置、速度和方向)。到此这篇论文就讲解完了，这篇论文说到底就是解决了一个语义特征的数据关联问题以及如何将语义特征融入优化去获得路标和摄像头姿态，这也是语义slam最核心的两个问题。
接下来粗略的介绍两篇国内的工作，清华的DX-SLAM，这个其实也不算语义slam，它的本质还是用低级图像特征，只不过他是用深度学习的特征点来代替传统基于几何的特征点，他是在orbslam2的框架上做的，重新做了一个重定位和回环检测模块，这个我是跑过orb和superpoint两种特征点，可以看到这种基于学习的特征点对于视角变化和低光照环境有更强的鲁棒性，我觉得可以考虑在地下停车场环境中如果需要稀疏点云地图，可以用这种基于学习的特征点。

这篇DS-SLAM也是清华的，他是结合语义信息和运动特征点检测，来剔除每一帧中的动态物体，从而提高位姿估计的准确性，同时建立了一个八叉树地图，他是在orbslam2的框架上加了一个语义分割和稠密地图生成的线程，我主要关注作者是如何去对动态物体进行剔除的，它是由两个步骤组成的，首先是进行一个运动一致性检测，作者提出一种基于多视图几何的算法，就是计算当前帧的像素点到极线的距离，若大于设定的阈值则视为动态点，这里他用到了基础矩阵和RANSAC，这个方法能检测物体是否运动，但很难提取出完整动态区域的轮廓，因为它是针对像素的，所以作者接下来就联合语义分割的结果，他认为人这类标签的对象都是属于可移动类别的，因此，将这两个步骤的结果结合起来，如果在一个物体被分类为可移动类别，且在它的语义分割边界内，有足够数量的由运动一致性检测得到的运动点，那么这个物体的所有点都被视为动态的，然后去除这些运动点再进行运动估计就会得到一个更加准确的位姿。

下边这篇印度帝国理工的鸟瞰图单目多体slam就涉及到动态物体定位的问题，首先左上图是说多体slam中存在的一个问题是用一个移动的相机对一个移动的物体进行三角定位是不可能的，因为在拍摄第二张图像时，该物体已经移动走了。而且三角化很难得到深度信息，因为这种背面投射的光线会在错误的位置相交。当汽车沿着黄线移动时，许多可能的轨迹会投射到图像中的相同位置。这篇论文在bev视图中解决了这些问题。它建的是一个鸟瞰的道路地图，用一个语义分割去保留路面上的特征点，然后通过一个固定的相机高度加内外参将路面图像点转成道路上定高的地图点云，再聚合传感器随时间的观测将得到的稠密点云投影到栅格图上得到道路和车道边界两种BEV占据网格图，为了解决动态对象遮挡车道路面问题，作者用了两个网络将单独的占据网格图转成模态的车道点云。对于动态物体的定位，作者是用一个单目深度估计网络去得到一个伪雷达的深度图，然后再用一个Frustum-PointNet网络去定位车辆，同时用2D检测结果作为定位来源，最后是都投到BEV视角下，就得到了车辆在bev图中的定位信息。在后端作者是提出了一种轻量级的在线位姿图表示，融合了场景中多个对象的约束。其中包括场景中车辆的动态信息和场景中的路标即地图点的静态信息，这种动态信息把连续时刻的相机运动，车辆运动和相机-车辆运动都约束起来，静态信息是用地图点去约束相机和车辆，最后相加得到损失函数。

对于在地图上加上物体，就要知道物体在三维地图中的表达方式，现在主流上有四种表达方式，前两种更偏向于三维重建，这里就不说了，主要来看第三种。

这是CMU的一篇工作，如何从单目利用2D检测框，辅助VP来恢复三维立方体结构就不说了，这里我主要关注如何在传统SLAM的静态假设上，追踪动态物体并实现相机位姿估计和运动物体位姿估计。那么对于这种物体级的slam，要进行运动估计首先要解决的问题就是数据关联，这也是语义slam中的一个研究点和难题，前面我介绍了一篇语义slam的数据关联，它是利用EM法来解决数据关联的，这篇论文它对静态物体和动态物体分别采用不同的关联方法：对于静态物体，首先是将提取到的特征点和2D检测框检测的对象关联起来，如果一个特征点至少两帧都在同一个2D检测框里并且它到这个2D检测框构成的立方体中心的三维距离小于1m，那么这个特征点就和这个2D检测的对象关联起来，并表示为同一种颜色，然后就去匹配不同帧之间的两个对象，如果这两个2D检测框对象的共享特征点最多且数量超过10个，就把这两个对象匹配起来了，同时，对于匹配上的点过少的框，认为是动态物体而去除掉。

对于动态物体，特征点是直接用2D KLT 稀疏光流算法来跟踪，像素跟踪后，动态特征的3D位置通过考虑物体运动完成三角化。存在的问题是当像素位移很大，即当一辆车靠近相机时，稀疏光流的跟踪算法会失效，所以作者又采用了一种视觉对象跟踪算法，去跟踪对象的2D边界框，并从上一帧中预测其位置，然后与当前帧中检测到的重叠比最大的边界框进行匹配。

将静态物体和动态物体关联起来后，就可以联合相机位姿，物体，和地图点这三个地图成员去做BA，那么我们就看一下作者是如何把语义物体的约束融入BA的，这个函数保留了特征点slam中相机地图点的重投影误差，加上了相机与物体的约束和物体与地图点的约束，物体与地图点的约束很简单，就是地图点要在这个物体的3D框里边，先是将这个地图点转换为长方体框架，然后与3D框的尺寸进行比较，得到三维误差，相机与物体的约束分为二维测量误差和三维测量误差，二维测量误差就是物体投影到二维平面的二维边界框与检测框的误差，也就是左图中红色框和蓝色框的误差，三维测量误差是由相机坐标系下位姿和已有3D框测量的位姿的对数误差加上物体的尺寸差构成的，那么现在就将语义物体的约束加到BA公式中进行完整的优化了，这就是真正意义上的语义slam。

除了立方体表示物体，还有椭圆体表示物体，但是椭圆体的物体表示只是一种近似，它的检测框和实际测量的检测框不可能完全重合，所以这个quadricSLAM的贡献只是加入了物体层级的路标，对精度提升并没有帮助，但相反cubeslam的精度提升很大，在kitti07序列中几乎和真值重合，我觉得是因为车的检测框满足立方体的模型，那么加入这种语义对象做优化就很合理。

接下来是一篇最近的物体级建图的论文dsp-slam，基于先验的物体重建就不说了，还是关注它的数据关联和联合地图优化。它关联的是检测和地图中最近的物体，它定义的一个检测包括2D的检测框，2D的分割掩膜，三角化得到的3D点云深度和3D检测框初始的位姿，是针对不同的数据来源设计不同的数据关联方法，对于雷达输入的点云，是比较3D边界框检测和重建物体之间的距离，然后关联距离最近的，对于单目和双目图像，是计算2D边界框检测和物体之间匹配特征点数量，在多个关联中，保留最近的一个。无关联的检测初始为新对象，关联上的检测只进行位姿估计。它的联合地图优化的损失函数和上一篇cubeslam很像，就是没有约束物体和点，我觉得是因为它单个物体的三维重建是基于表面一定数量的特征点的，就隐性包含了这种约束。
这两篇论文的基础框架都是orbslam，就是把一个物体级的三维重建算法加到一个传统slam算法中，它们的数据关联还是都要用到特征点的，然后都在地图优化中加入物体与相机和物体与地图点的约束，值得考虑的问题是相比于车辆，在一个稀疏点云地图的基础上怎样把一些更有用的地面标志比如减速带，箭头或者空中元素路灯、车牌这种加到地图中，增加地图的丰富性和拓扑性，然后怎样用这种语义特征配合点特征去进行定位？

接下来就是韩国KAIST的一篇Road-SLAM，这篇论文比较早，是开了利用IPM图像构建子地图进行ICP匹配的一个先河，它是用车辆的单个前视相机的IPM语义特征来建图的，大致流程是在里程计上投影IPM图像的二值化点生成车道线和标记点云，为了避免IPM造成的大的透视失真，点云生成限制在了相机附近的感兴趣区域，然后经过点云语义分割和随机森林分类后，将道路标记和周围车道组成的特征定义为子地图，进行子地图间的ICP去匹配局部地图得到这两个局部地图之间的相对姿态，把这个相对姿态用于全局姿态图的优化，以纠正漂移。姿态图优化的损失函数由里程计和回环检测构成的。在Road-SLAM里点云语义分割是用传统的方法，分类是采用机器学习的方法。

而在AVP-SLAM里，是用一个U-Net网络对图像进行语义分割得到了比Road-SLAM更精确的语义特征，同时是采用了四个环视相机生成环视IPM图，感知范围更广，为了解决停车场中存在的一些无纹理区域提取不到语义特征的问题，在定位中采用了EKF框架，将里程计与视觉定位结果结合起来。在该滤波器中，使用里程计进行预测，并使用视觉定位结果进行更新。不仅提高了系统的鲁棒性，而且使估计的轨迹更加平滑，其他的流程就和road-slam一样。它存在的问题是语义点云会提供错误的数据关联，导致运动估计不准，还有虽然ICP是非常好的点云配准方法，但它仍然属于暴力匹配，匹配效率低下而且容易出错。

针对这两个问题，有两篇论文分别去解决。

小米科技的AVP-Loc做的工作是直接使用停车场的高清矢量地图进行定位，它提出了一种基于语义分割的环视图与高清矢量图相匹配的异构数据关联方法和一种利用分布在矢量图之间的独特道路标志组合的封闭形式重定位的策略，匹配方法大致就是用了一种类匹配策略，对地图中不同形式表现的道路标线用不同的方法匹配点和线的矢量段。左上角下边的这个图红色代表的是ICP重定位的初始姿态，绿色是收敛到的局部最小值，ICP算法它很依赖于初始姿态的接近程度，初值不好会陷到局部最小值里，而回不到实际的位置。这个AVP-Loc就提出了一种新的算法，如左上图所示，无论噪声如何扰动，解都会收敛到相同的最小值。具体就是作者把减速带、箭头和仪表段这三个基本的离散道路标记组合起来，当作一种地标的标签特征，每一个地标它都有自己独特的空间特征，就是它在地图中的位置，重定位是通过将局部地标候选物与矢量图中的对应物进行距离信息的匹配来实现的。

上交的这篇鸟瞰图混合边缘slam解决的就是数据关联的问题，当某些道路标记或停车点边缘临时位于相机焦点的射线方向上时，可能会被错误地删除，因此，当前图像上提取的边缘通常是不完整和不稳定的，这将使基于关键帧的策略难以获得鲁棒的相对姿态估计。作者的思想就是既然我处理不了这种车道线的点云，我就直接不用车道线和减速带建图，而是用鸟瞰边缘和自由空间边缘两个边缘构建地图，同时用高斯滤波器平滑地更新局部地图并填补边缘间小的空白，用滑动窗口去除添加到局部地图中不正确的分割边缘，最后将重要的边缘尽可能地保留在局部地图中，使用最近邻搜索建立起局部地图上边缘的数据关联，再通过这个函数计算位姿。

以上这几种利用车道线等地面语义标志建图的地下停车场语义slam，它有个好处就是高度是给定的，所有的语义物体都是地面上的，然后用里程计去做坐标系变换，维护子地图，在子地图间用ICP做回环检测拼成全局地图，而姿态图优化就用里程计和回环帧的约束，但是存在的问题除过以上两种，还有landmark基本都是车道线，纵向精度受影响，建图过程中大部分区段视觉是没起约束作用，那么我们是否可以考虑将这种车道线的语义加入优化中去优化地图的位姿。

然后同样是秦通的工作，这个就是面向于城市道路建图了，涉及到一个云端数据的处理，建图还是和avp-slam类似，其中为了解决语义分割带来的噪声，利用统计的方法来过滤噪声，具体实现是把地图分为小网格，对插入语义点的语义标签得分进行计数，将得分最高的语义标签表示为网格的类。它存在的问题我觉得是定位偏差导致多次轨迹无法重叠，表现在地图上就是车道线很粗，还有车道线欠约束，时间长了精度可能下降，再者就是如何将空中元素加到地图中。归根结底还是怎么在优化中约束车道这种语义特征。

对于空中元素在语义地图上的使用，阿里巴巴有一篇基于稀疏视觉语义特征的SLAM，这篇工作的亮点在于它用不同的方法去参数化道路上的一些常见的语义实体，比如对于道路元素，是集成了级联深度模型来检测标准化的道路元素，对于路灯和标志，是通过其代表性深度关键点为骨架和边界建模，对于车道是通过分段立方样条曲线来参数化。对于语义实体的特征追踪，是用IMU预积分得到的T作为相对位姿的初值，使用匈牙利匹配策略在像素空间中以实例级和像素级方式关联地面特征。对于竖直的Pole Objects语义特征如路灯、标志牌，提取FREAK描述子然后使用光流法追踪，加上代表同一个地图点在两个相邻关键帧之间的投影。为了把语义元素引入状态估计中，作者设计了5种优化变量，包括检测到的语义特征点对应的3D空间点，相机可视范围内地面对象对应的地面参数，极点对象的竖直平面参数，车道线对象的每个样条曲线的控制点，初始化样条曲线的数据关联变量，基于这5类优化变量，对每个图像关键帧的位姿应用了三种类型约束，包括点观测系数，样条观测系数和共面观测系数，使用GNSS-VIO给出的位姿初始化所有优化变量，初始化结束后，在vio基础上得到一个因子图开融合水平地面、竖直平面、语义特征点、c样条曲线控制点，对相机位姿产生约束。

最后就是西交的一篇大尺度室外环境地标的视觉语义SLAM，通过坐标系变换和贝叶斯更新，将三维地图与相关语义信息进行融合。利用基于高斯分布的模糊归属关系实现地标数据融合，建立地标数据的拓扑语义图。

接下来说一下我对基于视觉语义路标的室外SLAM建图的理解，我觉得有两种方案，一种是在稀疏点云地图加高层次的语义特征，一种是直接用车道线等语义特征去建图，这个也包括在停车场做avp，前者能做的就是怎么用这些静态的语义特征去丰富地图和增加地图的拓扑性，或者去考虑如何去除道路上的动态物体，后者存在的问题就是语义特征描述信息的缺失，会导致错误的数据关联，从而导致错误的位姿和地标估计，怎么去在输入数据去改进配准的方法，或者如何将语义特征加入优化去提高slam的精度。