论文阅读记录SuMa SuMa++

首先是关于SuMa的阅读，SuMa是一个完整的激光SLAM框架，核心在于“基于面元(surfel)”的过程，利用3d点云转换出来的深度图和法向量图来作为输入进行SLAM的过程，此外还改进了后端回环检测的过程，利用提出的面元的概念和使用到的虚拟帧来优化回环检测的过程。
SuMa的核心分为以下几个步骤：

1. SuMa预处理

预处理的部分将3d点云转换为两张图，原文用的词是vertex map和normal map，这里直接翻译为顶点图和法向量图。
预处理的过程本质上就是建立一个3d到2d的转换，原始的点云是3d的，顶点图和法向量图都是2d的，所以需要一个转换关系，论文首先使用的是点云到顶点图的转换，对于点云中的每个点，有：

其中出现的两个参数分别是

其中r是深度，f是竖直方向的视野范围
此外w和h分别表示顶点图的大小。对于顶点图中的每个点，论文都使用$V_D$来表示。得到了顶点图后，利用其相邻的关系，就可以得到法向量图，就像其名字一样，法向量图存储的是每个位置上的法向量（normal），在计算时可以直接使用顶点图中的相邻元素进行计算：

为了保证计算的有效性，对于边界的元素，如果位置超过了图像的范围，也就是说位置不合法了，这种情况直接不参与计算，只保留合法位置的法向量信息。

在经过预处理之后，原本的3D点云地图就被转换成两张二维的图像，后续的里程计和回环检测都是在这两张图的基础上

2. SuMa地图表示

前面看过的论文里，地图的表示方法有很多种，最普通的基于特征点、特征线，稍微进阶一些的比如基于物体的就是用的物体地图，而LOAM也可以看作是基于边和面的地图，在SuMa中作者提出了一种基于面元的地图。

面元即论文中的surfel这个单词，面元地图指的就是面元作为地图中的基本元素，一个面元就相当于一个小平面，可以抽象为一个小的圆形，每个面元用一个位置向量、一个法向量和一个半径表示，此外还保留两个时间戳，分别是创建面元的时间和最后一次更新面元的时间。

除此之外，为了体现面元的置信度，方便后续筛选掉错误的面元，论文对每个面元设置了一个优势比（odds ratio），用来体现一个面元的可靠程度，其计算方法为：

其中的odds为一个函数，具体表示为：

$p_{stable}$和$p_{prior}$表示面元是一个稳定面元的概率和先验概率，这其实是一个不断更新的过程，如果确定两帧的两个面元对应的是地图上的同一个面元，那么更新时必然是一个迭代的更新，原面元法向量与待整合面元的法向量夹角越小、距离越近，对应到公式中的$exp$项也就越接近1，整体也就向着$p_{stable}$的方向移动，从而让面元的可靠程度越高。

此外，关于时间戳信息，引入这个量主要是为了将地图中的面元区分为两部分：动态图和静态图。动态图记为$M_{active}$而静态图记为$M_{inactive}$，里程计进行位姿估计的过程主要是用动态图，而回环检测的部分则使用静态图。最近更新时间超过一定阈值就将其记为静态图，否则放在动态图。

3. SuMa里程估计

前面提到里程计估计这部分只用动态图，也就是距离当前不太远的内容。这部分就是利用这些内容，通过最小化一个偏差，来得到一个帧间位姿变换。论文使用的是类似于重投影误差的方法或者说类似ICP的方法，对于当前帧的顶点图的每个点，根据位姿变换T投影到上一帧，找到上一帧的顶点图中距离其最近的一点，从而得到该点所在的面元，计算投影点到面元的距离并求和，最小化这个距离，就是我们优化过程的目标函数：

其中，$T_{C_{t?1}{C}_t}^{(k)}u$是将当前帧的点转换到上一帧，$v_u$是上一帧与之对应的点，$n_u^T$是对应点的面元的法向量，因为是计算到面元距离，所以要乘以一个法向量

通过迭代优化图中的T，从而得到一个更加准确的位姿变换T，这个过程称为帧到模型的ICP。可以看到目标函数中是对当前帧顶点图的所有点进行距离的统计，所以必然需要一定的筛选措施，这里除了去除投影后位置不合法的点，还将角度偏差过大和距离偏差过大的点也去除了，这里的角度偏差过大指的是法向量图中对应位置的角度的偏差。
在优化的过程中，先用上一帧和上上帧之间的帧间变换作为初始值，之后利用高斯牛顿法进行后端的非线性优化，最终计算出帧间变换T。

4. SuMa地图更新

面元计算这部分，首先对当前帧的每个点，计算其所在面元的半径，公式如下：

其中p和clamp都是函数，具体表示为：


这个半径表示的是当前这个点所在的面元的可能的半径，但是不是保留这个点还需要判断。判断的过程首先利用前面得到的帧间变换矩阵，将当前帧的信息投影到世界坐标系下，在这一步我们需要用到前面保留的2d-3d之间的转换关系，这个关系可以给我们指明点之间是怎样的一个观察方法，按照这个方法，我们沿着当前点的视线延长，看看延长线能否与一个地图上的面元相交，如果没有相交，那么根据当前点的信息新建一个面元添加到地图中；如果存在一个面元，那么就检测距离与角度的偏差值：

• 如果符合条件，则认为当前的点所在的面元和地图上的面元是一个面元，如果新的测量更加准确，比如说半径更小，就进行整合操作：
  
  这时前面计算的半径就能用到了，而且与位置和法向量的整合方法不同，半径的整合是完全替代，直接用新的、更小的半径代替前面的半径。之后更新面元的可信度。
• 如果不符合条件，就调低面元的可信度，除此之外不做其它操作。在一次更新之后，去除过旧的面元和可信度过低的面元。

5. SuMa回环检测

回环检测的部分首先是要获得一个候选回环帧，论文中使用的方法是遍历静态图，从中找出距离当前帧最近的一帧，公式如下：

这里所谓的最近是只考虑了平移没有考虑旋转，这是因为旋转在寻找距离最近的时候并不会产生影响。得到最近的帧之后，利用存储的位姿变换关系，可以得到这两帧之间的旋转和平移关系：

对于这两个关系，旋转可以直接拿来使用，但是平移上论文又引入了一个参数量：

这里是考虑到单纯使用重叠程度作为是否出现回环的判断标准容易出现误判，所以这里在平移上增加了一个系数，从而产生了三个初值，从这三个初值中选一个最优值作为回环的匹配程度。
除此之外，论文还提出了一个虚拟帧，虚拟帧本质上就是投影后的候选回环帧和当前帧的整合，具体做法是先利用位姿变换关系，将候选回环帧投影到当前位置，之后采用与前面一样的视角检测方法，如果视线上存在更近的点，就用这个点去替代投影帧上的点，以此构成虚拟帧。
得到虚拟帧之后，采用相同的方法计算帧间残差，因为增加了参数，所以是三个位姿去计算残差，从中选择最小的作为评判回环是否出现的标准。如果出现了回环，还要持续检测后面一个时间段内的位姿变换，必须保证一个时间段内位姿的变换与候选回环帧之间保持一致性才可以认为真正出现了回环。如果真的出现了回环，就利用位姿图进行优化，对回环进行优化。

总的来说在回环检测这部分，首先要遍历静态图，选择距离最近的一帧作为候选回环帧，根据候选回环帧和当前帧制作虚拟帧，利用虚拟帧和三个位姿变换计算相似度，如果相似度满足阈值且后续一个时间段内都符合条件，就认为真正出现回环，通过图优化进行回环校正。

6. SuMa++

SuMa++则是在SuMa的基础上引入了语义相关的内容，因为大部分的框架都是相似的，这里就简单记录一下。

首先SuMa++使用RangerNet++来获得点的语义标签，产生一个语义mask用来标记点的语义信息。其次考虑到错误分类会对后序的内容产生影响，SuMa++引入了一种洪泛算法来消除错误标签。洪泛算法以语义mask和顶点图为输入，输出修正之后的语义mask，伪代码如下：

整个过程主要是对边缘信息的处理，首先对于语义mask中的一个点，如果在其邻域范围内，有其它语义信息的点，那么就认为这个点是边缘点，将这个点去除，全部处理一遍之后，相当于将两个物体边缘的部分去除掉了。之后再对这些去掉的空白位置，采用就近分配的原则，重新为其分配标签。最终效果为：

很直观的可以发现，从( a )图到( c )图，物体与物体之间的边界区分更加明显，可以更好的为后续的动态物体的去除作铺垫。

除此之外，考虑到对于SLAM过程而言，制图起着关键作用的，主要是静态的物体，而非动态物体，因此SuMa++将动态物体去除掉，主要方法是在更新地图时，检测地图与当前帧对应点的语义一致性，简单来说就是如果是同一个位置但是标签变了，那么就认为这个位置上的面元是动态物体上的一个面元，通过在可信度函数是增加语义一致性项来加速置信度的降低。

但是作者也提到了，在场景特征不那么丰富的情况下，这种过滤方法会让特征不足，从而影响SLAM的效果。

最后SuMa++还使用了语义ICP，在里程计的目标函数部分，增加一个语义权重项，通过比较地图点与扫描点之间的语义一致性来调整权重，也就是说不仅空间上要存在一致性，语义上也要满足一致性。

SuMa++在SuMa的基础上增加了语义信息，将语义信息与面元相结合，语义获取部分使用Rangenet++来获取语义mask，并通过洪泛算法对mask中的物体边界部分进行修正。语义的应用方面，SuMa++利用语义的一致性，通过比较地图点和当前帧的同一位置上的语义信息，将对制图过程意义不那么明显的动态物体筛除，此外还将语义信息应用在里程计的目标函数中，为目标函数增加语义权重，从而体现出语义一致性。