Abstract: We investigate uncertainty quantification of 6D pose estimation from learned noisy measurements (e.g., keypoints and pose hypotheses). 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 defining it. An appealing simplification of PURSE–motivated by the bounded state estimation error assumption in robust control– 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 geometric interpretation of the nonconvex PURSE, 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 or the intersection of multiple geodesic balls, and this perspective allows us to design an algorithm to densely sample the boundary of the PURSE through strategic random walks that are efficiently parallelizable on a GPU. We then use the miniball algorithm by Gärtner (1999) to compute the MEGB of PURSE samples, leading to an inner approximation of the true MEGB. Our algorithm is named CLOSURE (enClosing baLl frOm purSe boUndaRy samplEs) and it enables computing a certificate of approximation tightness by calculating the relative ratio between the size of the inner approximation and the size of the outer approximation GRCC from Tang, Lasserre, and Yang (2023). Running on a single RTX 3090 GPU, CLOSURE achieves the relative ratio of 92.8% on the LM-O object pose estimation dataset, 91.4% on the 3DMatch point cloud registration dataset and 96.6% on the LM object pose estimation dataset with an average runtime below 0.3 seconds. Obtaining comparable worst-case error bound but 398×, 833× and 23.6× faster than the outer approximation GRCC, CLOSURE enables uncertainty quantification of 6D pose estimation to be implemented in real-time robot perception applications.
