We investigate uncertainty quantification of 6D pose estimation from keypoint measurements. Assuming unknown-but-bounded measurement noises, a pose uncertainty set (PURSE) is a subset of SE(3) that contains all possible 6D poses compatible with the measurements. Despite being simple to formulate and its ability to embed uncertainty, the PURSE is difficult to manipulate and interpret due to the many abstract nonconvex polynomial constraints. An appealing simplification of PURSE is to find its minimum enclosing geodesic ball (MEGB), i.e., a point pose estimation with minimum worst-case error bound. We contribute (i) a dynamical system perspective, and (ii) a fast algorithm to inner approximate the MEGB. Particularly, we show the PURSE corresponds to the feasible set of a constrained dynamical system, and this perspective allows us to design an algorithm to densely sample the boundary of the PURSE through strategic random walks. We then use the miniball algorithm to compute the MEGB of PURSE samples, leading to an inner approximation. Our algorithm is named CLOSURE (enClosing baLl frOm purSe boUndaRy samplEs) and it enables computing a certificate of approximation tightness by calculating the relative size ratio between the inner approximation and the outer approximation. Running on a single RTX 3090 GPU, CLOSURE achieves the relative ratio of 92.8% on the LM-O object pose estimation dataset and 91.4% on the 3DMatch point cloud registration dataset with the average runtime less than 0.2 second. Obtaining comparable worst-case error bound but 398x and 833x faster than the outer approximation GRCC, CLOSURE enables uncertainty quantification of 6D pose estimation to be implemented in real-time robot perception applications.

翻译：我们研究基于关键点测量的6D姿态估计的不确定性量化。在假设测量噪声未知但有界的情况下，姿态不确定性集合（PURSE）是SE(3)的子集，包含所有与测量兼容的6D姿态。尽管PURSE易于构建且能嵌入不确定性，但由于包含众多抽象的非凸多项式约束，其操控与解释十分困难。PURSE的一个有效简化方法是寻找其最小包围测地球（MEGB），即具有最小最坏情况误差界的点姿态估计。我们的贡献包括：(i) 一种动态系统视角，以及(ii) 一种快速逼近MEGB内部的算法。特别地，我们证明PURSE对应于约束动态系统的可行集，这一视角使能够设计通过策略性随机行走密集采样PURSE边界的算法。随后利用最小包围球算法计算PURSE样本的MEGB，从而得到内部逼近。我们的算法命名为CLOSURE（通过采样PURSE边界求最小包围球），该算法通过计算内部逼近与外部逼近的相对尺寸比，实现逼近紧致性证明。在单张RTX 3090 GPU上，CLOSURE在LM-O物体姿态估计数据集达到92.8%的相对比率，在3DMatch点云配准数据集达到91.4%，平均运行时间低于0.2秒。在获得可比的最坏情况误差界的同时，CLOSURE的计算速度比外部逼近算法GRCC快398倍至833倍，从而使6D姿态估计的不确定性量化能够在实时机器人感知应用中实现。