【论文阅读】GCLO: Ground Constrained LiDAR Odometry with Low-drifts for GPS-denied Indoor Environments

一、公式与约束

这篇文章主要的是利用连续平面来提升激光SLAM在室内停车场的效果，所以必须要约束的就是平面的表达方法。论文一共使用了雷达坐标系、IMU坐标系以及世界坐标系三种坐标系，分别用L B W来表示。

论文中平面采用的是无限延伸的平面，而在现有的研究中，空间无限延伸平面主要有三种表示方法：HF表示法、空间坐标系以及单位四元数。这三种方法有如下缺点：
1）HF包含法向量n和平面到坐标系原点的距离，这往往是过参数化的，所以它会在最小二乘优化中受到奇异信息矩阵的影响，并且难以维护法向量。
2）球坐标系表示包含具有方位角和仰角的法向量，但是，当仰角等于 ±π /2 时，会产生歧义。
3）单位四元数表示平面存在的问题在于，目前尚未能清楚描述四元数和平面之间的物理联系虽然在CP表示的情况下，当给定坐标系的原点位于平面上时，也会遇到奇异性问题。

针对于这些问题，论文使用了最近点表示法（cp）来表示空间中的无限平面，CP表示法是直接引用的其他论文中的内容，是用驻留在平面上且最接近当前帧原点的3D点来表示这个平面，引用自“LIPS: Lidar-inertial 3d plane slam”这篇论文。这种表示方法简单，但是在转换的过程中比较麻烦，所以在这篇论文中，空间的无限平面使用了两种坐标系来表示，就像PL-VIO一样，一般表示用CP表示法，在转换坐标系的时候，用HF表示法。

具体来说，对于落在平面上的一个点p，在从a到b的位姿变换过程中会符合：

而对于HF表示法，落在平面上的点会满足：

两个式子整合可以得到：

结合下面的两个式子，可以解出HF表示法的两个参数：

最后用这两个参数，再转换为b帧上的CP表示法的平面：

最后这两个公式给出了平面方程在不同坐标下下的转换关系，需要经过HF表示法作为中间值，但是论文没有给出CP表示法与HF表示法的转换关系，所以可能在实际代码中需要额外计算并保存一下HF表示法的两个参数。

二、基于地面约束的激光里程计

地面的约束主要是为了解决当激光入射角过大时产生的纵向上的偏移。

左右图可以明显看到，在地下停车场的实验场景下，没有采用地面校正的左图明显点云产生了向上的偏移，整体呈现一个弓形的状态，而使用了地面约束的右图则明显将点云拉到了正确的幅度下。

对于这篇论文提出的GCLO，大体框架如下图所示：

论文着重讲了下面两个部分：

scan-to-map的点云注册方法

对于一帧已经校正过的点云，首先使用点到面的ICP的方法，计算当前帧点云和局部地图之间的位姿变换，也就是所谓的scan-to-map的匹配方法，这样就可以建立当前帧的点和协方差和局部地图之间的转换关系。

滑动地图更新策略

地图的更新主要包括两部分，对于地图中已有的旧点，也就是转换之后距离小于阈值的点，认为是之前的旧点在当前帧又出现了一次，此时选择融合策略，用旧点更新地图点，同时利用协方差滤除不准确性过高的点。对于新点，则直接加入到地图中去。

三、地面的约束

这篇论文主要的改进点就是使用了地面约束，这部分我们理顺一下地面的相关内容。

地面的提取与描述

地面的提取主要是从点云中提取出地面点，并用这些地面点获得一个平面的表示方程，这篇论文使用的方法是对每个关键帧（应该是补充了关键帧的筛选机制），从点云中分离出地面点，之后对地面点使用RANSAC来提取平面并滤除异常点，最后提取出平面的方程表达，并且考虑到RANSAC提取的结果存在一定的噪声，这篇论文中使用了一个类似最小二乘的方法来提取地面的方程：

通过高斯牛顿法来最小化上式，从而得到一个合适的用CP表示法表示的平面方程：

平面的连续性估计

这部分主要是讲如何确定不同帧的两个平面是同一个平面，使用的方法是比较CP表示法的差距。

这个式子中f表示的是转换函数，用于将平面方程转换到对应的帧，e值则真正用于衡量相似度，e值过大则认为地面与旧的地面是不同的两个地面。除此之外，计算e的过程中还需要考虑不确定性：

平面提供的约束

一旦我们建立了不同帧之间的平面匹配关系，就可以利用这个关系来将点云“拉下来”

具体的做法是对于每一对已经建立联系的平面方程，统一转换到世界坐标系下，用世界坐标系下的残差作为优化的目标。

这部分残差将作为因子图的一部分，参与到后续的优化中，通过在优化目标中加入对应的项来将点云拉回到正常的位置，从而避免出现“弓形”状态的点云地图。