Provably Guaranteed Polytopic Uncertainty Quantification for SLAM

01 Motivation

对于自动驾驶、无人机导航这类安全攸关（safety-critical）场景，SLAM 只给出位姿和地图的点估计是不够的：下游的 planning / control 需要显式知道感知误差的边界才能做出安全决策。已有的 UQ 方案要么没有形式化保证，要么依赖过强的建模假设，要么只处理位姿估计而非完整的 SLAM 流水线。For safety-critical scenarios such as autonomous driving and UAV navigation, it is not enough for SLAM to output only point estimates of the pose and the map: downstream planning / control needs to know the bounds of the perception error explicitly in order to make safe decisions. Existing UQ approaches either offer no formal guarantee, rely on overly restrictive modeling assumptions, or address only pose estimation rather than a complete SLAM pipeline.

"Many existing works either offer no formal containment guarantee, rely on restrictive modeling assumptions, or focus only on pose estimation rather than a complete SLAM pipeline."

概率式 UQ（Probabilistic UQ）通常假设测量噪声服从高斯分布，用逆 Fisher 信息矩阵（FIM）或 Hessian 在最优解处近似协方差；论文指出这种做法有三点局限：(i) 测量不确定性往往偏离高斯、在动态场景中还会变化；(ii) 用 FIM/Hessian 本身就再次假设了高斯，容易低估真实不确定性；(iii) 由此得到的置信区间不提供确定性的（deterministic）包含保证。Probabilistic UQ usually assumes Gaussian measurement noise and approximates the covariance by the inverse Fisher information matrix (FIM) or the Hessian at the optimum; the paper points out three limitations of this practice: (i) measurement uncertainty often deviates from Gaussian and may vary in dynamic scenes; (ii) using the FIM/Hessian itself assumes Gaussianity once again and tends to underestimate the true uncertainty; (iii) the confidence intervals so obtained provide no deterministic containment guarantee.

02 Method

整个框架建立在三个基本 UQ 原语（primitives）之上：forward UQ（给定点与位姿的不确定性集合，推出刚体变换后点的不确定性集合，用于建图）、backward UQ（给定点对应关系的不确定性集合，反推位姿的不确定性集合，用于位姿跟踪）、以及 pose compound（把相对位姿的不确定性与已有全局位姿的不确定性复合成新的全局位姿不确定性）。每个模块都产出一个经过认证（certified）的多面体，只要输入边界是确定性的，输出边界就继承确定性保证。The whole framework rests on three fundamental UQ primitives: forward UQ (given the uncertainty sets of a point and of a pose, derive the uncertainty set of the point after the rigid-body transformation; used for mapping), backward UQ (given the uncertainty sets of point correspondences, invert them into the uncertainty set of the pose; used for pose tracking), and pose compound (compose the uncertainty of a relative pose with that of an existing global pose into a new global pose uncertainty). Every module outputs a certified polytope: as long as the input bounds are deterministic, the output bounds inherit the deterministic guarantee.

三模块 + Conformal PredictionThree primitives + Conformal Prediction

局部点云的不确定性由 conformal prediction (CP) 从标定数据中以指定的目标漏覆盖率 δ（实验中取 δ = 0.01）校准得到，再转成多面体 𝒫(A, b) = {p | Ap ≤ b}。定位（localization）可以走 relative framework（逐帧估计相对位姿，再与历史全局位姿做 pose compound）或 global framework（直接把当前局部点云与全局地图匹配，做全局位姿跟踪）两条路线；建图（mapping）用 forward UQ 把新观测的局部点转换到全局坐标系；当检测到 loop closure 时执行 smoothing，进一步收紧轨迹和地图的不确定性集合。The uncertainty of the local point cloud is calibrated by conformal prediction (CP) from calibration data at a prescribed target miscoverage rate δ (δ = 0.01 in the experiments), and then turned into a polytope 𝒫(A, b) = {p | Ap ≤ b}. Localization can take either the relative framework (estimate the relative pose frame by frame, then pose compound it with the historical global pose) or the global framework (match the current local point cloud directly against the global map for global pose tracking); mapping uses forward UQ to transform newly observed local points into the global frame; when a loop closure is detected, smoothing is performed to further tighten the uncertainty sets of the trajectory and the map.

Pose compound 的两种算法Two algorithms for pose compound

论文给出 direct 和 indirect 两种 pose compound 计算方法。数值仿真表明：indirect compound 算法下的位姿不确定性比 direct 算法增长得更快——这与后续 backward UQ 实验中的观察一致。The paper gives two ways of computing the pose compound, direct and indirect. Numerical simulation shows that the pose uncertainty under the indirect compound algorithm grows faster than under the direct one — consistent with the observation in the subsequent backward UQ experiments.

03 Experiments

实验分三部分：(1) 基本 UQ 模块的仿真验证（forward / backward / pose compound）；(2) 数值 SLAM 仿真（50m×50m×50m 工作空间，圆形轨迹）；(3) Replica 数据集上的真实 SLAM 实验。backward UQ 部分与位姿 UQ 基线 CLOSURE（inner approximation）和 GRCC（outer approximation）做对比。The experiments come in three parts: (1) simulation validation of the fundamental UQ primitives (forward / backward / pose compound); (2) numerical SLAM simulation (50m×50m×50m workspace, circular trajectory); (3) real SLAM experiments on the Replica dataset. The backward UQ part is compared with the pose UQ baselines CLOSURE (inner approximation) and GRCC (outer approximation).

Table I：backward UQ 的 conservatism test——从原始集合 𝒯 中采样 1000 个位姿样本，统计落在各方法估计集合内的比例。CLOSURE 只是内近似（inner approximation），不保证包含，包含率在不同 trial 间波动很大；GRCC 与本文方法都是 100% 包含，验证了两者的 guaranteed 性质。Table I: conservatism test of backward UQ — 1000 pose samples are drawn from the original set 𝒯 and the fraction falling inside the set estimated by each method is counted. CLOSURE is merely an inner approximation, does not guarantee containment, and its containment rate fluctuates widely across trials; both GRCC and our method reach 100% containment, confirming the guaranteed nature of the two.

Ablation：只注册新观测地标 vs 每帧重新注册所有可见地标Ablation: registering only newly observed landmarks vs re-registering all visible landmarks every frame

为验证上述解释，论文做了一组消融：把 global framework 改成每帧重新注册所有可见地标（而非只注册新观测到的地标）。"The ablation results show that the estimated uncertainty increases significantly, especially when the trajectory returns to the starting point, which verifies our claim."To verify the above explanation, the paper runs an ablation: the global framework is changed so that all visible landmarks are re-registered at every frame (instead of only the newly observed ones). “The ablation results show that the estimated uncertainty increases significantly, especially when the trajectory returns to the starting point, which verifies our claim.”

与概率基线的对比Comparison with the probabilistic baseline

论文还实现了一个线性-高斯设定下计算精确后验的概率基线（不含 SE(3) 约束），用 99% 置信水平的协方差椭球表示不确定性。在 global framework、约 15 个点/帧的中等观测密度下，到第 20 帧时该基线未能覆盖 9/20 的位姿估计和 26/120 的地图点，而本文方法实现了 full containment。在 relative framework、约 5 个点/帧时，到第 30 帧，概率基线平移椭球体积为 3.12，本文多面体体积为 1.2，"showing about 3× slower growth"。The paper also implements a probabilistic baseline that computes the exact posterior under a linear-Gaussian setting (without the SE(3) constraint), representing uncertainty by covariance ellipsoids at the 99% confidence level. Under the global framework with a moderate observation density of about 15 points per frame, by frame 20 this baseline fails to cover 9/20 of the pose estimates and 26/120 of the map points, while our method achieves full containment. Under the relative framework with about 5 points per frame, by frame 30 the translational ellipsoid volume of the probabilistic baseline is 3.12 and the volume of our polytope is 1.2, “showing about 3× slower growth”.

计算开销Computational cost

MATLAB 实现，32GB RAM + Intel Core Ultra 7 265 CPU，平均每帧 19 个点时：forward UQ 9.3s、backward UQ 0.0036s、direct pose compound 12s、indirect pose compound 0.5s、loop-closure smoothing 每帧 29.55s（20 帧共 591s）。瓶颈在于 forward UQ 和 direct pose compound 反复求解的 SDP 优化。MATLAB implementation, 32GB RAM + Intel Core Ultra 7 265 CPU, with an average of 19 points per frame: forward UQ 9.3s, backward UQ 0.0036s, direct pose compound 12s, indirect pose compound 0.5s, loop-closure smoothing 29.55s per frame (591s for 20 frames in total). The bottleneck lies in the SDP optimizations repeatedly solved by forward UQ and direct pose compound.

04 Limitations