0. 简介
局部几何信息即法线和点分布在基于激光雷达的同时定位与地图构建（SLAM）中是至关重要，因为它为数据关联提供了约束，进一步确定了优化方向，最终影响姿态的准确性。然而即使在使用KD树或体素图的辅助下，估计法线和点分布也是耗时的任务。为了实现快速法线估计，《LOG-LIO: A LiDAR-Inertial Odometry with Efficient Local Geometric Information Estimation》研究了激光雷达扫描帧的结构信息，提出了一种新颖的快速近似最小二乘（FALS）方法，通过预先计算的方位角信息，当新的扫描帧到达时，估计法线仅需要点的距离信息，为了高效估计点的分布，我们将ikd树扩展到体素管理地图，并在保持法线一致性的同时增量更新其点云分布。对于满足基于法线的可见性和一致性检查的扫描点，我们设计了一种稳健且准确的分层数据关联方案，其中点到曲面的关联优先于点到平面的关联，在分布趋于收敛后，再进一步固定体素以平衡时间消耗和表示的正确性。文中的公式较多，所以我们主要先了解具体表示以及主要内容。该工作实现已在https://github.com/tiev-tongji/LOG-LIO 开源，还将Ring FALS作为独立的法线工具开源在https://github.com/tiev-tongji/RingFalsNormal。
1. 主要贡献
地图体素内逐步更新点云分布以保持空间信息的正确性，同时与法线保持一致性。为了平衡时间消耗和表示的正确性，我们在扩展的ikd树上管理地图，并在分布收敛后进一步固定分布。本工作的主要贡献如下：
- Ring FALS一种利用特定激光雷达的结构信息的新型快速近似最小二乘法法线估计器，与PCL相比快速而准确，满足LIO系统的实时要求。
- 考虑地图体素内点分布的鲁棒且准确的分层数据关联方案，其中点到曲面的关联优先于点到平面的关联，大尺度优先于小尺度。
2. 准备性工作
2.1 符号表示
我们现在定义本文中使用的符号和坐标系。我们将W视为世界坐标系，I_k和L_k分别表示与第k次LiDAR扫描相关的IMU和LiDAR坐标系。^aT_b ∈ SE(3)表示从坐标系b到坐标系a的欧几里德变换，由旋转矩阵^aR_b ∈ SO(3)和平移向量^at_b ∈ \mathbb{R}^3组成。n_r表示来自环FALS的法向量，e_d表示与分布的最小特征值对应的特征向量（详见第2.4节）。
2.2 LiDAR观测模型
图1. LiDAR观测模型和多尺度surfel关联的示意图。(a) 紫红色线条表示红色点的射线。八个点是Ring FALS用来估计红色点法线的邻域点。(b) 五个蓝色椭圆代表最近体素对应的绿色查询点在扩展ikd-tree中的小尺度surfel。橙色椭圆代表由小尺度surfel合并而成的大尺度surfel。绿色查询点首先与大尺度surfel关联。如果大尺度surfel不能满足第2.4节和第4.2节的条件，则与其所在体素的小尺度surfel关联
在实践中，LiDAR通过结合目标表面的方位和距离测量来获取一个点的三维坐标[18]，[19]，如图1(a)所示。LiDAR观测模型如下：
其中，r_i表示距离，θ_i表示方位角，φ_i表示垂直角度，代表目标点的水平和垂直结构信息的v相对于LiDAR。对于旋转式LiDAR，扫描点也可以根据其环索引和相对于LiDAR的方位角排列成映射表T，如下所示：
其中 row_i = ring(p_i) 并且 column_j = θ_i
2.3 最小二乘法法线估计
给定表面的一个子集n个三维点p_i，其中i = 1, 2, …, n，最小二乘法找到法线向量n = (n_x, n_y, n_z)和标量d，使得最小化方程（3）
正态分布的闭式解是协方差矩阵（方程（4）中的）最小特征值所对应的特征向量。
其中 \tilde{p} = 1/n \sum^n_{i=1} p_i
2.4 Surfel
我们将体素内的平面性ρ定义为类似于SLICT[6]的方式，并进一步引入γ如下：
其中，λ_1、λ_2、λ_3是协方差矩阵M的特征值，满足λ_1 < λ_2 < λ_3。我们定义一个表示surfel的体素，其ρ_i大于1.0且γ_i大于100，其中ρ_i越大表示采样点在表面上的分布越平坦，γ_i越大表示分布与线性几何形状的接近程度越低。一个surfel由体素内点p的平均位置和法线e_d表示，其中e_d是对应于λ_1的特征向量。通过合并分布，可以将多个小尺度的surfel合并成一个大尺度的surfel，如图1(b)所示。
3. 系统描述
LOG-LIO的流程如图2所示。对于新的输入扫描，我们首先估计点云的法线。然后根据它们的局部几何信息，对未畸变的点云和地图进行关联。我们结合IMU的测量值，并通过iEKF优化车体的姿态。优化完成后，新的点将被添加到由扩展ikd树管理的地图中，并且体素内的分布将被逐步维护。
我们的系统以IMU帧作为车体坐标系，系统状态x可以表示为：
其中^WR^T_I表示相对于^Wp^T_I和^Wv^T_I，它们分别代表IMU在世界坐标系（即第一个IMU参考系）中的方向、位置和速度。b^T_ω和b^T_a分别表示陀螺仪和加速度计的偏置，^Wg^T是已知的世界坐标系中的重力向量。
3.1 数据预处理
LOG-LIO使用环形FALS（第IV-A节）来估计每个输入点的法向量，表示为n_r。随后，使用体素网格下采样和基于IMU测量的反向传播来进行点减少和畸变校正。
3.2 数据关联(重点内容)
在数据关联的开始阶段，将上一帧的IMU测量值进行积分，以预测姿态\hat{x}_k。利用这个预测，将每个新的输入点^Lp_i转换到世界坐标系中，即^Wp_i = ^W\hat{T}_I {^IT_L} {^Lp_i}。然后，数据关联分为三个连续的步骤：
3.2.1 初始对应
对于查询点^Wp_i，我们首先搜索与其k个最近的地图点对应的k个最近的地图体素。
3.2.2 可见性和一致性检查
如果地图点的法向量与射线（从查询扫描点到LiDAR中心的向量）之间的夹角大于90度，则候选关联地图点可能对LiDAR不可见。这种情况通常发生在室内，其中一个物体（例如墙壁）的两个平面彼此靠近，这被称为双面问题[20]。这种错误的对应关系直接被消除。通过计算查询点的法向量与关联地图点的法向量之间的平均夹角来评估关联地图点的一致性。如果平均夹角大于阈值α（本文中α = 60°），则认为它是不一致的关联并丢弃它。
3.2.3 分层关联
对于满足可见性和一致性检查的查询点，进行分层关联，其中点到面的关联优先于点到平面的关联，大尺度的surfels优先于小尺度的surfels。
Surfel是对局部表面的一种更精确和灵活的表示，而不是通过稀疏地图点拟合的平面，因为surfels是通过点的分布建模的，这不仅意味着局部表面的位置，还意味着其形状。大尺度的surfels可以通过合并多个小尺度的分布来近似表示（图1(b)），并且对噪声不敏感，因此提供比小尺度的surfels更稳健的约束。然而，如果关联的地图点不在大尺度的surfels内（即任何地图点到surfels的距离大于阈值），我们认为大尺度的surfels质量较低。此时，我们采用与小尺度的surfels进行关联。
对于与小尺度的surfels相关的约束，我们将查询点与其所在的体素的surfel进行关联，该体素必须已经固定。如果该体素无法满足surfel的标准（第2.4节），我们将采用点到平面的关联。
3.3 姿态优化
我们采用FAST-LIO2中的iEKF来优化姿态。预测步骤通过将最新优化的状态\tilde{x}_{k−1}与协方差矩阵P_{k−1}进行融合来实现。
对于残差计算，给定世界坐标系中的点^Wp_i，残差z_i被计算为：
其中，n_j是与p_i相关的surfel或平面的归一化法向量，^Wq_j是位于相关元素上的点。
然后，我们用\hat{x}_k和\hat{P}_k分别表示传播的状态和协方差。它们代表了状态的先验高斯分布。通过将先验分布和从方程（12）中的点到surfel和点到平面的关联的测量模型结合起来，我们得到了最大后验估计（MAP），如下所示：
在时间k，其中\tilde{x}^κ_k是第κ次迭代更新的误差，H^κ_i和H^κ_j是关于\tilde{x}^κ_k的雅可比矩阵，R_i和Q_j来自原始测量噪声。卡尔曼增益可以高效地计算，计算负载取决于状态维度而不是测量维度[13]，[14]。
3.4 地图管理
LOG-LIO使用扩展的ikd树和体素来管理地图，并将每个体素分配给相应的树节点。
为了解决当新的点添加到体素中时重新计算点分布所带来的计算负担，我们根据第IV-B节的描述逐步维护每个体素内的点分布。
评论（0）
您还未登录，请登录后发表或查看评论