雷达系列论文翻译（十）：Scan registration using segmented region growing NDT
Scan registration using segmented region growing NDT
这篇论文可被视为对上一篇论文的系统性阐述版本,由同一作者完成,其中针对每一算法,本研究进行了深入分析并对其进行了系统性阐述
摘要
基于矩形体素栅格划分的正态分布变换配准算法用于将点云数据划分为三维网格单元，并对每个单元内的点进行高斯分布建模以实现精准配准过程。然而，在实际应用中发现当扫描区域跨越栅格边界时，在体素栅格划分下会导致目标函数梯度计算不一致的问题影响优化效果。为此提出了一种分段区域增长改进方法（SRG-NDT），该方法通过排除地面点数据以显著提升配准效率的同时保持了与传统NDT方法相同的定位精度特性。具体而言，在处理剩余非地面点集时采用聚类分析技术可减少目标函数参数规模并构建平滑连续的目标函数从而提高优化收敛速度与计算效率。实验结果表明在城市和森林环境中SRG-NDT方法达到了与现有算法相当的定位精度水平但其相较于ICP、G-ICP及传统NDT算法在计算时间上分别实现了约90.1%、95.3%及94.5%的时间节省
引言
扫描配准方法在移动机器人的建图算法中扮演关键角色。许多传感器通过点云形式提供环境信息，并包括RGBD相机、LIDAR和双目相机等设备。基于两次扫描间的重叠几何关系来确定相对变换参数有助于融合多源点云数据，在未知环境中实现自动操作通常是一个必要的步骤。
基于扫描配准技术的最流行建图方法主要包括Besl和Kay(1992)；Chen等人（1991）；Zhang(1994)提出的广义迭代最近点(G-ICP)算法；Segal等人（2009）提出的正态分布变换(NDT)方法；Magnusson等人（2007年）；Stoyanov等人（2012a）所提出的改进型ICP算法等
NDT算法作为一种替代显式计算最近邻对应关系的方法存在，
它通过将点云表示为多个高斯分布来进行建模。
在配准过程中，
通过第二次扫描中从各高斯分布采样点的概率值来计算重叠度。
为了实现对齐扫描，
采用非线性优化方法确定变换参数，
使得重叠度最大化；
然而由于采样点跨越单元边界，
导致代价函数出现不连续性。
基于体素栅格的离散化模型生成的高斯分布在模拟环境方面存在一定局限性；
这些模型仅在单个尺度下对每个栅格内的点进行局部建模，
可能导致无法充分捕捉扫描中存在的大型特征信息，
从而影响整体对齐效果。
本研究中采用分割与聚类方法旨在提升NDT算法的收敛性和实时性能。具体而言，在这项工作中提出了分割区域生长NDT(SRG-NDT)，该方法通过识别并充分利用环境中的自然特征为NDT算法生成高斯聚类。首先采用基于高斯过程的分割算法去除扫描中的地平面，并结合高效计算的区域生长算法对剩余点进行聚类。根据自然特征进行聚类，在与标准NDT相比可显著减少所需分布数量的同时仍能精确模拟复杂环境。此外地面分割的应用使得场景分解为独立的地平面与障碍物部分这一划分可被用于更高层次的任务规划与执行。为了实现配准目标场景扫描中每个点的成本计算基于参考扫描所有聚类的信息而无需额外参数配置即可完成精确匹配过程。
本研究通过融合外部环境中的视觉信息及激光雷达数据，并结合WPI校园内森林区域的真实场景数据，在模拟与实际环境并行运行条件下开展实验验证。具体而言，在评估指标分析部分详细阐述了以下三项关键成果：研究表明，在精确度评估方面,SRG-NDT算法表现不逊于当前最先进的人工智能匹配技术；进一步分析发现,通过剔除地面反射点,不仅有效降低了计算开销,还能维持与完整点云处理相当的匹配精度；对比现有主流算法（ICP,G-ICP,NDT）,本算法在计算效率上实现了显著提升,平均处理时间分别减少了90.1%,95.3%,94.5%。
这项研究具体阐述了其工作流程。具体来说，在第2部分中涉及现有技术中的扫描配准与聚类方法的相关研究。第3部分深入探讨了扫描配准问题，并系统性地分析了NDT方法的技术基础。在第4部分中，则详细介绍了一种新型的SRG-NDT算法及其在地面分割和动态增长聚类方面应用的具体细节。第5部分总结并展示了该算法的关键实验结果与性能评估指标。最后，在结论部分（即第6节），我们归纳分析了本研究的主要发现，并展望了未来可能的研究方向与技术改进空间。
相关工作
广为人知且应用最广泛的扫描配准技术是ICP算法（由Besl与McKay于1992年、Chen与Medoni于1991年以及Zhang于1994年各自阐述）。该算法旨在通过最小化参考扫描与场景之间对应点的欧几里得距离来确定变换参数。为了建立对应关系，系统会根据参考扫描中的每个点在场景中找到与其最近的邻接点，并认为这两者为对应的点对。当这些对应点之间的总欧几里得距离不再减少时，则会反复执行此过程直至收敛。值得注意的是，在不同场景中由于移动传感器的距离不同导致的采样密度变化会对结果产生影响；因此，在采样密度较低的地方可能会出现匹配失败的情况。鉴于此，在传统方法难以处理复杂地形时往往会出现低收敛性的问题。为此Segal等人提出了广义ICP (G-ICP)方法（Segal等人2008），该方法通过对每个局部邻域内的表面法线进行计算，并仅考虑那些具有相似表面法线方向的两点间的配准关系从而提高了匹配效率；同时这种方法还考虑了传感器噪声以及采样特性的影响从而显著提升了收敛速度及范围。基于上述理论基础G-ICP方法已被成功应用于多种配准任务中包括对齐高分辨率激光雷达数据（如SICK LiDAR）以及相机数据等；其中初始阶段通常会利用高维尺度不变特征（SIFT）特征进行粗略对齐随后再利用G-ICP方法进行精确配准（如Pandey等人2011b）。然而尽管上述两种方法在许多情况下表现良好但它们都存在一个共同的问题即都需要建立最近邻对应关系这一过程不仅计算量较大而且也被证明是限制传统配准算法效率的关键因素之一
扫描配准的另一种方法是无NDT。NDT算法最初是由Biber和Strasser(2003)提出的，用于2D扫描配准和建图，后来扩展到三维(Magnusson和Duckett，2005；Takeuchi和Tsubouchi，2006)。NDT算法将参考扫描表示为一组高斯分布，这些高斯分布将参考扫描的表面局部建模为概率密度函数。给定所需变换的参数估计值，分配一个分数来量化参考扫描和变换后场景扫描之间的重叠量。重叠分数是通过在高斯分布上评估变换后的场景扫描点来确定的。为了配准两对点云，执行非线性优化以确定变换参数，使得重叠分数最大化。Stoyanov等人(2012a)引入了分布到分布的NDT匹配的扩展，它为新扫描和参考扫描生成高斯分布，并最小化两次扫描的分布之间的L2L_2距离以进行配准。分布到分布的扩展可以改善收敛盆地，并缩短计算时间。我们将点到分布的NDT算法称为NDT-P2D，将分布到分布的NDT算法称为NDT-D2D。
近年来
尽管研究表明NDT算法相较于ICP算法能够在配准效果上体现出更高的质量(Magnusson等人, 2009)，但它仍存在一个显著缺陷即其不连续性的代价函数。基于高斯分布的方法是通过将点云划分为矩形网格单元来构建的。因此导致用于确定变换参数的非线性代价函数缺乏明确定义因为在优化过程中点通常会跨越单元边界从而导致优化过程难以收敛。尽管重叠网格单元和三线性插值等策略在实践中已被证明是有效的但由于这些技术仅能部分缓解而不彻底消除该问题因此无法完全消除不连续性带来的影响。此外通过直接模拟环境中的自然特征来实现的话则会减少所需的高斯分布数量
基于多尺度NDT（Multiscale Normal Distributions Transform）的方法也被广泛应用于扫描匹配领域，并在其后得到了实践层面的高度认可（Takeuchi和Tsubouchi, 2006；Magnusson等人, 2007; Kaminade等人, 2008; Das和Waslander, 2012）。尽管该多尺度方法能够在扫描之间存在较大的位移时实现精确配准效果（即能够实现精确配准），但其所需选择尺度大小与数量的问题往往不够明确（即选择往往不够明确），并且该方法会将算法计算成本乘以所包含的不同比例尺的数量（即计算成本会因比例尺数量不同而有所变化）。作者先前提出了一种基于k-均值聚类的方法来处理二维激光扫描数据（即采用k-means算法进行数据划分），这种方法在不同比例尺下都会带来更好的性能（即随着聚类数量增多而提升性能）。随后为了防止出现局部极小值问题（即避免陷入局部最优解的情况），这些聚类集又被用于从粗到细逐步优化的过程当中。这种方法带来了平滑与连续性的代价函数特性（即代价函数具有良好的平滑性与连续性），然而这种基于k-均值的方法却难以有效地扩展至三维点云数据中（因为无法很好地适应三维空间中的复杂分布特征）。
Moosmann等人(2009)在三维空间中也进行了扫描分割工作；他们采用了基于图论的方法，在满足局部凸性条件的前提下实现了激光扫描数据的分割。该方法通过计算表面法线信息来进行 scan segmentation；然而，在稀疏 scan 数据情况下或当场景包含大量树、灌木及树叶等对象时；很难获得高质量的表面法线信息。在点云数据构建三维体素网格的过程中；高斯过程（GPs）与最近邻聚类算法已被验证能够在稀疏激光 scan 数据上实现良好的 segmentation 效果(Douillard等人, 2011)，并被成功应用于 ICP 扫描配准技术中；其中相邻点对之间的对应关系被限定于同一区域内的连续段(Douillard等人, 2012)。Golovinskiy等人(2009年)还提出了分层方法来实现三维点云聚类；而 Klasing等人(2008年)则采用径向有界最近邻(RBNN)图对三维数据进行了聚类分析。尽管上述技术具有较好的前景；但对于大多数自然特征位于地面以上且相互分离的情况；使用三维最近邻聚类技术并非最佳选择；此时可采用更为简单的方法：从上往下垂直观察二维区域的增长情况。区域增长算法作为一种高效的计算型 segmentation 方法；已经在计算机视觉领域得到了广泛的应用(Haralick and Shapiro, 1985)
问题描述
1.扫描配准
我们定义了一个包含N_p个点的集合P={p₁,..., p_Nₚ}来表示点云。每个点pi属于三维空间R³（i=1,…, N_p）。在本研究中使用了两个不同的扫描：场景扫描和参考扫描，并希望通过配准过程使它们相互对齐。为此，在这两种扫描之间确定了相对变换参数，并将其应用于场景扫描的所有数据点上以实现配准目标。这些相对变换参数由一个6维向量ϵ=[tx, ty, tz, φ, θ, ψ]^T∈SE(3)给出。需要注意的是，在此过程中旋转参数采用了3-2-1欧拉角表示法来进行建模。基于估计得到的参数ϵ，则可将来自场景扫描的所有数据点通过特定变换映射至参考坐标系中。
这个变换函数T_ϵ: R³→ R³可表示为：
T_ϵ(p) = [R(φ) R(θ) R(ψ)] p + [t_x t_y t_z]^T (2)
其中,R_φ代表绕xx轴进行φ度的旋转变换矩阵,R_θ代表绕yy轴进行θ度的旋转变换矩阵,R_ψ代表绕zz轴进行ψ度的旋转变换矩阵,而[t_x,t_y,t_z]^T则表示连接两个坐标系原点位置关系的平移向量
该算法旨在寻找两组点云数据之间的最优变换矩阵PRPR与PSPS；主要目标是确定一组参数设置\epsilon^{*} ∈ SE(3)，使得两次 scan 结果尽可能吻合；在理论分析中我们通常会构建一个评估函数Λ: SE(3) → ℝ\mathbb{SE}(3)} \rightarrow \mathbb{R}来量化两次 scan 的重叠程度；其最优解满足方程（2）：
注
2.NDT扫描配准
一种空间建模技术称为NDT（Normal Distributions Transform），它利用这种方法对三维空间中的散乱点集进行处理。具体而言，在一个二维栅格外划分网格时，在每个单元内假设存在多个离散的位置测量值服从高斯分布。随后通过对这些测量值进行计算得到一个连续的概率密度函数，在每个网格单元内形成一个平滑且连续的概率曲面图层。这种曲面图层能够有效地描述三维物体的整体形状特征并提供精确的空间定位信息。与传统的基于体积的存在感知方法不同的是，在占据栅格的方法中我们关注的是特定位置是否被物体所占据；而在ND T方法中，则是针对每个位置计算其附近区域内存在采样点的概率密度值从而构建了空间细分网格模型
NDT配准算法开始于将参考扫描占据的空间细分为固定大小的矩形栅格单元，c∈R3c \in \mathbb{R}3，其中参考扫描中所有栅格单元的集合被表示为CRCR。定义单元格中参考扫描点的集合，cic_i，为PciR={p∈PR：p∈ci}P^R_{c_i} = {p \in P^R：p \in c_i}。给定第ii个单元，占据该栅格的参考扫描点被用来为代表性的高斯分布N(μiR,ΣiR)\mathcal{N}(\mu_iR,\Sigma_iR)生成均值μiR\mu_iR和协方差矩阵ΣiR\Sigma_iR。高斯分布可以解释为一个生成过程，该过程对单元内的局部表面点PciRP_{c_i}^R进行建模。假设参考扫描表面点的位置是从这个分布中得出的，那么在栅格cic_i内测量点到一个点pp的可能性可以建模为
ρci(p)=exp(−(p−μi)TΣi−1(p−μi)2)(3) \rho_{c_i}(p) = exp(- \frac{(p-\mu_i)^T \Sigma_i^{-1} (p - \mu_i)}{2}) \tag{3}
图1给出了2D激光扫描的NDT概率分布图的例子。
基于点云由分段连续与分段可微 Gauss 分布求和建模的特点，在实际应用中我们通常会采用数值优化工具来实现场景 scan 和 reference scan 的配准过程。这种配准方法能够有效度量 reference scan 在经过参数估计 ε 转换后与 scene scan 中各采样点所处 Gauss 分布区域之间的覆盖程度。 Magnusson 研究表明，在对应于每个采样点位于 Gauss 分布单元内的区域上评估转换后的 scene scan 中各采样点的位置信息即可得到该方法的基本实现框架 (Magnusson 等人, 2007)。
涉及至高斯分布的成本函数评估场景中，则需考虑所有配对的高斯分布在参考扫描中的表现情况。然而，在Stoyanov等人（2014）的研究中，则为了降低计算复杂度，在最近邻的位置上仅进行成本函数计算以减少运算量。这种简化方式虽然有效降低了运算负担但会导致目标函数出现不连续性并且当均值点之间的对应关系发生变化时将无法保证变换空间中的可导性和二阶可导性因而缺乏必要的数学性质支持相关方法则采用了三线性插值策略来处理具有不连续性的代价函数并基于邻近栅格的概率密度重新估算梯度和Hessian矩阵（Magnusson等人, 2007）。尽管这种插值方法能在一定程度上缓解这一缺陷但它仍然无法彻底解决原始问题因为原始的目标函数仍然存在不可导性和非光滑特性
需要注意的是，在解析梯度gg和Hessian矩阵HH方面，在两个NDT成本函数之间具有可操作性。这有助于提升所选非线性优化方法的整体效率。在点到分布匹配过程中，我们定义了在场景扫描中选取一个点及其变换参数下对应的梯度函数∏g(pk,ϵ)\prod_{g}(p_k,\epsilon)以及Hessian贡献函数∏H(pk,ϵ)\prod_H(p_k,\epsilon)。这些函数用于计算对应点pkp_k处的关联梯度及其海塞贡献项。初始参数估计值由变量符号^\hat{\epsilon}\hat{\epsilon}表示，在满足||g|| < ϵndt||g| < \epsilon_{ndt}这一条件时（其中||·||表示向量范数），优化过程将终止并退出循环运算阶段）。为了实现这一目标，默认情况下采用了带线搜索的牛顿法（如Biber与Strasser (2003)所示），但也可以采用其他如LevenBerg-Marquardt或Broyden-Fletcher-Goldfarb-Shanno(BFGS)等算法（Magnusson, 2009）。
本文提出的方法:SRG-NDT
为了实现SRG-NDT算法，在该算法的基础上进行了优化工作。该算法通过三种方式实现了对NDT技术的有效改进：第一种方法是采用第4.1节所述的方法去除扫描过程中的地面数据；第二种方法是针对不同环境特点设计了适应性更强的数据处理流程；第三种方法则优化了计算效率以适应复杂场景需求。具体而言，在典型的野外环境中（如森林边缘地带），地面上的数据通常不如树冠、围栏等特征提供充足的匹配信息；因此，在进行大规模三维建模时需要特别注意这些区域的数据质量对最终结果的影响。由于激光扫描过程中地面点密度较高且分布不均，在后续处理中可能会导致较大的计算开销并影响最终结果的质量；为此我们采用了基于体素化的降噪滤波技术来进行初步处理，并结合多级分辨率分析进一步提高了数据的质量和一致性
此外，在第4.2节中所述的区域生长方法被用来对非地面点执行聚类任务。在执行聚类步骤的过程中，在环境中识别出自然特征如树木和灌木丛，并将其组织成SRG-NDT模型中的高斯分布。这种区域增长聚类方法本质上是一种从环境中提取具有代表性的特征的技术。假设当所有地面点都被去除后，在空间上这些特征将彼此分离，并均匀地分布在NDT配准所需的适当配置中。
最后阶段, 以适应对 scan 区域进行聚类处理, 我们对第 4 节所述 NDT 成本计算方法进行了调整。SRG-NDT 算法通过评估所有由 region growing 方法识别出的 polyMap 聚类中的 Gaussian 分布集合, 来处理相应的 point cloud 坐标转换问题。同样地,该算法还对 scene scan 中每一个 Gaussian 分量与 reference scan 中相应分量之间的关系进行了比较分析, 并将其用于计算对应 point 集间的距离评估。这一过程在计算资源上是可接受且高效的, 因为 scan 中识别出的实际 cluster 数量通常远少于体素离散化带来的 Gaussian 分布数量。将所有 scan 中检测到的实际 Gaussian 分布纳入考量后, 在优化过程中能够自然地突出最可能对应的 cluster, 并构建一个连续且可导的目标函数以指导后续优化工作
1. 地面分割
用于这项工作的地面分割算法的基础最初由Douillard等人于2011年提出
1.1 径向栅格绑定
为了进行径向栅格绑定，在激光扫描坐标系下的激光扫描的x−yx-y平面首先被分割成NaN_a个角扇区。每个扇区覆盖的角度τa\tau_a由下式给出
τa=2πNa(7) \tau_a = \frac{2\pi}{N_a} \tag{7}
每个扇区进一步细分为NlN_l个基于线性范围的bin。每个线性范围bin覆盖的距离τl\tau_l由下式决定
τl=RmaxNb(8) \tau_l = \frac{R_{max}}{N_b} \tag{8}
其中RmaxR_{max}是给定扫描预期的最大范围测量值。定义第ii个扇形和第jj个线性范围的bin为ζij⊆R2\zeta_{ij} \subseteq \mathbb{R}^2，以及所有bin的集合为Ξ\Xi，这里∣Ξ∣=NaNl|\Xi| = N_aN_l。定义点云PP中投影落在栅格ζij\zeta_{ij}的点为
Pij={p∈P：(px,py)∈ζij}(9) P_{ij} = { p \in P：(px,py) \in \zeta_{ij} } \tag{9}
为了执行快速地面分割，从栅格点集PijP_{ij}构造一组元组。为了构造元组集合，为每个栅格单元内的点确定一个原型点。对于地面分割的情况，最好在每个栅格单元中对原型点进行建模，以便它最好地代表地面。每个单元格内的点PijP_{ij}的原型点ζij\zeta_{ij}被选为具有最低zz坐标的点
xij=arg minp∈Pijpz x_{ij} = \argmin_{p \in P_{ij}}{p^z}
最后，使用每个栅格单元中的点的原型点，可以定义每个角扇区的元组集合。定义与原型点一个栅格中原型点xijx_{ij}相关联的元组为yij=(rij,hij)∈R2y_{ij} = (r_{ij},h_{ij}) \in \mathbb{R}^2。对于元组表示，第一个元素rij∈Rr_{ij} \in \mathbb{R}是范围分量，而hij∈Rh_{ij} \in \mathbb{R}是高度分量。使用栅格单元的原型点xijx_{ij}，元组的范围和高度分量计算如下
rij=(xijx)2+(xijy)2(11) r_{ij} = \sqrt{(x_{ij}x)2 + (x_{ij}y)2} \tag{11}
hij=xijz(12) h_{ij} = x_{ij}^{z} \tag{12}
范围元素rijr_{ij}是从原型点xijx_{ij}到传感器原点的xx和yy分量的欧几里得距离，高度元素hijh_{ij}只是原型点的zz分量。角扇区ii的元组集合YiY_i被定义为
Yi={yij：j∈{1,...,Nl}}(13) Y_i = { y_{ij}：j \in { 1,...,N_l } } \tag{13}
1.2 GP回归
地面分割是通过执行GP回归来完成的。关于GPs用于地面地形建模的彻底探索，见Vasude-van等人(2009年)。GP回归提供了一个概率框架，在给定一组训练数据的情况下，为一组查询输入预测一组高度值。假设GP模型要用范围-高度元组集的子集训练，表示为̄Yˉi⊆Yi\bar{Y}i \subseteq Y_i。协方差函数用于量化用于回归的数据点之间的相关性。在这项工作中，使用了平方指数协方差函数，κ：R2→R\kappa：\mathbb{R}^2 \to \mathbb{R}。假设设计了两个标量数据点b1∈Rb_1\in \mathbb{R}和b2∈Rb_2 \in \mathbb{R}之间的关联是我们希望获得的。使用平方指数协方差函数，相关性由下式给出
κ(b1,b2)=σf2exp(−12l2(b1−b2)2) \kappa(b_1,b_2) = \sigma_f^2 exp(- \frac{1}{2l^2}(b_1 - b_2)^2)
其中l∈Rl \in \mathbb{R}是长度尺度参数，σf∈R\sigma_f \in \mathbb{R}是输入方差，它们被称为GP模型的超参数。给定协方差函数，可以提出地面分割的回归问题，该问题试图预测元组集Yi{Y}i的相关范围值的高度值，假定GP模型已经使用来自训练集Yˉi\bar{Y}i的数据训练。使用方差函数，定义与查询和训练范围数据相关的四个矩阵。定义Kqq∈R∣Yi∣×∣Yi∣\mathcal{K}{qq} \in \mathcal{R}^{|Y_i| \times |Y_i|}作为待查询集合的自协方差矩阵。令K(j,k)\mathcal{K}(j,k)为K\mathcal{K}的第(j,k)(j,k)个元素。为了简化表示法，将一个元组元素yk∈Yiy_k \in Y_i的范围和高度值分别表示为rkr_k和hkh_k。然后，用等式(14)给出的平方指数协方差函数来填充矩阵Kqq\mathcal{K}{qq}的每个元素，
Kqq(j,k)=κ(rj,rk),∀yj,yk⊆Yi(15) \mathcal{K}{qq}(j,k) = \kappa(r_j,r_k), \forall y_j,y_k \subseteq Y_i \tag{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|}的定义。对于任意y_j \in Y_i且y_k \in {\bar{Y}}_i（其中j,k=1,2,\dots,n），我们可以定义互协方差函数为\mathcal{K}(j,k) = \kappa(r_j,r_k)（16）。同时根据对称性关系可得\mathcal{K}_{tq} = (\mathcal{K}_{qt})^\top（17）。而对于属于{\bar{Y}}_i中的任意两个样本点y_j,y_k（其中j,k=1,2,\dots,m），自协方差函数被定义为\mathcal{K}_{tt}(j,k) = \kappa(r_j,r_k)（18）。
基于自相关与互相关矩阵的方法下述内容中所述
根据联合分布，可以给出条件均值和条件方差的表达式。条件均值和协方差表达式产生预测方程。换句话说，使用训练数据和预测方程，可以为一组待查询的距离值预测地面高度值的向量λ∗\lambda{*}和相应的的协方差Vλ∗V_{\lambda{}}。条件均值和方差预测方程如下
λ∗=Kqt[Ktt+σn2I]−1λ(20) \lambda^{} = \mathcal{K}{qt}[\mathcal{K}{tt} + \sigma_n^2 I]^{-1} \lambda \tag{20}
Vλ∗=Kqq−Kqt[Ktt+σn2I]−1Ktq(21) V_{\lambda^{*}} = \mathcal{K}{qq} - \mathcal{K}{qt}[\mathcal{K}{tt} + \sigma_n^2 I]^{-1} \mathcal{K}{tq} \tag{21}
定义方法γ(Yi,Yˉi)\gamma(Y_i,\bar{Y}i)，它使用训练数据Yˉi\bar{Y}i和待查询点YiY_i为角扇区ii生成四个相关矩阵Ktt\mathcal{K}{tt}、Ktq\mathcal{K}{tq}、Kqt\mathcal{K}{qt}和Kqq\mathcal{K}{qq}
基于预测方程...
1.3 使用GP回归和增量采样一致性的地面分割
INSAC算法与GP回归模型协同作用于激光扫描数据中的地面点识别工作。将径向栅格划分为扇区后，在每个扇区内部的所有地面点均具有独立标识特征。如前所述，在第4.1.1节中详细阐述了三维扫描数据如何被扇区分割并绑定的过程。为了初始化地面分割过程，请先从每个扇区中选取距离扫描原点不超过阈值δo\delta_{o}范围内的高度元组点集作为种子集；随后GP回归模型将在这些种子集上进行训练以生成训练点Yˉi\bar{Y}i集合。在查询集YiY_i中剩余未被选中的原型点将依据GP回归模型进行评估筛选；当前迭代阶段的所有内点元组将被收集到内点集合YinY{in}中，并生成新的GP回归模型用于进一步分析；不断重复上述过程直到无法再添加新的内点为止；当所有候选内点都被耗尽后，则剩余的所有bin中的原型点都将被分类为地物或非地物特征区域；针对每一处地物bin中的剩余待分类样本，则需比较其高度分量与当前地物原型的高度分量差异是否小于预先设定的地物判别阈值δg\delta_{g}；若满足该条件，则认为该待分类样本属于地物区域；基于此定义的操作符Pg←ϑ(P)P^{g} \leftarrow \vartheta(P)将接收原始激光扫描数据PP并返回相应的地物特征区域集合PgP^g；算法1完整记录了上述地面分割方法的具体实施流程（见图2），而图3展示了典型激光扫描数据下的分割效果示例
该论文中所描述的内容较为晦涩难懂, 相较于直接观察算法本身而言, 直接阅读算法可能会更加直观易懂一些. 本文所提出的假设基础在于, 在距离传感器原点附近且处于一定范围内的一组典型样例被认定为地面特征, 并用于初始化高精度平面(GP)模型. 该假设计实上是合理的, 在一般情况下接近车辆的位置通常不会存在较大的障碍物.
非地面点的聚类
通过将稀疏性处理引入非地面点群落划分过程，并结合区域增长算法进行迭代优化以实现高精度地物分类目标
基于第4.1.1节所描述的类似径向栅格技术框架，在为简化处理过程而采用单一索引的前提下，定义径向栅格单元ζi∈R2\zeta_i \in \mathbb{R}2及其所属单元集合为Ξ\Xi。针对非地面点云数据集PngP{ng}中的点集PiP_i（i=1,...,|\Xi|i=1,...,|\Xi|），定义属于栅格单元ζi\zeta_i的空间点集为
{p∈Png：(px,py)∈ζi}(24) { p \in P{ng}：(px,p^y) \in \zeta_i } \tag{24}
其对应的空间点集全体构成集合P={P1ng,...,P|\Xi|ng}\mathcal{P} = { P_1{ng},...,P_{|\Xi|}{ng} }。根据聚类理论定义聚类空间γ⊆Png\gamma \subseteq P^{ng}及其集合Γ={γ1,...,γNΓ}\Gamma = {\gamma_1 ,..., \gamma_{N_\Gamma}}其中NΓN_{\Gamma}表示总的聚类数量该值在初始化前未知。通过从均匀分布U\mathcal{U}中随机采样初始簇中心索引ii的方式生成初始簇体即令μi∈R3\mu_i ∈ ℝ³为对应空间点集PiP_i的质心并定义距离函数d：ℝ³×ℝ³→ℝd:\mathbb{R}^3 × ℝ^3 → ℝ计算两个质心之间的距离表示如下
d(Pi,Pj)=||μi−μj||m(25) d(P_i,P_j) = || μ_i - μ_j ||m \tag{25}
其中mm指定了所采用的距离范数类型基于上述距离计算结果可确定每个空间点集PiP_i的空间最近邻区域ΩΩ即满足以下条件的所有空间点集
Ω={Q∈P:d(Q,Pi)<δnn}\backslash Pi(26) Ω = { Q ∈ 𝒫 : d(Q,P_i) < δ{nn} } ∖ P_i (26)
其中δnnδ_{nn}}作为单个质心PiP_i的空间领域阈值被设定为此处的距离计算结果的最大允许偏差值。
对于随机选取的一组点PiP_i来说，在生成其最近邻之后将该邻集Ω\Omega加入到已有的最近邻集合中。在该邻集中任意一个元素PjP_j都会被用来与原始点集PiP_i比较以确定两者是否具有合并的可能性。为了实现两组数据集的合并操作我们定义了一个函数ϱ：P×P→{0,1}\varrho：\mathcal{P} \times \mathcal{P} \to {0,1}来判断两组数据是否可合并性问题解决方法为若这两组数据能够被合并则函数ϱ(Pi,Pj)\varrho(P_i,P_j)返回1否则返回0在此合并函数中我们可以采用不同的度量方法来进行比较例如基于欧氏距离计算各数据点间的平均值或者通过高斯分布间的L2L_2距离来进行比较此外我们还可以利用高斯拟合方法来实现这一功能如果相邻的数据桶能够被合并则它们会被加入到开放型的数据列表QoQ_o中其中QokQ_o^k表示列表中的第kk个元素而相邻的数据桶也会从原始数据集中移除并被整合到当前聚类γcur\gamma_{cur}中随后系统会探索并评估QoQ_o中最接近的对象每当一个数据桶被探索过后其最近邻也会被自动加入到列表QoQ_o中当列表QoQ_o为空时系统会从原始数据集中选择一个新的数据桶继续上述操作直至所有非空的数据桶都被分配给某个聚类为止该算法的具体实现过程如算法2所示
这玩意其实就是一个简化版本的DBSCAN聚类
定义方法\Gamma \leftarrow \eta(Png)\Gamma \leftarrow \eta(P^{ng})$, 它处理非地面点的点云数据, 并基于区域增长算法生成相应的聚类集合. 图3展示了聚类完成后所形成的高斯分布的一个具体实例.
SRG-NDT代价函数
基于场景扫描上应用区域增长聚类算法所得的所有高斯分布基础上修改NDT代价函数重新构建该方法，并对分割后参考扫描中的每个点进行评估。其中参数估计̄ϵ\epsilon作用于场景扫描中的点p其中参数估计̄ϵ\epsilon作用于场景扫描中的点p, 生成变换后的点pˉ=Tϵ(p)\bar{p}=T_{\epsilon}(p)。这些变换后的点通过NDT评分函数ρ\rho进行评估，并与对应的高斯聚类γj\gamma_j进行比较（如图所示）。该评分机制能够有效突出最可能对应的高斯聚类，并为优化提供连续且可微分的目标函数框架。与标准NDT算法类似，在计算目标函数解析梯度gg的同时也能获得海塞矩阵HH信息（如式(27)所示），这有助于显著提升非线性优化效率[1]。优化过程从预设的参数估计初始值ϵ^\hat{\epsilon}出发进行搜索，在满足梯度范数||g||< ϵndt时停止迭代计算（如图所示）。值得注意的是由于目标函数具有良好的连续性性质因此解析梯度与海塞矩阵在整个参数空间内均具有良好定义这保证了采用基于梯度的方法能够确保全局收敛性
算法3中给出了具有点到分布匹配的SRG-NDT的总结。
对于基于点云配准的SRG-NDT算法，在实现点云匹配时采用了与该方法相似的技术手段。值得注意的是，在处理两次激光扫描数据时，并非仅进行简单的分割和聚类处理；而是通过将每次扫描所得的高斯分布集合代入到对应的NDT代价函数中进行对比分析以确定最优匹配关系
综上所述
实验结果
此部分将根据具体情况进行更新；此外，在此之前也会考虑对其他基于NDT改进方法的论文核心内容进行更新。