【论文阅读】PSF-LO: Parameterized Semantic Features Based Lidar Odometry

这篇论文是阿里研究院的一篇论文，主要内容是结合语义、激光SLAM的一个前端里程计部分，除了语义信息与激光SLAM的整合，论文还使用了基于物体的思想，只保留了方便建图定位的静态四类物体：道路、建筑物、交通信号灯和柱状物。使用RangeNet++来获取点云的语义信息，之后对每类物体都采用五个参数进行描述，在里程计部分则使用论文提出的PSF帧，通过最小化四类物体的重投影误差来得到帧间位姿变化，除此之外，论文还通过垂直平面的提取、速度幅度与运动朝向相结合的判断策略等方法，针对于动态静态物体的筛选做出了优化。

一、公式约定与总览

作为一篇SLAM的文章，目标必然是获得一个帧间位姿变化T，这篇文章使用的是3D点云，所以属于激光SLAM的范畴，对于字母约定上，右下角标表示时间，右上角标则表示坐标系。此外，关于语义信息的获取，论文直接使用RangeNet++来获得语义信息，之后来提取物体并构建PSF帧，整个SLAM都是依赖于PSF帧所建立的。

二、PSF的提取

所谓PSF，本质上就是参数化的语义特征，或者说是对物体的参数化，我们可以利用语义将同类的点凑成整个物体，对这个物体添加描述性信息，比如说中心点位置、半径等，那么在后续的SLAM过程中，就可以利用物体的描述性信息来进行优化或者校正。

在这篇论文中，作者主要提出了四种PSF，也就是四类物体，对每个PSF都用五个参数进行描述，其中表达式参数（equation coefficient）根据物体类型的不同，有三维和六维两种选择。五类参数如下：

在这部分中，输入是带有语义的点云，输出则是包含有很多PSF的PSF帧。下面对四类PSF分开介绍：
①道路 Road PSF
为了克服道路点近处密集远处稀疏的问题，论文使用了不同分辨率的方形网格，近处的网格较小而远处的网格较大，图示如下：

通过对点云做2D投影（个人感觉是向地面投影），之后对不同网格内的点使用RANSAC进行离群点的剔除，进而获得五个参数中的CS参数，对于道路而言，这个参数是三维的，公式如下：

其中dπ表示的是原点到平面的标量距离，nπ是指向原点到平面的单位法向量。除此之外，权重用内点与全部点数量的比例来计算，最后两个参数则需要使用外接矩形来实现，中心点和四个角点都用外接的矩形来计算。

②建筑物 Building PSF
这一部分的实现与地面很类似，也是需要划定网格然后做投影，但是区别在于，考虑到建筑物在投影后并不一定是一条直线，比如墙角的点云投影到地面，其形状应该是一个直角，如果用RANSAC去剔除，结果只会保留一条直角边，这显然是不合适的，针对这一点，论文的做法是首先将网格旋转45°，之后对标记有建筑物的点做投影，这里对每个网格重复做RANSAC进行提取，只不过提取之后并不结束，重复提取直到网格内点的数目少于一定数量，才认为提取完毕。

③交通信号 Traffic sign PSF
由于交通信号的点云一般都是相对独立的，所以可以直接使用欧氏聚类来进行二次分割，之后使用与建筑物相同的提取方法进行提取。
④柱状物 Pole-like PSF
柱状物和交通信号很像，也是用欧氏聚类做处理，之后参数cs用如下的公式来记录：

其中Pp表示拟合线上最接近原点的点，Np是直线指向z方向的单位方向向量。其余的参数都与道路是一样的。

三、基于PSF的激光里程计

在里程计的部分，论文主要做了两个对LOAM的改进，一方面是滤除了一些不可靠的角点，另一方面是增加了语义一致性的权重：

其中Ns表示的是拥有相同标签的地理特征的数目，权重w会随着语义一致性差而降低。后面的部分个人看的也一知半解，大体感觉就是利用了类似重投影误差的思想，结合了物体的参数做的里程计。由于参数中的cs项维度不唯一，所以有两种投影方法，对于三维的cs，投影方法为：

这里论文将单个PSF物体记为S，一个PSF帧记为Fs，将最近几帧投影到世界坐标系下组成的子图记为Qs，对于这种设定，我们可以根据记录的帧间位姿变换T，将子图里的所有帧投影到世界坐标系下，对于某个帧，选择其在世界坐标系下的最近的临近点，利用这些临近点，制作一个平均点：

那么这个点就是抽象的准确位置，以此就可以计算一个类似于重投影误差的误差：

同理，使用六维cs的柱状物的误差函数计算方法可以写为：

同理可以写出四类的PSF的误差函数，对误差函数求和，就可以得到总的误差函数f：

之后就变成一个优化问题，寻找一个T使得误差函数最小。

四、动态与静态物体的筛选

论文选择了静态物体去进行SLAM的过程，对静态物体与动态物体的筛选也做了优化。论文提出的筛选包括四步：物体提取、关联、速度估计和分类。

在速度估计这部分，论文使用的是另一篇文章的方法，但是该方法容易受到物体上表面的影响，所以作者改进了一下。首先是将点做二维投影，投影到极坐标上，个人感觉依然是向z轴方向上投影，投影到地面上后划分为极坐标，对每个点，制作一个存放有其周围点的子集：

对这些点，统计其来自的雷达的线数。因为雷达以此scan是从上到下好几线的，这里个人理解就是在统计线的个数，因为不同的线上，如果竖直方向投影靠近，那说明是投影在了一个竖直的面上，如果有很多线的点都很接近，说明这个垂直的平面很大，也就是我们要的稳定的垂直平面。保留这些点，再利用原来的方法，就可以得到更加准确的估计。
此外，关于状态的估计，普遍的方法有两种：利用速度的规模和朝向的一致性。论文将这两种同时纳入考虑，通过贝叶斯滤波进行整合：

其中前项表示的是朝向的一致性而后项则是速度规模，其中：