ICP Variants算法模块化总结和ICP效果影响分析

ICP Variants算法模块化总结和ICP效果影响分析

本文主要参考Fran¸cois Pomerleau及其团队在2013年发表在Autonomous Robots上的论文:,摘要其中关键的论证和数据分析部分,并尝试结合个人理解独立完成的翻译和展开.

附赠自动驾驶最全的学习资料和量产经验：链接

1. 前言

本人在之前整理IROS2022的论文资料过程中，发现在“Point Cloud Registration”这一方向上有好几个团队做了非常创新和有意思的研究（可详见链接：ICRA 2022论文汇总与整理），然后在尝试研读和理解其中感兴趣的几篇论文后发现自己在“Point Cloud Registration”这个论题中都没有积累出基本的认识，论文阅读起来的感觉还是太过于“空洞”，这里空洞的原因在于本人没有充足的实验和项目经历以支撑我去理解各个论文中的创新点、贡献点的实验验证。
抛开大佬们做出的最新的方案和技术分享，我尝试去在“Point Cloud Registration”中形成一个最基本的认识和知识架构：

第一，Point Cloud Registration的本质问题是什么？第二，Point Cloud Registration最核心的技术方案是什么？第三，有哪些因素能够影响Point Cloud Registration的结果？

顺着上述几个基本问题的思路，“Point Cloud Registration”的主要目的其实就是找出（计算）两“块“点云之间的刚性变换关系（Rigid Transformation），那基于这一个”答案“，“Point Cloud Registratio”的相关技术和理论思路是可以被其他更大的领域方向所包含，比如三维重建研究、医疗成像相关应用研究和机器人学中SLAM和Localization的研究，等等。而在解决“Point Cloud Registration”这一问题的上，最广为人知和“有效“的技术方法就是Iterative Closest Point（ICP）算法了，进一步分析”影响Point Cloud Registration结果的因素“也就可以转化成分析”影响Iterative Closest Point 匹配结果的因素“了。

本文主要目的就是根据前人科学家和研究人员的工作，去分析和总结ICP 及其Variants算法的核心算法流程（由哪些必须的模块组成），并对比分析各个ICP Variants算法在实际环境数据集上的表现效果和差异性分析。最终，希望能持续地学习和跟踪相关方法以形成对ICP Variant算法知识体系的深入理解：明白为什么Iterative Closest Point\Point Cloud Registration在有些场景下有效、在有些场景的表现不理想，对算法的进一步的优化或者工程应用提供思路，等。

具体的来说，在本人的其他文章中经常提到的SLAM问题在退化场景、非结构化场景的解决效果往往不尽如人意，同一算法在不同的应用场景中的效果也是千差万别，等等此类的问题，我都希望通过这篇文章基于点云配准算法的表现来完成分析。

本篇文章将从ICP算法模块开始，先对各个经典的ICP Variants算法进行简单的总结，再分析和整理ICP算法的流程链条与模块。在ICP算法模块化总结和分析的基础上，开始ICP各个模块对算法效果影响的对比分析方法论阐明，再进行对比结果的分析与评价，最后完成全文的总结。

2. ICP算法模块

2.1 经典ICP Variants 算法的简单总结

回顾本人这些年来看过的ICP Variants算法，从标准的ICP算法论文开始，到经典的ICP Variants算法G-ICP、PL-ICP、P2PL-ICP、Voxel-ICP、CSM和NDT，再到最近几年的LiTAMIN、LiTAMIN2的关于激光配准模块优化的LO和CoBigICP等。每一种ICP Variants算法都有其核心的特点：

• 标准ICP基于两个假设：1.对应的pairsofpoints的数量和对应关系在迭代中保持不变，2.两个点云中的点完全相同，发展设计出的算法核心在于1. 通过在Euclidean Distance一定范围的点建立点间的对应关系，2. 通过构建误差优化问题模型，最小化对应点之间的Euclidean Distance平方和来求解每次迭代的点云之间的刚性变换关系（Rigid Transformation），3. 收敛条件可以是一定的迭代次数或者迭代间差异小于一定阈值。

• PLICP：算法设计的核心1. 将当前帧数根据初始位姿（initial pose）投影到参考帧（reference scan）坐标系下，2. 对于当前帧（reading scan）的每一个点云i，用最临近法在参考帧中找到最近的两个点（j1,j2），3. 计算点对之间的误差并去除误差过大的outliers，4. 最后根据最小化误差函数求解出RT。

• P2PL-ICP：在PLICP的算法上，将误差项的模型改变为点到面的距离（point-to-plane metric）

• GICP：该算法则在框架中结合了P2P和P2PL算法的方案，1. 误差度量中既考虑了当前帧（reading scan）中点的信息，也包含了参考帧（reference scan）的面信息，2. 算法中利用平面法向量给优化问题模型的目标函数的每一个误差“点对“（pair of points）赋予权重以降低outliers和bad初值的影响，3. GICP算法中还在优化问题建模中考虑了传感器的噪声模型；

• Voxel-ICP：算法的主要改动在于1. 将参考帧和当前帧的点云数据分割为3D体素，以体素代替点云，2. 在将点云转化到3D栅格后考虑体素的形状参数以进行后续的优化匹配流程；

• NDT：NDT算法也是不再以点云数据为处理对象，1. 配准时也是适应点云转化后的体素， 2. 但NDT的体素会再拥有一个独特的特征：基于点云的多维变量正态分布函数参数， 3. 随后在优化匹配流程中也是使用每个栅格体素中的正态概率分布特征构建最小负对数似然函数进行求解；

• CSM：此处的CSM指的是cartographer中使用的correlative scan matcher，不是combined scan matcher和canonical scan matcher。correlative scan matcher的算法核心模块中与标准ICP不同的地方在于，1. 算法中参考帧（reference scan）可以是一系列scans形成的meta-scan，2. 参考帧点云在预处理时会通过“占据概率栅格地图”的更新算法插入子图中维护和更新每个栅格概率形成“占概率栅格子图”，3. 随后的匹配优化模块也是根据当前帧在“占据概率栅格子图”中对应栅格的得分构建出优化问题模型。

• LiTAMIN & LiTAMIN2：该新型的ICP算法被认为具有超高的计算效率和精度，该系列算法的贡献点简要的说明起来就是1. 在第一版算法中也是基于正态分布概率的形式来进行匹配，但引入了F范数和正则化协方差矩阵对损失函数进行归一化操作来简单的实现了稳定性，随后在第二版算法中将原先point-to-normal分布的匹配方案修改成了distribution-to-normal分布的方案以减少计算时的点数，优化计效率，2. 第二版算法中利用一新的distance metric，将对称的KL-散度引入到反映两个概率分布之间差异的ICP代价函数中。代价函数不仅包括点之间的距离，还包括分布形状之间的差异，2. ，

• CoBigICP：也是一种基于概率形式的ICP变体算法，1. 其引入了相关熵（correntropy）的误差度量来作为噪声模型，以排除外点和构建优化模型目标函数，2. 该算法中还提出了一个有意思的寻找参考帧和当前中对应点对（pair of corresponding points）的方法，即在参考帧和当前帧中进行双向匹配的方法来构建一个对称点对模型。

以上只是本人粗略了解的ICP Variants算法的核心方案，而且看完论文的时间比较久远了，若有总结不正确的地方，还望大佬们斧正批评。

虽然仅仅简单讨论了上述较为经典和有影响力的ICP算法，但总觉得还是缺少了某一种更为重要的思考，我们其实可以发现若是停留在针对每一种ICP Variants算法总结和分析，而不进行相互间联系分析的话，还是没法入门ICP Variants算法的研究领域。

2.2 ICP 算法模块化分析

根据上一小节的思考，不妨再进一步的分析ICP Variants算法间的优化逻辑和结构联系。当然了，ICP算法的研究工作体量是丰富的、庞杂的，基于文章的篇幅和个人的知识能力是没法在这里尽善尽美地展开说明，但本文会尽力聚焦在ICP算法的核心模块上进行分析总结。

基于ETH Zurich的Roland Siegwart、 Laval University的François Pomerleau和National Institute for Research in Computer Science and Control的Francis Colas团队的工作，他们一致认为ICP Variants算法可以归结为执行多个处理序列的迭代算法，对于每一步和每一个模块（比如Data association和Error minimization model）都有多种策略可以选择和改变，并且每种策略都需要特定的参数。

一般的ICP算法流程链以二维或三维的两个点云作为输入，并估计两帧点云之间的平移和旋转变换矩阵参数，以构建出的优化问题模型最小化点云对齐误差。再者，一般把第一个点云称为参考点（reference），把第二个点云称为当前点（reading），ICP算法的本质就是试图将当前值与参考值对齐。为此，它首先对点云进行过滤，然后遍历一系列的处理模块。对于每次迭代，它将当前帧（reading scan）中的点与参考帧（reference scan）中的点关联起来，并找到一个将对齐误差最小化当前帧到参考帧的刚体变换。更具体地说，ICP算法链由几个步骤组成并分由模块实现，其步骤及对应的模块类型可有以下六种:

• 数据过滤（Data filtering）:此步骤适用于参考点云（reference point cloud）和当前点云（reading point cloud）。在此步骤中，0个或多个“DataPointsFilter”模块将一个点云作为输入，转换它并生成另一个云作为输出。转换可能会添加信息，例如表面法线，或者可能会改变点的数量，例如通过随机移除一些点。

• 刚体变换（Transformation）: 当前点云（reading point cloud）被旋转和平移。额外的数据，如曲面法线，也被转换。

• 数据关联（Data association）:匹配模块将当前点云（reading point cloud）的点链接到参考点云（reference point cloud）中的点。常见的方法就是通过基于kd树的快速k最近邻匹配来寻找该关联的点对（pair of points）数据。

• 离群点过滤（Outlier filtering）:零个或多个离群点过滤模块可以直接删除(hard rejection)和/或加权(soft rejection) 当前点云与参考点的匹配点之间的对应链接关系。该过滤的标准可以是一个固定的最大授权距或者是一个因素的中位数距离等。针对随后的最小化匹配优化步骤，若是通过加权对应链接关系进行过滤的话，权重为0的点将直接被忽略。

• 误差最小化（Error minimization）:误差最小化模块主要的工作就是考虑计算点云之间的变换矩阵以使参考点云（reference point cloud）和当前点云（reading point cloud）之间的定义的度量误差最小化。在该误差最小化的优化模块中，系统是可以在最开始定义不同的误差函数，比如常见的“点对点“度量和”点对平面“度量。

• 收敛检查（Convergence checking）:零个或多个ConvergenceChecker模块可以根据某些条件停止迭代。例如，收敛条件可以是循环执行的次数，也可以与匹配误差量相关。因为ICP各模块可以被系统流程链接，所以系统中应该定义模块之间的关系必须通过or条件达成一致，而所有的and条件都在单个模块中定义。

综上，若是根据该ICP算法模块化分析结构再去看2.1中各个ICP算法的贡献点和不同，就能够更加宏观地理解和理清算法间的关系和进化脉络了。例如，PL-ICP在标准ICP算法模块上的主要改进就是在于“数据关联“模块和”误差最小化“模块中误差度量（distance metric）的优化，同理P2PL-ICP和PL-ICP也是在于“数据关联“模块和”误差最小化“模块中误差度量（distance metric）的优化的不同，再看LiTAMIN & LiTAMIN2，除了上述两个模块的改进外，还针对”数据过滤“和”离群点过滤“模块进行优化改动。

3. 对比分析方法论

根据第二部分的分析将ICP Variants算法的脉络大抵地分为了六个核心模块，并简单的分析了经典ICP Variants算法中每种贡献点是集中在了哪些核心模块中。但是每个核心模块对ICP算法的最终结果的影响效果都相同吗？每个核心模块又各主要受哪些内在、外在因素影响呢？前人研究学家的工作出发点是什么样的呢？若是想进一步的在ICP Variants算法领域做出贡献的话，又该从何下手呢？

这些问题的解决必然是需要进行一系列的科学严谨地实验研究来完成对比总结的，本人认为也只有在分门别类的基础上，再去理解各类别各模块之间的关系和影响力才能到达深入理解范畴的起点或者激发出工程应用的好点子。

此处再一次感谢前人科学家们的工作和贡献，Rusinkiewicz和Levoy在ICP算法变体比较方面的开创性工作推动了Scan Registration领域的重大进展。Wulf及其团队提出一种与ICP密切相关的SLAM评估方法:他们使用pairwise scans和使用meta-scans(一系列的scans)进行SLAM以比较各类ICP Variants算法，同时他们观察到，与pairwise scans匹配相比meta-scans会更慢地积累误差但增大计算压力。除此之外，还有Roland Siegwart、 François Pomerleau和Francis Colas团队的关于ICP Variants算法在Real-World数据集上的对比分析工作。

3.1 方法论概述

此处，将结合前人研究人员的工作和总结，对更强、更科学、更严谨和更完善的实验方法论需求进行概述，同时该方法论也将秉承着科学实验方法的三个原则(即比较性、再现性/可重复性和论证性/解释性)，并允许研究人员获得内在(例如，计算时间，使用的参数，参数行为)和外在(例如，准确性，精度)量的洞察。谈到此处，可能就有读者明白我考虑的分类原则是什么了，没错，本文提及的对比实验方法论考虑的重心即为两类：

1. A set of external factors

2. A set of performance metrics

• 前者“a set of external factors”可理解成影响算法效果的外在因素，比如我们常说的**“算法所应用的场景”（适用于室内场景、适应于城市复杂场景、适应于高速场景亦或者适应于强结构性的场景），而一个“优秀”的ICP算法除了需要在其论文中注明的场景中精度达到预期，其鲁棒性稳定性也是众多研究者们看重的方面（总不能在某个场景是state-of-art，换了场景就成了piece-of-shit吧），另一个研究学者们看重的点是“不同ICP算法在配准初值不准确时的容忍度”，面试中常问到的“ICP和NDT算法收敛特性有什么不同”也是大家考虑的点，以及“不同ICP算法对点云间低空间覆盖率（lowspatialoverlap）的敏感度”**也是值得关注的问题方向。

• 后者“a set of performance metrics”，顾名思义就是评价ICP算法的评价度量方式了，**不同的ICP算法在不同的评价度量方式下必然会得到不同的结论，只有robust的评价度量才能用以描述完整的ICP Variants算法。**比较常见的“不太严谨”的方案是将ICP算法的高维度研究结果用低维度的性能度量去描述（6DoF的问题研究在结果的performance metric选择上却使用2DoF的状态）。Wulf O.及其团队研究ICP Variants算法在3D SLAM轨迹精度表现，但使用2D的ground-truth来计算每个算法的maximum error和标准差作为performance metrics。还有研究团队使用计算出的“与ground-truth的欧几里得距离差值”和“旋转矢量参数化后的方向差值”定义了两个独立的均方根(rms)误差(即一个在平移分量上，另一个在旋转分量上)来表征性能。基于旋转误差量的表征方式，Huynh及其团队提出了科学文献中使用的六种不同类型的SO(3) 距离的评估，她们认为欧拉角的距离误差不能使用norm of rotation vector parametrization，而应是在单位球面上的测地线距离。以上的performance metric各有优劣，取长补短整合出最robust的评价度量方式即是最终的目的。

3.2 针对Sensitivity to Input的讨论

大抵上可以将input分为“initial-pose misalignment”，“scan outliers”和“data sets”。由于icp算法本质上是一种近似的局部收敛算法，其结果与初始姿态（initial-pose）有关，而初值通常是由积累的里程计或启发式运动模型等惯性测量提供的，由此提供的初值精度有限且不确定性随着观测间隔时间的增加而增加。因此，评估一个ICP 算法如何基于各种初值收敛接近正确的姿态是很重要的鲁棒性研究。

• 针对**“initial-posemisalignment”**的控制，作为一个合理的近似，公认的较好的操作是通过在ground-truth pose上添加扰动（perturbations）来生成初始对准空间（space of initial-pose alignment）。

• 给Scan Matching带来困难的另一个外在因素就是**异常值的数量（amount of scan outliers）**了。准确的说，outliers主要是影响在数据关联（Data association）模块，通过一定的数据关联metric，若是当前帧中的点在参考帧中找不到对应的点（无法生成pair of corresponding points），ICP则有因错误匹配而收敛到错误的局部最优风险。 **进一步的分析scanoutliers的来源，其主要来自于三个方面：动态障碍物体带来的返回值，传感器的FoV与发布频率和传感器的运动方式。**第一种由动态障碍物体引发的极端情况是：上一帧参考帧点云都是打到静态环境的返回值，下一帧当前帧点云都是打到动态障碍物的返回值，那么由此input的匹配必然不会输出精确的结果，第二种由于传感器FoV和运动引发的极端情况是：当传感器做快速的旋转运动，而传感器的FoV很小和发布频率很低，则参考帧点云和当前帧点云也必然是难以找到合理的对应点对（pair of corresponding points），从而影响匹配效果。

• 最后，数据集的场景环境也是一个关键的变量，往往对ICP算法的结果产生很大的影响，但数据集的场景环境作为一个外在因素变量是可以严格且方便控制的（直接在同一数据集上运行多种ICP Variants算法和同一ICP算法在不同数据集场景）。

3.3 针对Evaluation Metrics的讨论

在完成对输入量的处理和各个ICP核心模块的功能后，就能获取每个pair of points的匹配结果并作为后续计算误差分布中的样本。在给定的data set环境和给定的input扰动水平下，对所有pair of points的累积误差分布可以简化对特定ICP算法解的样本分析，在不同的data set环境中也可以给特定ICP算法解的生成误差边缘分布（marginal distribution of error）。为了保证完备性和严谨性，每个pair of points的样本都基于ICP算法的刚性变换矩阵结果SE(3)来计算生成对应的位移和旋转误差量，具体的公式如下：

Tr和Tg分别表示ICP算法得出的变换矩阵和ground-truth的变换矩阵，et表示位移误差，er表示旋转误差，值得说明的是该旋转误差定义为直接从旋转矩阵得到的测地线距离（Geodesic distance）。

在获得每一个样本的误差度量后，将尝试对整体系统的准确度和精确度进行数据评估，由于ICP算法的误差分布模型不太符合高斯概率分布规律，经验均值和协方差并不是解释精度和准确度的真正指标值，因此多数的研究学者选择了中位数和分位数来进行描述系统的误差分布（标记为A50, A75, A95，以分别作为误差分布的概率0.5(即中位数)，0.75和0.95的分位数）。当A50、A75和A95的值接近于零时，则可以表明计算结果是准确的（accurate）。如果这些分位数之间的差异很小，则表明计算结果是精确的（precise）。

最后，每个ICP算法的计算耗时也应该作为关键的指标用以评价算法效率，但算法实现的编程语言选择、程序员技术水平、并行度等都是影响时间性能的因素。故此一般来说，时间评价应做定性的测试评价即可（除非所有这些因素都被控制并尽可能的统一）。

3.4 方法论总则协议

首先，ICP Variants算法应始终与普遍接受的ICP baseline算法进行比较，后续的“实验结果及评价”部分也是主要针对ICP baseline算法进行分析讨论；其次，ICP Variants算法需要在足够的数据上进行比较以降低过拟合的风险，并确保具有统计学意义的解释，此处的“足够的数据”既包括在不同的scan overlapping级别上涵盖不同类型的环境，还包括不同扰动级别的input（根据数据集的特点：主要是环境中元素的尺度和传感器的噪声，提出三种不同的扰动水平：易、中、难）；最后，应该根据误差的分布来进行实验比较，而不是只对单个结果进行比较。

4. 实验结果及评价

4.1 数据集介绍

该部分选择的数据集均来自：“Challenging Laser Registration" 数据集，Pomerleau, F., Liu, M., Colas, F., Siegwart, R.:Challenging Data Sets for Point Cloud Registration Algorithms. The International Journal of Robotics Research (2012)。

• **Apartment：**在室内公寓场景的30Hz的Kinect RGB-D 传感器数据；

图：室内公寓场景

• **Stairs：**在七层楼梯环境的“搜寻营救”机器人的传感器数据（需要说明的是由于爬楼的过程，该机器人的编码器数据很不可靠）；

图：楼梯场景

• ETH： ETH的室内建筑场景，环境中有柱子和拱门的大走廊；

• **Gazebo：**基于Gazebo仿真环境搭载的覆盖在公园凉亭上的酒树环境；

• **Wood：**周围有茂密的植被的小路场景；

• **Plain：**覆盖高寒植被的小凹盆地环境。

图：Gazebo覆盖在公园凉亭上的酒树环境

以上这六个数据集大体上涵盖了各种类型的环境:人工的和自然的，杂乱的和开放的，同质的和高度可变的。另外，前人学者们估计了上述六个数据集的点云重叠率，下图表示为为扫描A中同时存在于扫描b中的点的百分比。深红色表示高重叠，深蓝色表示低重叠。对角线元素的比例为1。

4.2 扰动量（pertuibations）影响讨论

上文提及到initial-pose的输入包括不同扰动级别，根据数据集的特点：主要是环境中元素的尺度和传感器的噪声，提出三种不同的扰动水平：易、中、难。具体的数据生成标准如下表所示：

表：每个分量的标准差和每个扰动水平的样本数

基于六个数据集环境和三种不同级别的input扰动量，下图表征出了易、中、难三个扰动集的平移误差累积概率函数，填充的背景显示了各自的理论分布。

图：平移误差累积概率函数

上图中线条的不平滑部分主要是由于样本数量不够导致的拟合过程生成了跳变，但大抵上是完全可以得出initial-pose上的扰动量对ICP算法结果的影响量。

4.3 ICP优化模块参数影响讨论

首先，考虑数据过滤（Data filtering）模块参数，在point-to-point的情况下，以均匀概率对两个点云进行下采样。研究者们将当前帧点云数保持在(0.001，0.01，0.05，0.1，0.5，1.0) 的比率和使用(0.001，0.01，0.05，0.1，1.0) 的比率作为参考帧的点云数目保持率。在point-to-plain的情况下，因为需要提取法线，所以使用sampling-surface-normal功能并将探索大小的阈值设为(5、7、10、20、100、200)。对于当前帧点云，则使用与point-to-point相同的下采样方法寻找(0.001，0.01，0.05，0.1，0.5，1.0)的比率。经过实验比对，研究学者得出该模块的优化得到point-to-point的参考帧和当前帧的比值为0.05，point-to-plain的当前帧的比值为0.05，参考帧阈值为7。

其次，数据关联（Data association）模块则选择使用Kd-tree进行临近搜索来进行pair of points的查询，而需要考虑的参数值包括：（1）参考帧和当前帧中的每个点相关联的最近邻居的数量，（2）最近邻返回数的容忍度，（3）最大临近搜索窗口大小。需要说明的是，针对不同的ICP Variants算法数据关联（Data association）模块的技术实现有很多种，此处仅选择KD-Tree作为一个例子进行说明。

随后，离群点过滤（Outlier filtering）模块进行简单的hard-code强制过滤一定比例的点云数量，在数据关联的基础上，point-to-point保留75%最近的点，为point-to-plain保留70%最近的点。实际的众多ICP Variants算法会有更新颖和高效的方式进行outliers的判断与过滤，此处的强制过滤只是简化实验流程。

4.4 实验结果

**重点：**该小节中提及的所有实验结果和总结都是基于Roland Siegwart、 François Pomerleau和Francis Colas团队的关于ICP Variants算法在Real-World数据集上的对比分析工作完成的。

通过对比2种典型的ICP算法在6个不同数据集上的随机选择的35对点云样本，在输入中分“易、中、难“的3种级别每轮进行64个perturbation的输出，一共得出80640个ICP计算匹配结果，并将所有的配准结果进行基于3.3小节中Evaluation Metrics的误差计算得到下表：

表：对于不同的扰动(简单(EP)，中等(MP)，困难(HP))所得的总体精度误差。上:位姿误差[m]，底部:旋转误差[rad]，较暗的色调对应较高的误差

由上表，整体平移误差结果表明，point-to-plane (A50 = 0.76 m)的**准确度（accuracy）比point-to-point (A50 = 0.97 m)的高20%。当观察A95和A50之间的差异时，优势就相反了，这表明point-to-point的精确度（precision）提高了30%。point-to-plane旋转的准确度（accuracy）增益为40%，而point-to-plane旋转的精确度（precision）**优势保持在30%。总的来说，通过上表可以得出：p2point更加precise，而p2plane更加accurate。

再来分析算法所应用的场景环境对匹配效果的影响力，实验数据处理成下图：

图：所有扰动和数据环境下点对面和点对点性能的比较。粗红条对应A50(即中位数);蓝色矩形的上端是A75，虚线的上端是A95

由上图可以看到在ETH数据集下A95分位数都超出了图表范围，pont-to-piont的位移误差在12.16m，而point-to-plane的位移误差在16.87m。不论在那种数据集环境下，每种ICP算法的准确性结果都遵循着同一结构级别，也就是大体上来说，A50误差量point-to-plane会更小，但A75和A95的增益point-to-plane会很大，由此我们可以得出point-to-plane在“恶劣情况“下的表现并不会保证能比point-to-point好。

另一个需要在数据集环境的影响力对比中进行说明，ETH和Plain数据集分别包含了长走廊和平原空旷环境（都是非结构化、特性信息少的退化环境场景），但由于ETH中是带有重复元素的长走廊，因此很容易受到相似特征的干扰引发错误匹配（陷入局部最优），故ETH在A95的误差表现上反而还差与Plain场景的A95误差量。

接下来的图表表达了在低扰动干扰下数据集环境的影响，**这种情况一般都发生在移动机器人的作业场景下（能够保持较低的不确定性）。**除了Wood和Plain数据集之外，所有环境的中值误差都在10厘米以下。虽然被认为是半结构化的环境，和其他数据集环境下的结果相比，Gazebo数据集与Apartment数据集一样都保持更低的误差。

图：低扰动水平下的环境影响，灰色条纹对应A50、A75和A95的分位数

进一步的，基于point-to-plane拥有更好性能的结论，针对同一数据集场景同一ICP算法下，不同扰动下的输入对算法的效果影响如下图所示：

图：点对面icp算法的累积误差概率。三种扰动水平对背景填充的Apartment数据集的影响对应初始扰动的理论曲线

由上图可得出，理想情况下，所有pair of scans在配准后的误差将比它们的相关扰动水平更少。另外还可由上图观察到，对于所有的扰动类型，大约25%的配准结果仍然呈现比初始扰动更差的转换。Roland Siegwart及其团队认为，基于上述六个数据集的点云重叠率的计算图标，可以看到Apartment数据集的点云重叠率在不同的位置下也存在较高的重叠率，而基于point-to-plane的ICP算法对一系列不同的重叠比率的鲁棒性较弱由此导致了“大约25%的配准结果仍然呈现比初始扰动更差的转换“的问题。

最后，下图显示了在所有环境和所有扰动类型上scan间预先计算的重叠率和平移误差之间的关系。对重叠相同的pair of points（scans）每个bin（大小为0.08）提取统计量A50、A75和A95。基于point-to-point和point-to-plane的ICP算法共享相同的离群点过滤（Outlier filtering），该Module调优处理70%和75%的异常值。这导致point-to-point和point-to-plane的ICP算法遵循相同的趋势，导致在低重叠值时性能较差。在0.30 ~ 0.38的重叠范围内，误差中值误差大于2m。

图：在所有环境和所有扰动类型上，scan重叠率与点到平面的位移误差之间的相关性

5. 总结

本文中在数据误差结果统计时使用的A95统计数据可能看起来有些excessive，但众多研究学者和工业界大佬们认同的一点是，A95它意味着若是有一个误差超过20的匹配结果是远不被接受的，这是非常重要的一点，并且这也是一个稳定的、鲁棒的系统和一个“脆弱的“系统之间的区别。

基于本文中的实验对比，点到平面的ICP算法对于以下应用场景来说是稳定的:

• 第一，环境类型可以被控制为高度结构化的、特征明显且丰富的;

• 第二，机器人运动时点云数据保持高度的重叠率;

• 第三，状态估计是作为匹配的初值位姿误差应保持在10 cm和10°以内；

以上的条件在实验室实验中很常见，但在实际应用中往往难以保证时时刻刻保持。

本文中离群点过滤（Outlier filtering）模块都是采用的hard-code的形式强制过滤一定比例的点云数目，但基于环境场景的不同应当使用更灵活的异常值抑制算法以应对重叠率的可变性情况。

最后，从第二部分中举出的9种经典ICP Variants算法开始，完成了对各个经典的ICP Variants算法的简单总结和ICP算法的流程链条与模块的分析和整理。通过第三部分和第四部分的内容完成ICP各个模块对算法效果影响的对比分析和评价，希望来深入的理解每种算法创新的内在驱动力和对ICP Variant算法进行相互间的联系理解，以尝试入门ICP Variants算法的研究领域，为今后在ICP Variants算法工程上的应用、问题的解决和研究领域上的贡献理清思路和方法论。