Complementary Perception for Handheld SLAM论文笔记
1 摘要
我们提出了一种方法，用于对一般的三维环境建图（其中三维环境并非处处都能满足足够的几何特征或视觉信息，并且手持型的设备的运动也是不受约束的。）连续时间的SLAM算法整合了雷达，相机，惯性测量单元到一个补充设备中，所有的传感器都对最后的优化结果进行约束。提出的算法被设计用于扩展可构图环境，增强生成的手持地图的可靠性。所提出的算法的关键组成部分是将深度不确定性结合到视觉特征中，这对于噪声平滑是有效的，并且允许具有和不具有深度估计的特征以统一的方式建模。表现结果：建图区域更广。
2 引言
多数LSTM的算法前提是：运动能够通过相邻时刻内精准的特征匹配被估计到。然而，感知数据不满足精准的计算匹配的条件。例如，基于雷达的SLAM 难以在平地环境或长走廊环境下进行实现，视觉SLAM 难以在缺乏特征的环境下，如办公楼，或光照差的区域下进行实现。
较差的环境条件生成了假阳性的特征匹配结果，导致了较差的运动估计。如图1所示，雷达传感器无法在当前环境下进行较好的特征匹配，导致了树在多个位置下都出现了。
针对该问题, 我们开发了一种基于雷达、相机和IMU的SLAM系统. 通过对比实验, 证明了所提出方法在复杂环境下的鲁棒性.
- 同时利用三种传感器形式获得的测量结果进行连续时间下的轨道估计；
- 通过深层的不确定性进行广义的视觉特征匹配。
- 提出了一种新的用于估计特征深度和深度不确定性的方法
- 结果好
4 系统背景
方法建立于Bosse et al. in [1], [5].提出的连续时间的雷达和IMU SLAM算法。这个非实时的算法通过雷达的旋转来提供3D区域覆盖。算法的目的是通过最小化下面的误差项来拟合满足激光和IMU测量结果的轨道。
e_{rvel}^{I}代表估计角速度与IMU测得角速度\omega_{meas}之间的差值。其中\omega_{bias}被定义为当前估计的向量偏移；而矩阵B则用于将IMU坐标系转换为世界坐标系。误差项e_{acc}^{I}代表估计加速度a(x)与IMU测得加速度a_{meas}之间的差异。
其中g代表重力加速度常数,a_{bias}代表当前估计值中的加速度偏差项,其采样频率为100Hz.
误差e^L(x)评估了雷达测量特征在不同时刻下的偏差程度.这些特征都是基于同一表面元在不同扫描时刻捕获的数据.系统采用sweep_matching进程来进行特征匹配操作.
表面元被建模为3D世界中基于当前轨道信息进行多尺度体素化的结果,并在每个体素内计算出均值s及其对应的协方差矩阵Ω
协方差矩阵中特征值最小的那个特征向量决定了一个平均表面的标准\hat n；该协方差矩阵的特征值表征了该表面片椭球体的形状。最平滑的那个表面片被选中，在80厘米范围内匹配其他相邻的表面片。两个被匹配的面团a和b用于计算下面所述的误差公式：
其中\hat{n}_{a,b}是a与b所对应的协方差矩阵之和的最小特征向量。为了使a与b更加接近而进行了误差最小化操作。
轨道估计x在初始化阶段基于IMU测量值进行确定。通过将节点分段线性地平移以及分段线性地旋转来进行误差的重复最小化过程。
接着，在每一步迭代中都会针对当前最新的轨迹估计进行线性化处理以求得最优解。
在多观测框架下利用洛伦兹函数计算残差后确定的数据误差权重\omega_{ab}能够反映不同观测之间的差异。
利用上述方法构建了一个基于改进卡尔曼滤波器的数据融合模型进而实现了高精度的位置状态估算。
系数r是用来平滑离散值的系数，遵循退火方案，其中阈值随着每次迭代而减小。对于IMU误差，权重是固定的，并根据IMU的准确度进行选择。
优化器状态由0.1秒间隔的校正节点组成，用于5秒的测量窗口。 然后窗口前进一秒并且优化新轨迹，使得它与先前窗口共享初始姿势。 继续local registration进程，直到处理完所有测量。 然后执行全局regestration阶段，其在整个测量集合上应用相同的算法，但具有更大的时间间隔10s以保持计算成本可管理。 第二阶段负责全局回环检测。
如在[1]，[5]中，我们还使用上述误差项来求解激光器和IMU之间的空间和时间偏移，并估计IMU偏差中的任何漂移。 为简洁起见，我们省略了这些细节，因为它们的作用很小，并且不需要他们的描述来理解我们的贡献。 最后，离线校准IMU到激光变换，并且在每个数据收集开始时使用静止时段来确定IMU偏差的初始估计。
方法
我们的耦合方法将视觉角度的测量结果整合到上述的目标中。之后轨道的估算结果依赖于 在用于移动的环境中雷达特征和视觉特征的匹配误差。
我们用雷达数据来估计视觉特征的深度，用以消除单目相机中的尺度漂移问题。
简而言之，我们的方法扩展了[1]中的方案，通过：从图像序列中提取2D特征，利用雷达数据来估计每个特征的深度，找到视觉特征的匹配，将匹配约束添加到目标中。
- A 提取视觉特征
本研究中的视觉特征提取方法要求所选取的特能够具有高度的唯一性，并能在连续滚动的5秒窗口中实现精确匹配。其计算效率需达到理想水平以确保实时处理能力。通过实验验证可知：Abramovitz算法及其描述函数均表现优异。
每帧检测到的视觉特征的数量对于维持平衡视觉模态置信度与其他传感器测量数据的有效性而言具有决定性作用；为了实现对平衡视觉模态置信度和其他传感器测量数据的有效监控与管理，在每帧中我们调节并设定一个合适的特征数量k；通过设定一个动态可变的阈值λ_min（即最小允许测度），我们可以有效地排除那些在当前条件下表现不足的数据指标。
- B估计特征的深度
为了将视觉特征与目标关联起来，在我们的算法中识别这一挑战是尤为重要的部分。
由于激光雷达与视觉模态的本质不同，在对复杂地形如树木和灌木等几何形状建模时，激光雷达的插值效果并不理想。
特别地，在对复杂地形如树木和灌木等几何形状建模时, 激光雷达的插值效果并不理想.
为此, 我们通过将光线投射至最新更新的世界地形图多尺度视图来推断深度信息. 这一过程利用了表面元的方向性法线特性, 从而能够有效忽略后方遮挡物并精确重构三维场景边界. 此外, 该光线投射过程在图3中进行了可视化展示.
我们利用如下公式估算视觉特征的深度d_a：
其中相机当前位姿为t,对应表面元的位置由s_a表示,并且η为投射光线的单位向量.通过协方差矩阵计算误差值,并对这些误差值进行优化.那些与射线源距离过远的候选表面元将被滤除（满足条件l_a(d_a) > l_{max}）;同时,那些时间间隔过长的表面元也会被剔除（满足条件|τ - v_a| > Δ_{τ_{\text{max}}}）。我们将排除所有背向表面上存在的元素,并对剩余的部分按照以下公式进行排序:
其中ψ_a代表对应表面元的宽度。
我们估算深度的不确定性。
它被均值为d、标准差为 σ = ||η||_Ω 的高斯分布所近似（其中被选中的表面元在投射光η下的协方差矩阵中的元素）。
只有当候选表面元的所有视觉特征满足l_a(d_a) > l_{max}时才被保留下来。
这些视觉特征超出雷达范围，则设置σ = ∞而仍然提供二维到二维的映射约束。
- C 匹配视觉特性
保留一组视觉特性用于求取对应的一组特性对。 选定两个特定的特性描述符 a 和 b 来确定这些对应关系，并确保 a 和 b 之间的 Hausdorff 距离不超过 1，并且每个 a 都与其对应的 b 是 FLANN 算法[23]中的近 邻。 为了确保所选特 征的时间间隔超过 Δτmin（如 表 I 所示），这种匹配方法仅考虑时间上 独立的关系。
注：改动说明
回顾一下我们的深度估计与其光线方向相关的高斯不确定性。通过在横向添加微小的不确定性,我们可以将二维特征点反向投影至世界坐标系中的三维点pi,并采用如下协方差矩阵来建模其位置上的不确定性
其中σ_i σ_0是当前光线方向η_i和它的横向方向σ_0的值如表2，
上述公式表示了三种距离的统一模型
我们提出的方法中计算成本最高的步骤是光线-网格元素相交测试。 自然地，在五秒钟滚动窗口内的图像特征和n_s表面, 该成本是每秒记录时间的O(n_fn_s)。 通过在找到特征对应之后进行深度计算, 则可将复杂度降低到对于n_m匹配对的O(n_m n_s)。 标准空间加速技术（例如移动体素网格）可以进一步降低这种复杂性, 但是每个实例的成本相当于30个标量乘法以拒绝l_{max}横向距离之外的所有表面, 并且$n_m n_s≈2,000,000是一个典型值, 因此未加优化的方法"足够"具有运行时间不敏感的特点
用于衡量优化轨道与全局标定轨道之间的覆盖差异程度的指标为：该指标数值越小越好；根据测量结果（如表3所示），我们可以评估其性能。
