GCLO：具备地面约束的适用于停车场AVP任务的lidar里程计(ICRA 2022)

2022-07-23 08:09 作者:3D视觉工坊 0人读过 | 我要投稿

论文阅读：GCLO: Ground Constrained LiDAR Odometry with Low-drifts for GPS-denied Indoor Environments (ICRA 2022)

Motivation

在具体的工程实践中，lidar在地下停车场容易沿垂直方向向上漂移，导致建图结果不佳。具体原因是当入射角较大时，激光雷达测量的深度可能会出现偏差。因此，当车辆在地面上移动时，从道路上观察到的点会稍微弯曲，LO 估计的轨迹会沿垂直方向漂移。因此为了消除 LiDAR 测量偏差并压缩可预测的位姿漂移，通常做法是需要针对不同的传感器调整比例因子，并且需要根据环境温度产生差异，所以为了提升鲁棒性，加入地面平面的考量减少无关项的影响是一个实用的选择。下图是motivation的可视化，具体表达的是见图结果的地面点与拟合的地平面之间的距离的俯视图，a图表示当车辆在地下停车场的地平面上移动时，LO 结果向上漂移，所以最终地图里表示的地面是一个扭曲向上的弓形，b图表示当采用本文提出的方法后的地面见图效果。

作者：晃晃悠悠的虚无周 | 微信公众号「3D视觉工坊」

Contribution

提出了一种利用地面观测约束来减少多层室内和平坦室外环境中的位姿漂移的 LO 方法。
提出了一种通过检测连续关键帧之间的急剧变化来关联地面平面地标的地面匹配方法。
基于KITTI 和 HIK 数据集的实验结果说明了准确性，并且讨论了在算法泛化方面所存在的局限性。

Content

关于无限平面参数化的讨论

本文最终选定的无限平面参数化方式为closest point(CP)，未选的方式包括：Hesse form(HF), 球坐标系表示和单位四元数表示，他们分别具有如下缺点：1）HF包含法向量n和平面到坐标系原点的距离，这往往是过参数化的，所以它会在最小二乘优化中受到奇异信息矩阵的影响，并且难以维护法向量。2）球坐标系表示包含具有方位角和仰角的法向量，但是，当仰角等于 ±Π /2 时，会产生歧义。3）单位四元数表示平面存在的问题在于，目前尚未能清楚描述四元数和平面之间的物理联系虽然在CP表示的情况下，当给定坐标系的原点位于平面上时，也会遇到奇异性问题。但是由于地平面与车辆上配备的 LiDAR 有一定距离，因此本文可以避免 CP 表示的奇异性。另外由于本文将地面建模为无限平面地标，因此在不同帧之间转换地面参数至关重要，所以采用 HF作为地面转换的中间参数使得转换方程变得容易，具体过程如下：假设帧a存在一个地面用HF表示为：

，点是该平面的点，即

，给定a帧和b帧之间存在变换：

，带入可得：

，从而可以得到新的HF表示：

，最后将HF表示转为CP表示：

2.系统框架

系统框架如上图。在预处理模块中，使用EKF方法融合 IMU 和车轮编码器，以提供高频运动估计结果。LiDAR 点云的运动畸变通过使用运动估计结果来补偿。去畸变的 LiDAR 点云基于点到平面的 ICP 算法来估计连续扫描帧之间的相对位姿变换。维护以传感器为中心的滑窗，从而减轻 LiDAR 扫描的稀疏性并提高 LO 的准确性。在每个关键帧，滑动图被记录为局部地图。从局部地图中分割地平面，并使用加权最小二乘法估计 CP 参数。之后，关联在不同关键帧观察到的地平面，并且构建一个位姿图并联合优化残差。3.以传感器为中心滑窗的scan2map

1）Scan2map注册假设在时间k的时候，全局位姿为，并且滑窗地图已经基于帧进行了维护，当一个新的扫面帧在时间k+1进入的时候，通过点到面的ICP估计出相对变换和对应的协方差。滑窗中点的坐标和不确定性协方差可以通过下述公式变换到新入帧：

其中，

是点关于旋转分量的雅可比。2）以传感器为中心的滑窗地图更新滑窗图在更新观测特征的同时需要过滤掉具有较大不确定性的点。主要步骤如下：a)首先，新注册的激光点需要先检查预定义的距离阈值，之后和滑窗地图点关联。b)然后，选择关联的滑窗点，并将它们的不确定性重置为关联的新扫描点的协方差矩阵，称之为观测误差。c)具有大不确定性的滑窗点会被过滤掉。在实验中，只要对应的协方差矩阵的迹大于阈值，该点就会被消除。d)最后，将没有关联成功的激光点作为新的观测值添加到滑窗图中。3)地面提取和建模考虑到停车场的地面大多都平坦，所以采用无限平面进行建模。对于每个关键帧，首先从以传感器为中心的滑窗地图中分割出地面点。然后基于这些分割出的地面点集应用RANSAC获得初始的CP表示。RANSAC获得的表示肯定有噪声，因此定义一个加权最小二乘问题来最小化点到地面的距离：

其中，是第i个测量的不确定性的逆，是点pi的不确定性。关于残差对于的雅克比可以定义为：

相应的局部增量可以表示为：

新的平面参数可以根据局部增量进行更新：

最终的不确定性可以近似为：

4)地面平面匹配假设已知j个历史关键帧：

，并且每个关键帧都有从里程计获得的位姿和其对应的观察到的地面参数及其不确定性，接下来的问题可以表述为：如何确定哪个历史地平面与当前地平面共面。一个直接的方案是通过在欧式空间的一个相同的坐标系中比较它们的参数来匹配地平面。但是如果漂移过大的话会错误地关联地平面。相反，基于一个先验知识 ( 地面通常是结构化比较好的，并且在论文motivation的环境（室内停车场）中，地面连接存在明显差异。)，

如上图所述，当地面变化的时候，CP变化极其明显，因此可以通过如下公式检测CP参数的剧烈变化从而关联对应的地平面

其中f()代指：

然后通过如下公式估计的不确定性（假设短时间内的LO结果足够精确，所以不需要计算LO的不确定性）

5)联合位姿图优化在确定地平面之间的匹配关系后，将地面约束融合到位姿图优化框架中，从而压缩 LO 的漂移，如下图所示。

假设在世界坐标系存在一个地面P，其CP表示为：。然后位姿为的帧观测到地面P得到对应的CP表示和HF表示

，运用之前提到的公式，以HF为中介，将平面在当前坐标系转换到全局坐标系，从而可以从观测到的CP与估计的CP之间构建对应的残差：

残差关于和的雅可比表示为：

最后通过LM方法联合优化LO和地平面残差：

6)其他细节本文在每个关键帧提取一个地平面作为地面观测。但是，此策略不适用于所有目标环境。首先，CP 参数可能会因减速带而产生噪声。其次，地面不能总是被建模为平面地标，因为弯曲和螺旋坡道在地下停车场很常见。第三，当关键帧位于多层路口的交叉区域时，该关键帧对应两个地平面。采用如下几种方法来处理上述情况。首先，使用 Huber 损失函数来减少异常值（例如减速带）对 CP 参数估计的影响。其次，通过计算残差和不确定性来评估地面的平面度。平面度低的地面不会被建模为平面地标，相应的观测约束也不会被融合。第三，当地面匹配不明确时，在某些指标（平面度、点数）下只选择一个地平面。这样，无论关键帧在停车场的哪个位置，每个关键帧都只拥有一个具有精确估计CP参数的地平面。4.实验

本文采用的数据集是HIK和KITTI。HIK是海康内部数据集，包含了三种场景，室内，校园环境，城市公路。KITTI不作赘述，大家都知道。最终的实验数据记录如下：

注意，上表中的A/B,A指的是GCLO*, B指的是GCLO，GCLO*代表不加入地面约束。

注意，上图中，kitti分成了两个子集，子集1旨在说明自己算法有优势的场景，子集2旨在分析为什么算法在这些场景不行，作者以Seq.09举例分析，如下图所示，虽然每个关键帧的地面是局部平坦的，但地面的坡度是逐渐变化的。最终不正确的关联结果会导致更高的位姿漂移。因此与 GCLO* 相比，GCLO 结果的旋转和平移误差分别增长了 36% 和 18%。

Conclusion

这篇文章的作者利用多层室内和平坦室外环境的地面约束来优化 LO 的位姿漂移。虽然最初是为 AVP 应用而设计的，但实验证明对于其他环境也很有价值，并且也通过实验结果对缺点进行了分析与讨论，展现出了有希望的改进方向，主要原因就是由于grounds的建模和关联方法使得GCLO对一般场景的适应性较差。所以后续可以考虑整合其他的建模方法或者额外的语义信息，以提高准确性和鲁棒性。本文仅做学术分享，如有侵权，请联系删文。

3D视觉工坊精品课程官网：3dcver.com
1.面向自动驾驶领域的多传感器数据融合技术
2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
9.从零搭建一套结构光3D重建系统[理论+源码+实践]
10.单目深度估计方法：算法梳理与代码实现
11.自动驾驶中的深度学习模型部署实战
12.相机模型与标定(单目+双目+鱼眼）
13.重磅！四旋翼飞行器：算法与实战
14.ROS2从入门到精通：理论与实战
15.国内首个3D缺陷检测教程：理论、源码与实战
16.基于Open3D的点云处理入门与实战教程