LiTAMIN：基于正态分布几何近似的SLAM
LiTAMIN：基于正态分布几何近似的SLAM
编辑丨当SLAM遇见小王同学
声明: 本文只是个人学习记录，侵权可删。论文版权与著作权等全归原作者所有，小王自觉遵守《中华人民共和国著作权法》与《伯尔尼公约》等国内外相关法律，本文禁止转载！！
“ 本文提出了一种3D LiDAR SLAM方法，该方法提高了采用具有正态分布簇的局部近似几何的ICP算法的准确性，鲁棒性和计算效率。与以前的基于正态分布的ICP方法相比，作者的ICP方法可以通过Frobenius范数和正则协方差矩阵对成本函数进行归一化。以前的方法通过主成分分析，其计算成本比LiTAMIN高。而且，LiTAMIN可以减少不正确的闭环约束的影响。实验结果表明，LiTAMIN优于包括LOAM，LeGO-LOAM等方法。 ”
简介
SLAM是自主移动机器人的一项基本技术。特别是LiDAR SLAM被广泛使用，因为它在室内和室外环境中均表现出稳定的性能。LiDAR的SLAM方法分为两类：基于匹配和基于特征。基于匹配的方法采用几何配准技术，例如迭代最近点（ICP）和正态分布变换（NDT）。这些方法通过直接使用扫描点来提供准确的位置估计。但是，它们在计算上效率不高，因为它们使用大量的点进行稳定配准。基于特征的方法的计算效率得到了广泛研究。这些方法提取特征并使用例如线段和平面。但是，当环境中的几何特征不足时，这些方法将变得不准确且不稳定。
本文介绍了一种基于匹配的SLAM方法，该方法提高了ICP的计算效率和鲁棒性。作者认为，提高ICP算法的稳定性对于提高基于匹配的SLAM方法的整体性能至关重要。LiAMIN使用正态分布近似几何形状，这种方法不是照原样使用从LiDAR获得的点云，而是修改了点云以减少点数并使密度更均匀。这样可以实现更快，更稳定的SLAM。还为ICP提出了一个新的成本函数，以便可以有效，稳定地进行最小化。此外引入了一个指标，以减少LiDAR SLAM在闭环检测中离群值的影响。实验结果表明，与最先进的方法相比，LiTAMIN所实现的改进。图1是LiTAMIN的结果。
系统概述
图2是系统LiTAMIN的介绍
LiDAR Odometry块包含两个线程：使用我们的ICP方法的位姿跟踪线程和使用跟踪线程结果的局部建图线程。
里程计模块不使用全局地图来避免扫描丢失，而是使用包含里程计的局部地图。建图更新周期是恒定的，因为局部建图线程不执行闭环。系统通过从局部地图中删除旧点来减少不一致的影响。通过跟踪线程累积每个局部扫描来构建局部地图，以处理一次扫描的稀疏性。
关键帧制作模块输出局部地图和局部地图之间的相对位姿，同时累积LiDAR里程计并每10 m设置关键帧。通过将关键帧写入存储设备（例如硬盘驱动器或固态驱动器），可以减少内存使用量。
作者认为跟踪线程计算出的轨迹对于10 m的行程范围足够准确，这是建立关键帧的扫描累积距离。在位姿图上闭环后，系统以关键帧为单位改变全局地图。
位姿图优化器可校正关键帧之间的最近相对位姿，并检测位姿图中的循环。当检测到循环候选者时，优化器将从存储中读取必要的关键帧。闭环检测将距离当前位置半径30 m范围内的所有关键帧列为循环候选，然后应用基于ICP的闭环进行处理。
系统将ICP应用于每个循环候选者。有时，闭环检测器线程会相对于里程计而延迟进行ICP处理。由于里程计的计算独立于全局地图，因此尽管全局建图可能会延迟，但延迟不会影响总的计算结果。应用ICP之后，系统将所有相对位姿（包括误差）插入到位姿图中，还消除了位姿图优化中的错误位姿。
快速稳定的ICP
在需要实时处理的SLAM系统中，ICP方法必须在准确性和鲁棒性之间取得平衡，以获取计算效率。标准ICP和其他可靠的方法直接采用点云。点云中的优化初始解和均匀点密度这些方法是理想的。尽管一些鲁棒且准确的ICP方法可以确保在没有初始解决方案的情况下实现全局最优，但它们具有很高的计算成本。对于需要对每个帧进行恒定ICP处理的SLAM，这些方法不切实际。减少3D点的数量是提高计算效率的最有效解决方案之一。许多基于ICP的SLAM系统经常使用具有体素网格和正态分布的ICP方法，因为它们可以减少计算成本，同时仍保留足够的几何信息。其中，NDT 和GICP是最流行的方法。
我们的目标是提高这些基于正态分布的方法的准确性和鲁棒性，同时实现可与基于特征的方法相媲美的计算效率。表I列出了我们的ICP方法与其他方法之间的区别。
1 地图表示和点关联
体素网格或KD树用于SLAM系统中的地图表示和对应搜索。体素网格表示在计算效率方面具有优势，因为体素的数量显着低于原始点云中的点数。相反，KD树表示在配准的准确性和鲁棒性方面具有优势。关于找到对应点，KD树表示可以找到具有最近邻搜索的关联点，而体素网格表示不能保证NN搜索。关于计算成本，体素网格表示在对应搜索中具有优势，因为体素网格的计算成本为O(N)，而kd树的计算成本为O(Nlog(N))。
LiTAMIN结合了两种表示形式的优点。LiTAMIN通过体素滤波将LiDAR数据表示为减少的点集数量，其中每个体素由单个点表示，特别是体素中包含的3D点的质心。该图还由体素网格表示，以减少点的数量并使密度更均匀。点关联由具有KD树表示的NN搜索确定。作者实现了1 m的体素大小；因此，与关键帧对应的局部地图的大小为200 m×200 m×40 m。
2 成本函数和避免退化
LiTAMIN采用类似于NDT和GICP的方式，采用局部几何形状近似于正态分布的ICP，这应该能够应对协方差矩阵的退化。如果局部几何形状是一个平面，则协方差矩阵的最小特征值是0或非常小；因此，表一中NDT和GICP的成本函数与退化的协方差矩阵不同。如果协方差矩阵不具有逆函数，则某些基于NDT的方法将应用PCA并将表示形式更改为点到平面距离度量。
GICP通过在PCA之后应用以下变换来使用协方差矩阵C：
但是，由于PCA应用于所有体素具有较高的计算成本，因此PCA的这种稳定化技术不适合快速计算。为了降低成本，作者提出以下协方差转换：
将λ凭经验设置为10-6，因为该数字对应于标准偏差为1 mm的正态分布；该数字不会影响ICP结果，因为LiDAR的测量误差范围为几厘米。用GICP中的diag（1,1，ε）替换特征值表明，特征值的大小不会影响ICP结果的准确性。协方差矩阵的退化方向对GICP的准确性有着重要作用。考虑到这一点，通过Frobenius范数对协方差矩阵进行归一化，因为使用特征值缩放矩阵不会影响几何配准。
Frobenius范数通过特征值的平方和指示矩阵的比例，定义如下：
3 鲁棒闭环
作者采用迭代加权最小二乘法的方法。此外还提出了一种简单直观的加权方法。
位姿图优化方法中的成本函数如下：
尽管ICP结果提供了和，但这些值可能存在异常值，因为ICP无法保证全局最优。
考虑到误差
的方法通过以下权重计算解决了这个问题：
因为σ和σ的值取决于ICP的精度，所以通过考虑ICP输出最佳值时的误差来确定数量。
在最佳情况下，通过采用角速度ω（ω，ω，ω）来考虑旋转矩阵的以下小变化
当旋转矩阵足够小时，由于的对角元素变为零，因此误差eR仅具有非对角元素，并且可以近似为
因为将ω看作是任意轴的旋转角度，所以可以得出结论，ICP结果的旋转角度精度在最佳值的3度以内，并且将值设置为σ=3√2度。同样，因为作者认为平移的精度约为10厘米
作者将值设置为σ= 0.1 m。当和大于容许误差时，接近零。作者的方法可以将ICP的失败结果排除为异常值，因为权重为零时相应的约束不会影响成本函数。
实验
在本实验中，作者评估了LiTAMIN和其他最新方法生成的轨迹的准确性，并将其与从图3所示实验设备获得的地面真实轨迹进行了比较。
获得了四个变化的评估数据集：使用手推车，步行，轮椅和Segway。
每种SLAM方法都使用VLP-16 LiDAR重建轨迹。而且还使用了精密测量仪器，Leica Pegasus系统和RTK-GPS来获取地面真实数据。在本实验中，将其他四种SLAM方法与LiTAMIN进行了比较，评估是使用具有Intel Core i7-7820HQ处理器的笔记本电脑进行的。
评估的误差指标是均方根误差（RMSE），计算为
此外，还评估了高度误差，
地平面，
从轨迹切出的线段中的误差，
表II列出了误差评估的结果。在该表中，红色结果表示最佳精度，蓝色结果表示次佳精度，而粗体结果则表示每个误差度量的第三佳准确性。
图4显示了地面真实轨迹和每种方法估算的轨迹。
图5显示了LiTAMIN结果。
表III列出了每种方法的计算时间以及实际时间。
从使用所有VLP-16数据帧（没有帧丢失和线程休眠）构建地图的总计算时间中获得了结果。字体颜色和粗体以与表II中相同的方式对计算时间进行排名。表IV列出了我们SLAM系统中每个执行功能的每个功能的平均计算时间。
为了评估LiTAMIN在大循环情况下的准确性，在较大的环境中使用Segway数据集的相同实验设备进行了实验。图6示出了每种方法的轨迹和地面真相。
图7显示了LiTAMIN叠加在航空照片上的结果。
表V列出了错误评估的结果。
讨论
与基于特征的方法LeGOLOAM和LOAM相比，基于ICP的方法在1 m和3 m的短段中更加准确。此结果表明，基于ICP的方法比基于特征的方法进行局部姿态跟踪的准确性更高。
表II中Segway数据集的准确性降低表明它的轨迹是最难确定的。PCA方法和LeGO-LOAM在Segway数据集的较长片段中显示出较差的准确性。但是，与其他方法相比，这两种方法在较短的片段中并没有明显差的结果。这些结果源于以下事实：这两种方法在轨迹的一部分期间丢失了位姿跟踪。
可以看出，对于长度超过100 m的长段，具有闭环的LiTAMIN与没有闭环的LiTAMIN相比有所改善。然而，对于短轨迹而言，这种改进并不明显。这表明闭环检测可以提高轨迹的一致性，但不能提高局部几何形状的准确性。尽管在PCA方法和hdl中可以类似地看到这种趋势，但它表明应该提高局部跟踪精度以提高局部地图几何的准确性。
结论
本文介绍了一种SLAM系统，该系统具有更高的准确性，鲁棒性和计算效率。LiTAMIN的主要贡献是归因于通过正态分布近似局部几何形状的ICP更加稳定，以及通过简单直观的加权实现鲁棒的闭环检测。实验结果表明，比其他最新的SLAM方法更准确，并且对于某些数据集是稳定的，而其他方法则遇到位姿跟踪失败。而且尽管是基于ICP的系统，但其计算效率可与基于特性的最快方法LeGO-LOAM媲美。
本文仅做学术分享，如有侵权，请联系删文。
下载1
在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2
在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。
下载3
在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。