Systematic Handling of Heterogeneous Geometric Primitives in Graph-SLAM Optimization 笔记

1. 简介

传统引入线、面的SLAM系统中，首先需要对线与面的三维表达分别定义，然后基于各自的数学模型定义点、线、面之间关系的误差函数。而本文的特点是提出了一套统一的表达方式来描述点线面三种表达，在该框架下彼此的误差函数能够很紧凑地用同一个公式表达出来，从而提高图优化过程的收敛和鲁棒性。
在本文中，作者提出了一种图优化后端，该后端支持由点，线或平面组成的地图。该后端可以表示同构（点-点，线-线，平面-平面）和异构测量（点在线上，点在面上，线在面上）的误差函数。与其独立地处理所有情况，本系统使用统一的公式，既可以得出紧凑的结果，又可以得出简洁的实现。使用更高维的几何表达和基于该表达的约束条件，可提高系统鲁棒性，并扩大收敛范围，如图所示。系统在真实和合成数据集上做了验证和评估。同时，源代码已经开放。

2. 算法

2.1 匹配元表达点线面

论文提出点线面可在高维度下统一表达，这种表达被称为匹配元（matchables）,其本质是退化的二次曲面表达，1）点描述为没有半径的球；2）线描述为没有半径的圆柱；3）平面描述为两个没有距离的平面。
点x在任何实体上都可以用下列公式描述：

其中p向量描述了该实体的中心，A是对称矩阵，特征值分解可以得到其旋转R和描述形状的对角矩阵Λ，其Λ的非零特征值为1，上公式将成为点x到该实体最近点的欧几里得平方距离。

2.2 点线面约束

在统一了点、线、面的表达在匹配元的框架下后，它们彼此间的误差可以统一用该公式描述：

其中Ma与Mb为任意两个匹配元。此外：注意到误差表达中并非所有元素都是有效的，如考虑两点之间的误差，我们仅仅关注彼此的距离，因而它们的朝向是无效的；同理，考虑线和平面的误差，方向向量应该是正交的。因此统一描述的思路是，在该误差向量之外，还定义一个信息矩阵，用信息矩阵的对应项抑制那些无效的量。
信息矩阵，其中A表示用于剔除上式中残值误差的非相关分量的“激活矩阵”：

而具体的取值根据两个匹配元所属的类型(point, line, plane)而查表得到：

3. 实验结果

3.1 实验数据

为了证明该方法提供了更广阔的收敛域和鲁棒性，使用g2o包中的g2o_模拟器的扩展来生成由点、平面和线组成的合成世界，如图示出了模拟场景示例。

并且考虑了三种规模的图，分别用g0, g1, g2表示，规模大小依次增大，分别是100节点，1000节点和10000节点。其中
• all: a matchable in the map is sensed through all sensing modalities (homogeneous and not)
• hom: a matchable in the map generates a measure of the same type (point-point, line-line, plane-plane).
• non-hom: a matchable in the map generates a non homogeneous measurement.
• point: only the point matchables are considered.

3.2 结果

在大规模图的实验下，收敛速度会更快，且收敛时得到的整体误差值更接近最优误差值，意味着距离最优解更近，体现了系统鲁棒性和收敛性质的提升。

作者也在ICL-NUIM数据集上验证了与经典算法ProSLAM和ORB-SLAM2的绝对轨迹误差比较，该系统的敛性质和鲁棒性都有一定的提升。SA-SHA是作者前个版本的方法，SA-SHAGO是该方法。