itemizestditemize

#  CLOSURE: Fast Quantification of Pose Uncertainty Sets

Author Names Omitted for Anonymous Review. Paper-ID [192]  Yihuai Gao12, Yukai
Tang13, Han Qi4 and Heng Yang4  2Stanford University<EMAIL_ADDRESS>3Tsinghua University<EMAIL_ADDRESS>4Harvard University,
<EMAIL_ADDRESS>
###### Abstract

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 $\mathrm{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 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 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 and $91.4\%$ on the 3DMatch point
cloud registration dataset with the average runtime $<0.2$ second. Obtaining
comparable worst-case error bound but $398\times$ and $833\times$ faster than
the outer approximation GRCC, CLOSURE enables uncertainty quantification of 6D
pose estimation to be implemented in real-time robot perception applications.

††1 equal contribution.

##  I Introduction

6D pose estimation (_i.e.,_ a 3D rotation and a 3D translation) from images
and point clouds is a longstanding problem in robotics and vision and finds
extensive applications in localization and mapping [44], robotic manipulation
[21], virtual and augmented reality [49], and autonomous driving [43].

A popular paradigm for pose estimation is to first detect salient _keypoints_
in the sensor data –often done using deep neural networks [18, 54, 39, 38]–
and then leverage the _maximum likelihood estimation_ (MLE) framework to
estimate the optimal pose that best fits the keypoint measurements. Formally,
let $x=(R,t)\in\mathrm{SO}(3)\times{{\mathbb{R}}^{3}}:=\mathrm{SE}(3)$ be the
unknown pose to be estimated, one first extracts $N$ keypoints $y_{i}\in{\cal
Y},i=1,\dots,N$ from the raw sensor data and assume

$\displaystyle g(x,y_{i})=\epsilon_{i},\quad i=1,\dots,N,$ (1)

where $g:\mathrm{SE}(3)\times{\cal Y}\rightarrow{{\mathbb{R}}^{m}}$ is a known
residual function that measures the discrepancy between keypoint $y_{i}$ and
pose $x$. When $y_{i}$ is a perfect (_i.e.,_ noise-free) measurement, $g$
evaluates to zero; when $y_{i}$ is noisy, $\epsilon_{i}\in{{\mathbb{R}}^{m}}$
describes the measurement noise in neural network predictions. We give two
instantiations of (1) that will be the focus of this paper.

######  Example 1 (Object Pose Estimation [52, 29]).

Let $y_{i}=(z_{i},Z_{i})\in{{\mathbb{R}}^{2}}\times{{\mathbb{R}}^{3}}$ be a
pair of matched 2D image keypoint and 3D object keypoint. The function

$\displaystyle g(x,y_{i})=z_{i}-\Pi(RZ_{i}+t)$ (2)

describes the _reprojection error_ of the 3D keypoint $Z_{i}$, where
$\Pi(\cdot)$ is the camera projection
function.111$\Pi:{{\mathbb{R}}^{3}}\rightarrow{{\mathbb{R}}^{2}}$,
$\Pi(v)=[v_{1}/v_{3},v_{2}/v_{3}]^{\mathsf{T}}$.

######  Example 2 (Point Cloud Registration [53, 18]).

Let $y_{i}=(a_{i},b_{i})\in{{\mathbb{R}}^{3}}\times{{\mathbb{R}}^{3}}$ be a
pair of matched 3D keypoints in the source and target point clouds,
respectively. The function

$\displaystyle g(x,y_{i})=b_{i}-(Ra_{i}+t)$ (3)

describes the _Euclidean distance_ between the keypoints.

Given the detected keypoints $\\{y_{i}\\}_{i=1}^{N}$, one then formulates

$\displaystyle\min_{x\in\mathrm{SE}(3)}\sum_{i=1}^{N}\rho(g(x,z_{i})),$ (4)

whose solution provides an estimate of the unknown pose $x$. For example, when
the noise $\epsilon_{i}$ is assumed to follow a standard Gaussian
distribution, then choosing $\rho(g(x,z_{i}))=\|g(x,z_{i})\|^{2}$ makes the
solution of (4) to be the maximum likelihood estimator. In practice, there are
often keypoint measurements that do not follow (1) and lead to very large
$\epsilon_{i}$, commonly known as an _outlier_. To regain robustness to
outliers, $\rho$ in (4) is modified to be a _robust loss_ [27, 9] and the
optimization (4) is referred to as _M-estimation_ (_i.e.,_ MLE-like
estimation).

Uncertainty Quantification. Despite the many algorithmic advances in solving
(4) [13, 3, 15], _e.g.,_ with certifiable global optimality guarantees [51],
we argue there are two fundamental issues with the M-estimation framework.
First, the starting assumption that $\epsilon_{i}$ follows a Gaussian-like
distribution (up to the removal of outliers) is not justified. For example,
[46] shows noises generated by neural network keypoint detections in Example 1
fail almost all statistical multivariate normality tests [30], regardless of
whether outliers are removed. We reinforce this empirical observation in
Supplementary Material and provide similar results showing real noises in
keypoint matches of Example 2 also deviate far from a Gaussian distribution.
Second, the solution of (4) provides a _single point estimate_ that usually
comes with no _uncertainty quantification_ , which is crucial when the pose
(state) estimations need to be used for downstream decision-making [20, 16].
Though it is possible to use the inverse of Fisher Information at the optimal
solution to approximate the uncertainty, such approximation (i) again builds
on the assumption that $\epsilon_{i}$ is Gaussian, and (ii) is known to
underestimate the true uncertainty [45]. Recent works [40, 53, 14] provided
uncertainty estimation for a few robot perception problems. However, the
uncertainty either depends on uncheckable assumptions and cannot be computed
[40, 53], or build on machinery that only applies to estimators based on
expensive semidefinite relaxations [51].

Set-Membership Estimation with Noise Calibration. An alternative (albeit less
popular) estimation framework that this paper advocates for the purpose of
practical and rigorous uncertainty quantification is that of _set-membership
estimation_ (SME), widely known in control theory for system identification
[36, 35, 48]. Instead of placing distributional assumptions on the measurement
noise $\epsilon_{i}$ in (1), SME assumes the noise is _unknown but bounded_ ,
_i.e.,_

$\displaystyle\sqrt{\epsilon_{i}^{\mathsf{T}}\Lambda_{i}\epsilon_{i}}=:\|\epsilon_{i}\|_{\Lambda_{i}}\leq\beta_{i},\
\ i=1,\dots,N$ (5)

for positive definite matrices $\Lambda_{i}\in\mathbb{S}^{d}_{++}$ and
positive noise bounds $\beta_{i}>0$. With this assumption, and invoking (1), a
_Pose UnceRtainty SEt_ (PURSE) can be formulated as [52]

$S=\left\\{x\in\mathrm{SE}(3)\mid\|g(x,y_{i})\|_{\Lambda_{i}}\leq\beta_{i},i=1,\dots,N\right\\},$
(PURSE)

_i.e.,_ the set of all possible poses that are compatible with the
measurements $y_{i}$ and the assumption (5). The careful reader may ask “ _why
should we trust the unknown-but-bounded noise assumption ( 5) in SME more than
the Gaussian-like noise assumption in MLE_?” The answer is that recent
statistical tools for distribution-free uncertainty calibration [41], _e.g.,_
_conformal prediction_ [2], enables rigorous estimation of $\Lambda_{i}$ and
$\beta_{i}$ from a calibration dataset. For instance, [52] demonstrated how to
calibrate noises generated by a pretrained neural network for Example 1, and
in Supplementary Material we show how to calibrate neural network detection
noises for Example 2 using conformal prediction. In fact within robotics, the
SME framework has recently also gained popularity in simultaneous localization
and mapping (SLAM) for guaranteed error analysis [22, 37]. However, the SLAM
literature used _interval analysis_ to bound the estimation error of 2D poses,
while in this paper we focus on uncertainty quantification of 3D rotations and
translations.

Computational Challenges. Although SME provides a natural description of
uncertainty and its unknown-but-bounded noise assumption can be made practical
by modern uncertainty calibration tools, it does bring computational
challenges because the (PURSE) is an abstract subset of $\mathrm{SE}(3)$
defined by many nonconvex constraints (more precisely, polynomial inequalities
[52]). This makes PURSE difficult to be manipulated, _e.g.,_ draw samples,
visualize and interpret, extract a point estimate, compute volume, to name a
few. Therefore, it is desirable to simplify the (PURSE). Motivated by the
common practice in robust perception-based control [20, 28, 19] that assumes
the state estimation has bounded error, it is appropriate to simplify PURSE as
a point estimate together with a _worst-case error bound_ , _i.e.,_ an
_enclosing geodesic ball_ 222Since $\mathrm{SE}(3)$ is a Riemannian manifold,
we use the term “geodesic”. centered at the point estimate.333Given an
enclosing geodesic ball, one can draw samples from PURSE and estimate its
volume by performing rejection sampling. Since it is desired to find an
enclosing geodesic ball with _minimum conservatism_ , we wish to compute the
_minimum enclosing geodesic ball_ (MEGB), _i.e.,_ the smallest geodesic ball
that encloses the original PURSE (to be formulated precisely in Section II).
Towards this goal, [52] proposed an algorithm to compute the worst-case error
bound for any point estimate, and [46] showed how to compute a hierarchy of
enclosing geodesic balls (with decreasing sizes) that asymptotically converge
to the MEGB. There are, however, two shortcomings in [52, 46]. First, both
works provide _outer_ approximations of the MEGB and it is unclear how
conservative they are (_e.g.,_ how much larger are these approximations
compared to the true MEGB). Second, they rely on semidefinite relaxations for
the outer approximation, which are too expensive to be practical for real-time
robotics applications (see Section V).

Contributions. We contribute an algorithm that quantifies the uncertainty of
PURSE in real time for both Examples 1-2. The key perspective is that,
_algebraically_ , (PURSE) is defined by unstructured nonconvex constraints,
but _geometrically_ , (PURSE) corresponds to precisely the feasible sets of
certain constrained dynamical systems. This observation leads to a natural
algorithm: we first generate random samples in (PURSE) as initial states of
the dynamical system, and then perturb the dynamical system properly until it
reaches the _boundary_ of the feasible set. Effectively, this algorithm aims
to densely sample the boundary of PURSE by _strategic random walks_ , which
lends itself to fast parallization. Using the samples on the PURSE boundary,
we run the fast _miniball_ algorithm by Gärtner [24] to return a geodesic ball
enclosing the samples. We name our algorithm _enClosing baLl frOm purSe
boUndaRy samplEs_ (CLOSURE). This geodesic ball in turn becomes an _inner_
approximation of the MEGB. By comparing the _relative ratio_ between the size
of the inner approximation computed by CLOSURE and the size of the outer
approximation computed by [46], we obtain a _certificate of tightness_ for the
approximation. We test CLOSURE on the LM-O dataset [11] for Example 1 and the
3DMatch dataset [56] for Example 2 and demonstrate that (i) with an average
runtime of $0.1879$ second, CLOSURE computes inner approximations with average
relative ratio $92.8\%$ for Example 1 and (ii) with an average runtime of
$0.1774$ second, CLOSURE computes inner approximations with average relative
ratio $91.4\%$ for Example 2; both using a single NVIDIA RTX 3090 GPU. This
means CLOSURE is $398\times$ and $833\times$ faster than the outer
approximation algorithm in [46], respectively, making it feasible in real-time
robot perception systems.

Paper Organization. We formalize the notion of MEGB and discuss high-level
outer and inner approximation strategies in Section II. We then focus on the
inner approximation: in Section III we show how PURSE corresponds to the
feasible sets of constrained dynamical systems and in Section IV we detail the
CLOSURE algorithm. We provide experimental results in Section V and conclude
in Section VI.

##  II Minimum Enclosing Geodesic Ball

Figure 1: MEGB $B(C^{\star},D^{\star})$ of a simple rectangle $S$ and MEGB
$B(\hat{C},\hat{D})$ of the subset $\hat{S}\subset S$. Note that
$\hat{D}<D^{\star}$ but $B(\hat{C},\hat{D})\not\subset
B(C^{\star},D^{\star})$.

Given the uncertainty set $S$ defined in (PURSE), denote
$S_{{R}}\subseteq\mathrm{SO}(3)$, $S_{{t}}\subseteq{{\mathbb{R}}^{3}}$ as the
projection of $S$ onto $\mathrm{SO}(3)$ and ${{\mathbb{R}}^{3}}$,
respectively. Consider the optimization problems

$\min_{C\in\mathrm{SO}(3)}\max_{R\in S_{{R}}}\
\mathrm{dist}_{\mathrm{SO}(3)}(C,R)$ (6)

and

$\min_{c\in{{\mathbb{R}}^{3}}}\max_{t\in S_{{t}}}\
\mathrm{dist}_{{{\mathbb{R}}^{3}}}(c,t)$ (7)

where
$\mathrm{dist}_{\mathrm{SO}(3)}(C,R):=\arccos((\mathrm{tr}\left(C^{\mathsf{T}}R\right)-1)/2)$
and $\mathrm{dist}_{{{\mathbb{R}}^{3}}}(c,t):=\|c-t\|$ denote the geodesic
distance metrics in $\mathrm{SO}(3)$ and ${{\mathbb{R}}^{3}}$, respectively.
Let $C^{\star}$ (resp. $D^{\star}$) and $c^{\star}$ (resp. $d^{\star}$) be the
optimizer (resp. optimum) of (6) and (7), then the minimum enclosing geodesic
ball (MEGB) of $S_{{R}}$ is

$\displaystyle
B_{\mathrm{SO}(3)}(C^{\star},D^{\star})=\left\\{R\in\mathrm{SO}(3)\\!\mid\\!\mathrm{dist}_{\mathrm{SO}(3)}(R,C^{\star})\\!\leq\\!D^{\star}\right\\},\\!\\!$
(8)

and the MEGB of $S_{{t}}$ is

$\displaystyle
B_{{{\mathbb{R}}^{3}}}(c^{\star},d^{\star})=\left\\{t\in{{\mathbb{R}}^{3}}\mid\mathrm{dist}_{{{\mathbb{R}}^{3}}}(t,c^{\star})\leq
d^{\star}\right\\}.$ (9)

By the minimax nature of problems (6)-(7), we have

$S_{{R}}\subseteq B_{\mathrm{SO}(3)}(C^{\star},D^{\star}),\quad
S_{{t}}\subseteq B_{{{\mathbb{R}}^{3}}}(c^{\star},d^{\star}),$

_i.e.,_ the geodesic balls enclose the original uncertainty sets. See Fig. 1
for an illustration of the MEGB for a 2D rectangle. The center
$(C^{\star},c^{\star})\in\mathrm{SE}(3)$ is the best point estimate achieving
_minimum_ worst-case error bounds of $(D^{\star},d^{\star})$. We choose to
seperate the MEGB for $\mathrm{SO}(3)$ and ${{\mathbb{R}}^{3}}$ because their
distance metrics have different units, _i.e.,_ degrees and meters. A classical
result in Riemannian geometry [1] states the MEGB exists and is unique under
mild conditions. However, solving the minimax optimizations (6)(7) is known to
be intractable [4]. We review several algorithmic choices for solving (6)(7).

Geodesic Gradient Descent. A general recipe leveraging the _geodesic
convexity_ of $f(C):=\max_{R\in S_{{R}}}\mathrm{dist}_{\mathrm{SO}(3)}(C,R)$
in $C$ can be constructed as follows: start with an initial rotation
$C^{(0)}$; then at each iteration (a) find the rotation $R$ in $S_{{R}}$ that
attains the maximum distance to the current iterate $C^{(k)}$, and (b) move
along the geodesic from $C^{(k)}$ to $R$ by a portion of $\gamma\in(0,1)$,
which is equivalent to performing (sub-)gradient descent on $f(C)$. Due to
geodesic convexity of $f(C)$, this algorithm is guaranteed to converge with
well-known complexity analysis [57]. However, computing the maximum distance
from $S_{{R}}$ to $C^{(k)}$ in step (a) boils down to solving a nonconvex
optimization and is in general not implementable. In Supplementary Material we
attempt this algorithm with step (a) implemented by the semidefinite
relaxation proposed in [52] and show that it does converge but is impractical
due to the excessive runtime (near one hour).

Since exactly solving (6)(7) using gradient descent is impractical, we turn to
approximation algorithms.

Outer Approximation. [46] proposed an algorithm that produces a hierarchy of
enclosing geodesic balls that asymptotically converge to the MEGB from above.
This algorithm, however, has two drawbacks. First, the convergence is not
detectable and therefore it is unclear how conservative is the computed
enclosing geodesic ball compared to the true MEGB. Second, this algorithm is
also built upon semidefinite relaxations and it is too slow to be practical
(see Section V).

We focus on inner approximations in this paper.

Inner Approximation. If in addition to the outer approximation provided by
[46], one can produce an _inner_ approximation of the MEGB, then by comparing
the relative ratio between the sizes of the inner and outer approximations, a
certficate of approximation tightness can be obtained. The next result
provides a straightforward way to compute inner approximations.

######  Proposition 3 (Inner Approximation of MEGB).

Let $\hat{S}_{{R}}\subseteq S_{{R}}$ and $\hat{S}_{{t}}\subseteq S_{{t}}$ be
nonempty subsets. Consider the optimizations

$\displaystyle\min_{C\in\mathrm{SO}(3)}\max_{R\in\hat{S}_{{R}}}\
\mathrm{dist}_{\mathrm{SO}(3)}(C,R)$ (10)

and

$\displaystyle\min_{t\in{{\mathbb{R}}^{3}}}\max_{t\in\hat{S}_{{t}}}\
\mathrm{dist}_{{{\mathbb{R}}^{3}}}(c,t),$ (11)

and denote their optimizers (resp. optima) to be $\hat{C}$ (resp. $\hat{D}$)
and $\hat{c}$ (resp. $\hat{d}$). Then $B_{\mathrm{SO}(3)}(\hat{C},\hat{D})$ is
no greater than the MEGB (8) and $B_{{{\mathbb{R}}^{3}}}(\hat{c},\hat{d})$ is
no greater than the MEGB (9), _i.e.,_

$\displaystyle\hat{D}\leq D^{\star},\quad\hat{d}\leq d^{\star}.$ (12)

###### Proof.

In (6) and (10), for any $C\in\mathrm{SO}(3)$ denote

$\displaystyle f(C):=$ $\displaystyle\max_{R\in
S_{{R}}}\mathrm{dist}_{\mathrm{SO}(3)}(C,R),$ $\displaystyle\hat{f}(C):=$
$\displaystyle\max_{R\in\hat{S}_{{R}}}\mathrm{dist}_{\mathrm{SO}(3)}(C,R).$

Since $\hat{S}_{{R}}\subseteq S_{{R}}$, we have

$f(C)\geq\hat{f}(C),\quad\forall C\in\mathrm{SO}(3).$

Therefore,

$\min_{C\in\mathrm{SO}(3)}f(C)=D^{\star}\geq\hat{D}=\min_{C\in\mathrm{SO}(3)}\hat{f}(C).$

The proof for $\hat{c}\leq c^{\star}$ is similar. ∎

Proposition 3 states that, given any nonempty subsets of the original PURSE
$S_{{R}}$ and $S_{{t}}$, solving the minimax problems (10)-(11) leads to valid
inner approximations. We remark that here the word “inner” is abused in the
sense that we can only guarantee the MEGB of $\hat{S}_{{R}}$ (resp.
$\hat{S}_{{t}}$) is _smaller_ than, but _not enclosed by_ , the MEGB of the
original PURSE $S_{{R}}$ (resp. $S_{{t}}$). Fig. 1 gives such an
counterexample. Nevertheless, we chose to use “inner” approximation to
parallel the “outer” approximation in previous works. To make Proposition 3
useful, we need to strategically choose the subsets $\hat{S}_{{R}}$ and
$\hat{S}_{{t}}$ such that (i) the minimax problems are easier to solve, and
(ii) the inner approximations are as tight (large) as possible. Requirement
(i) is easy to satisfy if we choose the subsets as _discrete samples_ ,
_i.e.,_ we approximate the original PURSE as a point cloud of poses. In this
case, the minimax problems can be solved exactly and efficiently [24], which
we will further explain in Section IV. To satisfy requirement (ii), we
leverage the intuition that the MEGB must touch the PURSE at a finite number
of _contact points_ [50, 34, 26] that lie on the boundary of the PURSE
$\partial S_{{R}}$ and $\partial S_{{t}}$. Therefore, if we can densely sample
$\partial S_{{R}}$ and $\partial S_{{t}}$, then there is high probability that
the contact points will be included and the inner approximation will be close
to the true MEGB. For example, the MEGB of the rectangle in Fig. 1 is exactly
the same as the MEGB of the discrete point cloud containing four corners of
the rectangle. Therefore, finding good samples on the boundary of the PURSE is
crucial to compute tight inner approximations of the MEGB.

However, the (PURSE) is defined by abstract constraints and there exists no
general algorithms that can sample its boundary, _i.e.,_ points at which some
of the inequalities become equalities. In the next section, we will show that,
despite being algebraically unstructured, the PURSE has simple geometrical
structures that can be exploited to sample its boundary.

##  III A Dynamical System Perspective

(a) Constrained dynamical system for (PURSE2D3D).
---
(b) Constrained dynamical system for (PURSE3D3D).
Figure 2: Constrained dynamical systems whose feasible sets correspond to (a)
(PURSE2D3D) for Example 1 and (b) (PURSE3D3D) for Example 2.

Plugging the $g$ functions in (2) and (3) to the definition of (PURSE), we
explicitly write down the PURSE for Example 1

$S=\left\\{(R,t)\mid\|z_{i}-\Pi(RZ_{i}+t)\|_{\Lambda_{i}}\leq\beta_{i},\forall
i\right\\},$ (PURSE2D3D)

and for Example 2

$S=\left\\{(R,t)\mid\|b_{i}-Ra_{i}-t\|_{\Lambda_{i}}\leq\beta_{i},\forall
i\right\\}.$ (PURSE3D3D)

Although the constraints are complicated in $(R,t)$, they actually define
quite simple geometric sets [55].

PURSE2D3D. Each constraint in (PURSE2D3D) asks $\Pi(RZ_{i}+t)$ –the
reprojection of $Z_{i}$– to lie inside an ellipse on the image plane. Fig.
2(a) depicts the geometric constraints: the line from the camera center to
each 3D keypoint, _i.e.,_ the bearing vector, needs to pass through the
ellipse of bounded measurement noise. This is precisely a constrained
mechanical system where there is (i) a prismatic joint between each 3D
keypoint and each bearing vector, and (ii) a spherical joint between each
bearing vector and the camera center. The $N$ 3D keypoints form a rigid body
(as shown by the connected links in Fig. 2(a)) that can move in 3D space
subject to the bearing vectors not passing the 2D ellipses.

PURSE3D3D. Each constraint in (PURSE3D3D) enforces $Ra_{i}+b_{i}$ –the rigid
transformation of $a_{i}$– to lie inside an ellipsoid centered at $b_{i}$.
Fig. 2(b) depicts the geometric constraints: each of the ellipsoid forms a
shell fixed in 3D space, and the rigid body formed by 3D points
$\\{a_{i}\\}_{i=1}^{N}$ (again showned by the connected links) can freely move
in 3D space subject to not escaping the ellipsoidal shells.

This geometric perspective shows if one can simulate the constrained dynamical
systems in Fig. 2, _e.g.,_ under random disturbances, then everytime the
dynamical system “hits” the boundary of the ellipses or shells, we obtain a
pose sample on the boundary of PURSE. However, to the best of our knowledge,
there does not exist an off-the-self simulator for constrained dynamical
systems as the dynamics of these systems are described by _differential
algebraic equations_ that are much harder to solve than ordinary differential
equations [32, 12]. Nevertheless, in the next section we show we can customize
a simulator that basically simulates the rigid bodies as if there are no
constraints until constraint violation happens.

##  IV The CLOSURE Algorithm

The CLOSURE algorithm contains three steps, which is overviewed in Algorithm
1.

1 Input:  a pose uncertainty set (PURSE) $S$; RANSAG trial number $N_{{\sf
RANSAG}}$; simulation parameters $\mathcal{P}$;

2 Output:  miniball center pose $s^{*}\in\mathrm{SE}(3)$ ; radius of rotation
miniball $\hat{D}$; radius of translation miniball $\hat{d}$

3

4$S_{0}\leftarrow$ RANSAG ($S$, $N_{{\sf RANSAG}}$);

5 $\hat{\partial S_{R}}\leftarrow$ sample_rotation_boundary($S_{0}$, $S$,
$\mathcal{P}$);

6 $\hat{\partial S_{t}}\leftarrow$ sample_translation_boundary($S_{0}$, $S$,
$\mathcal{P}$);

7 $R^{*},\hat{D}\leftarrow$ minimum_enclosing_geodesic_ball($\hat{\partial
S_{R}}$);

8 $t^{*},\hat{d}\leftarrow$ minimum_enclosing_ball($\hat{\partial S_{t}}$);

9 return: $s^{*}\leftarrow(R^{*},t^{*})$, $\hat{D}$, $\hat{d}$

10

Algorithm 1 CLOSURE Overview

1 Input:  initial poses $S_{0}\subset\mathrm{SE}(3)$; PURSE $S$; base angular
velocity magnitude $\omega_{0}$; time step decay factor $\gamma$; translation
perturbation scale $t_{p}$; random walk trial number $N_{W}$; iteration number
$N_{I}$; perturbation number $N_{P}$; optimal perturbation number $N_{P}^{*}$;
time step scaling number $N_{T}$;

2 Output:  sampled boundary poses $\hat{\partial S_{R}}\subset\mathrm{SO}(3)$
as an inner approximation of $\partial S_{R}$;

3

4$\bar{R}\leftarrow\text{proj}_{\mathrm{SO}(3)}(\sum_{(R_{j},*)\in
S_{0}}R_{j})$;

5 $\bar{t}\leftarrow\frac{1}{|S_{0}|}\sum_{(*,t_{j})\in S_{0}}t_{j}$;

6 $\hat{\partial S_{R}}\leftarrow\emptyset$;

7 for  _ $(R_{0},t_{0})\in S_{0}$_ do

8 for  _ $n\leftarrow 1$ to $N_{W}$_ do

9 $\omega\leftarrow$ init_angular_velocity$(R_{0},\bar{R},\omega_{0})$;

10 $R^{*}\leftarrow R_{0},t^{*}\leftarrow t_{0}$;

11 % Iterate $N_{I}$ times so that the evolved pose gets close to $\partial
S_{R}$

12 for  _ $i\leftarrow 1$ to $N_{I}$_ do

13 % Randomize $N_{P}$ translation perturbations for  _$j\leftarrow 1$ to
$N_{P}$_ do

14 $t_{j}\leftarrow t^{*}+$ perturbation($t_{p}$);

15 $d_{j}\leftarrow\text{dist}(R^{*},t_{j},\partial S)$;

16

17 end for

18 % Pick out $N_{P}^{*}$ perturbations that drag the pose away from $\partial
S$

19 $\\{j_{k}\\}_{k=1}^{N_{P}^{*}}\leftarrow$
top_k_indices($\\{d_{j}\\}_{j=1}^{N_{P}}$, $N_{P}^{*}$);

20 for  _$k\leftarrow 1$ to $N_{p}^{*}$_ do

21 for  _$m\leftarrow 1$ to $N_{T}$_ do

22 $\Delta T\leftarrow\gamma^{m-1}$

23 $R_{km}\leftarrow$ update_rotation($R^{*}$, $\omega$, $\Delta T$);

24 $I_{km}\leftarrow$ in_purse($R_{km},t_{j_{k}}$);

25

26 end for

27

28 end for

29 % Find the optimal pose that is still in $S$ and has the maximum rotation
movement

30 $m_{0}\leftarrow\min\left\\{m\mid\exists k\text{\ s.t.\
}I_{km}=1\right\\}$;

31 $k_{0}\leftarrow\left\\{k\mid I_{km_{0}}=1\right\\}$;

32 $R^{*}\leftarrow R_{k_{0}m_{0}}$;

33 $t^{*}\leftarrow t_{k_{0}m_{0}}$;

34

35 end for

36

37 $\hat{\partial S_{R}}\leftarrow\hat{\partial S_{R}}\cup R^{*}$;

38

39 end for

40

41 end for

42return: $\hat{\partial S_{R}}$

43

Algorithm 2 $\partial S_{R}$ sampler

1 Input:  initial poses $S_{0}\subset\mathrm{SE}(3)$; PURSE $S$; base angular
velocity magnitude $\omega_{0}$; time step decay factor $\gamma$; translation
perturbation scale $t_{p}$; random walk trial number $N_{W}$; iteration number
$N_{I}$; perturbation number $N_{P}$; optimal perturbation number $N_{P}^{*}$;
time step scaling number $N_{T}$;

2 Output:  sampled boundary poses $\hat{\partial S_{R}}\subset\mathrm{SO}(3)$
as an inner approximation of $\partial S_{R}$;

3 $\omega\leftarrow$ init_angular_velocity($S_{0},\omega_{0},N_{W}$);

4 $v\leftarrow$ init_center_velocity($S_{0},v_{0},N_{W}$);

5 $(R^{*},t^{*})\leftarrow$ repeat($S_{0},N_{W}$);

6 $\Delta T\leftarrow(1,\beta,\beta^{2},\cdots,\beta^{N_{T}-1})$;

7

8for  _ $i\leftarrow 1$ to $N_{I}$_ do

9 $R\leftarrow$ repeat($R^{*},N_{P}$);

10 $t\leftarrow$ repeat($t^{*},N_{P}$) +
perturbation($t_{p},|S_{0}|N_{W}N_{P}$);

11 $d\leftarrow$ dist($R,t,\partial S$);

12 $j\leftarrow$ top_k_indices($d,N_{P}^{*}$);

13 $\tilde{t}\leftarrow$ repeat($t_{[j]},N_{T}$);

14 $\tilde{R}\leftarrow$
update_rotation(repeat($R^{*},N_{P}^{*}$)$,\omega,\Delta T$);

15 $I\leftarrow$ in_purse($\tilde{R},\tilde{t}$);

16 $R^{*},t^{*}\leftarrow$ find_farthest_rotation($\tilde{R},\tilde{t},I$);

17

18 end for

19return: $\hat{\partial S_{R}}\leftarrow R^{*}$

Algorithm 3 $\partial S_{R}$ parallel sampler

Step I: Initialize Starting Poses. Given a (PURSE) $S$, we first sample a set
of initial poses $S_{0}$ from $S$ using an algorithm proposed in [52] called
_random sample averaging_ (RANSAG). The basic idea of RANSAG is to leverage
the well-established _minimal solvers_ [31] to quickly generate candidate
poses and check if they belong to PURSE.

  * •

Example 1: In each RANSAG trial, we randomly select 3 constraints (_i.e.,_ 3
ellipses in Fig. 2(a)) and find a 2D point in each constraint. We then apply
the perspective-3-point (P3P) minimal solver [23] to obtain a candidate pose
$s$. We add $s$ into the set $S_{0}$ if $s$ satisfies (PURSE2D3D).

  * •

Example 2: In each RANSAG trial, we randomly select 3 pairs of corresponding
points, and apply Arun’s Method [6] to compute a candidate pose $s$. We put
$s$ into $S_{0}$ if $s$ satisfies (PURSE3D3D).

Step II: Strategic Random Walk. In this step, we sample poses that are close
to the boundary of the PURSE, $\partial S$, starting from the initial samples
$S_{0}\subset S$ by Step I. The basic idea is to add random perturbations to
every pose in $S_{0}$ until they hit the boundary of the PURSE and violate the
defining constraints. We engineer several techniques to make this idea more
efficient.

  * •

Walk away from the center. We first find the average rotation $\bar{R}$ and
average translation $\bar{t}$ of $S_{0}$ (line 7-7). For each pose
$(R_{0},t_{0})$ in $S_{0}$, we initialize $N_{W}$ walks with randomized
angular or center velocities while pointing outwards $\bar{R}$ and $\bar{t}$,
which explicitly encourages the samples to move away from the center and
explore the boundary. To be exact, when sampling the rotation boundary
$\partial S_{R}$, suppose $(u,\theta)$ is the axis-angle representation of the
relative rotation between $R_{0}$ and $\bar{R}$, we initialize the angular
velocity $\omega$ as $\omega=\omega_{0}(u+\tilde{u})$, where $\omega_{0}$ is
angular velocity magnitude and $\tilde{u}\in\mathbb{R}^{3}$ is a random unit
vector. Similarly, when sampling the translation boundary $\partial S_{t}$, we
initialize the center velocity $v$ as
$v=v_{0}\left((t_{0}-\bar{t})/\|t_{0}-\bar{t}\|+\tilde{v}\right)$, where
$\tilde{v}\in\mathbb{R}^{3}$ is a random unit vector. However, in Example 1,
$S_{t}$ in the camera projection direction appears significantly longer than
the other two directions as shown in Fig. 3(a). To balance the geometrical
singularity, we apply additional normalization to the translation velocity
according to the PCA analysis of $S_{0}$.

  * •

$\bm{\partial S\neq\partial S_{{R}}\times\partial S_{{t}}}$. A vanilla walking
strategy is to perform $N_{I}$ iterations of rigid body movement with a
variety of perturbations and step sizes (reflected in the time step lengths)
so that the evolved poses can get close enough to $\partial S$. However,
recall that our starting goal is to sample $\partial S_{{R}}$ and $\partial
S_{{t}}$, not $\partial S$. One may think that $\partial S=\partial
S_{R}\times\partial S_{t}$, but this is in general _not true_. In fact,
$\partial S_{R}=\partial(S|_{\mathrm{SO}}(3))\subset(\partial
S)|_{\mathrm{SO}}(3)$ and $\partial
S_{t}=\partial(S|_{\mathbb{R}^{3}})\subset(\partial S)|_{\mathbb{R}^{3}}$. In
words, the boundary of $S_{{R}}$ (resp. $S_{{t}}$) is a subset of the boundary
of $S$ projected onto $\mathrm{SO}(3)$ (resp. ${{\mathbb{R}}^{3}}$). This
means even if $(R,t)\in\partial S$, it is not guaranteed that $R\in\partial
S_{R}$ and $t\in\partial S_{t}$. Please refer to Supplementary Material for an
intuitive example. With this observation in mind, we sample $\partial S_{R}$
and $\partial S_{t}$ separately. The $\partial S_{R}$ sampler is shown in
Algorithm 2, and the $\partial S_{t}$ sampler is similar and presented in
Supplementary Material for brevity.

When sampling $\partial S_{R}$ (resp. $\partial S_{t}$), we fix the angular
velocity $\omega$ (resp. center velocity $v$) and add random perturbations to
center translation (resp. rotation). In each iteration step, we first find top
$N_{p}^{*}$ perturbations out of $N_{p}$ that _drags the pose away from
$\partial S$_, _i.e.,_ maximizing the distance to $\partial S$. The distance
between a pose $(R,t)\in\mathrm{SE}(3)$ and $\partial S$ is defined
heuristically as

$\text{dist}(R,t,\partial
S)=\min_{i}\left(\beta_{i}-\|g_{i}((R,t),y_{i})\|_{\Lambda_{i}}\right),$ (13)

where $g_{i}$ is the estimation error defined in (2) or (3). Then we apply
different step scales $\gamma^{m-1},m=1,\cdots,N_{T}$ to the pose after each
perturbation. Among all the $N_{p}^{*}N_{T}$ poses, we find the one that is
still in $S$ and has the maximum rotation (resp. translation) movement to
update the optimal pose $(R^{*},t^{*})$. Finally, after $N_{I}$ iterations, we
add the optimal pose to $\hat{\partial S_{R}}$. In this way, we can sample
$|S_{0}|N_{W}$ poses that are close enough to $\partial S_{R}$ (resp.
$\partial S_{t}$) from different approaching directions. Algorithm 2
summarizes the strategic random walk that samples $\partial S_{{R}}$.

  * •

Parallelization. To improve the sampling speed, we implement Algorithm 2 with
parallel programming using CuPy,444https://cupy.dev/, a python interface for
NVIDIA CUDA library. presented in Algorithm 3. We notice that $(R^{*},t^{*})$
is the only variable that is updated consecutively in each iteration, while
all the other loops are independent and can be executed in parallel.
Specifically, there are $|S_{0}|N_{W}$ walks in total with different initial
velocities. In each iteration, the $N_{P}$ perturbations can be applied to the
optimal poses in parallel, from which we can find $N_{P}^{*}$ optimal
perturbations that maximize the distance to $\partial S$ according to (13).
Next, we apply all $N_{T}$ time step scales in a roll and broadcast to
$N_{P}^{*}$ optimal perturbations for object movement simulation. At the end
of each iteration, we update the optimal poses $(R^{*},t^{*})$ with the
farthest movements. With in-depth analysis of the sampling algorithm, the
steps with heaviest computation are 8 and 8. In these steps,
$|S_{0}|N_{W}N_{P}$ and $|S_{0}|N_{W}N_{P}^{*}N_{T}$ poses555In a typical
case, $|S_{0}|\approx 100,N_{W}=2,N_{P}=150,N_{P}^{*}=10,N_{T}=15$, thus
$|S_{0}|N_{W}N_{P}=|S_{0}|N_{W}N_{P}^{*}N_{T}\approx 3\times 10^{4}$. are
checked, while checking each pose requires to calculate (PURSE2D3D) or
(PURSE3D3D). Executing these steps in parallel significantly reduces the time
cost compared with CPU based implementation.

Step III: Miniball. In this step, after acquiring a set of rotations
$\hat{\partial S_{R}}$ and translations $\hat{\partial S_{t}}$ close to
$\partial S_{R}$ and $\partial S_{t}$ respectively, we calculate the MEGB of
$\hat{\partial S_{R}}$ and $\hat{\partial S_{t}}$. The _miniball_
algorithm666https://people.inf.ethz.ch/gaertner/subdir/software/miniball.html
introduced in [24] provides a fast implementation that can exactly compute the
MEGB of a point cloud in Euclidean space. This means we can directly apply it
to solve (11) and compute $B_{{{\mathbb{R}}^{3}}}(\hat{c},\hat{d})$ because
$\hat{\partial S_{t}}\subset{{\mathbb{R}}^{3}}$ lives in an Euclidean space.
However, we cannot directly apply miniball to $\hat{\partial S_{R}}$ because
it lives in $\mathrm{SO}(3)$. Fortunately, [46, Proposition A12, A13] points
out, under mild conditions, computing the MEGB of a set on $\mathrm{SO}(3)$ is
equivalent to computing the MEGB of the corresponding set on the space of unit
quaternions, which in turn is equivalent to simply treating the unit
quaternions as points in ${{\mathbb{R}}^{4}}$. Therefore, we can apply the
same algorithm after embedding the rotations in ${{\mathbb{R}}^{4}}$ and solve
(10) to compute $B_{\mathrm{SO}(3)}(\hat{C},\hat{D})$. Thanks to the
efficiency of the miniball algorithm, the runtime of this step is negligible
compared to RANSAG and the strategic random walk.

In summary, the CLOSURE algorithm takes the PURSE as input, and outputs
$B_{\mathrm{SO}(3)}(\hat{C},\hat{D})$ and
$B_{{{\mathbb{R}}^{3}}}(\hat{c},\hat{d})$, serving as inner approximations of
the true MEGB. By taking advantage of efficient GPU parallel computing,
CLOSURE can be executed around 0.2 seconds with satisfying performance and is
suitable for real-time applications.

##  V Experiments

We test CLOSURE on two real datasets, the LM-O dataset [11] for object pose
estimation (Example 1) and the 3DMatch dataset [56] for point cloud
registration (Example 2). Since CLOSURE takes in the (PURSE) description, we
briefly describe how we process the datasets to obtain PURSE descriptions.

  * •

LM-O dataset. The LM-O test dataset contains 1214 images each capturing 8
different objects on a table and the goal here to estimate the 6D pose of each
object while quantifying uncertainty. We leverage the pretrained semantic
keypoint detector [38] to detect 2D semantic keypoints $\\{z_{i}\\}_{i=1}^{N}$
of each object that are matched to the manually labeled 3D keypoints
$\\{Z_{i}\\}_{i=1}^{N}$ ($N$ is around 10). To calibrate the uncertainty of
the 2D keypoint detections, we follow the conformal prediction approach in
[52], which produces descriptions of pose uncertainty in the form of
(PURSE2D3D).

  * •

3DMatch dataset. The 3DMatch test dataset includes 1623 pairs of point clouds
and the goal here is to estimate the 6D rigid transformation between each pair
of clouds while quantifying uncertainty. We leverage the pretrained DGR
network [18] that detects salient keypoint matches
$\\{a_{i},b_{i}\\}_{i=1}^{K}$ between each pair of clouds. To calibrate the
uncertainty of the keypoint matches, we design a similar conformal prediction
procedure as [52] with $400$ pairs in the calibration dataset. The details of
the conformal prediction design are presented in Supplementary Material. After
conformal prediction, pose uncertainty is given in the form of (PURSE3D3D)
with $N=50$.

###  V-A Effectiveness of the Boundary Sampler

A key idea in CLOSURE is that it tries to apply strategic random walks to
sample the boundary of the PURSE. Here we investigate how effective is this
boundary sampling strategy on two examples. To do so, we compare CLOSURE with
pure RANSAG, _i.e.,_ sampling inside PURSE without any motivation to sample
the boundary. We choose one example in the LM-O dataset and one example in
3DMatch dataset. We compare a more accurate parameter set of CLOSURE++ with
pure RANSAG for 100000 trials, both running around 1 second. Fig. 3 plots the
sampling results of the two algorithms. We use the stereographic projection to
visualize the rotation samples (_i.e.,_ we convert rotations to unit
quaternions and use stereographic projection to represent unit quaternions on
a 3D sphere). We can clearly see that the samples from CLOSURE++ are more
spread-out than those from RANSAG only. For this reason, the geodesic balls
enclosing the CLOSURE++ samples are larger than the geodesic balls enclosing
the RANSAG samples.

|
---|---
(a) An example from LM-O.
|
(b) An example from 3DMatch.
Figure 3: Effectiveness of PURSE boundary sampler on (a) an example from LM-O,
and (b) an example from 3DMatch. Left: rotation, Right: translation.

###  V-B Certificate of Approximation Tightness

We then perform a large-scale experiment of uncertainty quantification on all
the test samples of LM-O and 3DMatch (excluding the calibration samples),
where we compare the performance of CLOSURE with other baseline algorithms.
Our algorithm CLOSURE runs on a single CPU with a RTX 3090 GPU, while the
other baselines run on a workstations with 128 x AMD Ryzen Threadripper PRO
5995WX 64-Cores CPUs. Specifically, RANSAG and RANSAG+fmincon run with MATLAB
Parallel Computing Toolbox.

####  V-B1 LM-O

We implement two versions of CLOSURE

  * •

CLOSURE: the default CLOSURE algorithm with parameters $N_{{\sf
RANSAG}}=1500$, $\omega_{0}=1$, $v_{0}=2$, $\gamma=0.5$, $R_{p}=0.2$,
$t_{p}=0.1$, $N_{W}=2$, $N_{I}=5$, $N_{p}=150$, $N_{T}=15$.

  * •

CLOSURE++: the more accurate algorithm with parameters $N_{{\sf
RANSAG}}=1500$, $\omega_{0}=1$, $v_{0}=2$, $\gamma=0.5$, $R_{p}=0.2$,
$t_{p}=0.1$, $N_{W}=10$, $N_{I}=10$, $N_{p}=150$, $N_{T}=15$.

We implement the following baselines

  * •

RANSAG: pure RANSAG sampling with the miniball algorithm. We choose the number
of pure RANSAG trials so that its runtime is roughly one second on the
workstation with parallel workers.

  * •

RANSAG+fmincon: to encourage the RANSAG initial samples to walk to the
boundary of PURSE, we explicitly solve an optimization problem

$\displaystyle\text{Problem}(i)=\max_{(R,t)\in
S}\|z_{i}-\Pi(RZ_{i}+t)\|_{\Lambda_{i}}$ (14)

where $z_{i}$ is the detected 2D key point, $Z_{i}$ is the 3D key point, and
maximizing the objective explicitly pushes the rigid body system in Fig. 2(a)
to hit the boundary of the $i$-th ellipse. The algorithm proceeds as in
Algorithm 4, and we solve the optimization problems using Matlab function
fmincon with parallel workers.

  * •

GRCC: the algorithm from [46] that computes an outer approximation of the MEGB
of PURSE with relaxation order $\kappa=2$ for translation and $\kappa=3$ for
rotation. It provides an outer approximation of the MEGB (_i.e.,_ $\bar{D}$
and $\bar{d}$), which is used to calculate $\eta_{R}$ and $\eta_{t}$ for other
methods.

We investigate the runtime of these five algorithms, and more importantly,
study how tight the inner and outer approximations of the MEGB are.
Specifically, GRCC is the only algorithm that produces upper bounds on the
size of the true MEGB. Let $\bar{D}$ and $\bar{d}$ be the radii of the
enclosing geodesic balls produced by GRCC on $\mathrm{SO}(3)$ and
${{\mathbb{R}}^{3}}$, respectively. The other four algorithms all produce
lower bounds on the size of the true MEGB. Let $\hat{D}$ and $\hat{d}$ be the
radii of the geodesic balls produced by these inner approximation algorithms.
Define the _relative ratios_

$\displaystyle\eta_{{R}}:=\frac{\hat{D}}{\bar{D}}\leq
1,\quad\eta_{{t}}:=\frac{\hat{d}}{\bar{d}}\leq 1.$ (15)

Clearly, $\eta_{{R}}=1$ (resp. $\eta_{{t}}=1$) certifies that the true MEGB
has been found, and the closer $\eta_{{R}},\eta_{{t}}$ are w.r.t $1$ the
tighter the inner approximations are. Therefore, $\eta_{{R}},\eta_{{t}}$
produce certificates of approximation tightness.

1 Input:  Calibration dataset ${\cal D}$, a pose uncertainty set (PURSE) $S$;
RANSAG trial number $N_{{\sf RANSAG}}$; key point search index set ${\cal I}$;

2 Output:  sampled boundary poses $\hat{\partial S}\subset\mathrm{SE}(3)$ in
${\sf PURSE}$; center pose $s^{*}\in\mathrm{SE}(3)$; radius of minimum
enclosing geodesic ball $\hat{D}$; radius of minimum enclosing ball $\hat{d}$

3

4$S_{0}\leftarrow$ RANSAG ($S$, $N_{{\sf RANSAG}}$);

5 $\hat{\partial S}\leftarrow S_{0}$;

6 for  _i $\in{\cal I}$_ do

7 for  _$(R_{0},t_{0})\in S_{0}$_ do

8 $(R,t)\leftarrow$ fmincon(Problem($i$), init:$(R_{0},t_{0})$);

9 if  _$(R,t)\in S$_ then

10 $\hat{\partial S}\leftarrow\hat{\partial S}\cup\\{(R,t)\\}$;

11 end if

12

13 end for

14

15 end for

16$R^{*},\hat{D}\leftarrow$ minimum_enclosing_geodesic_ball($(\hat{\partial
S})|_{\mathrm{SO}(3)}$);

17 $t^{*},\hat{d}\leftarrow$ minimum_enclosing_ball($(\hat{\partial
S})|_{\mathbb{R}^{3}}$);

18 return: $\hat{\partial S}$, $s^{*}\leftarrow(R^{*},t^{*})$, $\hat{D}$,
$\hat{d}$

19

Algorithm 4 RANSAG+fmincon

Results. Table I summarizes the average runtime and the relative rotation and
translation ratios. We observe that (i) CLOSURE runs below $0.2$ seconds on
average and attains $\eta_{{R}}>0.92,\eta_{{t}}>0.97$, making it the method of
choice for real-time uncertainty quantification; (ii) with more runtime
budget, CLOSURE++ runs below 1 second but boosts the relative ratios to almost
$1$; (iii) RANSAG, without the strategic random walks to sample the boundary,
runs slower with worse relative ratios, again showing the value of the PURSE
boundary sampler. (iv) RANSAG+fmincon attains the best $\eta_{{R}}$ but the
worst $\eta_{{t}}$, showing that the translation uncertainty set is more
difficult to sample for an optimization-based approach; (v) the GRCC
algorithm, despite being the only algorithm that can produce outer
approximations, is too slow for real-time applications. We believe the results
in Table I show that CLOSURE and CLOSURE++ can be real-time alternatives of
the GRCC because the amount of underestimation of the uncertainty is very
minor.

TABLE I: Performance of 5 methods on LM-O (Example 1)  | avg runtime (seconds) | avg $\eta_{{R}}$ | avg $\eta_{{t}}$
---|---|---|---
CLOSURE | $\bm{0.1879}$ | $0.9280$ | $0.9781$
CLOSURE++ | $0.7563$ | $0.9660$ | $\bm{0.9918}$
RANSAG | $1.8368$ | $0.8916$ | $0.8791$
RANSAG+fmincon | $6.1095$ | $\bm{0.9726}$ | $0.8119$
GRCC | $74.9724$ | $\backslash$ | $\backslash$

####  V-B2 3DMatch

We implement two versions of CLOSURE

  * •

CLOSURE: $N_{{\sf RANSAG}}=1500$, $\omega_{0}=0.5$, $v_{0}=0$, $\gamma=0.5$,
$R_{p}=0$, $t_{p}=0.1$, $N_{W}=2$, $N_{I}=5$, $N_{p}=150$, $N_{T}=15$.

  * •

CLOSURE++: $N_{{\sf RANSAG}}=1500$, $\omega_{0}=0.5$, $v_{0}=0$, $\gamma=0.5$,
$R_{p}=0$, $t_{p}=0.1$, $N_{W}=20$, $N_{I}=10$, $N_{p}=150$, $N_{T}=15$

We implement the same RANSAG and GRCC baselines as in the LM-O case. We choose
to not compare with RANSAG+fmincon due to its excessive runtime. We then
investigate the runtime of these algorithms as well as the certifcates of
approximation tightness as defined in (15).

Results. Table II summarizes the average runtime and the relative rotation and
translation ratios. We observe that (i) CLOSURE runs below $0.2$ seconds on
average and attains $\eta_{{R}}>0.91,\eta_{{t}}>0.93$, which benefits real-
time uncertainty quantification. (ii) CLOSURE++ consumes more time but
achieves the most accurate relative ratios. (iii) RANSAG runs slow and has the
worst relative ratios. (iv) GRCC is too slow for real-time applications.

TABLE II: Performance of 4 methods on 3DMatch (Example 2)  | avg runtime (seconds) | avg $\eta_{{R}}$ | avg $\eta_{{t}}$
---|---|---|---
CLOSURE | $\bm{0.1774}$ | $0.9140$ | $0.9364$
CLOSURE++ | $1.3362$ | $\bm{0.9318}$ | $\bm{0.9563}$
RANSAG | $1.2820$ | $0.8480$ | $0.8689$
GRCC | $147.8408$ | $\backslash$ | $\backslash$

###  V-C Time Decomposition

We analyze the time decomposition of CLOSURE. Our algorithm consists of three
steps as described in Section IV: (1) RANSAG sampling, (2) strategic random
walk, (3) miniball. The result of the time decomposition in Example 1 and
Example 2 are shown in Table III. We see that for CLOSURE, the time of the
RANSAG sampling is comparable to that of strategic random walk, and the
miniball time is negligible.

TABLE III: Time decomposition of CLOSURE and CLOSURE++.  | RANSAG | Random walk | Miniball
---|---|---|---
Example 1 CLOSURE | $0.0900$ | $0.0915$ | $0.0064$
Example 1 CLOSURE++ | $0.0869$ | $0.6235$ | $0.0459$
Example 2 CLOSURE | $0.0883$ | $0.0838$ | $0.0053$
Example 2 CLOSURE++ | $0.0874$ | $1.1997$ | $0.0491$

###  V-D Ablation: Sensitivity to Parameters

We study how the parameters of CLOSURE impact its efficiency, _i.e.,_ how the
performance (in terms of relative ratios) and runtime changes with the input
parameter set. We present the results for sampling the rotation boundary
$\partial S_{R}$ of (PURSE3D3D) and refer the interested reader to
Supplementary Material for more results.

Specifically, though a large parameter set is available to tune, the
parameters that have the largest impact on the efficiency of CLOSURE are
iteration step number $N_{I}$ and number of the parallel workers
$N_{W},N_{T},N_{P},N_{P}^{*}$. We initialize the parameter set with
$N_{I}=5,N_{W}=20,N_{T}=15,N_{P}=150,N_{P}^{*}=10,\omega=0.5,\beta=0.5,t_{p}=0.1$.
When setting new values to each parameter, the rest of the parameters are kept
the same as the initial values. During the study, we fix the RANSAG output of
each experiment and compare the performance of different parameter values. The
results are shown in Fig. 4, which demonstrate that CLOSURE’s performance is
quite robust to parameter tuning. For all the parameter choices, CLOSURE
achieves relative ratio $\eta_{{R}}>90\%$ and its runstime is around 1 second.

---
Figure 4: The ratio-runtime curve of different parameters.
$N_{I}\in\\{2,5,10,20\\}$, $N_{W}\in\\{2,5,10,20,50\\}$,
$N_{P}\in\\{20,50,100,150,200\\}$, $N_{T}\in\\{5,10,15,20,25\\}$,
$N_{P}^{*}\in\\{2,5,10,15,20\\}$

It is clear that both runtime and $\eta_{R}$ grows as the all numbers
increase, especially when the numbers start from a small value. However, as
$N_{I}$ and $N_{W}$ grow higher, $\eta_{R}$ saturates around $0.93$. This
might be a result that the outer approximation $\bar{D}$ provided by GRCC is
not tight enough. Therefore, even if the sampler fully explores $\partial
S_{R}$, the relative ratio will not reach $1$. In the mean while, the
sensitivity is not significant as the $\eta_{R}$ stays above $0.9$ for a wide
range of parameter values.

###  V-E How Large is the Pose Uncertainty?

The previous results focused on showing the relative ratios that are
informative for the approximation performance of CLOSURE. In Fig. 5, we show
the cumulative distribution of absolute sizes of the inner approximations of
MEGB, just so the reader is aware of how much uncertainty is induced from the
calibrated noise bounds on the keypoint measurements generated by modern
neural networks. From the cumulative distribution we can see that the rotation
uncertainty is significant for both cases with a long-tail distribution. For
translation, uncertainty from the LM-O dataset is also large. This is because:
(i) compared with Example 2, Example 1 loses one dimension; (ii) The DGR
network is more recent thus may be better trained. These numbers above show
that there is a significant amount of pose uncertainty from robot perception.
We hope, now that CLOSURE enables fast quantification of the pose uncertainty,
future research can investigate ways to _reduce_ uncertainty in the pose
estimation and perceptual learning pipeline.

|
---|---
Figure 5: Absolute radius of the geodesic ball of rotation and translation, in
CLOSURE for Example 1 and Example 2. Left: rotation, Right: translation.

##  VI Conclusion

We introduced CLOSURE, a GPU-accelerated fast algorithm that can quantify pose
uncertainty from learned 2D-3D and 3D-3D keypoint matches in real time. The
key perspective that led to the design of CLOSURE is that the pose uncertainty
set (PURSE), despite being algebraically unstructured, corresponds to the
feasible set of a constrained dynamical system. The key algorithmic component
of CLOSURE is a strategy to generate random walks towards the boundary of the
PURSE, and the key enabler for the efficiency of CLOSURE is parallel computing
in GPUs. With two real-world datasets, we demonstrate the efficiency and
effectiveness of CLOSURE in producing tight uncertainty estimation of 6D
poses.

## Acknowledgments

We would like to thank William Zhang for drawing the 3D visualizations in Fig.
2.

###  -A Non-Gaussian Measurement Noise in Example 2

In this section, we provide numerical evidence that the noise in measurements
generated by either handcrafted features or learned features in point cloud
registration (Example 2) do not follow a Gaussian distribution.

Our strategy is to compute the noise vector $\epsilon_{i}$ from (1) as

$\epsilon_{i}=b_{i}-(Ra_{i}+t),i=1,\dots,N,$

where $y_{i}=(a_{i},b_{i})\in{{\mathbb{R}}^{3}}\times{{\mathbb{R}}^{3}}$ is a
pair of matched 3D keypoints in the source point cloud and the target point
cloud respectively, typically available through matching (handcrafted or
learned) features of the point clouds. We evaluate the noises on the popular
3DMatch dataset [56] using two feature matching algorithms: (i) a handcrafted
feature FPFH [42], and (ii) a learned feature FCGF [17].

This procedure produces a set of $N=1,431,186$ and $N=8,410,386$ pairs of
matching points $y_{i}=(a_{i},b_{i})$, respective for FPFH and FCGF. We want
to test if the noise $\left\\{\epsilon_{i}\right\\}_{i=1}^{N}$ are drawn from
a multivariate Gaussian (normal) distribution. To do so, we use the R package
MVN [30] that provides a suite of popular multivariate normality tests well
established in statistics. To consider potential outliers in the noise vectors
$\left\\{\epsilon_{i}\right\\}_{i=1}^{N}$, we run MVN on $\alpha\%$ of the
noise vectors with smallest norms, and we sweep $\alpha$ from 1 (_i.e.,_ keep
only the $1\%$ smallest noise vectors) up to 100 (_i.e.,_ keep all noise
vectors). Then we provide perspective plots and the Chi-square quantile-
quantile (Q-Q) plots of the noise vectors under different inlier thresholds.

Percentage Mardia Henze-Zirkler Royston Doornik-Hansen Energy $1\%$ NO NO NO
NO NO $5\%$ NO NO NO NO NO $10\%$ NO NO NO NO NO $20\%$ NO NO NO NO NO $40\%$
NO NO NO NO NO $100\%$ NO NO NO NO NO

TABLE A4: MVN test for FPFH method on $\alpha\%$ percentage of the noise
vectors with smallest norms. (MVN test for FCGF method has the same result.)

Table A4 shows all the MVN tests indicate that the noise residuals do not
follow a Gaussian distribution even after filtering potential outliers and no
matter whether the matching points are calculated with handcrafted or deep
learning based methods. Fig. A6 and Fig. A7 show the perspective plots (top)
and the Chi-square quantile-quantile (Q-Q) plots (bottom) of the empirical
density functions under different inlier thresholds, in comparison to that of
a Gaussian distribution. We can see that the empirical density functions
deviate far away from a Gaussian distribution, and are difficult to
characterize. This motivates the set membership estimation framework in
Section I.

Figure A6: Perspective plots (top) and Chi-square Q-Q plots (bottom) of the
noise vectors generated with handcreafed features (FPFH) from the 3DMatch
dataset. From left to right, they are respectively for noise residual range
5cm, 10cm, 20cm, 50cm, 100cm. The right-most graph shows the perspective plot
for a Gaussian distribution that can be used as comparison.

Figure A7: Perspective plots (top) and Chi-square Q-Q plots (bottom) of the
noise vectors generated with learned features (FCGF) from the 3DMatch dataset.
From left to right, they are respectively for noise residual range 5cm, 10cm,
20cm, 50cm, 100cm. The right-most graph shows the perspective plot for a
Gaussian distribution that can be used as comparison.

###  -B Uncertainty Calibration for Example 2

1 Input:  Calibration Point Cloud pairs with matched keypoints ${\cal
D}=\\{\\{a_{j},b_{j}\\}_{j=1}^{N_{i}}\\}_{i=1}^{N_{{\sf Calibration}}}$; error
rate $\epsilon$

2 Output:  Calibration threshold $\alpha$

3

4for  _$i\in\\{1,\cdots,N_{{\sf Calibration}}\\}$_ do

5 $N_{i}\leftarrow$ number of correspondences in the $i$-th point cloud;

6 $(R_{i},t_{i})\leftarrow$ ground truth pose of the $i$-th point cloud;

7 $\\{a_{j},b_{j}\\}_{j=1}^{N_{i}}\leftarrow$ correspondences of the $i$-th
pair of point clouds;

8 $\\{w_{j}\\}_{j=1}^{N_{i}}\leftarrow$ weights of the correspondences;

9
$\alpha_{i}\leftarrow\max_{j=1,...,N_{i}}(\|R_{i}a_{j}+t_{i}-b_{j}\|^{2}\times
w_{j})$;

10

11 end for

12Sort $\alpha_{\pi(1)}\geq\alpha_{\pi(2)}\geq\cdots\geq\alpha_{\pi(N_{{\sf
Calibration}})}$;

13 $\alpha\leftarrow\alpha_{\pi(N_{{\sf Calibration}}\times(1-\epsilon))}$;

14 return: $\alpha$

Algorithm 5 3DMatch conformal calibration

The sketch of the calibration process for the Example 2 is shown in Algorithm
5.

Basic Setup. Given a pair of point clouds, the DGR network [18] outputs a set
of matched keypoints $\\{(a_{j},b_{j})\\}$ together with weights $w_{j}$, with
$w_{j}$ indicating the confidence of the $j$-th match being valid. Thus, we
leverage the weights of the features output by the DGR network as the
confidence function in the calibration and test process. Also, we normalize
the 2-norm of the weights of each point clouds to keep weights across
different point clouds to have the same scale. To (1) reduce the calculation
burden of the PURSE set, (2) prune the outliers to reduce the uncertainty of
the PURSE set, we only select the top $50$ correspondences with the largest
weights for the calibration and the test process.

Calibration process. In the calibration process, we follow the inductive
conformal prediction procedure [2] and randomly choose 400 pairs of point
clouds for calibration from the 3DMatch dataset with ground truth poses, and
use the DGR network to output the features and the weights of the features.
The calibration process of the PURSE set in Example 2 is carried out as
follows: for the $i$-th point cloud, denote the ground truth pose as
$(R_{i},t_{i})$, the correspondences as $\\{a_{j},b_{j}\\}_{j=1}^{N_{i}}$ and
the weights as $\\{w_{j}\\}_{j=1}^{N_{i}}$. We then calculate
$\|R_{i}a_{j}+t_{i}-b_{j}\|^{2}\times w_{j}$. We denote
$\alpha_{i}=\max_{j=1,...,N_{i}}(\|Ra_{j}+t-b_{j}\|^{2}\times w_{j})$. Then we
sort all the $\alpha_{i}$ as
$\alpha_{\pi(1)}\geq\alpha_{\pi(2)}\geq\cdots\geq\alpha_{\pi(400)}$ and for a
given error rate $\epsilon=20\%$, we select $\alpha=\alpha_{\pi(400\times
80\%)}=\alpha_{\pi(320)}$.

Test process. Then in the test process, for the $i$-th point cloud, we use the
DGR network to output the features and the weights of the features. We denote
the correspondences as $\\{a_{j},b_{j}\\}_{j=1}^{N_{i}}$ and the weights as
$\\{w_{j}\\}_{j=1}^{N_{i}}$. We formulate the PURSE set as:

$\left\\{(R,t)|\|Ra_{j}+t-b_{j}\|^{2}\leq\frac{\alpha}{w_{j}},\
j=1,\cdots,N_{i}\right\\}$

###  -C Geodesic Gradient Descent

In this section, we focus on finding the MEGB on $\mathrm{SO}(3)$ for rotation
through geodesic gradient descent, as the geometry for ${{\mathbb{R}}^{3}}$ is
simpler and already studied in prior work [7]. We specify the algorithm
framework introduced in [5] to $\mathrm{SO}(3)$ and prove that it is equal to
geodesic gradient descent. We then provide convergence analysis and numerical
experiments to verify the results.

Inner maximization problem. We first focus on the inner maximization problem.
Let ${\cal S}_{R}\subset B(O,\rho)$ be a nonempty compact subset of
$\mathrm{SO}(3)$ contained in the open ball $B(O,\rho)$ at center $O$ with
radius $\rho\leq\frac{\pi}{2}$, consider the function

$\displaystyle f:B(O,\rho)\rightarrow$ $\displaystyle{{\mathbb{R}}}$
$\displaystyle c\mapsto$ $\displaystyle f(c)=\displaystyle\max_{s\in{\cal
S}_{R}}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s).$ (A16)

If $\rho\leq\frac{\pi}{2}$, we can guarantee the continuity and convexity of
$f$. Specifically, we have the following proposition:

######  Proposition A4 (Convexity and (Sub)gradient).

The following properties hold true for $f$:

  1. (i)

$f$ is Lipschitz continuous with $L=4\rho$, _i.e.,_
$\left|f(c_{1})-f(c_{2})\right|\leq
L\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},c_{2}),\forall c_{1},c_{2}\in
B(O,\rho)$;

  2. (ii)

$f$ is strictly convex when $\rho=\frac{\pi}{2}$;

  3. (iii)

$f$ is $\mu$-strongly convex when $\rho<\frac{\pi}{2}$, with
$\mu=2\rho\mathrm{cot}\left(\rho\right)$.

###### Proof.

Lipschitz continuity of $f$. Pick any two points $c_{1},c_{2}\in B(O,\rho)$,
since ${\cal S}_{R}$ is a nonempty compact set, the “$\max$” in (A16) is
attained, say at $s_{1}$ and $s_{2}$, respectively ($s_{1}$ and $s_{2}$ are
not necessarily unique). Consequently, we can write

$\displaystyle\left|f(c_{1})-f(c_{2})\right|$ $\displaystyle=$
$\displaystyle\left|\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{1},s_{1})-\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{2},s_{2})\right|$
$\displaystyle=$
$\displaystyle\left(\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})+\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})\right)\left|\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})\right|$
$\displaystyle\leq$ $\displaystyle
4\rho\left|\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})\right|$

where the last inequality holds because of the triangle inequality

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}(c_{i},s_{i})\leq\mathrm{dist}_{\mathrm{SO}(3)}(c_{i},O)+\mathrm{dist}_{\mathrm{SO}(3)}(O,s_{i})<2\rho,\quad
i=1,2.$

It remains to show
$\left|\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})\right|\leq\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},c_{2})$.
First, the inequality trivially holds when
$\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})=\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})$.
Second, when
$\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})>\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})$,
we have

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})>\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})\geq\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{1}),$

with the last inequality due to $s_{2}$ attains the maximum (squared) distance
to $c_{2}$. Hence,

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})$
$\displaystyle\leq\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{1})\leq\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},c_{2}),$

where the last inequality follows again from the triangle inequality. Third,
when
$\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})<\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})$,
we similarly have

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{1})$
$\displaystyle\leq\mathrm{dist}_{\mathrm{SO}(3)}(c_{2},s_{2})-\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},s_{2})\leq\mathrm{dist}_{\mathrm{SO}(3)}(c_{1},c_{2}).$

Therefore, $f$ is Lipschitz continuous with $L=4\rho$.

To show $f$ is convex, we first need the domain $B(O,\rho)$ to be convex. This
is evident in [25, Lemma 6] for $\rho\leq\frac{\pi}{2}$. We then show $f$ has
a positive (semi-)definite Hessian in $B(O,\rho)$.

Convexity of $\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$. We first analyze the
gradient and Hessian of $\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$ at $c\in
B(O,\rho)$ with $s\in{\cal S}_{R}\subset B(O,\rho)$. By definition, the
gradient and Hessian of $\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$ are the
gradient and Hessian of the following function w.r.t.
$x\in{{\mathbb{R}}^{3}}$, a tangent vector at $c$:

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c\mathrm{Exp}\left(x\right),s)=\mathrm{dist}_{\mathrm{SO}(3)}^{2}(\mathrm{Exp}\left(x\right),c^{\mathsf{T}}s),$

where $\mathrm{Exp}\left(x\right)=\exp\left(x^{\wedge}\right)$ is the
composition of the “∧” map and the exponential map. Let
$c^{\mathsf{T}}s\in\mathrm{SO}(3)$ be a rotation about the unit axis $w$ with
angle $\gamma$ (_i.e.,_ the axis-angle representation for $c^{\mathsf{T}}s$ is
$(w,\gamma)$). We then recall the following cosine rule in $\mathrm{SO}(3)$
about geodesic triangles.

######  Lemma A5 (Cosine rule in $\mathrm{SO}(3)$ [25, Proposition 2]).

Let $A_{1},A_{2},A_{3}\in\mathrm{SO}(3)$ be three points forming a triangle
and let $a_{1},a_{2},a_{3}$ be the lengths of the three geodesic line
segments. If $a_{3}$ is the length of the smaller geodesic arc between $A_{1}$
and $A_{2}$, then

$\displaystyle\mathrm{cos}\left(\frac{a_{3}}{2}\right)=\mathrm{cos}\left(\frac{a_{1}}{2}\right)\mathrm{cos}\left(\frac{a_{2}}{2}\right)+\mathrm{sin}\left(\frac{a_{1}}{2}\right)\mathrm{sin}\left(\frac{a_{2}}{2}\right)\mathrm{cos}\left(\angle
A_{1}A_{3}A_{2}\right),$ (A17)

where $\angle A_{1}A_{3}A_{2}$ is the angle formed by the geodesic segments
$A_{1}A_{3}$ and $A_{2}A_{3}$ at point $A_{3}$.

Using Lemma A5 and letting $A_{1}=\mathrm{Exp}\left(x\right)$,
$A_{2}=c^{\mathsf{T}}s$, and $A_{3}=\mathrm{I}_{3}$, we have

$\displaystyle a_{1}=\gamma,\quad a_{2}=\left\lVert x\right\rVert,\quad
a_{3}=\mathrm{dist}_{\mathrm{SO}(3)}(\mathrm{Exp}\left(x\right),c^{\mathsf{T}}s),$
$\displaystyle\mathrm{cos}\left(\angle
A_{1}A_{3}A_{2}\right)=\frac{x^{\mathsf{T}}w}{\left\lVert x\right\rVert}.$

Invoking the cosine rule (A17), we obtain

$\displaystyle\mathrm{cos}\left(\frac{a_{3}}{2}\right)=\mathrm{cos}\left(\frac{\gamma}{2}\right)\mathrm{cos}\left(\frac{\left\lVert
x\right\rVert}{2}\right)+\mathrm{sin}\left(\frac{\gamma}{2}\right)\mathrm{sin}\left(\frac{\left\lVert
x\right\rVert}{2}\right)\frac{x^{\mathsf{T}}w}{\left\lVert
x\right\rVert}\Longrightarrow$
$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}(\mathrm{Exp}\left(x\right),c^{\mathsf{T}}s)=a_{3}$
$\displaystyle=2\mathrm{arccos}\left(\mathrm{cos}\left(\frac{\gamma}{2}\right)\mathrm{cos}\left(\frac{\left\lVert
x\right\rVert}{2}\right)+\mathrm{sin}\left(\frac{\gamma}{2}\right)\mathrm{sin}\left(\frac{\left\lVert
x\right\rVert}{2}\right)\frac{x^{\mathsf{T}}w}{\left\lVert
x\right\rVert}\right).$ (A18)

We want derivatives up to second order, so we perform a second-order Taylor
expansion for (-C):

$\displaystyle
a_{3}=2\mathrm{arccos}\left(\mathrm{cos}\left(\frac{\gamma}{2}\right)\left(1-\frac{\left\lVert
x\right\rVert^{2}}{8}\right)+\frac{1}{2}\mathrm{sin}\left(\frac{\gamma}{2}\right)x^{\mathsf{T}}w+o(\left\lVert
x\right\rVert^{3})\right)$

Finally, we have the gradient

$\displaystyle\nabla\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)=\nabla_{x}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(\mathrm{Exp}\left(x\right),c^{\mathsf{T}}s)|_{x=0}=-\gamma
w^{\mathsf{T}},$ (A19)

where $(w,\gamma)$ is the axis-angle representation of $c^{\mathsf{T}}s$.
Equation (A19) states that the gradient of
$\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$ at $c$ points in the negative
direction of the geodesic from $c$ to $s$ (_i.e.,_ along the direction from
$s$ to $c$), and has magnitude equal to the length of the geodesic. When
$c=s$, $\gamma=0$ and the gradient is equal to zero. Similarly, for the
Hessian we have

$\displaystyle\nabla^{2}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)=$
$\displaystyle\nabla_{xx}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(\mathrm{Exp}\left(x\right),c^{\mathsf{T}}s)|_{x=0}$
$\displaystyle=$ $\displaystyle\nabla_{x}(2a_{3}\nabla_{x}a_{3})|_{x=0}$
$\displaystyle=$ $\displaystyle
2(\nabla_{x}a_{3})^{\mathsf{T}}\nabla_{x}a_{3}|_{x=0}+2a_{3}\nabla_{xx}a_{3}|_{x=0}$
$\displaystyle=$ $\displaystyle
2ww^{\mathsf{T}}+2\gamma\nabla_{xx}a_{3}|_{x=0}$ $\displaystyle=$
$\displaystyle\displaystyle
2ww^{\mathsf{T}}+\gamma\mathrm{cot}\left(\frac{\gamma}{2}\right)\left(\mathrm{I}_{3}-ww^{\mathsf{T}}\right).$
(A20)

Note that

$\displaystyle\lim_{\gamma\rightarrow
0}\gamma\mathrm{cot}\left(\frac{\gamma}{2}\right)=\lim_{\gamma\rightarrow
0}\frac{(\gamma\mathrm{cos}\left(\frac{\gamma}{2}\right))^{\prime}}{(\mathrm{sin}\left(\frac{\gamma}{2}\right))^{\prime}}=\lim_{\gamma\rightarrow
0}\frac{\mathrm{cos}\left(\frac{\gamma}{2}\right)+\frac{\gamma}{2}\mathrm{sin}\left(\frac{\gamma}{2}\right)}{\frac{1}{2}\mathrm{cos}\left(\frac{\gamma}{2}\right)}=2,$
(A21)

which implies the Hessian at $c$ is equal to $2\mathrm{I}_{3}$ when $c=s$ and
$\gamma=0$. Let $(w,u,v)$ be a set of orthonormal basis in
${{\mathbb{R}}^{3}}$ (_i.e.,_ choose $u,v$ as two orthogonal unit vectors in
the plane perpendicular to $w$), we have

$\displaystyle\nabla^{2}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)w=$
$\displaystyle 2w,$ (A22a)
$\displaystyle\nabla^{2}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)u=$
$\displaystyle\gamma\mathrm{cot}\left(\frac{\gamma}{2}\right)u,$ (A22b)
$\displaystyle\nabla^{2}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)v=$
$\displaystyle\gamma\mathrm{cot}\left(\frac{\gamma}{2}\right)v,$ (A22c)

which states the Hessian has one eigenvalue equal to $2$ and two eigenvalues
equal to $\gamma\mathrm{cot}\left(\frac{\gamma}{2}\right)$. Since
$\gamma\mathrm{cot}\left(\frac{\gamma}{2}\right)\in(2\rho\mathrm{cot}\left(\rho\right),2]$
when $\gamma\in[0,2\rho)$,777Note that since $c,s\in B(O,\rho)$, we have
$\gamma=\mathrm{dist}_{\mathrm{SO}(3)}(c,s)\leq\mathrm{dist}_{\mathrm{SO}(3)}(c,O)+\mathrm{dist}_{\mathrm{SO}(3)}(O,s)<2\rho$.
we have

$\displaystyle\nabla^{2}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)\succ
0,\quad\forall c,s\in B\left(O,\rho\right)\text{ with }\rho=\frac{\pi}{2},$
(A23)

and $\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$ is strictly convex on
$B(O,\rho)$ with $\rho=\frac{\pi}{2}$;

$\displaystyle\nabla^{2}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)\succeq
2\rho\mathrm{cot}\left(\rho\right)\mathrm{I}_{3},\quad\forall c,s\in
B(O,\rho)\text{ with }\rho<\frac{\pi}{2},$ (A24)

and $\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$ is strongly convex on
$B(O,\rho)$ with $\rho<\frac{\pi}{2}$.

Convexity of $f$. We then proceed to show the convexity of $f$. Let
$c_{1},c_{2}\in B(O,\rho)$, and let $c_{\alpha}$ be the $\alpha$-midpoint
along the geodesic from $c_{1}$ to $c_{2}$ with $\alpha\in(0,1)$. Because
${\cal S}_{R}$ is compact, we can write

$\displaystyle f(c_{\alpha})=\max_{s\in{\cal
S}_{R}}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{\alpha},s)=\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{\alpha},s_{\alpha})$
(A25)

with $s_{\alpha}\in{\cal S}_{R}$ a point that attains the maximum distance to
$c_{\alpha}$ (note the point $s_{\alpha}$ needs not be unique). By the strict
convexity of $\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{\alpha},s_{\alpha})$ when
$\rho=\frac{\pi}{2}$, one obtains

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{\alpha},s_{\alpha})<(1-\alpha)\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{1},s_{\alpha})+\alpha\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{2},s_{\alpha}).$
(A26)

One now notices that by definition of $f$,

$\displaystyle f(c_{i})=\max_{s\in{\cal
S}_{R}}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{i},s)\geq\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{i},s_{\alpha}),i=1,2.$
(A27)

Hence, combining (A25)-(A27), we conclude with the strict convexity of $f$
when $\rho=\frac{\pi}{2}$:

$\displaystyle f(c_{\alpha})<(1-\alpha)f(c_{1})+\alpha f(c_{2}).$ (A28)

When $\rho<\frac{\pi}{2}$, from (A24) we know
$\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$ is $\mu$-strongly convex with
$\mu=2\rho\mathrm{cot}\left(\rho\right)$. Consequently, (A26) can be modified
according to the definition of geodesic strong convexity [10, Definition 11.5]

$\displaystyle\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{\alpha},s_{\alpha})$
$\displaystyle\leq$
$\displaystyle\displaystyle(1-\alpha)\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{1},s_{\alpha})+\alpha\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{2},s_{\alpha})-$
$\displaystyle\frac{\mu}{2}\alpha(1-\alpha)\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{1},c_{2})$
$\displaystyle\leq$ $\displaystyle\displaystyle(1-\alpha)f(c_{1})+\alpha
f(c_{2})-\frac{\mu}{2}\alpha(1-\alpha)\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{1},c_{2}),$
(A30)

where the second inequality is again by the definition of $f$ in (A27).
Combining (A25) and (A30) we obtain the geodesic $\mu$-strong convexity of
$f$:

$\displaystyle f(c_{\alpha})\leq(1-\alpha)f(c_{1})+\alpha
f(c_{2})-\frac{\mu}{2}\alpha(1-\alpha)\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c_{1},c_{2}).$

∎

Subdifferential of supreme functions. To derive our final theorem on solving
subdifferential of supreme functions on Riemannian manifolds (_e.g.,_
$\mathrm{SO}(3)$), we first need to introduce some definitions and lemmas.

######  Definition A6 (Directional derivative [47, Section 3, Definition
4.1]).

Let $f:M\rightarrow\mathbb{R}$ be a function on a Riemannian manifold $M$. The
directional derivative of $f$ at $x\in M$ in the direction $v\in T_{x}M$ is
defined as

$\displaystyle f^{\prime}(x;v)=\lim_{t\rightarrow
0^{+}}\frac{f(\gamma(t))-f(x)}{t}.$ (A31)

where $\gamma:\in[-\delta,\delta]\rightarrow M$ is a geodesic segment on $M$
with $\gamma(0)=x$ and $\gamma^{\prime}(0)=v$.

For a geodesically convex function $f$, for a given $x$ and $\gamma$,
$f\circ\gamma:[-\delta,\delta]\rightarrow\mathbb{R}$ is convex. Thus
$\frac{f\circ\gamma(t)-f\circ\gamma(0)}{t}$ is non-decreasing on $t>0$, and we
have [47, Section 3, Theorem 4.2]

$\displaystyle
f^{\prime}(x;v)=\inf_{t\in(0,\delta]}\frac{f\circ\gamma(t)-f\circ\gamma(0)}{t}.$
(A32)

######  Definition A7 (Subdifferential on Riemannian manifold).

Let $f:M\rightarrow\mathbb{R}$ be a convex function on a Riemannian manifold
$M$. The subdifferential of $f$ at $x$ is defined as

$\displaystyle\partial f(x)=$ $\displaystyle\\{v\in T_{x}M|f(\gamma(t))\geq
f(x)+t\langle v,\gamma^{\prime}(0)\rangle,$ (A33) $\displaystyle\quad\forall
t>0,\forall\gamma:[-\delta,\delta]\rightarrow\mathbb{R}\quad
s.t.\gamma(0)=x\\}.$

We can also use directional derivatives to define the subdifferential.

######  Proposition A8 (Equivalent expression of subdifferential on Riemannian
manifold [47, Section 3, Theorem 4.8]).

For every $x\in M$, $\partial f(x)$ is non-empty, convex and compact.
Moreover, we have

$\displaystyle\partial f(x)=\\{v\in T_{x}M|f^{\prime}(x;w)\geq\langle
v,w\rangle,\quad\forall w\in T_{x}M\\}.$ (A34)

Then we can derive the following theorem on solving subdifferential of supreme
functions on Riemannian manifolds.

######  Theorem A9 (Danskin’s Theorem on Riemannian manifolds).

Let ${\cal S}\subset M$ be a compact set on a Riemannian manifold $M$, and let
$\phi:M\times{\cal S}\rightarrow\mathbb{R}$ be continuous and such that
$\phi(\cdot,s)$ is geodesically convex for all $s\in{\cal S}$.

  1. (i)

The function $f:M\rightarrow\mathbb{R}$ defined by $f(x)=\max_{s\in{\cal
S}}\phi(x,s)$ is geodesically convex and has directional derivative given by

$\displaystyle f^{\prime}(x;v)=\max_{\bar{s}\in{\cal
S}(x)}\phi^{\prime}(x,\bar{s};v),\quad\forall x\in M,v\in T_{x}M.$ (A35)

where $\phi^{\prime}(x,s;v)$ is the directional derivative of $\phi$ at
$(x,\bar{s})$ in the direction $v$ and ${\cal S}(x)=\\{\bar{s}\in{\cal
S}:f(x)=\phi(x,\bar{s})\\}$ is the set of maximizing points of
$\phi(x,\cdot)$.

  2. (ii)

If $\phi(\cdot,s)$ is differentiable at all $s\in{\cal S}$ and
$\nabla_{x}\phi(x,\cdot)$ is continuous on ${\cal S}$ for each $x$, then

$\displaystyle\partial
f(x)=\mathrm{conv}\left(\\{\nabla_{x}\phi(x,\bar{s}):\bar{s}\in{\cal
S}(x)\\}\right).$ (A36)

###### Proof.

The structure of the proof follows the Euclidean version in [8, Proposition
B.25] but with different definitions according to the Riemannian manifold.

Convexity of $f$. Similar to the proof of Proposition 1. The conclusion holds
for an arbitrary geodesic convex function $\phi(c,s)$.

Directional derivative of $f$. According to the definition of $f$, for any
$\bar{s}\in{\cal S}(x)$, we have $f(x)=\phi(x,\bar{s})$ and
$f(\gamma(t))\geq\phi(\gamma(t),\bar{s})$ for all $t\in[-\delta,\delta]$. Thus
we have

$\displaystyle\frac{f(\gamma(t))-f(x)}{t}\geq\frac{\phi(\gamma(t),\bar{s})-\phi(x,\bar{s})}{t}.$
(A37)

We take the limit $t\rightarrow 0^{+}$ on both sides and obtain
$f^{\prime}(x;v)\geq\phi^{\prime}(x,\bar{s};v)$ for all $\bar{s}\in{\cal
S}(x)$. Thus we have

$\displaystyle f^{\prime}(x;v)\geq\max_{\bar{s}\in{\cal
S}(x)}\phi^{\prime}(x,\bar{s};v)$ (A38)

To prove the reverse inequality, we consider $\\{t_{k}\\}\rightarrow 0^{+}$
and let $x_{k}=\gamma(t_{k})$ where velocity $\gamma^{\prime}(t_{k})=v_{k}$.
For each $k$, we find $s_{k}\in{\cal S}(x_{k})$. Since ${\cal S}$ is compact,
there exists a subsequence $\\{s_{k_{j}}\\}$ converging to some $s_{0}\in{\cal
S}$. Without loss of generality, we assume $\\{s_{k}\\}$ converges to $s_{0}$.
Since $\phi$ is continuous, we have
$\phi(x_{k},s_{k})\rightarrow\phi(x,s_{0})$. Thus, for any $s\in{\cal S}$, we
have $\phi(x_{k},s_{k})\geq\phi(x_{k},s)$. We let $k\rightarrow+\infty$ and we
obtain $\phi(x,s_{0})\geq\phi(x,s)$. Thus $s_{0}\in{\cal S}(x)$ and
$f(x)=\phi(x,s_{0})$. We now have

$\displaystyle f^{\prime}(x;v)\leq$
$\displaystyle\displaystyle\frac{f(x_{k})-f(x)}{t_{k}}$ (A39) $\displaystyle=$
$\displaystyle\displaystyle\frac{\phi(x_{k},s_{k})-\phi(x,s_{0})}{t_{k}}$
(A40) $\displaystyle\leq$
$\displaystyle\displaystyle\frac{\phi(x_{k},s_{k})-\phi(x,s_{k})}{t_{k}}$
(A41) $\displaystyle\leq$
$\displaystyle\displaystyle-\phi^{\prime}(x_{k},s_{k};-v_{k})$ (A42)
$\displaystyle\leq$
$\displaystyle\displaystyle\phi^{\prime}(x_{k},s_{k};v_{k})$ (A43)

The inequality in (A43) is the result of (A32) applied on
$\phi^{\prime}(x_{k},s_{k};-v_{k})$. The last inequality is proved in [47,
Section 3, Theorem 4.2]. We now consider function
$g_{k}(\cdot)=\phi(\gamma(\cdot),s_{k}):\mathbb{R}\to\mathbb{R}$, and
$g(\cdot)=\phi(\gamma(\cdot),s_{0})$. From [47, Section 3, Theorem 4.2],
$g_{k},g$ are convex functions. By the continuity of function $\phi,\gamma$,
$\lim_{k\to\infty}g_{k}(t_{k})=g(0)$. Then we apply the following lemma.

######  Lemma A10 ([8, Proposition B.23]).

Suppose $g:\mathbb{R}\to\mathbb{R}$ is a convex function and $\\{t_{k}\\}\to
t$ is a convergent sequence. If $g_{k}:\mathbb{R}\to\mathbb{R}$ is a sequence
of convex functions with property that $\lim_{k\to\infty}g_{k}(t_{k})=g(t)$.
Then we have

$\limsup_{k\to\infty}g_{k}^{^{\prime}}(t_{k})\leq g^{^{\prime}}(t)$

###### Proof.

In this proof, $g(\cdot)^{^{\prime}}$ and $g_{k}(\cdot)^{^{\prime}}$ denotes
the right directional derivative.

Suppose $\mu>g^{^{\prime}}(t)$, then since $g$ is convex, we can choose a
$\bar{t}$, such that $\forall\Delta t<\bar{t}$, we have:

$\displaystyle\frac{g(t+\Delta t)-g(t)}{\Delta t}<\mu$ (A44)

Thus, for relatively large $k$, we have:

$\displaystyle\frac{g_{k}(t_{k}+\Delta t)-g_{k}(t_{k})}{\Delta t}<\mu$ (A45)

But letting $\Delta t\to 0^{+}$, this implies:

$\displaystyle\limsup_{k\to\infty}g_{k}^{^{\prime}}(t_{k})<\mu$ (A46)

Since this inequality holds for any $\mu>g^{^{\prime}}(t)$, so we can conclude
that:

$\displaystyle\limsup_{k\to\infty}g_{k}^{^{\prime}}(t_{k})\leq
g^{^{\prime}}(t)$ (A47)

∎

We can see that the directional derivative of $g_{k},g$ is just the
directional derivative we have defined for $\phi$ along the geodesic. So that
we have

$\displaystyle
f^{\prime}(x;v)\leq\limsup_{k\to\infty}\phi^{\prime}(x_{k},s_{k};v_{k})\leq\phi^{\prime}(x,s_{0};v)$
(A48)

Since $s_{0}\in{\cal S}(x)$ is arbitrary, we have
$f^{\prime}(x;v)\leq\max_{\bar{s}\in{\cal S}(x)}\phi^{\prime}(x,\bar{s};v)$.
This relation together with inequality (A38) proves the equality (A35).

Subdifferential of $f$. Since we assumed $\phi(\cdot,s)$ is Riemannian
differentiable for all $s\in{\cal S}$, we have
$\partial_{x}\phi(x,s)=\\{\nabla_{x}\phi(x,s)\\}$ and for any geodesic segment
$\gamma$ with $\gamma(0)=x$, we have
$\phi(\gamma(t),s)\geq\phi(\gamma(0),s)+t\langle\nabla_{x}\phi(x,s),\gamma^{\prime}(0)\rangle$.
Thus for all $\bar{s}\in{\cal S}$ we have

$\displaystyle f(\gamma(t))=$ $\displaystyle\displaystyle\max_{s\in{\cal
S}}\phi(\gamma(t),s)$ $\displaystyle\geq$
$\displaystyle\displaystyle\phi(\gamma(t),\bar{s})$ $\displaystyle\geq$
$\displaystyle\displaystyle\phi(x,\bar{s})+t\langle\nabla_{x}\phi(x,\bar{s}),\gamma^{\prime}(0)\rangle$
$\displaystyle=$ $\displaystyle\displaystyle
f(x)+t\langle\nabla_{x}\phi(x,\bar{s}),\gamma^{\prime}(0)\rangle$ (A49)

Therefore $\nabla_{x}\phi(x,\bar{s})\in\partial f(x)$. Consider a convex
combination of two gradients

$\displaystyle
v=\alpha\nabla_{x}\phi(x,\bar{s}_{1})+(1-\alpha)\nabla_{x}\phi(x,\bar{s}_{2}),\quad\alpha\in[0,1],$
(A50)

we still have

$\displaystyle f(\gamma(t))=$ $\displaystyle\displaystyle\max_{s\in{\cal
S}}\phi(\gamma(t),s)$ $\displaystyle\geq$
$\displaystyle\displaystyle\alpha\phi(\gamma(t),\bar{s}_{1})+(1-\alpha)\phi(\gamma(t),\bar{s}_{2})$
$\displaystyle\geq$
$\displaystyle\displaystyle\alpha\left(\phi(x,\bar{s}_{1})+t\langle\nabla_{x}\phi(x,\bar{s}_{1}),\gamma^{\prime}(0)\rangle\right)$
$\displaystyle+(1-\alpha)\left(\phi(x,\bar{s}_{2})+t\langle\nabla_{x}\phi(x,\bar{s}_{2}),\gamma^{\prime}(0)\rangle\right)$
$\displaystyle=$
$\displaystyle\displaystyle\alpha\phi(x,\bar{s}_{1})+(1-\alpha)\phi(x,\bar{s}_{2})+t\langle\alpha\nabla_{x}\phi(x,\bar{s}_{1})$
$\displaystyle+(1-\alpha)\nabla_{x}\phi(x,\bar{s}_{2}),\gamma^{\prime}(0)\rangle$
$\displaystyle=$ $\displaystyle\displaystyle f(x)+t\langle
v,\gamma^{\prime}(0)\rangle$ (A51)

Thus $v\in\partial f(x)$. Therefore

$\displaystyle\partial
f(x)\supset\mathrm{conv}\left(\\{\nabla_{x}\phi(x,\bar{s}):\bar{s}\in{\cal
S}(x)\\}\right).$ (A52)

To prove the reverse inclusion, we use the hyperplane separation theorem. By
the continuity of $\nabla_{x}\phi(x,\cdot)$ and the compactness of ${\cal S}$,
we have ${\cal S}(x)$ is compact and $\\{\nabla_{x}\phi(x,\bar{s})|s\in{\cal
S}\\}$ is also compact. If $d\in\partial f(x)\subset T_{x}M$ while
$d\notin\mathrm{conv}\left(\\{\nabla_{x}\phi(x,\bar{s}):\bar{s}\in{\cal
S}(x)\\}\right)$, according to the strict separating theorem: there exist
$v\in T_{x}M$ and $\mu\in\mathbb{R}$ such that

$\displaystyle\langle
d,v\rangle>\mu>\langle\nabla_{x}\phi(x,\bar{s}),v\rangle,\quad\forall\bar{s}\in{\cal
S}(x)$ (A53)

Thus $\langle d,v\rangle>\max_{\bar{s}\in{\cal
S}(x)}\langle\nabla_{x}\phi(x,\bar{s}),v\rangle=f^{\prime}(x;v)$, which
contradicts the equivalent definition of subdifferential in Proposition A8.
Therefore $\partial
f\subset\mathrm{conv}\left(\\{\nabla_{x}\phi(x,\bar{s}):\bar{s}\in{\cal
S}(x)\\}\right)$ and together with (A52) we obtain the equality (A36). ∎

The key takeaway of the theorem above is that, if $f$ satisfies some convexity
and continuity conditions, the subdifferential of a $\min\max f$ is just the
convex hull of the gradients of the function $f$ at the maximizer. For our
problem, we have $f(c)=\max_{s\in{\cal
S}_{R}}\mathrm{dist}_{\mathrm{SO}(3)}^{2}(c,s)$, the problem is equivalent to
the $f(c)=\max_{s\in{\cal S}_{R}}\|c-s\|_{F}^{2}$. Thus, we just need to find
the gradients of the function $\|c-s\|_{F}^{2}$ at the maximizer $s$ to obtain
the subdifferential of $f$, which is just the vector points to the maximum
distance point.

SDP-based geodesic gradient descent.

The original maximization problem is presented as follows:

$\displaystyle\max_{R,t}$ $\displaystyle\|R-R_{0}\|_{F}^{2}$ subject to
$\displaystyle(R,t)\in{\sf PURSE}$ (A54)

The PURSE set constraint in Example 2 contains multiple polynomial
constraints. For how to formulate the PURSE set constraint in Example 1 into
polynomial constraints, we refer to [52], and for how to solve this problem
through relaxations, we refer to the original paper [33], and two appendices
in [52, 46].

1 Input:  PURSE $S$; initial rotation center $R_{0}$; overall iteration steps
$N_{I}$;

2 Output:  rotation center $R^{*}$; sampled boundary poses $\hat{\partial
S_{R}}$;

3$\hat{\partial S_{R}}\leftarrow\emptyset$;

4

5for  _$i\leftarrow 1$ to $N_{I}$_ do

6 $(\hat{R},\hat{t})\leftarrow$ sdp_relaxation($R_{i-1}$, $S$); % Find the
poses in $S$ with the maximum distance with $R_{i-1}$

7 $R_{i}$ = SLERP($R^{*}$, $\hat{R}$, $1/i$); % Apply spherical linear
interpolation (SLERP) with ratio $1/i$ to find the next step

8 $\hat{\partial S_{R}}\leftarrow\hat{\partial S_{R}}\cup\\{\hat{R}\\}$;

9

10 end for

11% calculate $R^{*}$ as the average rotation of the last $10$ $R_{t}$

12 $R^{*}\leftarrow\text{proj}_{\mathrm{SO}(3)}\sum_{t=T-9}^{T}R_{t}$;

13 return: $R^{*}$, $\hat{\partial S_{R}}$

Algorithm 6 SDP-based geodesic gradient descent

Based on the analysis above, the key problem turns into finding the maximum
distance point to the current point $R_{0}$ on ${\cal S}_{R}$. Maximizing a
convex function on a convex set is already difficult to solve, let alone our
problem here is a non-convex basic semialgebraic set. We leverage a hierarchy
of convex relaxations based on sums-of-squares (SOS) programming [33].

With the properties discussed in Theorem A9, Algorithm 6 can be regarded as a
subgradient method on $\mathrm{SO}(3)$. We then apply the following theorem to
guarantee the linear convergence of our subgradient method.

######  Theorem A11 (Subgradient Descent Convergence Rate [57, Theorem 11]).

If $f$ is geodesically $\mu$-strongly convex and $L_{f}$-Lipschitz, and the
sectional curvature of the manifold is lower bounded by $\kappa\leq 0$, then
the subgradient method with $\eta_{s}=\frac{2}{\mu(s+1)}$ satisfies

$f\left(\overline{x}_{t}\right)-f(x^{*})\leq\frac{2\zeta(\kappa,D)L_{f}^{2}}{\mu(t+1)},$

where $\overline{x}_{1}=x_{1}$, and
$\overline{x}_{s+1}=\exp_{\overline{x}_{s}}\left(\frac{2}{s+1}\exp_{\overline{x}_{s}}^{-1}(x_{s+1})\right)$.

---
Figure A8: The distance between the rotation center output and the
intermediate rotations at different steps
$\epsilon=\mathrm{dist}_{\mathrm{SO}(3)}(R^{*},R_{i})$ acquired by SDP solver.
The slope in the log scaled figure indicates the convergence rate of Algorithm
6.

We apply numerical experiments to validate the convergence rate of Algorithm 6
using commercial optimizer MOSEK888https://www.mosek.com/. We run the SDP
iteration for $100$ steps and calculate the difference between the rotation at
each step and the final result
$\epsilon_{i}=\mathrm{dist}_{\mathrm{SO}(3)}(R_{i},R^{*})$. We then plot the
$\epsilon_{i}$ against the iteration number $i$ in Figure A8 in logarithm
scales. The $\log(\epsilon_{i})$ decreases linearly with the iteration number
$\log(i)$ with a slope close to $1$, which supports the theoretical conclusion
of Theorem A11.

However, solving SDP is extremely time-consuming, and such an iterative method
cannot be implement in parallel. Running on a laptop with a 12-core Intel
i5-13500H CPU, each step takes more than $15$ seconds to solve, thus the
overall algorithm with $100$ steps takes more than $25$ minutes to run.
Therefore, this method is impractical in real-time applications.

###  -D Boundary Sampler Algorithms

In this section, we give a more detailed explanation of the translation
boundary $\partial S_{t}$ sampler in Algorithm 7. We also provide its
parallelized version in Algorithm 8.

####  -D1 Translation Boundary $\partial S_{t}$ Sampler

1 Input:  initial poses $S_{0}\subset\mathrm{SE}(3)$; PURSE $S$; base center
velocity magnitude $v_{0}$; time step decay factor $\gamma$; rotation
perturbation scale $\theta_{p}$; random walk trial number $N_{W}$; iteration
number $N_{I}$; perturbation number $N_{P}$; optimal perturbation number
$N_{P}^{*}$; time step scaling number $N_{T}$;

2 Output:  sampled boundary poses $\hat{\partial S_{t}}\subset\mathbb{R}^{3}$
as an inner approximation of $\partial S_{t}$;

3

4$\bar{R}\leftarrow\text{proj}_{\mathrm{SO}(3)}(\sum_{(R_{j},*)\in
S_{0}}R_{j})$;

5 $\bar{t}\leftarrow\frac{1}{|S_{0}|}\sum_{(*,t_{j})\in S_{0}}t_{j}$;

6 $\hat{\partial S_{t}}\leftarrow\emptyset$;

7 for  _ $(R_{0},t_{0})\in S_{0}$_ do

8 for  _ $n\leftarrow 1$ to $N_{W}$_ do

9 $v\leftarrow$ init_center_velocity$(t_{0},\bar{t},v_{0})$;

10 $R^{*}\leftarrow R_{0},t^{*}\leftarrow t_{0}$;

11 % Iterate $N_{I}$ times so that the evolved pose gets close to $\partial
S_{t}$

12 for  _ $i\leftarrow 1$ to $N_{I}$_ do

13 % Randomize $N_{P}$ translation perturbations for  _$j\leftarrow 1$ to
$N_{P}$_ do

14 $R_{p}$ = perturbation($\theta_{p}$);

15 $R_{j}\leftarrow R_{p}R^{*}$ ;

16 $d_{j}\leftarrow\text{dist}(R^{*},t_{j},\partial S)$;

17

18 end for

19 % Pick out $N_{P}^{*}$ perturbations that drag the pose away from $\partial
S$

20 $\\{j_{k}\\}_{k=1}^{N_{P}^{*}}\leftarrow$
top_k_indices($\\{d_{j}\\}_{j=1}^{N_{P}}$, $N_{P}^{*}$);

21 for  _$k\leftarrow 1$ to $N_{p}^{*}$_ do

22 for  _$m\leftarrow 1$ to $N_{T}$_ do

23 $\Delta T\leftarrow\gamma^{m-1}$

24 $t_{km}\leftarrow$ update_translation($t^{*}$, $R_{j_{k}}$, $v$, $\Delta
T$);

25 $I_{km}\leftarrow$ in_purse($R_{j_{k}},t_{km}$);

26

27 end for

28

29 end for

30 % Find the optimal pose that is still in $S$ and has the maximum rotation
movement

31 $m_{0}\leftarrow\min\left\\{m\mid\exists k\text{\ s.t.\
}I_{km}=1\right\\}$;

32 $k_{0}\leftarrow\left\\{k\mid I_{km_{0}}=1\right\\}$;

33 $R^{*}\leftarrow R_{k_{0}m_{0}}$;

34 $t^{*}\leftarrow t_{k_{0}m_{0}}$;

35

36 end for

37

38 $\hat{\partial S_{t}}\leftarrow\hat{\partial S_{t}}\cup t^{*}$;

39

40 end for

41

42 end for

43return: $\hat{\partial S_{t}}$

44

Algorithm 7 $\partial S_{t}$ sampler

In general, $\partial S_{t}$ sampler is similar to $\partial S_{R}$ sampler,
but in $\mathbb{R}^{3}$ instead of $\mathrm{SO}(3)$. However, there are two
differences significantly impacting the result of relative ratio $\eta_{t}$
for the two examples that need to be considered.

For object pose estimation (Example 1), we need to initialize the center
velocity with additional scaling along a specific axis. As shown in Fig. 3(a),
we notice that $S_{t}$ (on the right) is in a needle-like shape with long
expansion in one direction but very thin in two other orthogonal directions.
If we uniformly randomize center velocities of the random walks, most of the
walks will stop close to the center of $S_{t}$. Therefore, the two points on
the two ends of $S_{t}$ are not likely to be reached.

To address this problem, we apply additional scaling to the translation
velocity according to the principle component analysis (PCA) of
$S_{t0}=\\{t_{0}|(R_{0},t_{0})\in S_{0}\\}$. Specifically, we first calculate
the PCA of $S_{t0}$ so that we obtain the first weight vector ${u_{1}}$ which
is parallel to the longest axis of $S_{t}$. In this way, we can rescale the
center velocity based on the expansion of $S_{t0}$ on $3$ PCA weights. Thus,
the center velocity is more likely to point to the direction of the longest
axis of $S_{t}$. In fact, the algorithm already performs well if we directly
use the first PCA weight as the center velocity.

For point cloud registration (Example 2), we found that $\partial S_{R}$
sampler is more efficient in sampling $\partial S_{t}$ than the $\partial
S_{t}$ sampler in Algorithm 7. We explain this surprising result with the
following reason.

Suppose the first point cloud is $A\in\mathbb{R}^{3\times N}$, a transform in
$(R,t)\in\mathrm{SE}(3)$ represents $RA+t$. During the random walk process,
suppose we find an additional transform $(\tilde{R},\tilde{t})$, and then the
overall transform is $(\tilde{R}R,\tilde{R}t+\tilde{t})$. The rotation part is
a direct multiplication of the two rotation matrices, therefore $\partial
S_{R}$ is efficiently sampled even if we only vary $\tilde{R}$. However, when
we use Algorithm 7 to sample $\partial S_{t}$, the translation part
$\tilde{R}t+\tilde{t}$ not only depends on $\tilde{t}$, but also on
$\tilde{R}$. Even if we extensively explore $\tilde{t}$ in the random walks,
the overall translation could expand even wider due to the independent term
$\tilde{R}t$ which is not optimized in the $\partial S_{t}$ sampler. Thus
$\partial S_{t}$ might not be fully explored.

####  -D2 Translation Boundary $\partial S_{t}$ Sampler with Parallelization

Similar to the parallelized $\partial S_{R}$ sampler, we implement the
parallelized version of $\partial S_{t}$ sampler on NVIDIA GPUs using CuPy. We
annotate all the dimensions of each variable, so that readers can easily
understand the parallelized computation pipeline. We provide more detailed
explanations of the GPU operators as follows.

  * •

repeat($A$, $N$): repeat the array $A$ for $N$ times along certain dimension.

  * •

matmul($A$, $B$): parallel matrix multiplication of $A$ and $B$. If $A$ or $B$
has more than 2 dimensions, the last two dimensions are treated as the matrix
dimensions and all other dimensions are broadcasted.

  * •

find_farthest_translation($R$, $t$, $I$): find the pose that is still in $S$
(indicated by $I$) and has the maximum translation movement from all the poses
in $R$ and $t$. This is equivalent to line 7-7 in Algorithm 7 but with
parallel computation on all $|S_{0}|N_{W}$ walks.

The other operators not mentioned here are similar to the ones in the non-
parallelized version but applies to all input values simultaneously.

1 Input:  initial poses $S_{0}\subset\mathrm{SE}(3)$; PURSE $S$; base center
velocity magnitude $v_{0}$; time step decay factor $\gamma$; rotation
perturbation scale $\theta_{p}$; random walk trial number $N_{W}$; iteration
number $N_{I}$; perturbation number $N_{P}$; optimal perturbation number
$N_{P}^{*}$; time step scaling number $N_{T}$;

2 Output:  sampled boundary poses $\hat{\partial S_{t}}\subset\mathbb{R}^{3}$
as an inner approximation of $\partial S_{t}$;

3 $v\leftarrow$ init_center_velocity($S_{0},v_{0},N_{W}$); %
$(|S_{0}|,N_{W},3)$

4 $(R^{*},t^{*})\leftarrow$ repeat($S_{0},N_{W}$); % $R^{*}$:
$(|S_{0}|,N_{W},3,3)$, $t^{*}$: $(|S_{0}|,N_{W},3)$

5 $\Delta T\leftarrow(1,\beta,\beta^{2},\cdots,\beta^{N_{T}-1})$; % $(N_{T})$

6

7for  _ $i\leftarrow 1$ to $N_{I}$_ do

8 $R_{p}\leftarrow$ perturbation($\theta_{p},|S_{0}|N_{W}N_{P}$); %
$(|S_{0}|,N_{W},N_{P},3,3)$

9 $R\leftarrow$ matmul($R_{p}$, repeat($R^{*},N_{P}$)); %
$(|S_{0}|,N_{W},N_{P},3,3)$

10 $t\leftarrow$ repeat($t^{*},N_{P}$); % $(|S_{0}|,N_{W},N_{P},3)$

11 $d\leftarrow$ dist($R,t,\partial S$); % $(|S_{0}|,N_{W},N_{P})$

12 $j\leftarrow$ top_k_indices($d,N_{P}^{*}$); % $(|S_{0}|,N_{W},N_{P}^{*})$

13 $\tilde{R}\leftarrow$ repeat($R_{[j]},N_{T}$); %
$(|S_{0}|,N_{W},N_{T},N_{P}^{*},3,3)$

14 $\tilde{t}\leftarrow$
update_translation(repeat($t^{*},N_{P}^{*}$)$,\tilde{R},v,\Delta T$); %
$(|S_{0}|,N_{W},N_{T},N_{P}^{*},3)$

15 $I\leftarrow$ in_purse($\tilde{R},\tilde{t}$); %
$(|S_{0}|,N_{W},N_{T},N_{P}^{*})$

16 $R^{*},t^{*}\leftarrow$ find_farthest_translation($\tilde{R},\tilde{t},I$);

17

18 end for

19return: $\hat{\partial S_{t}}\leftarrow t^{*}$

Algorithm 8 $\partial S_{t}$ parallel sampler

###  -E Intuitive example for $\partial S\neq\partial S_{R}\times\partial
S_{t}$

Here we will give a simple example showing that by simply pushing the poses to
the boundary of PURSE set could never be enough for getting the tight
characterization of the boundary of the PURSE set with respect to rotation and
translation.

We suppose $R$ lies in the $x$-$y$ plane and satisfies $x^{2}+y^{2}\leq 1$.
$t$ lies on the z axis and satisfies $-1\leq z\leq 1$. And the PURSE set as
$S\coloneqq\\{(x,y,z)|x^{2}+y^{2}+z^{2}\leq 1\\}$. We remark that here $R$ and
$t$ do not refer to rotation and translation anymore (so with a bit abuse of
notation). We here take sampling the boundary of $R$ set here as an example.

We argue that, by simply sampling the boundary of the PURSE set, it’s hard to
densely sample from the $R$ set and $t$ set. See the visualization in Fig A9.
We can see that, if we just simply uniformly sample from the boundary of the
PURSE set, it won’t be efficient in sampling the boundary of the $R$ set, let
alone this is just an over-simplified 2D example.

Figure A9: Simply sample from PURSE set boundary leads to low sampling
efficiency of the $R$ set boundary. Left: Uniform sampling of the unit sphere,
Right: $R$ set (projection to the x-y plane)

Our algorithm proceeds as follows:

Step 1. For the initial point, we first perturbs the translation $t$ $N_{P}$
times. And we keep the top $N_{P}^{*}$ best perturbations.

Step 2. Then we move the rotation $R$ in some random direction. We keep
$N_{T}$ exponentially-decay movements and check which is the farthest valid
movement.

The visualization of the intuitive example is shown in Fig. A10. In each row,
the left plot shows the points in the 3D PURSE set, the middle plot shows the
$R$ set (projection onto the x-y plane), and the right plot shows the $t$ set
(projection onto the z axis). It’s easy to see that our algorithm could
efficiently sample the boundary of the $R$ set and $t$ set because as the
iteration proceeds, the sampled point is becoming closer to the boundary of
the $R$ set.

---
Figure A10: Plot for the intuitive example. Up: Step 1, Down: Step 2.
Left: 3D PURSE set, Middle: $R$ set (projection onto the x-y plane), Right:
$t$ set (projection onto the z axis)

## References

  * Afsari [2011] Bijan Afsari.  Riemannian lp center of mass: existence, uniqueness, and convexity.  _Proceedings of the American Mathematical Society_ , 139(2):655–673, 2011.
  * Angelopoulos and Bates [2021] Anastasios N Angelopoulos and Stephen Bates.  A gentle introduction to conformal prediction and distribution-free uncertainty quantification.  _arXiv preprint arXiv:2107.07511_ , 2021.
  * Antonante et al. [2021] Pasquale Antonante, Vasileios Tzoumas, Heng Yang, and Luca Carlone.  Outlier-robust estimation: Hardness, minimally tuned algorithms, and applications.  _IEEE Transactions on Robotics_ , 38(1):281–301, 2021.
  * Arnaudon and Nielsen [2013a] Marc Arnaudon and Frank Nielsen.  On approximating the riemannian 1-center.  _Computational Geometry_ , 46(1):93–104, 2013a.
  * Arnaudon and Nielsen [2013b] Marc Arnaudon and Frank Nielsen.  On approximating the riemannian 1-center.  _Computational Geometry_ , 46(1):93–104, 2013b.
  * Arun et al. [1987] K Somani Arun, Thomas S Huang, and Steven D Blostein.  Least-squares fitting of two 3-d point sets.  _IEEE Transactions on pattern analysis and machine intelligence_ , (5):698–700, 1987.
  * Bâdoiu and Clarkson [2003] Mihai Bâdoiu and Kenneth L. Clarkson.  Smaller core-sets for balls.  In _Proceedings of the Fourteenth Annual ACM-SIAM Symposium on Discrete Algorithms_ , SODA ’03, page 801–802, USA, 2003. Society for Industrial and Applied Mathematics.  ISBN 0898715385.
  * Bertsekas [1997] D P Bertsekas.  Nonlinear programming.  _Journal of the Operational Research Society_ , 48(3):334–334, 1997.  doi: 10.1057/palgrave.jors.2600425.  URL https://doi.org/10.1057/palgrave.jors.2600425.
  * Black and Rangarajan [1996] Michael J Black and Anand Rangarajan.  On the unification of line processes, outlier rejection, and robust statistics with applications in early vision.  _International journal of computer vision_ , 19(1):57–91, 1996.
  * Boumal [2023] Nicolas Boumal.  _An introduction to optimization on smooth manifolds_.  Cambridge University Press, 2023.
  * Brachmann et al. [2014] Eric Brachmann, Alexander Krull, Frank Michel, Stefan Gumhold, Jamie Shotton, and Carsten Rother.  Learning 6d object pose estimation using 3d object coordinates.  In _Computer Vision–ECCV 2014: 13th European Conference, Zurich, Switzerland, September 6-12, 2014, Proceedings, Part II 13_ , pages 536–551. Springer, 2014.
  * Braun and Goldfarb [2012] David J Braun and Michael Goldfarb.  Simulation of constrained mechanical systems—part i: An equation of motion.  _Journal of Applied Mechanics_ , 2012.
  * Bustos and Chin [2017] Alvaro Parra Bustos and Tat-Jun Chin.  Guaranteed outlier removal for point cloud registration with correspondences.  _IEEE transactions on pattern analysis and machine intelligence_ , 40(12):2868–2882, 2017.
  * Carlone et al. [2023] Luca Carlone et al.  Estimation contracts for outlier-robust geometric perception.  _Foundations and Trends® in Robotics_ , 11(2-3):90–224, 2023.
  * Chin and Suter [2022] Tat-Jun Chin and David Suter.  _The maximum consensus problem: recent algorithmic advances_.  Springer Nature, 2022.
  * Chou et al. [2022] Glen Chou, Necmiye Ozay, and Dmitry Berenson.  Safe output feedback motion planning from images via learned perception modules and contraction theory.  In _International Workshop on the Algorithmic Foundations of Robotics_ , pages 349–367. Springer, 2022.
  * Choy et al. [2019] Christopher Choy, Jaesik Park, and Vladlen Koltun.  Fully convolutional geometric features.  In _Proceedings of the IEEE/CVF international conference on computer vision_ , pages 8958–8966, 2019.
  * Choy et al. [2020] Christopher Choy, Wei Dong, and Vladlen Koltun.  Deep global registration.  In _CVPR_ , 2020.
  * Cosner et al. [2021] Ryan K Cosner, Andrew W Singletary, Andrew J Taylor, Tamas G Molnar, Katherine L Bouman, and Aaron D Ames.  Measurement-robust control barrier functions: Certainty in safety with uncertainty in state.  In _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , pages 6286–6291. IEEE, 2021.
  * Dean et al. [2020] Sarah Dean, Nikolai Matni, Benjamin Recht, and Vickie Ye.  Robust guarantees for perception-based control.  In _Learning for Dynamics and Control_ , pages 350–360. PMLR, 2020.
  * Deng et al. [2020] Xinke Deng, Yu Xiang, Arsalan Mousavian, Clemens Eppner, Timothy Bretl, and Dieter Fox.  Self-supervised 6d object pose estimation for robot manipulation.  In _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 3665–3671. IEEE, 2020.
  * Ehambram et al. [2022] Aaronkumar Ehambram, Raphael Voges, Claus Brenner, and Bernardo Wagner.  Interval-based visual-inertial lidar slam with anchoring poses.  In _2022 International Conference on Robotics and Automation (ICRA)_ , pages 7589–7596. IEEE, 2022.
  * Gao et al. [2003] Xiao-Shan Gao, Xiao-Rong Hou, Jianliang Tang, and Hang-Fei Cheng.  Complete solution classification for the perspective-three-point problem.  _IEEE transactions on pattern analysis and machine intelligence_ , 25(8):930–943, 2003.
  * Gärtner [1999] Bernd Gärtner.  Fast and robust smallest enclosing balls.  In _European symposium on algorithms_ , pages 325–338. Springer, 1999.
  * Hartley et al. [2013] Richard Hartley, Jochen Trumpf, Yuchao Dai, and Hongdong Li.  Rotation averaging.  _International journal of computer vision_ , 103:267–305, 2013.
  * Henk [2012] Martin Henk.  löwner-john ellipsoids.  _Documenta Math_ , 95:106, 2012.
  * Huber [2004] Peter J Huber.  _Robust statistics_ , volume 523.  John Wiley & Sons, 2004.
  * Khalil et al. [1996] IS Khalil, JC Doyle, and K Glover.  _Robust and optimal control_.  Prentice hall, 1996.
  * Kneip et al. [2014] Laurent Kneip, Hongdong Li, and Yongduek Seo.  Upnp: An optimal o (n) solution to the absolute pose problem with universal applicability.  In _Computer Vision–ECCV 2014: 13th European Conference, Zurich, Switzerland, September 6-12, 2014, Proceedings, Part I 13_ , pages 127–142. Springer, 2014.
  * Korkmaz et al. [2014] Selcuk Korkmaz, Dinçer Göksülük, and GÖKMEN Zararsiz.  Mvn: An r package for assessing multivariate normality.  _R JOURNAL_ , 6(2), 2014.
  * Kukelova et al. [2008] Zuzana Kukelova, Martin Bujnak, and Tomas Pajdla.  Automatic generator of minimal problem solvers.  In _Computer Vision–ECCV 2008: 10th European Conference on Computer Vision, Marseille, France, October 12-18, 2008, Proceedings, Part III 10_ , pages 302–315. Springer, 2008.
  * Kunkel [2006] Peter Kunkel.  _Differential-algebraic equations: analysis and numerical solution_ , volume 2.  European Mathematical Society, 2006.
  * Lasserre [2001] Jean B Lasserre.  Global optimization with polynomials and the problem of moments.  _SIAM Journal on optimization_ , 11(3):796–817, 2001.
  * Lasserre [2015] Jean B Lasserre.  A generalization of löwner-john’s ellipsoid theorem.  _Mathematical Programming_ , 152:559–591, 2015.
  * Li et al. [2023] Yingying Li, Jing Yu, Lauren Conger, and Adam Wierman.  Learning the uncertainty sets for control dynamics via set membership: A non-asymptotic analysis.  _arXiv preprint arXiv:2309.14648_ , 2023.
  * Milanese and Vicino [1991] Mario Milanese and Antonio Vicino.  Optimal estimation theory for dynamic systems with set membership uncertainty: An overview.  _Automatica_ , 27(6):997–1009, 1991.
  * Mustafa et al. [2018] Mohamed Mustafa, Alexandru Stancu, Nicolas Delanoue, and Eduard Codres.  Guaranteed slam—an interval approach.  _Robotics and Autonomous Systems_ , 100:160–170, 2018.
  * Pavlakos et al. [2017] Georgios Pavlakos, Xiaowei Zhou, Aaron Chan, Konstantinos G Derpanis, and Kostas Daniilidis.  6-dof object pose from semantic keypoints.  In _2017 IEEE international conference on robotics and automation (ICRA)_ , pages 2011–2018. IEEE, 2017.
  * Peng et al. [2019] Sida Peng, Yuan Liu, Qixing Huang, Xiaowei Zhou, and Hujun Bao.  Pvnet: Pixel-wise voting network for 6dof pose estimation.  In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , pages 4561–4570, 2019.
  * Rosen et al. [2019] David M Rosen, Luca Carlone, Afonso S Bandeira, and John J Leonard.  Se-sync: A certifiably correct algorithm for synchronization over the special euclidean group.  _The International Journal of Robotics Research_ , 38(2-3):95–125, 2019.
  * Roth [2022] Aaron Roth.  Uncertain: Modern topics in uncertainty estimation, 2022.
  * Rusu et al. [2009] Radu Bogdan Rusu, Nico Blodow, and Michael Beetz.  Fast point feature histograms (fpfh) for 3d registration.  In _2009 IEEE international conference on robotics and automation_ , pages 3212–3217. IEEE, 2009.
  * Shi et al. [2023] Jingnan Shi, Heng Yang, and Luca Carlone.  Optimal and robust category-level perception: Object pose and shape estimation from 2-d and 3-d semantic keypoints.  _IEEE Transactions on Robotics_ , 2023.
  * Song et al. [2021] Yang Song, Zhuqing Zhang, Jun Wu, Yue Wang, Liang Zhao, and Shoudong Huang.  A right invariant extended kalman filter for object based slam.  _IEEE Robotics and Automation Letters_ , 7(2):1316–1323, 2021.
  * Szeliski [2022] Richard Szeliski.  _Computer vision: algorithms and applications_.  Springer Nature, 2022.
  * Tang et al. [2023] Yukai Tang, Jean-Bernard Lasserre, and Heng Yang.  Uncertainty quantification of set-membership estimation in control and perception: Revisiting the minimum enclosing ellipsoid.  _arXiv preprint arXiv:2311.15962_ , 2023.
  * Udriste [1994] Constantin Udriste.  _Convex functions and optimization methods on Riemannian manifolds_ , volume 297.  Springer Science & Business Media, 1994.
  * Wang et al. [2018] Ye Wang, Vicenç Puig, and Gabriela Cembrano.  Set-membership approach and kalman observer based on zonotopes for discrete-time descriptor systems.  _Automatica_ , 93:435–443, 2018.
  * Wen et al. [2023] Bowen Wen, Wei Yang, Jan Kautz, and Stan Birchfield.  Foundationpose: Unified 6d pose estimation and tracking of novel objects.  _arXiv preprint arXiv:2312.08344_ , 2023.
  * Xie [2016] Miaolan Xie.  Inner approximation of convex cones via primal-dual ellipsoidal norms.  Master’s thesis, University of Waterloo, 2016.
  * Yang and Carlone [2022] Heng Yang and Luca Carlone.  Certifiably optimal outlier-robust geometric perception: Semidefinite relaxations and scalable global optimization.  _IEEE transactions on pattern analysis and machine intelligence_ , 45(3):2816–2834, 2022.
  * Yang and Pavone [2023] Heng Yang and Marco Pavone.  Object pose estimation with statistical guarantees: Conformal keypoint detection and geometric uncertainty propagation.  In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , pages 8947–8958, 2023.
  * Yang et al. [2020] Heng Yang, Jingnan Shi, and Luca Carlone.  Teaser: Fast and certifiable point cloud registration.  _IEEE Transactions on Robotics_ , 37(2):314–333, 2020.
  * Yang et al. [2021a] Heng Yang, Wei Dong, Luca Carlone, and Vladlen Koltun.  Self-supervised geometric perception.  In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , pages 14350–14361, 2021a.
  * Yang et al. [2021b] Heng Yang, Chris Doran, and Jean-Jacques Slotine.  Dynamical pose estimation.  In _Proceedings of the IEEE/CVF International Conference on Computer Vision_ , pages 5926–5935, 2021b.
  * Zeng et al. [2017] Andy Zeng, Shuran Song, Matthias Nießner, Matthew Fisher, Jianxiong Xiao, and T Funkhouser.  3dmatch: Learning the matching of local 3d geometry in range scans.  volume 1, page 4, 2017.
  * Zhang and Sra [2016] Hongyi Zhang and Suvrit Sra.  First-order methods for geodesically convex optimization.  In _Conference on Learning Theory_ , pages 1617–1638. PMLR, 2016.