文章目录
摘要
摘要—点匹配和位姿优化是激光雷达里程计中的两个重要过程。前者容易受到噪声和初始位姿估计的影响，而后者常常因为不正确的匹配点而陷入局部极小值。本文提出了一种新的点对相似性方法，结合了法向量、空间协方差矩阵的最小特征值和局部强度值的KL散度。在位姿优化步骤中，我们同时使用提出的点对相似性和面性作为权重。实验结果表明，该方法可以确保更高的准确性，并且能够在普通PC上以27帧每秒的速度运行。
一、介绍
作为智能车辆的基本技术之一，里程计广泛用于位姿估计，这对许多任务（如规划和控制）至关重要。它常用于自动驾驶 [1]、自动移动机器人 [2] 和SLAM [3], [4]。智能车辆最常用的传感器之一是激光雷达（LiDAR），激光雷达传感器能够直接提供精确的距离测量，通常具有厘米级误差。与视觉里程计相比，激光雷达里程计在光照变化下具有更高的准确性和鲁棒性 [5], [6]。给定两个不同坐标系中的点集，激光雷达里程计的目标是找到最佳对齐这两个点集的变换，如图1所示。
尽管在该领域进行了大量研究，点匹配和最小化仍然是激光雷达里程计中最具挑战性的问题。一组良好的匹配点将显著加速高精度的里程计。由于其概念简单且性能良好，最近邻方法在实践中非常流行，成功应用于众多现实任务。然而，这种方法以容易陷入局部极小值而著称。为了改善点匹配，之前的研究致力于采用更有效的特征 [7], [8] 或利用线或平面约束 [9], [10], [11]。其他特征，例如某一点的法向量和曲率，被广泛用于评估点候选者的相似性。然而，在同一平面上的点由于相同的局部结构，法向量或曲率值无法区分。此外，一些方法，例如在点匹配中应用平面（或线）点的方法，当点云中包含的平面不足时，并不总是可靠。
在给定匹配点的情况下，许多最小化方法对所有点对采用相等权重。然而，相等权重无法表示匹配质量和链接点位置的不确定性。一方面，可能存在错误匹配，这些匹配的权重应该较低；另一方面，即使是正确匹配，两个匹配点的位置也可能存在噪声，因此也会遗憾地导致低权重结果。
在本文中，我们提出了一种称为IGICP的强度-空间激光雷达描述符，适用于激光雷达里程计。点的表示通过将欧几里得坐标与表面法线和激光雷达强度相结合得到了扩展。所提出的表示用于计算点候选者的相似性。在最小化步骤中，每个点对的接收权重代表了配对可靠性和点的不确定性。匹配的可靠性通过所提出的相似性进行测量，同时点的不确定性由点的面性表示。本文的主要贡献如下：
我们提出了一种高效且稳健的点匹配方法。在此过程中，法向量、相应的特征值和反射强度的KL散度共同用于匹配点。结合局部几何特征和强度提供了较高的正确匹配率。还表明，KL散度的强度与距离和入射角有关。一般而言，KL散度随着距离因子的增大呈四次方增长。为了确保无范围的强度测量，我们建议对原始输入强度进行归一化。
-
我们提出了一种计算每个匹配权重的方法，该方法考虑了配对不确定性和点的不确定性。同时，所提出的相似性被用于评估匹配的配对不确定性，平均面性表示单个点的不确定性。
-
在KITTI、M2DGR和自收集的数据集上的实验结果表明，所提出的IGICP在大多数KITTI序列和所有M2DGR序列上，在绝对轨迹误差（ATE）方面优于现有最先进的方法（SOTA）。与GICP相比，IGICP在较少的迭代次数下实现收敛，因此所需的时间成本低于GICP。
本文的其余部分组织如下：第二部分给出了文献综述。第三部分推导了我们在增强强度和几何的ICP中的主要贡献。实验结果在第四部分中呈现，结论在第五部分中展示。
二、相关工作
A. 基于点的方法
激光雷达里程计可以分为三大类：基于点的方法、基于分布的方法和基于学习的方法。ICP [12] 是基于点方法的典型代表，也是最早的里程计工作之一。ICP 直接在最近点之间建立匹配，并根据欧几里得距离构建成本函数。然而，在寻找两个点云之间的一对一对应关系时，这仍然是一个具有挑战性的问题。因此，为了提高 ICP 的适用性，提出了点到线 ICP [13] 和点到面 ICP [14]，它们通过最小化点到线距离或点到面距离来估计变换。NICP [7] 进一步利用每个点的局部特征（法向量和曲率）来改善点匹配。LOAM [9] 根据每条扫描线上的点的曲率提取角点和平面点，并结合点到线和点到面模型来估计变换。LEGO-LOAM [15] 通过点云分割过滤噪声点，并将其分为地面和非地面点。作为 LOAM 的一个变种，LOAM-livox [16] 实现了针对具有小视场（FoV）的激光雷达的点选择和迭代位姿优化。R-LOAM [17] 通过点到网格对应关系扩展了 LOAM。MULLS [10] 提取更多特征（地面、立面、柱子、梁等）以提高抗噪声的鲁棒性。基于点的方法在位姿估计中通常表现出较高的精度，但 ICP 及其变种有时可能由于初始位姿不当而陷入局部极小值。
B. 基于分布和基于学习的方法
基于分布的方法也被广泛应用于激光雷达里程计。NDT [18]、[19]、[20]、[21] 是最早且最受欢迎的方法之一。在 NDT 中，目标点云首先被投票到体素网格中，其中网格中的点最终用正态分布来描述。然后，源点云的每个点通过将其投票到这些分布中获得一个分数，变换可以通过优化点分数来估计。NDT 用正态分布的方式描述目标点云，而 GICP [22] 则对源点云和目标点云均如此。VGICP [23] 通过体素化扩展了 GICP，以避免成本高昂的最近邻搜索，同时保持其精度。LITAMIN [24] 通过 Frobenius 范数的成本函数归一化和正则化协方差矩阵实现稳定。LITAMIN2 [25] 进一步引入对称的 KL 散度 [26] 来测量局部分布之间的差异，成本函数包括点之间的距离和分布的差异。NDT 及其变种在许多应用中被证明对初始位姿具有鲁棒性，但与基于点的方法相比，NDT 估计的位姿通常不够准确。因此，在实践中常常将 NDT 用作粗略位姿估计器，并采用其他准确的位姿估计方法（如 ICP）进行微调。
近年来，基于学习的方法在该领域引起了更多关注。LO-Net [27] 是一种端到端的方法，具有新的掩模加权几何约束损失。LodoNet [28] 将激光雷达扫描转化为图像，并重新构建激光雷达场景描述问题，以提取图像特征。DeepCLR [29] 提供了一种基于半径搜索和 mini-PointNet 的端到端架构，而无需提取显式的点对应关系。PWCLO-Net [30] 提出了使用层次嵌入掩模优化。最近，HPPLO-Net [31] 利用可微分加权点到面 SVD 来求解位姿矩阵。时间复杂度的平方增长限制了流行的变换模块在点云处理中的应用。TransLO [32] 将 3D 点投影到 2D 表面，并将 2D 投影输入到具有线性复杂度的局部变换器中。EfficientLO-Net [33] 是一个端到端框架，并采用层次嵌入掩模来过滤不匹配的点。基于学习的方法在准确的位姿估计方面具有优势。然而，由于缺乏大规模的激光雷达数据集，基于学习的方法的泛化能力仍然存在疑问。此外，这类工作需要高计算资源，通常需要通过 GPU 加速。
C. 点匹配
现有的选择对应关系的方法可以分为三类：最近邻（NN）、具有平面或线约束的 NN 以及特征匹配，如表 1 所示。将目标扫描中的最近邻点作为对应关系是最常见且最简单的方法 [12][22]。由于 NN 在欧几里得空间中的缺陷，它容易受到噪声、体素下采样和变换的影响。平面或线约束方法提取不同类型的关键点 [9][10][11]，例如角点、线点或面点，然后在同类点中寻找最近邻。这些方法需要准确的点分类，通常耗时较长。基于特征的方法通过使用局部结构或点特征（如法向量、曲率 [7][8][34][35][36] 或强度）来匹配点。然而，当局部几何特征相似时，例如同一平面上的一些点，这种方法可能无法有效工作。
D. 基于强度的方法
在过去的十年中，激光雷达传感器的强度在许多 SLAM 任务中取得了成功 [37][38]。Intensity-SLAM [39] 利用 KNN 之间空间距离和强度距离的加权和，这体现在后端图优化中，而我们关注的是前端的激光雷达里程计。除了对原始点云进行处理外，几种方法将点云投影到圆柱形图像中，从而受益于丰富的 2D 图像处理工具。SLO [40] 通过 SuperPoint 提取圆柱形图像的关键点，并通过匹配描述符在关键点之间进行全面搜索。然后，SLO 通过 RANSAC 确定性地迭代排除离群点，并以自我监督的方式利用新关键点。InTEnLOAM [41] 采用圆柱形图像处理点云，额外计算强度差异图像、法线图像和曲率图像，以及强度直方图。根据特征，InTEn-LOAM 将每个关键点分类为四类。MCGICP 扩展了 GICP，增加了颜色或强度等额外输入通道。MCGICP [42] 首先将 3D 点投影到其局部 2D 表面，然后计算带有强度或颜色生成权重的加权协方差。基于强度的方法通常假设局部子集具有一致的强度，但在实际中，强度值容易受到噪声、距离和视角的影响。
E. 加权代价函数
在ICP及其变体中，给定匹配点，最小化步骤尝试优化旋转R和平移t，从
arg
min
R
,
t
∑
w
i
∥
R
x
i
+
t
−
y
j
∥
2
\arg\min_{R, t}\sum w_{i}\left\|R x_{i}+t-y_{j}\right\|^{2}
argR,tmin∑wi∥Rxi+t−yj∥2
其中 x i x_{i} xi 和 y j y_{j} yj 形成一对匹配， w i w_{i} wi 是一次匹配的权重。权重设计可以分为三类：等权重、成对权重和逐点权重，如表 II 所示。等权重因其简单性而受到广泛选择，但容易受到错误匹配、噪声点或杂乱点的影响。成对权重通常根据计算的相似度使用局部几何特征作为权重。在 [34] 和 [36] 中，权重与一对点之间的距离呈反比相关。在 [43] 中，局部协方差矩阵特征值的差异用于形成几何权重。成对权重不考虑点本身的质量，因此总是需要提前过滤噪声点或杂乱点。
逐点权重方法则侧重于测量点的质量以形成匹配。在 [44] 中，研究人员根据点的深度设计权重。在 [45] 中，权重根据激光雷达光束的入射角进行分配。在 [46] 中，权重根据传感器误差模型形成。逐点权重忽略了对应关系的质量，当错误匹配率较高时，可能无法有效工作。
图 2. KITTI 00 中的点匹配示例。线条连接了匹配的点。 (a) 通过最近邻 (NN) 获得的匹配。 (b) 通过所提相似度获得的匹配。
三. 基于强度和几何的ICP
点匹配和最小化是ICP基础里程计中的两个重要步骤。在本节中，我们提出了我们的基于强度和几何增强的ICP。首先，我们详细阐述了所提出的点的相似性。然后，我们讨论了ICP最小化中的权重设计。
A. 基于强度和几何相似性匹配的概述
由于其简单和高效，欧几里得空间中的最近邻在选取对应点时很常见。对于具有复杂结构的点，如角落，NN（最近邻）常常无法获得正确的匹配。图2显示了从KITTI 00的第一和第二次扫描中提取的两个案例。在图2(a)中，大多数NN选择的对应点是错误的，垂直平面上的点与水平平面上的点匹配。我们的目标是通过引入可靠的特征，如图2(b)所示，来改进匹配。
我们使用两步点匹配策略，首先通过KNN（K最近邻）搜索找到匹配候选，然后通过特征匹配确定最终对应。对于每个源点，我们首先在目标扫描中搜索其K个最近邻作为候选。然后，对于每对候选点，我们定义几何相似性 S G S_{G} SG 和强度相似性 S I S_{I} SI，并据此计算最终相似性。我们
选择候选中相似性最高的一对作为匹配对。可以通过kd树搜索进行KNN搜索候选，因此保持了ICP的速度和简单性优势。随后的特征匹配则确保了它比仅使用NN更准确和可靠。
鉴于几何和强度相似性，一次匹配的最终相似性S定义为
S
=
S
G
⋅
S
I
(
2
)
S=S_{G}\cdot S_{I} \qquad(2)
S=SG⋅SI(2)
在接下来的两节中，将详细讨论几何相似性
S
G
S_{G}
SG 和强度相似性
S
I
S_{I}
SI。
B. 点对的几何相似性 S G S_{G} SG
除了欧几里得坐标，许多其他特征被引入来表示点，包括法线向量、曲率或强度值。点的法线向量在以前的工作中被广泛使用，以指示局部结构。在我们的几何相似性中，我们采用法线向量作为特征之一。对于一个点 p i p_{i} pi，我们首先计算其协方差矩阵 C i C_{i} Ci。然后，我们得到 C i C_{i} Ci 的特征值 λ 1 ≥ λ 2 ≥ λ 3 \lambda_{1}\geq\lambda_{2}\geq\lambda_{3} λ1≥λ2≥λ3 和相应的特征向量 v 1 , v 2 , v 3 v_{1}, v_{2}, v_{3} v1,v2,v3，其中 n = v 3 n=v_{3} n=v3 是 p i p_{i} pi 的法线向量， λ 3 \lambda_{3} λ3 是表示法线向量方向上点的分散程度的特征值。在许多以前的研究[7]、[8]、[34]中，曲率 κ \kappa κ 被用来描述点的局部结构，
κ = λ 3 λ 1 + λ 2 + λ 3 ( 3 ) \kappa=\frac{\lambda_{3}}{\lambda_{1}+\lambda_{2}+\lambda_{3}} \qquad(3) κ=λ1+λ2+λ3λ3(3)
尽管简单高效，我们认为曲率的计算在视图变化下并不稳健。为了说明这一点，图3中展示了一个2D激光雷达扫描的例子。在图3中，一个2D激光雷达传感器观察到平面上的一些点。设 r为传感器到平面的距离，
β
\beta
β 为2D激光雷达传感器的角度分辨率，
d
k
d_{k}
dk 为第k个点到垂直线的距离。那么，第k个点与第
(
k
−
1
)
(k-1)
(k−1) 个点之间的距离
Δ
d
k
\Delta d_{k}
Δdk 为
Δ
d
k
=
r
[
tan
(
k
β
)
−
tan
(
(
k
−
1
)
β
)
]
=
r
[
sin
β
cos
(
k
β
)
cos
(
(
k
−
1
)
β
)
]
≈
r
β
cos
2
(
k
β
)
(
4
)
\begin{align*}\Delta d_k&=r[\tan(k\beta)-\tan((k-1)\beta)]\\ &=r\left[\frac{\sin\beta}{\cos(k\beta)\cos((k-1)\beta)}\right]\\ &\approx\frac{r\beta}{\cos^2(k\beta)}\end{align*} \qquad(4)
Δdk=r[tan(kβ)−tan((k−1)β)]=r[cos(kβ)cos((k−1)β)sinβ]≈cos2(kβ)rβ(4)
其中在近似等式中我们假设足够小的角度分辨率
β
→
0
\beta\rightarrow 0
β→0 和
cos
(
k
β
)
≈
cos
(
k
−
1
)
β
\cos(k\beta)\approx\cos(k-1)\beta
cos(kβ)≈cos(k−1)β。
由于大多数现有方法采用KNNs进行协方差计算，(4) 则意味着侧面视图点的主要方差与正面视图点的方差非常不同。方差，或者说协方差矩阵的主导特征值。矩阵，随着(4)所示的
1
/
cos
4
(
k
β
)
1/\cos^{4}(k\beta)
1/cos4(kβ) 成比例增长，从而导致曲率估计不稳定。
除了侧面视图点的方差不稳定问题外，(4)还意味着平面点的两个主要方差不平等，理想情况下它们应该是相等的， λ 1 = λ 2 \lambda_{1}=\lambda_{2} λ1=λ2。激光雷达传感器的水平和垂直角度分辨率可能差异很大。因此，由于不同方向上的密度不平衡，垂直方向上的方差通常比水平方向上的方差大。
为了解决这个问题，我们用最小特征值 λ 3 \lambda_{3} λ3 代替曲率。对于本文中提到的平面点，表明 λ 3 \lambda_{3} λ3 是曲率的一个适当近似。对于一个平面点， λ 1 \lambda_{1} λ1 和 λ 2 \lambda_{2} λ2，或者两个主要主导方向的方差，比第三方向的方差大得多， λ 1 , λ 2 ≫ λ 3 \lambda_{1},\lambda_{2}\gg\lambda_{3} λ1,λ2≫λ3，并且 λ 1 = λ 2 \lambda_{1}=\lambda_{2} λ1=λ2 是一个常数标量，在由于不平衡的角度分辨率和视图变化而难以获得等式的情况下，如在图3和(4)中讨论的。通过用常数替换不稳定的 λ 1 \lambda_{1} λ1 和 λ 2 \lambda_{2} λ2，(3)变为
κ ≈ λ 3 2 λ 1 ( 5 ) \kappa\approx\frac{\lambda_3}{2\lambda_1}\quad(5) κ≈2λ1λ3(5)
方程(5)表明，对于平面点，它们的曲率 κ \kappa κ 大约等于 λ 3 \lambda_{3} λ3，最多有一个常数因子。因此，我们用最小特征值 λ 3 \lambda_{3} λ3 替换曲率，以避免 λ 1 \lambda_{1} λ1 和 λ 2 \lambda_{2} λ2 的不稳定计算，作为特征。很容易验证 λ 3 \lambda_{3} λ3 对视图变化是稳健的，如在图3中讨论的。
我们使用一个四维向量 v，由法线向量 n 及其对应的特征值 λ 3 \lambda_{3} λ3 组成，作为单个点的局部几何特征，
v = [ n , α λ 3 ] ( 6 ) v=\left[n,\alpha\lambda_3\right]\quad(6) v=[n,αλ3](6)
其中
α
\alpha
α 是一个增益常数，用于平衡
λ
3
\lambda_{3}
λ3 与法线向量 n 的比例。对于平面点的情况，由于
λ
3
\lambda_{3}
λ3 很小，所提出的几何特征 v 由法线向量主导。对于没有主导方向的大
λ
3
\lambda_{3}
λ3，
λ
3
\lambda_{3}
λ3 提供了对局部形状的额外约束。点对的几何相似性
S
G
S_{G}
SG 定义为它们几何特征之间的余弦距离，
S
G
=
v
i
v
j
T
∥
v
i
∥
∥
v
j
∥
(
7
)
S_G=\frac{v_i v_j^T}{\left\|v_i\right\|\left\|v_j\right\|}\qquad(7)
SG=∥vi∥∥vj∥vivjT(7)
C. 强度相似性 S I S_{I} SI
除了局部结构外，激光雷达传感器还默认输出一束光的反射强度。强度通常用于全局扫描匹配[47][48]，它关注的是整个点云而不是单个点。在[38]中，研究直接将强度作为额外的维度连接在特征中。众所周知，强度的主要弱点是它容易受到噪声、距离和视图角度的影响。因此，单点强度的直接应用总是导致匹配结果不佳。为了解决不稳定强度的问题，我们采用局部子集的强度值统计而不是单个点。我们假设围绕一个点 p i p_{i} pi 的强度值服从正态分布 N ( μ i , σ i 2 ) \mathcal{N}\left(\mu_{i},\sigma_{i}^{2}\right) N(μi,σi2)，其中 μ i \mu_{i} μi 是均值， σ i 2 \sigma_{i}^{2} σi2 是方差。我们使用KL散度来评估两个点之间的强度差异，
K L ( N i ∣ ∣ N j ) = log σ j σ i + σ i 2 + ( μ i − μ j ) 2 2 σ j 2 − 1 2 ( 8 ) KL\left(\mathcal{N}_i\mid\mid\mathcal{N}_j\right)=\log\frac{\sigma_j}{\sigma_i}+\frac{\sigma_i^2+\left(\mu_i-\mu_j\right)^2}{2\sigma_j^2}-\frac{1}{2} \qquad(8) KL(Ni∣∣Nj)=logσiσj+2σj2σi2+(μi−μj)2−21(8)
在点匹配中，强度相似性应该是对称的， K L ( N i ∣ ∣ N j ) = K L ( N j ∣ ∣ N i ) K L\left(\mathcal{N}_i\mid\mid\mathcal{N}_j\right)=K L\left(\mathcal{N}_j\mid\mid\mathcal{N}_i\right) KL(Ni∣∣Nj)=KL(Nj∣∣Ni)，但原始的KL散度未能满足这一要求。因此，我们使用对称的KL散度代替，
K L sym = 1 2 [ K L ( N i ∥ N j ) + K L ( N j ∥ N i ) ] = σ i 2 + ( μ i − μ j ) 2 4 σ j 2 + σ j 2 + ( μ i − μ j ) 2 4 σ i 2 − 1 2 ( 9 ) \begin{align*}K L^{\text{sym}}&=\frac{1}{2}\left[K L\left(\mathcal{N}_i\|\mathcal{N}_j\right)+K L\left(\mathcal{N}_j\|\mathcal{N}_i\right)\right]\\ &=\frac{\sigma_i^2+\left(\mu_i-\mu_j\right)^2}{4\sigma_j^2}+\frac{\sigma_j^2+\left(\mu_i-\mu_j\right)^2}{4\sigma_i^2}-\frac{1}{2} \qquad(9)\end{align*} KLsym=21[KL(Ni∥Nj)+KL(Nj∥Ni)]=4σj2σi2+(μi−μj)2+4σi2σj2+(μi−μj)2−21(9)
方程(9)中的 K L sym K L^{\text{sym}} KLsym 的范围是 [ 0 , + ∞ ) [0,+\infty) [0,+∞)。我们通过高斯函数将 K L sym K L^{\text{sym}} KLsym 标准化到 [ 0 , 1 ] [0,1] [0,1] 范围内，以获得强度相似性 S I S_{I} SI，
S I = e − ( K L s y m ) 2 2 τ 2 ( 10 ) S_I=e^{-\frac{\left(K L^{sym}\right)^2}{2 \tau^2}} \qquad(10) SI=e−2τ2(KLsym)2(10)
其中 τ \tau τ 是用户设计的高斯尺度参数。
D. 强度归一化
激光雷达传感器收集的强度受到许多因素的影响，因此在实践中是不稳定的。在如第三节所述计算强度相似性之前，我们首先对接收的强度值进行归一化。我们使用Lambertian模型构建传感器强度模型[49]，
I r = I o × ϕ 2 4 r 2 η sys η atm ρ cos θ ( 11 ) I_r=I_o\times\frac{\phi^2}{4 r^2}\eta_{\text{sys}}\eta_{\text{atm}}\rho\cos\theta \qquad(11) Ir=Io×4r2ϕ2ηsysηatmρcosθ(11)
其中
I
o
I_{o}
Io 和
I
r
I_{r}
Ir 分别是发射和接收的强度，
ϕ
\phi
ϕ 是接收器口径直径，
η
sys
\eta_{\text{sys}}
ηsys 和
η
atm
\eta_{\text{atm}}
ηatm 分别是系统和大气衰减，r 是从传感器到目标的距离，
θ
\theta
θ 是入射角，
ρ
\rho
ρ 是目标的反射率。假设
I
o
,
ϕ
,
η
sys
I_{o},\phi,\eta_{\text{sys}}
Io,ϕ,ηsys 和
η
atm
\eta_{\text{atm}}
ηatm 是常数，那么反射率
ρ
\rho
ρ，这是塑造目标点材料的重要属性，是
ρ
∝
r
2
I
r
cos
θ
(
12
)
\rho\propto\frac{r^2 I_r}{\cos\theta}\quad(12)
ρ∝cosθr2Ir(12)
因此，我们通过
I
=
I
r
r
2
cos
θ
(
13
)
I=I_r\frac{r^2}{\cos\theta} \qquad(13)
I=Ircosθr2(13)
来归一化接收的强度，并在强度相似性计算中使用 I。注意，许多传感器都带有部分校准功能作为默认[39]。对于这类数据，我们可以根据(12)选择性地归一化距离或入射角。由于法线向量也是我们构建几何相似性所必需的，特别是强度归一化(13)，
图 4. 所提权重的示意图。绿色线条表示具有高权重的对应关系，这些对应关系在相似度和计划性上都很高，而红色线条表示具有低权重的对应关系，这些对应关系在相似度和/或计划性上都很低。
图5展示了在KITTI00中第一次和第二次扫描之间提出的成对权重。为了观察方便，仅可视化了第一次扫描。可以看出，附近的平面点被赋予了高权重，因为它们在最小化过程中是稳定的。位于边缘或角落的点被赋予了低权重，因为点类型的混淆。非平面点，如来自树木的点，的权重非常低。
入射角 θ \theta θ 的计算引入了非常有限的额外计算负担。强度归一化在我们的相似性计算中是必不可少的。考虑一个理想情况，匹配两个分别由它们各自的强度正态分布 N i \mathcal{N}_{i} Ni 和 N j \mathcal{N}_{j} Nj 表示的扫描，两个扫描之间只有轻微的平移。我们假设在形成 N i \mathcal{N}_{i} Ni 时所有 KNNs 的恒定范围 r i r_{i} ri，以及 N j \mathcal{N}_{j} Nj 的 r j r_{j} rj，并设 r i = a r j r_{i}=a r_{j} ri=arj。然后，根据(11)中的Lambertian模型， μ j = a 2 μ i , σ j 2 = a 4 σ i 2 \mu_{j}=a^{2}\mu_{i},\sigma_{j}^{2}=a^{4}\sigma_{i}^{2} μj=a2μi,σj2=a4σi2，(9)中的 K L sym K L^{\text{sym}} KLsym 变为
K L r i = a r j s y m = 1 4 a 4 + a 4 4 + μ i 2 4 σ i 2 ( 1 a 4 + 1 ) ( 1 − a 2 ) 2 − 1 2 ( 14 ) K L_{r_{i}=a r_{j}}^{s y m}=\frac{1}{4 a^{4}}+\frac{a^{4}}{4}+\frac{\mu_{i}^{2}}{4\sigma_{i}^{2}}\left(\frac{1}{a^{4}}+1\right)\left(1-a^{2}\right)^{2}-\frac{1}{2} \qquad(14) KLri=arjsym=4a41+4a4+4σi2μi2(a41+1)(1−a2)2−21(14)
方程(14)意味着两个理想扫描的KL散度随着范围因子a的四次方增长。(12)中的归一化消除了范围变化的影响，确保了平移过程中KL散度保持恒定。
E. 加权代价函数
加权代价函数已成功应用于许多基于ICP的方法。然而，为匹配对塑造适当的配对不确定性和点不确定性仍然是一个挑战。在本节中，我们提出了我们的权重，既考虑了点对之间的相似性，也考虑了每个单点的质量。图4展示了所提出的权重方法的主要思想，图5显示了在匹配KITTI 00的第一和第二次扫描时所提出的权重。
激光雷达数据的稀疏性使得即使在ICP收敛后也无法获得单个点的确切重叠。点到平面匹配是克服激光雷达稀疏性问题的有前途的解决方案，通过引入平面约束。受平面约束的启发，我们使用协方差的特征值的平均平面度来评估点的不确定性。对于点对 p i p_{i} pi 和 p j p_{j} pj 及其协方差特征值 λ 1 i ≥ λ 2 i ≥ λ 3 i \lambda_{1}^{i}\geq\lambda_{2}^{i}\geq\lambda_{3}^{i} λ1i≥λ2i≥λ3i 和 λ 1 j ≥ λ 2 j ≥ λ 3 j \lambda_{1}^{j}\geq\lambda_{2}^{j}\geq\lambda_{3}^{j} λ1j≥λ2j≥λ3j，平均平面度定义为
P = 1 2 × ( λ 2 i − λ 3 i λ 1 i + λ 2 j − λ 3 j λ 1 j ) ( 15 ) P=\frac{1}{2}\times\left(\frac{\lambda_2^i-\lambda_3^i}{\lambda_1^i}+\frac{\lambda_2^j-\lambda_3^j}{\lambda_1^j}\right)\qquad(15) P=21×(λ1iλ2i−λ3i+λ1jλ2j−λ3j)(15)
对于配对不确定性，我们使用(2)中的S来表示匹配的质量。最终，一个匹配点对的权重是
w i = S F ⋅ P ( 16 ) w_i=S_F\cdot P\quad(16) wi=SF⋅P(16)
给定每个匹配点对的权重，我们最小化以下加权代价函数，
T = arg min T ∑ i [ w i d i T ( C ~ i B + T C ~ i A T T ) − 1 d i ] ( 17 ) T=\arg\min_{T}\sum_i\left[w_i d_i^T\left(\tilde{C}_i^B+T\tilde{C}_i^A T^T\right)^{-1} d_i\right]\qquad(17) T=argTmini∑[widiT(C~iB+TC~iATT)−1di](17)
其中
T = [ R t 0 1 ] , C ~ i = [ C i 0 0 1 ] , d i = R x i + t − y j ( 18 ) T=\left[\begin{array}{ll} R& t\\ 0& 1\end{array}\right],\tilde{C}_i=\left[\begin{array}{cc} C_i& 0\\ 0& 1\end{array}\right], d_i=R x_i+t-y_j\quad(18) T=[R0t1],C~i=[Ci001],di=Rxi+t−yj(18)
当 w i w_{i} wi 都等于单位值时，(17)与GICP相同。在我们的文章中，我们使用(16)来计算 w i w_{i} wi。注意，我们假设所有点都是平面的，并给出了相应的权重，因此通过将其特征值替换为 ( 1 , 1 , ε ) (1,1,\varepsilon) (1,1,ε) 来规范每个点的协方差矩阵，其中 ε \varepsilon ε 是一个小的正常数。为此，让 3 × 3 3\times 3 3×3 对角矩阵 Λ \Lambda Λ 包含协方差矩阵 C i C_{i} Ci 的特征值在对角线上，V 是对应的特征向量， C i = V Λ V T C_{i}=V\Lambda V^{T} Ci=VΛVT。然后，我们将 Λ \Lambda Λ 的对角元素替换为 ( 1 , 1 , ε ) (1,1,\varepsilon) (1,1,ε) 并通过 C i ← V Λ V T C_{i}\leftarrow V\Lambda V^{T} Ci←VΛVT 来规范 C i C_{i} Ci。通过规范协方差矩阵，非平面点将在(17)中产生大误差。在FasterGICP[51]中可以找到类似的方法，其中直接过滤掉误差大的点。
F. IGICP概述
我们方法的框架如图6所示。给定两个输入点云，我们首先对两个点云进行下采样，并为每个点找到KNNs。尽管下采样方法有所改进[52]、[53]，我们为了简化使用经典版本。然后我们计算以一个点为中心的协方差矩阵的三个特征值，并通过(6)构建点的几何特征。几何相似性就是两个几何特征的余弦值。然后我们计算强度的对称KL散度，并将缩放的KL散度作为强度相似性。总体的配对相似性就是几何和强度相似性的简单乘积。强度相似性，如(2)所示。对于源扫描中的每个点，我们在目标扫描中找到其KNNs，并选择KNNs中相似性最高的点作为匹配点。给定匹配的点对，我们为每对点分配两个权重，点的平面度权重P如(15)所示，以及配对相似性权重 S F S_{F} SF 如(2)所示。然后，我们优化加权ICP类代价函数以获得最终的里程计。
四、 结果
