雷达系列论文翻译（十）：Scan registration using segmented region growing NDT

Scan registration using segmented region growing NDT

这篇论文可以视为对上一篇论文的详述版本，作者为同一人，这篇中对于每一部分的算法给出了详细的公式和步骤

摘要

正态分布变换扫(NDT)描配准算法使用矩形体素栅格划分点云，然后将每个单元内的点建模为一组高斯分布。为了配准扫描需要执行非线性优化，然而当点跨越栅格边界时，基于体素栅格的方法导致不明确的代价函数导数。在这项工作中，提出了一种分段区域增长的NDT(SRG-NDT)变体，该变体首先从扫描中去除地面点，然后使用环境中的自然特征为NDT算法生成高斯聚类。地面点的移除显著地加速了扫描配准过程，而对配准精度并没有影响。通过对剩余点进行聚类，与基于体素栅格的NDT方法相比，SRG-NDT方法能够用更少的高斯分布对环境进行建模，这允许平滑和连续的代价函数，并保证优化将收敛。此外，使用相对数量较小的高斯分布允许运行时间的显著改善。在城市和森林环境中的实验表明，SRG-NDT方法能够达到与现有方法相当的精度，但是与ICP、G-ICP和NDT相比，计算时间平均减少了90.1%、95.3%和94.5%。

引言

扫描配准方法在移动机器人的建图算法中起着重要作用。许多传感器以点云的形式提供环境信息，如RGBD相机、LIDAR和双目相机。扫描配准算法依赖于扫描之间的重叠几何来确定两次扫描之间的相对变换。了解这些变换参数可以融合点云数据，这是未知环境中自动操作通常需要的。

最流行的用于建图的扫描配准方法是迭代最近点(ICP) Besl和ckay(1992)；Chen和Medioni(1991)；Zhang(1994)，广义迭代最近点(G-ICP) Segal等(2009)，正态分布变换(NDT)Biber和Strasser(2003)；Magnusson等人(2007年)；Stoy-anov等人(2012a)算法。ICP算法试图找到变换参数，使得最近邻点之间的欧几里德距离最小化。由于ICP在点之间完全形成对应关系，当点云稀疏时，扫描配准的质量会下降。G-ICP通过使用局部邻域计算每个点的表面法线，并使用表面法线信息来提高扫描配准的精度改善了ICP算法。然而，当点云稀疏时，或者当从非平面表面(如树木和灌木)采样点时，表面法线的质量很差。此外，ICP和G-ICP算法的主要瓶颈是用于识别点对应关系的昂贵的最近邻搜索，这通常在计算上是昂贵的。

NDT算法是一种不需要显式计算最近邻对应的替代方法，因为它将点云表示为一组高斯分布，而不是单个点。当执行配准时，一次扫描由基于体素的分布表示，并且根据第二次扫描中从高斯分布中采样的点的概率计算重叠分数。为了对齐扫描，执行非线性优化以确定变换参数，使得重叠分数最大化，然而由于点跨越单元边界，代价函数不连续。此外，基于体素栅格的NDT离散化产生的高斯分布不一定精确地模拟环境。这些分布仅在单个尺度上对每个栅格内的点进行局部建模，可能不包含扫描中存在的有助于广泛对齐的大的特征。

在这项工作中，我们应用分割和聚类技术来改善NDT算法的收敛性和实时性，特别是在室外环境和稀疏点云的情况下。介绍了分割区域生长NDT(SRG-NDT)，它识别和利用环境中的自然特征，为NDT算法生成高斯聚类。首先，使用基于高斯过程的分割算法从扫描中去除地平面，并且应用计算高效的区域生长算法来聚类剩余的点。根据自然特征进行聚类，与标准NDT算法相比，可以使用更少的分布来精确模拟环境。地面分割的应用还导致将环境分解成地面和障碍物部分，这可以用于更高级别的任务执行。为了执行配准，场景扫描中的每个点的代价是相对于来自参考扫描的所有聚类来评估的，从而产生平滑和连续的代价函数，这保证了优化的收敛性。类似地，在分布对分布的匹配中，场景扫描也被分割和聚类，并且相对于来自参考扫描的所有聚类来评估新扫描中每个聚类的代价。

验证该方法的实验是使用Ford视觉和激光雷达数据集的室外数据以及在伍斯特理工学院(WPI)森林环境中收集的数据进行的。实验给出了三个主要结果。首先，我们证明了SRG-NDT算法执行扫描配准的精度与现有的最先进的方法相当。其次，我们表明从点云中移除地面点减少了扫描配准所需的计算时间，同时保持了与使用完整点云执行的扫描配准相当的配准精度。最后，我们说明了与现有方法相比，SRG-NDT算法能够以显著更快的时间执行扫描配准，因为SRG-NDT与ICP、G-ICP和NDT相比计算时间平均减少了90.1%、95.3%和94.5%。

这项工作如下进行。第2节介绍了与现有扫描配准和聚类方法相关的工作。第3节阐述了扫描配准问题，并详细描述了NDT方法。在第4节中，我们介绍了SRG-NDT算法以及基于高斯过程的地面分割和分割增长聚类的相关细节。第5节介绍了SRG-NDT方法的实验结果。最后，第6节总结了我们的发现并提出了未来的工作方向。

相关工作

最著名和最广泛使用的扫描配准算法是ICP算法，由Besl和McKay (1992)、Chen和Medoni (1991)和Zhang (1994)独立介绍。ICP算法试图找到使参考扫描和场景之间的对应点的欧几里得距离最小化的变换参数。为了计算对应关系，对照参考扫描查询场景中的点，以确定参考扫描中的点，该点是到查询点的最小欧几里德距离。当对应点之间的欧几里得距离之和没有充分减小时，计算点对应关系和最小化对应点之间的欧几里得距离的过程被迭代执行，直到收敛。实际上，扫描的点密度通常会随着距离激光扫描仪的距离而减小，因此移动的车辆可以创建扫描，其中相同的空间区域以不同的密度进行采样。ICP算法在不同点密度的情况下有困难，因为它对每个点寻找单独的对应关系，并且不使用任何表面信息。为了解决ICP的缺点，Segal等人引入了广义ICP (G-ICP) Segal等人(2009)，该方法使用局部邻域计算每个点的表面法线，并且仅包括两次扫描之间相似表面法线结构的点之间的对应关系。G-ICP明确地将传感器噪声和采样特性考虑在内，因此与ICP相比提高了收敛范围和收敛速度。G-ICP已进一步应用于配准点云和相机数据的配准，其中初始扫描对准使用高维的具有尺度不变性的特征变换(SIFT)特征完成，最终使用G-ICP执行细化(Pandey等人，2011b)。ICP和G-ICP方法的一个主要缺点是需要产生最近邻对应关系。这个步骤通常计算量很大，并且已经被证明是ICP算法的瓶颈。

扫描配准的另一种方法是无NDT。NDT算法最初是由Biber和Strasser(2003)提出的，用于2D扫描配准和建图，后来扩展到三维(Magnusson和Duckett，2005；Takeuchi和Tsubouchi，2006)。NDT算法将参考扫描表示为一组高斯分布，这些高斯分布将参考扫描的表面局部建模为概率密度函数。给定所需变换的参数估计值，分配一个分数来量化参考扫描和变换后场景扫描之间的重叠量。重叠分数是通过在高斯分布上评估变换后的场景扫描点来确定的。为了配准两对点云，执行非线性优化以确定变换参数，使得重叠分数最大化。Stoyanov等人(2012a)引入了分布到分布的NDT匹配的扩展，它为新扫描和参考扫描生成高斯分布，并最小化两次扫描的分布之间的$L_2$距离以进行配准。分布到分布的扩展可以改善收敛盆地，并缩短计算时间。我们将点到分布的NDT算法称为NDT-P2D，将分布到分布的NDT算法称为NDT-D2D。

最近，点云数据的NDT表示的使用已显著扩展，包括地图表示、点云精度评估、定位和路径规划应用。扩展的方法都依赖于基于NDT的地图，因此可以极大地受益于基于NDT的扫描配准算法的进步，该算法允许鲁棒和快速的点云聚集。NDT占用地图(NDT-OMs)使用图的体素化的NDT表示和递归协方差更新程序，以概率方式将新的传感信息整合到NDT地图中。NDT-OM结合了标准的占用栅格地图的鲁棒性和NDT的紧凑空间表示(Saarinen等人，2013b)，并在用于动态环境(Stoyanov等人，2013)和基于图的SLAM的长期操作方法(Ein-horn和Gross，2013)时显示出极佳的结果。3D-NDT还被用于评估从不同传感器收集的点云数据的准确性(Stoyanov等人，2012b)，以及校正由激光扫描仪在获取扫描期间经历明显移动而导致的点云畸变(Almqvist等人，2013)。流行的蒙特卡罗定位(MCL)算法针对基于NDT的地图进行了重构，并通过实验证明，在静态和动态场景下，与占用栅格MCL相比，该算法提供了更高的定位精度(Saarinen等人，2013a)。使用基于NDT的地图表示的改进波前规划器被证明能够在半结构化三维环境中以计算高效的方式计算可行路径(Stoyanov等人，2010)。

尽管已经表明NDT算法能够相对于ICP产生更高质量的配准结果(Magnusson等人，2009)，NDT算法的一个缺点是其不连续的代价函数。由于高斯分布是通过用矩形网格单元划分点云而生成的，因此用于确定变换参数的非线性代价函数没有很好地定义，因为在优化过程中点跨越单元边界。结果，优化的收敛性得不到保证。代价函数的不连续性问题通常使用重叠网格单元(Biber和Strasser，2003)或网格单元之间的三线性插值(Magnusson等人，2007)来解决。尽管重叠网格单元和三线性插值等策略在实践中已被证明是有效的，但这些技术只能缓解不连续性，而不能完全消除它们。此外，典型的NDT扫描匹配将点云数据转换成与所选单元尺寸一致的一大组高斯分布。NDT算法的运行时间与分布的数量成线性比例，尽管朴素的离散化方法在模拟环境方面做了合理的工作，但运行时间的改进应该是有可能的。通过直接模拟环境中的自然特征，它生成了更少的高斯分布。

使用多尺度NDT的扫描匹配方法也被提出，并且在实践中被证明是改善算法收敛域的有效方法(Takeuchi和Tsubouchi，2006；Magnusson等人，2007年；Kaminade等人，2008年；Das和Waslander，2012年)。尽管多尺度方法能够在扫描之间有大的位移时提供精确的配准结果，但是所需尺度的大小和数量的选择并不总是清楚的，并且该方法将算法的计算成本乘以包括的尺度的数量。作者先前提出的多尺度方法提出了使用k-均值聚类对2D激光扫描进行划分，在多尺度上随着聚类数量的增加而增加(Das andWaslander，2012)。然后，为了避免局部极小值，这些聚类集被用于由粗到细的优化方法中。该方法导致平滑和连续的代价函数，但是，k-均值方法不能很好地扩展到三维点云数据，需要预先固定分区的数量。

Moosmann等人(2009)在3D中也进行了扫描分割，他们使用基于图的方法基于局部凸性准则分割激光扫描。该方法计算表面法线信息以分割扫描，然而，对于稀疏扫描，或者当环境由诸如树、灌木和树叶的对象组成时，很难产生高质量的表面法线信息。在点云数据的三维体素网格上运行的高斯过程(GPs)和最近邻聚类算法已被证明在稀疏激光数据上产生了良好的分割结果(Douillard等人，2011)，并被进一步应用于ICP扫描配准，其中相邻点之间的对应关系被约束为属于对应的段(Douillard等人，2012)。Golovinskiy等人(2009年)还使用分层方法演示了三维点云聚类，Klasing等人(2008年)使用径向有界最近邻(RBNN)图对三维数据进行了聚类。尽管前景看好，但对于大多数自然特征在空间上是分离的并且垂直于地面的环境，使用3D最近邻聚类技术是不必要的。在这种情况下，可以应用更简单的方法，如从自上而下的角度来看2D区域的增长。区域增长算法作为能够以计算高效的方式提供良好聚类结果的分割方法，在计算机视觉领域中已经得到很好的应用(Haralick andShapiro，1985)。

问题描述

1.扫描配准

将点云定义为点的集合 P = { p 1 , . . . , p N p } P = \{p_1,...,p_{N_p}\} P={p1​,...,pNp​​}，其中 p i ∈ R 3 f o r i ∈ { 1 ， . . . ， N P } p_i\in \mathbb{R}^3 for i\in \{1，...，N_P\} pi​∈R3fori∈{1，...，NP​}。一个点$p_j\in P$由三个分量组成， p j = { p j x ， p j y ， p j z } p_j=\{p^x_j，p^y_j，p^z_j\} pj​={pjx​，pjy​，pjz​}，分别指该点的$x$，$y$和$z$分量。在这项工作中，术语场景扫描和参考扫描用于表示我们希望使用扫描配准来对齐的两个扫描。为此，我们将确定相对变换参数并将其应用于场景扫描，以使其在参考扫描定义的坐标框架内与参考扫描对齐。在三维空间中，变换参数由$ϵ=[t_x，t_y，t_z，\phi ，\theta ，\psi {]}^T\in \mathbb{S}\mathbb{E}(3)$给出。请注意，旋转将由3–2–1欧拉角参数化定义。使用参数估计$ϵ$，将来自场景扫描的点$p\in P^S$映射到参考扫描的坐标系中的变换函数$T_{ϵ}:{\mathbb{R}}^3\to {\mathbb{R}}^3$如下：
$$T_{ϵ}(p)=\left[\begin{array}{c}{R}_{\phi }{R}_{\theta }{R}_{\psi }\end{array}\right]p+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]\text{\hspace{1em}(1)}$$

其中，$R_{\phi }$是绕$x$轴旋转角度$\phi$的旋转矩阵，$R_{\theta }$是绕$y$轴旋转角度$\theta$的旋转矩阵，$R_{\psi }$是绕$z$轴旋转角度$\psi$的旋转矩阵，以及$[t_x，t_y，t_z{]}^T$是两个坐标系原点之间的平移向量。

扫描配准算法试图找到两个点云扫描之间的最佳变换，即参考扫描$P^R$和场景扫描$P^S$。目标是确定最佳对齐两者的变换参数，以便两次扫描尽可能重叠。为了量化两次扫描之间的重叠量，需要一个评分函数，并用映射$\Lambda ：\mathbb{S}\mathbb{E}(3)\to \mathbb{R}$表示。3D扫描配准问题可以定义为确定最佳变换参数${ϵ}^{\ast }$，它满足
$${ϵ}^{\ast }=\underset{ϵ\in \mathbb{S}\mathbb{E}(3)}{\mathrm{argmin}}\Lambda (ϵ)\text{\hspace{1em}(2)}$$

2.NDT扫描配准

NDT是一种方法，通过这种方法，点云的各部分在栅格结构中表示为高斯分布。该变换将点云映射到光滑的表面表示，该表面表示被描述为一组局部分布，这些局部分布捕捉表面的形状。类似于占据栅格地图，NDT生成环境的细分表示。然而，占据栅格地图表示单元被占据的概率，而NDT表示在给定单元内每个位置测量到点云样本的概率。

NDT配准算法开始于将参考扫描占据的空间细分为固定大小的矩形栅格单元，$c\in {\mathbb{R}}^3$，其中参考扫描中所有栅格单元的集合被表示为$C^R$。定义单元格中参考扫描点的集合，$c_i$，为 P c i R = { p ∈ P R ： p ∈ c i } P^R_{c_i} = \{p \in P^R：p \in c_i\} Pci​R​={p∈PR：p∈ci​}。给定第$i$个单元，占据该栅格的参考扫描点被用来为代表性的高斯分布$\mathcal{N}({\mu }_i^R,{\Sigma }_i^R)$生成均值${\mu }_i^R$和协方差矩阵${\Sigma }_i^R$。高斯分布可以解释为一个生成过程，该过程对单元内的局部表面点$P_{c_i}^R$进行建模。假设参考扫描表面点的位置是从这个分布中得出的，那么在栅格$c_i$内测量点到一个点$p$的可能性可以建模为
$${\rho }_{c_i}(p)=exp(-\frac{(p-{\mu }_i{)}^T{\Sigma }_i^{-1}(p-{\mu }_i)}{2})\text{\hspace{1em}(3)}$$
图1给出了2D激光扫描的NDT概率分布图的例子。

由于点云是由高斯分布的分段连续和分段可微求和来建模的，因此可以使用数值优化工具来配准场景扫描和参考扫描。可以计算拟合分数，它量化了参考扫描和由参数估计$ϵ$变换后的场景扫描之间的重叠程度。Magnusson的对于点到点分布NDT配准的原始工作通过在对应于每个变换后点占据的NDT单元的分布上评估变换后场景扫描中的每个点来计算代价(Magnusson等人，2007)。点对点分布NDT的得分如下
$${\Lambda }_{ndt}^{p2d}(ϵ)=-\sum _{p\in P^S}{\rho }_{c_{cur}}{T}_{ϵ}(p)\text{\hspace{1em}(4)}$$
其中$c_{cur}$记录了被参数估计$ϵ$变换后的点$p$占据的栅格。Stoyanov等人(2012年a)提出了一种改进的NDT方法，这种方法产生了两个扫描的NDT表示，然后比较扫描的分布，以便将它们对齐。为了分布到分布的匹配，场景扫描$P^S$也被分成一组栅格$C^S$，其中包含在第$i$个栅格中的点被用来建模分布$\mathcal{N}({\mu }_i^S，{\Sigma }_i^S)$。为了对准扫描，一个优化被公式化以最小化参考和场景扫描的高斯分布集合之间的$L_2$距离。将参考扫描中第$i$个高斯分布的均值和场景扫描中的第$j$个分布均值之间的不同定义为
$${\beta }_{ij}=[T_{ϵ}({\mu }_j^S)-{\mu }_i^R]\text{\hspace{1em}(5)}$$
利用均值的差异，NDT分布对分布的得分函数如下
$${\Lambda }_{ndt}^{d2d}(ϵ)=-\sum _{i=1}^{|C^R|}\sum _{j=1}^{|C^S|}exp(-\frac{1}{2}{\beta }_{ij}^T[{ϵ}_r^T{\Sigma }_j^S{ϵ}_r+{\Sigma }_i^R{]}^{-1}{\beta }_{ij})\text{\hspace{1em}(16)}$$

分布到分布成本函数评估场景和参考扫描的所有成对的高斯分布。然而，在Stoyanov等人的工作中，为了减少计算，成本函数只在最近的高斯分布上计算。在最近高斯分布上的计算导致了代价函数的不连续性，这意味着代价函数是非光滑的，并且当均值点之间的对应关系改变时，梯度和海塞矩阵不存在于变换空间中。相关方法使用三线性插值方案处理不连续代价函数，该方案在计算梯度和海塞矩阵的贡献时考虑了邻近栅格的分布(Magnusson等人，2007)。然而，三线性插值并不能完全解决这个问题，因为代价函数仍然是不连续的。

应该注意的是，可以为两个NDT成本函数计算解析的梯度$g$和海塞矩阵$H$，这可用于提高所选非线性优化方法的性能。对于点到分布匹配，定义函数${\prod }_g(p_k,ϵ)$和${\prod }_H(p_k,ϵ)$，它们使用场景扫描中的一个点和变换参数来计算点$p_k$的关联梯度和海塞贡献。优化是根据参数估计值$\widehat{ϵ}$初始化的，一旦梯度的范数$||g||$小于用户指定的阈值${ϵ}_{ndt}$，优化就终止。Biber和Strasser (2003)使用带线搜索的牛顿法进行优化，然而，也可以使用其他方法，如LevenBerg-Marquardt或Broyden-Fletcher-Goldfarb-Shanno(BFGS)法(Magnusson，2009)。

本文提出的方法:SRG-NDT

为了执行SRG-NDT，NDT算法通过以下三种方式进行修改。首先，使用第4.1节中描述的地面分割方法移除扫描的地面点。在大型户外、类似田野的环境中，地面点通常不像其他环境特征，如大树、栅栏等能够提供足够的信息来用于扫描匹配。事实上，地面点会降低ICP和NDT的准确性。当处理地面时，扫描点密度也成为一个问题，因为非常靠近激光扫描的地面点比更远的地面点密度更大。点云的不均匀密度对于NDT来说是不可取的，因为车辆附近的高密度点会产生偏向传感器原点的高斯分布。扫描的非均匀密度可以使用基于体素的下采样滤波器进行滤波来进行一定程度的缓解，然而，体素滤波器的使用给扫描匹配增加了额外的计算，并且由于下采样过程的进行再一定程度上从整个扫描中去除了很多信息。尽管对扫描进行下采样将消除NDT的有偏分布问题，但它并不能解决代价函数不连续的问题。

其次，使用第4.2节中描述的区域生长方法对非地面点进行聚类。通过执行聚类步骤，环境中的自然特征，例如树木和灌木丛，被聚类以形成SRG-NDT的高斯分布。区域增长聚类方法可以被视为一种从环境中获取代表性特征的方法，假设一旦地面点被移除，这些特征就会在空间上分离，并且以一种对于NDT配准来说足够的聚类配置进行分布。

最后，为了适应扫描的区域聚类，NDT成本计算按照第4.3节所述进行了修改。SRG-NDT算法针对由区域增长方法识别的聚类的所有高斯分布集合来评估变换后的点。类似地，SRG-NDT通过对场景扫描中的每个高斯分布相对于参考扫描的所有高斯分布进行评分，应用于分布到分布的NDT的代价中。对所有分布的评估在计算上是可行的，因为在扫描中识别的聚类的数量通常明显小于基于体素的离散化所产生的高斯分布的数量。基于扫描中的所有高斯分布来计算代价允许最强的贡献来自最有可能的聚类，并为优化提供连续且可微的代价函数。

1. 地面分割

用于这项工作的地面分割算法的基础首先由Douillard等人(2011年)提出。他们的方法基于稀疏点云的二维GP模型进行地面分割。GPs的使用提供了一个概率框架，使用增量样本一致性(INSAC)方法来识别地面点。Tongtong等人(2011年)提出了一种改进的Douillard地面分割算法。为了减少计算量，Tongtong等人基于极坐标网格binging将扫描划分为多个扇区，并将地面分割分别应用于每个扇区。GP表示是根据每个扇区中包含的点为近似信号制定的。Tongtong等人的方法被证明比2D方法运行得更快，在分割质量上具有相当的结果。这项工作使用了Tongtong等人(2011年)的一维近似地面分割方法，并在本节中进行了简要总结。请注意，对于我们寻求车辆相对运动估计的应用，激光扫描更新之间的地平面不会发生显著变化，因此允许在扫描之间持续移除地面点。强烈倾斜变化的问题可以通过额外的传感器来缓解，例如惯性测量单元，它将允许在应用地面分割之前对激光扫描进行倾斜补偿。

1.1 径向栅格绑定

为了进行径向栅格绑定，在激光扫描坐标系下的激光扫描的$x-y$平面首先被分割成$N_a$个角扇区。每个扇区覆盖的角度${\tau }_a$由下式给出
$${\tau }_a=\frac{2\pi }{N_a}\text{\hspace{1em}(7)}$$
每个扇区进一步细分为$N_l$个基于线性范围的bin。每个线性范围bin覆盖的距离${\tau }_l$由下式决定
$${\tau }_l=\frac{R_{max}}{N_b}\text{\hspace{1em}(8)}$$
其中$R_{max}$是给定扫描预期的最大范围测量值。定义第$i$个扇形和第$j$个线性范围的bin为${\zeta }_{ij}\subseteq {\mathbb{R}}^2$，以及所有bin的集合为$\Xi$，这里$|\Xi |=N_a{N}_l$。定义点云$P$中投影落在栅格${\zeta }_{ij}$的点为
P i j = { p ∈ P ： ( p x , p y ) ∈ ζ i j } (9) P_{ij} = \{ p \in P：(p^x,p^y) \in \zeta_{ij} \} \tag{9} Pij​={p∈P：(px,py)∈ζij​}(9)
为了执行快速地面分割，从栅格点集$P_{ij}$构造一组元组。为了构造元组集合，为每个栅格单元内的点确定一个原型点。对于地面分割的情况，最好在每个栅格单元中对原型点进行建模，以便它最好地代表地面。每个单元格内的点$P_{ij}$的原型点${\zeta }_{ij}$被选为具有最低$z$坐标的点
$$x_{ij}=\underset{p\in P_{ij}}{\mathrm{argmin}}{p}^z$$
最后，使用每个栅格单元中的点的原型点，可以定义每个角扇区的元组集合。定义与原型点一个栅格中原型点$x_{ij}$相关联的元组为$y_{ij}=(r_{ij},h_{ij})\in {\mathbb{R}}^2$。对于元组表示，第一个元素$r_{ij}\in \mathbb{R}$是范围分量，而$h_{ij}\in \mathbb{R}$是高度分量。使用栅格单元的原型点$x_{ij}$，元组的范围和高度分量计算如下
$$r_{ij}=\sqrt{(x_{ij}^x{)}^2+(x_{ij}^y{)}^2}\text{\hspace{1em}(11)}$$
$$h_{ij}=x_{ij}^z\text{\hspace{1em}(12)}$$
范围元素$r_{ij}$是从原型点$x_{ij}$到传感器原点的$x$和$y$分量的欧几里得距离，高度元素$h_{ij}$只是原型点的$z$分量。角扇区$i$的元组集合$Y_i$被定义为
Y i = { y i j ： j ∈ { 1 , . . . , N l } } (13) Y_i = \{ y_{ij}：j \in \{ 1,...,N_l \} \} \tag{13} Yi​={yij​：j∈{1,...,Nl​}}(13)

1.2 GP回归

地面分割是通过执行GP回归来完成的。关于GPs用于地面地形建模的彻底探索，见Vasude-van等人(2009年)。GP回归提供了一个概率框架，在给定一组训练数据的情况下，为一组查询输入预测一组高度值。假设GP模型要用范围-高度元组集的子集训练，表示为̄${\bar{Y}}_i\subseteq Y_i$。协方差函数用于量化用于回归的数据点之间的相关性。在这项工作中，使用了平方指数协方差函数，$\kappa ：{\mathbb{R}}^2\to \mathbb{R}$。假设设计了两个标量数据点$b_1\in \mathbb{R}$和$b_2\in \mathbb{R}$之间的关联是我们希望获得的。使用平方指数协方差函数，相关性由下式给出
$$\kappa (b_1,b_2)={\sigma }_f^{2}exp(-\frac{1}{2l^2}(b_1-b_2{)}^2)$$
其中$l\in \mathbb{R}$是长度尺度参数，${\sigma }_f\in \mathbb{R}$是输入方差，它们被称为GP模型的超参数。给定协方差函数，可以提出地面分割的回归问题，该问题试图预测元组集$Y_i$的相关范围值的高度值，假定GP模型已经使用来自训练集${\bar{Y}}_i$的数据训练。使用方差函数，定义与查询和训练范围数据相关的四个矩阵。定义${\mathcal{K}}_{qq}\in {\mathcal{R}}^{|Y_i|\times |Y_i|}$作为待查询集合的自协方差矩阵。令$\mathcal{K}(j,k)$为$\mathcal{K}$的第$(j,k)$个元素。为了简化表示法，将一个元组元素$y_k\in Y_i$的范围和高度值分别表示为$r_k$和$h_k$。然后，用等式(14)给出的平方指数协方差函数来填充矩阵${\mathcal{K}}_{qq}$的每个元素，
$${\mathcal{K}}_{qq}(j,k)=\kappa (r_j,r_k),\forall y_j,y_k\subseteq Y_i\text{\hspace{1em}(15)}$$

类似地，待查询数据和训练数据之间的互协方差矩阵${\mathcal{K}}_{qt}\in {\mathbb{R}}^{|Y_i|\times |{\bar{Y}}_i|}$以及训练数据的自协方差矩阵$\mathcal{K}\in {\mathbb{R}}^{|{\bar{Y}}_i|\times |{\bar{Y}}_i|}$可以定义为
$$\mathcal{K}(j,k)=\kappa (r_j,r_k),\forall y_j\in Y_i,y_k\in {\bar{Y}}_i\text{\hspace{1em}(16)}$$
$${\mathcal{K}}_{tq}={\mathcal{K}}_{qt}^T\text{\hspace{1em}(17)}$$
$${\mathcal{K}}_{tt}(j,k)=\kappa (r_j,r_k),\forall y_j,y_k\in {\bar{Y}}_i\text{\hspace{1em}(18)}$$

利用自相关和互相关矩阵，GP模型将输入数据和训练数据之间的关系建模为联合分布。将来自训练集的高度值向量表示为向量，$\lambda =[h_1,...,h_{|{\bar{Y}}_i|}]$，其中$h\in {\bar{Y}}_i$。类似地，将向量${\lambda }^{\ast }$表示为要使用GP回归推断的高度值。然后将GP建模为联合分布
$$\left[\begin{array}{c}\lambda \\ {\lambda }^{\ast }\end{array}\right]\sim (0,\left[\begin{array}{cc}{\mathcal{K}}_{tt}+{\sigma }_n^{2}I& {\mathcal{K}}_{tq}\\ {\mathcal{K}}_{qt}& {\mathcal{K}}_{qq}\end{array}\right])\text{\hspace{1em}(19)}$$
其中${\sigma }_n^2$是一个额外的超参数，即过程噪声方差。

根据联合分布，可以给出条件均值和条件方差的表达式。条件均值和协方差表达式产生预测方程。换句话说，使用训练数据和预测方程，可以为一组待查询的距离值预测地面高度值的向量${\lambda }^{\ast }$和相应的的协方差$V_{{\lambda }^{\ast }}$。条件均值和方差预测方程如下
$${\lambda }^{\ast }={\mathcal{K}}_{qt}[{\mathcal{K}}_{tt}+{\sigma }_n^{2}I{]}^{-1}\lambda \text{\hspace{1em}(20)}$$
$$V_{{\lambda }^{\ast }}={\mathcal{K}}_{qq}-{\mathcal{K}}_{qt}[{\mathcal{K}}_{tt}+{\sigma }_n^{2}I{]}^{-1}{\mathcal{K}}_{tq}\text{\hspace{1em}(21)}$$
定义方法$\gamma (Y_i,{\bar{Y}}_i)$，它使用训练数据${\bar{Y}}_i$和待查询点$Y_i$为角扇区$i$生成四个相关矩阵${\mathcal{K}}_{tt}$、${\mathcal{K}}_{tq}$、${\mathcal{K}}_{qt}$和${\mathcal{K}}_{qq}$

根据预测方程，可以为来自待查询数据集合的给定的范围值生成被估计的高度。使用索引符号，${\lambda }_k^{\ast }$表达待查询高度向量${\lambda }^{\ast }$的第$k$个元素，类似地${\lambda }_k$表示训练高度向量$\lambda$的第$k$个元素。此外，$N_{\lambda }^{\ast }$表示向量${\lambda }^{\ast }$中的元素数量，注意$N_{\lambda }^{\ast }=|Y_i|$。为了根据GP模型确定查询点是否是内点，可以将预测的地面高度${\lambda }_k^{\ast }$与待查询点来自元组的实际地面高度$y_k=(r_k,h_k)\in Y_i$进行比较。要被视为GP模型的内点，必须满足两个标准：
$$V_{{\lambda }_k^{\ast }}<{\delta }_{model}\text{\hspace{1em}(22)}$$
$$\frac{h_k-{\lambda }_k^{\ast }}{\sqrt{{\sigma }_n^2+V_{{\lambda }_k^{\ast }}}}<{\delta }_{data}\text{\hspace{1em}(23)}$$
其中${\delta }_{model}$定义了对于测试点的协方差阈值，${\delta }_{data}$定义了测试点到GP模型期望值的归一化距离。测试点与${\delta }_{model}$的比较确保了GP模型中有足够的置信度来进行内点分类，与${\delta }_{data}$的比较使用预测的地面高度和测量的地面高度之间的距离来将待查询点分类为GP模型的内点。

1.3 使用GP回归和增量采样一致性的地面分割

INSAC算法与GP回归模型一起用于识别激光扫描中的所有地面点。径向栅格绑定的每个扇区的地面点都是单独标识的。如第4.1.1节所讨论的，三维扫描首先被绑定到扇区种。为了初始化地面分割，每个扇区的种子集被选择作为在激光扫描原点的阈值距离${\delta }_o$内的范围-高度元组点集。对于每个扇区，GP模型的训练点${\bar{Y}}_i$然后被选择作为扇区的种子点。查询集中的$Y_i$中剩余的原型点根据GP模型进行评估。当前迭代的内点元组被添加到内点集$Y_{in}$，并产生一个新的GP模型。添加内点和重新生成GP模型的过程被重复，直到没有其他内点可以添加到集合中。由于没有剩余的内点，所有的bin的原型点都被分类为地面或非地面点。为了对每个地面bin中的剩余点进行分类，将每个点的高度分量与地面原型点的高度分量进行比较。如果绝对高度差小于用户定义的阈值${\delta }_g$，则这些点也被归类为地面点。定义方法$P^g\leftarrow \vartheta (P)$，它接受点云$P$并返回地面点$P^g$。算法1总结了地面分割过程，图2给出了示例激光扫描的结果。

这个论文中的叙述相当的难以理解，直接看算法可能还容易理解一些，一个基本的假设就是距离传感器原点一定距离内的一些原型点会被认为是地面点并用来初始化GP模型，这个假设其实是符合实际的，因为在通常的情况下距离车非常近的地方一般是不会出现较大的障碍物的

非地面点的聚类

为了对稀疏点云的非地面点进行聚类，使用了基于点的bin的区域增长聚类技术。为地面分割过程中生成的点的bin分配被重新用于聚类算法。重用栅格和点的bin分配允许使用简单的区域增长方法(Haralick和Shapiro，1985)进行非地面点的快速聚类，该方法连接相邻栅格，这些栅格包含非地面点，并且满足由用户定义的合并条件(例如位于相邻栅格内的均值点之间的最大距离，或者来自相邻栅格的点分布之间的$L_2$距离)。

使用第4.1.1节中描述的类似径向栅格技术，除了为了简单起见使用单一索引外，定义一个径向栅格单元${\zeta }_i\in {\mathbb{R}}^2$以及所有这些单元的集合为$\Xi$。对于非地面点的点云$P^{ng}$，定义属于栅格${\zeta }_i$的点为
P i n g = { p ∈ P n g ： ( p x , p y ) ∈ ζ i } (24) P^{ng}_i = \{ p \in P^{ng}：(p^x,p^y) \in \zeta_i \} \tag{24} Ping​={p∈Png：(px,py)∈ζi​}(24)
并且所有单元点集的集合表示为 P = { P 1 n g , . . . , P ∣ Ξ ∣ n g } \mathcal{P} = \{ P_1^{ng},...,P_{|\Xi|}^{ng} \} P={P1ng​,...,P∣Ξ∣ng​}。定义一组点的聚类为$\gamma \subseteq P^{ng}$，聚类的集合为 Γ = { γ 1 , . . . , γ N Γ } \Gamma = \{\gamma_1 ,..., \gamma_{N_\Gamma}\} Γ={γ1​,...,γNΓ​​}，其中$N_{\Gamma }$是总的聚类总数，它最初是未知的。通过首先从随机均匀分布$\mathcal{U}$中随机采样一个初始的bin索引$i$来产生簇，使得$i\sim \mathcal{U}(i\in 1,...,|\Xi |)$。让${\mu }_i\in {\mathbb{R}}^3$为点集$P_i$的均值，定义距离函数$d：\mathcal{P}\times \mathcal{P}\to \mathbb{R}$，它返回点集$P_i$和$P_j$之间的距离，表示如下
$$d(P_i,P_j)=||{\mu }_i-{\mu }_j|{|}_m\text{\hspace{1em}(25)}$$
其中$m$指定要使用的范数类型。使用等式(25)，点集$P_i$的的最近邻集可以被定义为
Ω = { P ∈ P ： d ( P , P i ) < δ n n } \ P i (26) \Omega = \{ P \in \mathcal{P}：d(P,P_i) < \delta_{nn} \} \backslash P_i \tag{26} Ω={P∈P：d(P,Pi​)<δnn​}\Pi​(26)
其中${\delta }_{nn}$被确定为点集$P_i$的邻居的阈值。

对于随机选择的点集$P_i$，生成最近的邻居并将其添加到最近的邻居集$\Omega$。最近邻集合$P_j\in \Omega$的一个元素$P_j$被用来和$P_i$比较，以确定两者是否可以合并。这两个集合的合并使用函数 ϱ ： P × P → { 0 , 1 } \varrho：\mathcal{P} \times \mathcal{P} \to \{0,1\} ϱ：P×P→{0,1}。如果两个点集$P_i$和$P_j$可以合并，则函数$\varrho (P_i,P_j)$返回1，否则返回0。在合并函数中，可以使用不同的度量来比较两个点集，例如点的平均值之间的欧几里德距离，由点集形成的高斯分布之间的$L_2$距离，或者高斯拟合的测试。如果这些bin可以合并，则相邻的bin被添加到一个开放的点集列表$Q_o$中，其中的符号$Q_o^k$表示$Q_o$的第$k$个元素。相邻点集也被从所有点集的集合中移除，并被合并到当前聚类${\gamma }_{cur}$中。探索和评估了离$Q_o$最近的邻居。根据评估函数$\varrho$，当一个bin被探索时，它最近的邻居也被添加到$Q_o$中。一旦$Q_o$为空，这意味着没有其他最近的邻居可以被分配给当前的聚类，所以从$\mathcal{P}$中选择一个新的点集，并重复这个过程。这个过程一直持续到$\mathcal{P}$中的所有非空点集都被分配给一个聚类。区域增长算法在算法2中给出。

这玩意其实就是一个简化版本的DBSCAN聚类

定义方法$\Gamma \leftarrow \eta (P^{ng})$，它接受非地面点的点云，并返回由区域增长算法确定的聚类集合。图3给出了聚类完成后得到的高斯分布的一个例子。

SRG-NDT代价函数

根据在场景扫描上执行区域增长聚类算法产生的所有高斯分布，修改NDT代价函数，以对分割后参考扫描中的每个点进行评分。表示场景扫描中被参数估计̄$ϵ$变换后的点$\bar{p}=T_{ϵ}(p)$。变换后的点使用NDT评分函数$\rho$，可以相对于高斯聚类${\gamma }_j$进行评分，公式如下
$${\rho }_{{\gamma }_j}(\bar{p})=exp(-\frac{(\bar{p}-{\mu }_j{)}^T{\Sigma }_j^{-1}(\bar{p}-{\mu }_j)}{2})\text{\hspace{1em}(27)}$$
场景扫描中的点根据参考扫描中的所有高斯聚类进行评分，这允许最强的评分贡献来自最有可能的聚类，并且为优化提供了连续且可微分的代价函数。类似于标准的NDT算法，可以计算代价函数的解析梯度$g$和海塞矩阵$H$，这可以用来提高非线性优化的性能。优化从参数估计初始值$\widehat{ϵ}$开始，一旦梯度范数$||g||$小于用户指定的阈值${ϵ}_{ndt}$，优化就终止。请注意，由于成本是连续的，所以存在解析梯度和海塞矩阵，并且他们在整个变换参数空间上有着良好的定义，从而保证使用任何基于梯度的方法的优化的收敛性。

算法3中给出了具有点到分布匹配的SRG-NDT的总结。

对于分布到分布匹配的SRG-NDT，应用了类似的方法，除了对两次激光扫描都进行分割和聚类，并使用分布到分布NDT代价函数比较每次扫描的高斯分布集合。

总结一下，这篇论文主要的创新点有三个：1.基于GP回归和增量采样一致性的地面分割方法，这里可以使用其他的地面分割方法来代替，比如LeGO-LOAM里面的方法或者RANSAC平面拟合方法。2.基于区域增长的聚类方法，类似于DBSCAN，这里也可以考虑其他的自聚类方法，或者基于深度学习进行聚类。3。在前面两步提升了计算效率的情况下可以使用全部的高斯分布进行评估，从而确保了代价函数在整个参数空间上的连续可微性质，从数学上可以给出保证收敛的性质。

实验结果

这部分会看情况更新，也可能先更新其他的基于NDT的改进方法的论文核心部分！