# On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

Connor Holmes, Frederike Dümbgen, Timothy D Barfoot

► To cite this version:

Connor Holmes, Frederike Dümbgen, Timothy D Barfoot. On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics. IEEE Transactions on Robotics, 2024, 40, pp.4805-4824. 10.1109/TRO.2024.3475220. hal-04903260

HAL Id: hal-04903260

https://hal.science/hal-04903260v1

Submitted on 21 Jan 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

Connor Holmes *Student Member, IEEE*, Frederike Dümbgen *Member, IEEE*, Timothy D. Barfoot *Fellow, IEEE*

*Abstract*—In recent years, there has been remarkable progress in the development of so-called *certifiable perception* methods, which leverage semidefinite, convex relaxations to find *global optima* of perception problems in robotics. However, many of these relaxations rely on simplifying assumptions that facilitate the problem formulation, such as an *isotropic* measurement noise distribution. In this paper, we explore the tightness of the semidefinite relaxations of *matrix-weighted* (anisotropic) state-estimation problems and reveal the limitations lurking therein: matrix-weighted factors can cause convex relaxations to lose tightness. In particular, we show that the semidefinite relaxations of localization problems with matrix weights may be tight only for low noise levels. To better understand this issue, we introduce a theoretical connection between the posterior uncertainty of the state estimate and the certificate matrix obtained via convex relaxation. With this connection in mind, we empirically explore the factors that contribute to this loss of tightness and demonstrate that *redundant constraints* can be used to regain it. As a second technical contribution of this paper, we show that the state-of-the-art relaxation of scalar-weighted SLAM cannot be used when matrix weights are considered. We provide an alternate formulation and show that its SDP relaxation is not tight (even for very low noise levels) unless specific *redundant constraints* are used. We demonstrate the tightness of our formulations on both simulated and real-world data.

*Index Terms*—Localization, SLAM, Anisotropic, Certifiable, Optimization.

## I. INTRODUCTION

STATE-ESTIMATION is an integral component of modern robotics systems. Workhorse algorithms for state-estimation – such as localization and simultaneous localization and mapping (SLAM) – are now capable of estimating hundreds of thousands of states on a single processor in real time [55] and are far from the computational bottleneck of robotic systems. To obtain such levels of performance, these algorithms typically rely on local optimization methods (e.g., Gauss-Newton), which often exhibit super-linear convergence. In particular, SLAM has reached a high level of maturity in terms of both breadth and depth of understanding in the robotics community (see [4] and [28] for a comprehensive review of SLAM).

In recent years, we have seen a surge in interest in the use of convex, semidefinite relaxations to solve and certify the global

This work was supported by the National Sciences and Engineering Research Council of Canada (NSERC), by the Swiss National Science Foundation, Postdoc Mobility under Grant 206954, and by the ANR JCJC project NIMBLE (No. ANR-22-CE33-0008).

CH and TDB are with the University of Toronto Robotics Institute, University of Toronto, Toronto, Ontario, Canada. FD is with Inria, École Normale Supérieure, PSL University, Paris, France. Corresponding author: connor.holmes@mail.utoronto.ca

Manuscript received: August 14, 2023; Revised: May 1, 2024; Accepted: September 22, 2024.

This paper was recommended for publication by Editor Behnke, Sven upon evaluation of the Associate Editor and Reviewers' comments.

Digital Object Identifier (DOI): see top of this page.

<figure><img src="image_2.png" /><figcaption>Fig. 1. An example of a local and global minimum for stereo SLAM with 10 poses and 20 landmarks from the “Starry Night” dataset. Ground-truth landmarks are shown as grey stars, ground-truth poses as blue frames, pose/landmark estimates are shown as frames/stars coloured red and green for local and global minima, respectively. Both minima are based on the same set of measurements: stereo pixel coordinates converted to Euclidean coordinates (example shown at the top of the figure) and relative-pose measurements.</figcaption></figure>

optimality of robotic state-estimation and perception problems. In principle, these relaxations can be solved in polynomial time using interior-point methods [63]. However, much of the interest in these algorithms is related to recent improvements in runtime, which have made certifiable methods more attractive for *real-time* applications. These improvements have largely been brought to the robotics community by SE-Sync [54], which globally solves *pose-graph optimization* (PGO) – a cornerstone of modern SLAM algorithms – by leveraging the low-rank nature of its semidefinite program (SDP) relaxation. A series of extensions to this method have been and continue to be developed [55].

Despite the success of these methods, they often rely on simplifying assumptions in order to apply these fast, low-rank algorithms. Perhaps most striking is the assumption of isotropic noise models that pervades the majority of the literature. Isotropic noise is often an unrealistic assumption for modern robotics, especially when more realistic sensor models are considered. For example, [46] shows that conversion of stereo pixel coordinates to Euclidean coordinates yields noise distributions that should not be modeled isotropically. When the correct model is used, the resulting maximum-likelihood optimization includes *matrix* (rather than *scalar*) weighting factors.

The introduction of matrix weighting in state-estimation problems with rotations typically leads to solution methods that are more involved. For example, though a closed-form, global solution exists for scalar-weighted Wahba's prob-
lem [36], [37], [64],<fnref n="1" /> when matrix weights are introduced, an iterative, local solver must be used [8], [20].

We will see that introduction of matrix weighting can also have a profound effect on the convex relaxations of state-estimation problems. In many cases, problems that have tight relaxations in the scalar-weighted case have a duality gap when matrix weighting is used. These relaxations can be *tightened* by adding new constraints to the SDP, but the addition of these constraints may degrade the performance of low-rank optimization methods mentioned above. Therefore, it is paramount to understand the key causes of this loss of tightness.

In this paper, we explore the tightness of semidefinite relaxations of perception problems that have been generalized to include *matrix weights* and expose the limitations that result from this generalization. In the next section, we introduce works that are closely related to this paper and establish our contributions to the field. We then introduce the requisite background on measurement models and semidefinite relaxation methods in Section III. In Section IV, we explore the impact of matrix weighting on the formulation of two key state-estimation problems in robotics: localization and SLAM. We then draw an interesting theoretical connection in Section V between the posterior distribution of the state estimate and dual solution (or *certificate matrix*) of the semidefinite relaxation of the corresponding state estimation problem. In Section VI, we provide an in-depth, empirical study of the effects of matrix weighting, anisotropic noise, and stereo-camera measurements on the tightness of the semidefinite relaxations defined in Section IV, drawing connections to Section V. We also evaluate these relaxations on real-world datasets, both with and without redundant constraints, in Section VII. In particular, we show how our globally optimal solution to matrix-weighted Wahba's Problem can be used in a state-of-the-art, outdoor, stereo-localization pipeline. Finally, Section VIII presents our conclusions and ideas for future research in this area.

## II. RELATED WORK AND CONTRIBUTIONS

There is a large range of problems for which certifiable methods already exist. To name a few, methods for robust state estimation [69], [70], sensor calibration [31], [66], inverse kinematics [31], image segmentation [38], pose-graph optimization [54], multiple-point-set registration [19], [39], range-only localization [27], planar SLAM [43], and range-aided SLAM [50] have all been explored.

Many papers have considered the conditions under which a given problem can be certified. In particular, [21] shows that, under certain technical conditions, problems that have zero duality gap when unperturbed (no noise) continue to enjoy zero duality gap as long as the perturbation parameter is within a bound (i.e., the underlying problem has sufficiently low noise). For state-estimation problems, this bound is often (but not always) found empirically to be larger than noise levels encountered in practice [29], [54], [60].

At present, certifiable perception problems can be broadly categorized into two key groups: problems for which fast, low-rank solvers are available and problems that can be certified,

but must still rely on slower SDP solution methods (e.g., interior-point methods). The next subsections provide more detail on each of these two groups.

### A. Fast Certifiable Perception

More so than other problems, global optimization of *rotation synchronization* has been the subject of intense study in the vision community [15], [29], [65] and was one of the first to enjoy significant speed improvements by leveraging the *low-rank* structure of the SDP relaxation via the so-called *Riemannian Staircase* approach [5], [9].

Building off existing certification methods for PGO [17] and inspired by the success of Riemannian methods for rotation-synchronization, [54] introduced SE-Sync in the robotics community. SE-Sync solves PGO over $SE(d)$ by taking advantage of its *separable* structure [41], marginalizing the translation variables, and using the *Riemannian Staircase* to solve the resulting rotation-synchronization problem. It was later shown that this technique could be used without marginalizing translations [12] and extended to landmark-based SLAM [35], in both cases further exploiting problem sparsity. This method was also extended to a *distributed* framework in [60] and has even been integrated into the recent Kimera-Multi pipeline [59]. Not surprisingly, these developments have inspired further advances in the original rotational synchronization problem [24].

Some of these methods boast runtimes that even rival state-of-the-art, local methods (e.g., Gauss-Newton-based methods [23]), with the added guarantee of a global certificate [12], [40]. An excellent review of the current state of certifiable methods is provided in [55].

### B. Certifiable Perception with Redundant Constraints

Though SE-Sync and its derivatives are among the most performant algorithms in certifiable perception, there are several other certifiable perception problems for which low-rank methods do not result in significant performance improvements. In particular, this set of problems includes those whose semidefinite relaxations are not initially tight, but can be tightened through the addition of certain constraints. These constraints are referred as *redundant constraints* because they are redundant in the original formulation of the problem, but become nonredundant (and, indeed, linearly independent) in the lifted, semidefinite relaxation.

This 'tightening trick' has been known for some time in the optimization community [47], and has been applied to several problems [51], [56]. In the computer-vision community, redundant constraints have been used to tighten generalized essential-matrix estimation [72], relative-pose estimation between cameras [14], [30], and registration using 3D primitives [13]. The formulation given in the latter paper is a degenerate version of our single-pose, matrix-weighted localization formulation (see Section IV-A), in which the use of (degenerate) matrix weights is motivated by geometry rather than noise distribution.

Interestingly, both [30] and [14] showed that adding redundant constraints increases the level of measurement noise

<footnote n="1">¹Wahba's problem can also be referred to as point-set registration.</footnote>
for which their respective problems remain tight. In the context of robotics, [66] found a similar result when exploring the effect of redundant constraints on a sensor-calibration task. [70] introduced redundant constraints in conjunction with *graduated non-convexity* [67] to solve *robust* point-set registration globally. [68] and [69] extended these results to several other robust perception problems by leveraging the Lasserre-moment hierarchy [34], [42].

This hierarchy constitutes a powerful set of theoretical tools that are guaranteed to tighten the SDP relaxation of any *polynomial* optimization problem through the use of redundant variables and constraints.<fnref n="2" /> The caveat to this method is that it requires the addition of new variables and constraints – possibly *ad infinitum* – and can quickly become intractable in practice. Techniques such as *Douglas-Rachford Splitting* for certification [70] and the STRIDE algorithm [69] have improved runtimes when the moment hierarchy is used, but are still far from real time.

Since certification generally depends on the number of variables and constraints used, a more efficient tightening approach is search for a small subset of variables and constraints that is sufficient to render a given relaxation *tight*. This reflects the approach that we take in this paper as well as in our concurrent work [26], which searches for redundant constraints using a sampling approach.

### C. Our Contributions

The contributions of this paper are as follows:

* We demonstrate that introducing matrix weights (due to anisotropic noise distributions) into existing certifiable state estimation problems can severely impact the tightness of the underlying SDP relaxation.

* We show that a set of redundant constraints can be used to regain tightness in these problems. To do this, we leverage results in our concurrent paper [26], which *numerically* find a redundant constraint set for a *specific* problem. We interpret these numerical constraints to find algebraic constraints that can be applied to a broader range of problem instances.

* We establish a connection between classical probabilistic interpretations of uncertainty and the *dual* or *certificate* matrix and leverage this connection to further understand our empirical results and the effect of redundant constraints on tightness.

* We show that, while the SDP relaxation of matrix-weighted SLAM is intractable for large-scale problems, the relaxation of matrix-weighted Wahba's problem can be solved in near realtime. We apply the latter relaxation in an outdoor, stereo-localization pipeline on real-world data.

## III. BACKGROUND MATERIAL

In this section, we provide the reader with some background material and notation that will be useful for the understanding of subsequent developments.

### A. Notation

We denote matrices with bold-faced, capitalized letters, $A$, column vectors with bold-faced, lower-case letters, $a$, and scalar quantities with normal-faced font, $a$. Let $\mathbb{S}^n$ denote the space of $n$-dimensional symmetric matrices and $\mathbb{S}_+^n$ denote the space of $n$-dimensional symmetric positive semidefinite matrices. We equivalently write $A \succeq 0$ whenever $A \in \mathbb{S}_+^n$. Let $\| \cdot \|_F$ denote the Frobenius norm and let $\langle A, B \rangle$ denote the Frobenius inner product. Let diag($A_1, \dots, A_N$) denote the block-diagonal matrix with blocks corresponding to matrices $A_1, \dots, A_N$. Note that this includes the case where the $A_i$ are scalar (i.e., diag($a_1, \dots, a_N$), $a_i \in \mathbb{R}$). Let $I$ denote the identity matrix, whose dimension will be clear from the context or otherwise specified. Let $0$ denote the matrix with all-zero entries, whose dimension will be evident from the context. Let the subscript "0" denote the world frame. Let $t_i^j$ denote a vector from frame $i$ to frame $j$ expressed in frame $i$ and $C_{ij}$ denote a rotation matrix that maps vectors expressed in frame $j$ to equivalent vectors in frame $i$. For readability, we replace $t_i^{0}$ with $t_i$ and $C_{i0}$ with $C_i$. Let $\otimes$ denote the Kronecker product. Let $|S|$ denote the cardinality of the set $S$. Let $A^+$ denote the Moore-Penrose pseudoinverse of a given matrix $A$. Let $\text{vec}(A)$ denote the column-wise vectorization (reshape) of a given matrix $A$. Let $(\cdot)^\times$ denote the linear, skew-symmetric operator as defined in [8]. Let $[N] = \{1, \dots, N\} \subset \mathbb{N}$ be the set of indexing integers.

### B. MAP Estimation and the Fisher Information Matrix

In robotics, we often frame state estimation as *maximum-a-posteriori* (MAP) problems, in which the optimal estimate is given by

$$ \mathbf{x}_c^* = \arg \min_{\mathbf{x}_c \in \mathcal{M}} - \log(p(\mathbf{x}_c | \mathcal{D})), \quad (1) $$

where $p(\mathbf{x}_c|\mathcal{D})$ is the posterior distribution function of the estimated parameter, $\mathbf{x}_c$, given all of the available data, $\mathcal{D}$ and $\mathcal{M}$ characterizes the feasible set. In practice, it is common to approximate Problem (1) as follows:

$$ \mathbf{x}^* = \arg \min_{\mathbf{x} \in \mathbb{R}^p} - \log(p(\mathbf{x} | \mathcal{D})), \quad (2) $$

where the optimization variable, $\mathbf{x}$, has been locally parameterized to ensure that constraints are not explicitly required.<fnref n="3" /> Optimization then proceeds by iteratively updating the local parameterization until convergence is reached.

It is often the case that we wish to ascertain not only optimal estimates, but also the uncertainty associated with these estimates. To do so, we use the *Laplace approximation*, which models the posterior distribution as a Gaussian centered at the MAP estimate, $\mathbf{x}^*$, with inverse covariance equal to the so-called *Fisher Information Matrix* (FIM),

$$ \Sigma^{-1} = - \frac{\partial^2}{\partial \mathbf{x}^2} \log(p(\mathbf{x} | \mathcal{D})) \bigg|_{\mathbf{x} = \mathbf{x}_u^*} . \quad (3) $$

The FIM can be extracted directly from the Hessian of the cost function in (2) (or an approximation thereof)<fnref n="4" />. Its properties

<footnote n="2">²Note that this class of problems encompasses almost all of the certifiable perception problems to date.</footnote>

<footnote n="3">³When state estimates involve pose variables or orientations this unconstrained form can be derived using the Lie algebra vector space of SE(3) or SO(3).</footnote>

<footnote n="4">⁴When using Gauss-Newton to solve estimation problems, the Hessian is often approximated as the product of the Jacobian with its transpose.</footnote>
have been intensely studied by the robotic state-estimation community [18]. For instance, it is known that the minimum eigenvalues of the FIM (and their respective eigenvectors) characterize the worst-case uncertainty of an estimated parameter. Of particular interest here is the fact that the geometry of the measurement data (e.g., aligned uncertainty ellipsoids) can lead to degeneracy of the FIM [71].

### C. Measurement Models

In this paper, we define a directed graph, $\tilde{\mathcal{G}} = (\mathcal{V}, \tilde{\mathcal{E}})$, to keep track of poses and measurements. The vertex set $\mathcal{V} = \mathcal{V}_p \cup \mathcal{V}_m$ is the union of the set of vertices representing poses, $\mathcal{V}_p = [N_p]$, and the set of vertices representing landmarks, $\mathcal{V}_m = [N_m]$. We assume that the edge set $\tilde{\mathcal{E}} \subset \mathcal{V} \times \mathcal{V}$ is partitioned as $\tilde{\mathcal{E}} = \tilde{\mathcal{E}}_p \cup \tilde{\mathcal{E}}_m$, where $\tilde{\mathcal{E}}_p \subset \mathcal{V}_p \times \mathcal{V}_p$ represents relative-pose measurements and $\tilde{\mathcal{E}}_m \subset \mathcal{V}_p \times \mathcal{V}_m$ represents measurements of a landmark from a given pose. We assume that each edge, $(i, j) \in \tilde{\mathcal{E}}$, is associated with an error term, $e_{ij}$, and a matrix weight, $W_{ij}$. The $i^{th}$ pose is a member of the Special Euclidean group:

$$ \mathrm{SE}(3) = \{(C_i, t_i) : C_i \in \mathrm{SO}(3), t_i \in \mathbb{R}^3\}. \quad (4) $$

$C_i$ represents the rotation from the world frame to the $i^{th}$ pose frame and $t_i$ represents the translation vector from the world frame to the pose frame, expressed in the pose frame. Moreover, we will make use of the following homogeneous transformation to represent a given robot pose:

$$ T_i = \begin{bmatrix} C_i & -t_i \\ 0 & 1 \end{bmatrix}. \quad (5) $$

Here, we consider MAP optimization problems over pose and landmark variables in which the cost (log-posterior) can be expressed in the following factored form [8]:

$$ -\log(p(x|\mathcal{D})) = \sum_{(i,j) \in \tilde{\mathcal{E}}_p} J_{ij}^p + \sum_{(i,k) \in \tilde{\mathcal{E}}_m} J_{ik}^m, \quad (6) $$

$$ J_{ij}^p = e_{ij}^T W_{ij} e_{ij}, \quad J_{ik}^m = e_{ik}^T W_{ik} e_{ik}, $$

where $J_{ij}^p$ and $J_{ik}^m$ are the cost 'factors', with error terms, $e_{ij}$ and $e_{ik}$, that depend on the problem variables and weighting matrices, $W_{ij}$ and $W_{ik}$. The exact form of these terms are discussed in the next two sections. Though our formulation here is matrix-weighted in general, we focus on anisotropic noise for pose-landmark measurements, with relative-pose measurements remaining isotropic.

#### 1) Matrix-Weighted Pose-Landmark Measurements

Each edge, $(i, k) \in \tilde{\mathcal{E}}_m$, represents a measurement of the $k^{th}$ landmark from the $i^{th}$ pose. In robotics, the sensors that provide measurements of landmarks can often be modeled as

$$ d_{ik} = g(C_i m_0^{k0} - t_i) + \epsilon_{d,ik}, \quad \epsilon_{d,ik} \in \mathcal{N}(0, \Sigma_{d,ik}), \quad (7) $$

where $d_{ik}$ represents the (raw) measurement of landmark $k$ from the $i^{th}$ pose variable, $m_0^{k0}$ is the location of landmark $k$ in the global frame, $g(\cdot)$ is an invertible, smooth, non-linear function, and $\epsilon_{d,ik}$ is a zero-mean error term having normal distribution with associated covariance matrix, $\Sigma_{d,ik}$.

It is often desirable to convert measurements $d_{ik}$ to a more convenient form by inverting the measurement model (if possible) and defining the pseudo-measurement, $\tilde{m}_i^{ki} = g^{-1}(d_{ik})$, with approximate mean given by

$$ \mathbb{E}(\tilde{m}_i^{ki}) \approx C_i m_0^{k0} - t_i. $$

The deviation from the mean, given by

$$ e_{ik} = \tilde{m}_i^{ki} - (C_i m_0^{k0} - t_i), \quad (8) $$

is approximately Gaussian with zero mean and is exactly the error term for this measurement factor. Regardless of whether $\Sigma_{d,ik}$ represents isotropic noise, the (linearly-transformed) covariance of $\epsilon_{ik}$ is typically anisotropic and is given by

$$ \Sigma_{ik} = G^T \Sigma_{d,ik} G, \quad (9) $$

where $G$ is the Jacobian of the inverse measurement function $g^{-1}(\cdot)$ [46]. The weighting matrix in the cost factor in the MAP estimation, (6), is then given by the inverse of the covariance matrix, $W_{ik} = \Sigma_{ik}^{-1}$. Note that solving the problem under the simplifying assumption of isotropic noise – that is, $W_{ik} = \sigma_{ik}I$ with $\sigma_k \in \mathbb{R}$ – can be extremely detrimental to the quality of the final solution [45], [46].

A key example of such a situation arises in a common preprocessing step of stereo-vision problems, in which pixel and disparity measurements are converted to Euclidean point measurements by inverting a known camera model. It has been shown that this results in measurement uncertainty that is much larger in the depth direction of a given camera pose [7], [46] (c.f. Figure 1). A derivation of the inverse stereo measurement model and associated covariance is provided in Appendix E.

#### 2) Relative-Pose Measurements

Each edge, $(i, j) \in \tilde{\mathcal{E}}_p$, represents a relative-pose measurement, $(\tilde{C}_{ij}, \tilde{t}_{ij}^j) \in \mathrm{SE}(3)$ and its associated homogeneous transformation, $\tilde{T}_{ij}$. In a robotics context, these relative-pose measurements often represent IMU-based measurement information, dynamics-based prior belief propagation, or aggregate measurement data between keyframes. Similarly to [12], we define the following relative-pose error term:

$$ e_{ij} = \mathrm{vec}(\tilde{T}_{ij} T_j - T_i). \quad (10) $$

Though $W_{ij} \succeq 0$ can be an arbitrary, positive semidefinite matrix, we select weight matrices of the form $W_{ij} = \mathrm{diag}(\sigma_{ij}^2, \sigma_{ij}^2, \sigma_{ij}^2, \tau_{ij}^2)^{-1} \otimes I$, since it allows us to express our cost factor as

$$ \begin{aligned} J_{ij} &= \frac{1}{\sigma_{ij}^2} \| \tilde{C}_{ij} C_{j0} - C_i \|_F^2 + \frac{1}{\tau_{ij}^2} \| \tilde{t}_i^{ji} - \tilde{C}_{ij} t_j + t_i \|_2^2 \\ &= e_{ij}^T W_{ij} e_{ij}. \end{aligned} \quad (11) $$

where $\sigma_{ij}$ and $\tau_{ij}$ are scalar weights.<fnref n="5" /> This form of cost factor is similar to the cost used in pose-graph optimization problems (c.f. [12], [35], [54]) and is quadratic for our choice of pose variables, (4).<fnref n="6" />

<footnote n="5">⁵Note that these weights represent isotropic noise, although representing other distributions may be possible.</footnote>

<footnote n="6">⁶The choice to represent translation variables in the local frame ($t_i$) rather in the world frame ($t_0^{k0}$) allows us to keep the cost function quadratic in the variables.</footnote>
The relative-pose error formulation also allows for the addition of prior information about the pose variables to (6). In this case, the relative-pose error is defined with respect to the world frame:

$$e_{0j} = \mathrm{vec}(\tilde{T}_{0j}T_j - I). \qquad (12)$$

### D. Convex Relaxations of QCQPs

We review the well-known procedure for deriving convex, SDP relaxations of a standard form of polynomial optimization problem.<fnref n="7" /> This procedure was pioneered in [57] and has become the cornerstone of certifiably correct methods in robotics and computer vision [15], [55]. Here, we consider a homogeneous, quadratically constrained quadratic problem (QCQP) expressed in the following standard form:

$$\min_{\mathbf{z}} \{z^T \mathbf{Q} \mathbf{z} \mid z^T \mathbf{A}_i \mathbf{z} = 0, \forall i \in [N_c], z^T \mathbf{A}_0 \mathbf{z} = 1\}, \quad (13)$$

where $\mathbf{z} \in \mathbb{R}^n$ is the homogeneous optimization variable, $\mathbf{Q} \in \mathbb{R}^{n \times n}$ represents the quadratic cost, the $\mathbf{A}_i$ correspond to $N_c$ quadratic constraints, and $\mathbf{A}_0$ corresponds to the so-called homogenizing constraint [21]. Note that any problem with quadratic cost and constraints can be converted into a problem of this form [21]. This includes cost functions of the form given in (6), as long as the error terms are linear in the optimization variables and the constraints are quadratic.

In general, problems with quadratic equality constraints, such as QCQPs, are difficult to solve optimally because they are non-convex [11]. However, a popular approach to obtaining globally optimal solutions to QCQPs involves formulating the convex, SDP relaxation of the QCQP and subsequently showing that this relaxation is tight. In this context, ‘tight’ means that the relaxation and the original problem have the same optimal cost and equivalent minimizers.

It can be shown that Problem (13) is equivalent to the following problem [21]:

$$\begin{array}{ll}
\min_{\mathbf{X}} & \langle \mathbf{Q}, \mathbf{X} \rangle \\
\text{s.t.} & \langle \mathbf{A}_i, \mathbf{X} \rangle = 0, \quad \forall i \in [N_c] \\
& \langle \mathbf{A}_0, \mathbf{X} \rangle = 1, \\
& \mathbf{X} \succeq \mathbf{0}, \\
& \mathrm{rank}(\mathbf{X}) = 1,
\end{array}
\qquad (14)$$

where the last two constraints implicitly enforce the fact that $\mathbf{X} = \mathbf{x}\mathbf{x}^T$. In Problem (14), we see that all the non-convexity of the problem has been relegated to a single non-convex constraint on the rank. It follows that we can find a convex relaxation of Problem (14) by removing the rank constraint:

$$\begin{array}{ll}
\min_{\mathbf{Z}} & \langle \mathbf{Q}, \mathbf{Z} \rangle \\
\text{s.t.} & \langle \mathbf{A}_i, \mathbf{Z} \rangle = 0, \quad \forall i = [N_c] \\
& \langle \mathbf{A}_0, \mathbf{Z} \rangle = 1, \\
& \mathbf{Z} \succeq \mathbf{0}.
\end{array}
\qquad (15)$$

It is well known that if the optimal solution $\mathbf{Z}^*$ satisfies $\mathrm{rank}(\mathbf{Z}^*) = 1$ then the convex relaxation is tight to the original QCQP. Accordingly, this condition implies that the

solution can be factorized as $\mathbf{Z}^* = \mathbf{z}^*\mathbf{z}^{*T}$, where $\mathbf{z}^*$ is the globally optimal solution of Problem (13).

We note that Problems (15) and (14) have the same Lagrangian dual problem, which plays a central role in certifying global optima and can be expressed as follows:

$$\begin{array}{ll}
d^* = \max_{\mathbf{H}, \lambda, \rho} & -\rho \\
\text{s.t.} & \mathbf{H}(\lambda, \rho) = \mathbf{Q} + \rho \mathbf{A}_0 + \sum_{i=1}^{N_c} \lambda_i \mathbf{A}_i, \quad (16) \\
& \mathbf{H}(\lambda, \rho) \succeq \mathbf{0},
\end{array}$$

where $\rho$ and $\lambda = [\lambda_1 \cdots \lambda_{N_c}]^T$ are the Lagrange multipliers corresponding to the single homogenizing and the $N_c$ quadratic constraints, respectively.

Since Problem (15) is convex, it has the same optimal cost as its dual, Problem (16).<fnref n="8" /> When the relaxation is tight, the optimal cost of the original QCQP, Problem (13), will also be equal to the dual problem cost, a condition known as strong duality.

The complementary slackness conditions of (15) and its dual, (16), provide the following relationship between the rank of the primal and dual solution matrices,

$$\mathrm{rank}(\mathbf{Z}^*) \le n - \mathrm{rank}(\mathbf{H}^*) = \mathrm{corank}(\mathbf{H}^*), \quad (17)$$

where $\mathbf{H}^*$ represents the dual matrix at the optimal Lagrange multipliers $(\lambda^*, \rho^*)$.<fnref n="9" /> When (15) satisfies the so-called strict complementarity property, this inequality is binding (i.e., an equality). In the sequel, we will make use of this relationship to study the tightness of the relaxation by analyzing the dual matrix, $\mathbf{H}^*$, which is more directly connected to the problem data.

In practice, there are typically two ways that globally optimal solutions can be obtained for a QCQP with a tight semidefinite relaxation:

1) We can solve Problem (15) or Problem (16)<fnref n="10" /> directly and extract the globally optimal solution.

2) Given a candidate solution, $\hat{\mathbf{z}}$, found via fast optimization methods, we can certify its global optimality using the dual or certificate matrix, $\mathbf{H}$.

In robotics, there is a strong preference for the latter method, since it is often much more computationally efficient than the former. State-of-the-art methods leverage low-rank SDP techniques such as that of Burer and Monteiro (BM) [16] and the Riemannian Staircase [9], [54] to solve larger problem instances in real time.

However, these methods are far less performant when redundant constraints are introduced to tighten the semidefinite relaxation.<fnref n="11" /> As such, a key goal of this paper is to establish whether SDP relaxations of problems with matrix weights

<footnote n="7">⁷For the sake of brevity, our introduction of these problems and concepts in this section have been stated without proof. The interested reader is referred to [21], [63] and references therein for more extensive expositions.</footnote>

<footnote n="8">⁸This is a consequence of the fact that strong duality holds for convex problems under sufficient constraint qualifications [11]. In our case, we ensure that the SDPs that we solve satisfy the Linear Independent Constraint Qualification.</footnote>

<footnote n="9">⁹We replace $\mathbf{H}(\lambda, \rho)$ with $\mathbf{H}$ when it is clear from the context.</footnote>

<footnote n="10">¹⁰The dual problem is solved in [12] and is then used to recover the primal solution.</footnote>

<footnote n="11">¹¹For example, the BM approach requires a verification that the solution is a second-order critical point [10]. When redundant constraints are used, this verification step necessarily involves solving a different (non-low-rank) SDP, since they are not uniquely determined.</footnote>
require redundant constraints and, if so, whether it is nev-
ertheless possible to find global solutions efficiently enough
for robotics applications.

IV. PROBLEM FORMULATIONS

In this section, we provide QCQP formulations for the
matrix-weighted localization and SLAM problems that are
considered herein. We demonstrate how the costs can be
formulated in the standard quadratic form of Problem (13). In
the interest of brevity, we do not explicitly show the conversion
of the constraints into the standard form.

A. Matrix-Weighted Localization

In this section, we consider localization problems with ma-
trix weights. That is, we consider the problem of determining
the estimate of a sequence of poses given matrix-weighted
measurements of a set of *known* landmarks. We assume that
point correspondences are known and correct.

The maximum-likelihood estimate of the poses is given
by the solution to the following least-squares optimization
problem:

$$
\begin{align}
\min_{C_i, t_i, \forall i \in \mathcal{V}_p} & \sum_{(i,k) \in \bar{\mathcal{E}}_m} e_{ik}^T \mathbf{W}_{ik} e_{ik} \nonumber \\
\text{s.t.} \quad & C_i^T C_i = \mathbf{I}, \quad \forall i = \mathcal{V}_p, \tag{18} \\
& (c_i^1)^\times c_i^2 - c_i^3 = \mathbf{0}, \quad \forall i = \mathcal{V}_p, \nonumber \\
\text{where} \quad & e_{ik} = \tilde{m}_i^{ki} - C_i m_0^{k0} + t_i, \nonumber
\end{align}
$$

where the variable definitions are consistent with those pro-
vided in Section III-B and $c_i^j$ denotes the $j^{th}$ column of matrix
$C_i$. It has been shown that the two constraints on the $C_i$ in (18)
are sufficient to ensure that $C_i \in \text{SO}(3)$ and can be expressed
in the standard quadratic form of (13) [61]. Therefore, this
problem is a QCQP since $e_{ik}$ is linear in $C_i$ and $t_i$, and the
constraints are quadratic.

It is important to note that this problem is separable in each
set of pose variables, since there are no constraints or cost
elements linking any two poses. Therefore, the problem may
be divided into $N_p$ subproblems, each being equivalent to the
matrix-weighted version of Wahba's problem [20], [64].<fnref n="12" /> In
the scalar-weighted case ($W_{ik} = w_{ik}I$), there is a closed-
form, global solution to Wahba's problem [37]. However, for
the general matrix-weighted case, a key simplification of the
cost function is no longer possible and solutions must be found
iteratively with no guarantee of global optimality [7], [8], [22].

We collect the relevant optimization variables into a single
vector, $\mathbf{x}_i^T = [c_i^T \quad t_i^T \quad w]$, where $c_i = \text{vec}(C_i)$, which
allows us to re-express the cost element as

$$
J_{ik} = \mathbf{x}_i^T \mathbf{Q}_{ik} \mathbf{x}_i, \qquad (19)
$$

where the symmetric cost matrix, $Q_{ik}$, is given in Appendix D.
We have also introduced a so-called *homogenizing variable*, $w$,
which is subject to the *homogenizing constraint*, $w^2 = 1$ and
facilitates the reformulation of Problem (18) into the standard
form of Problem (13) [21], [66].

The full cost of Problem (18) can be constructed by per-
muting and summing the matrices, $Q_{ik}$, according to edges in
$\bar{\mathcal{E}}_m$ and a given pose variable ordering:

$$
z^T = [c_1^T \ t_1^T \ \cdots \ c_{N_p}^T \ t_{N_p}^T \ w] . \quad (20)
$$

In practice, it is more efficient to leverage the separability
of the problem and solve for each pose via separate instances
of Wahba's problem. Since each separate problem is small
in dimension (13-by-13), its SDP relaxation can be solved
quickly using modern interior-point solvers (see Section VII-A
for runtimes with this approach).

The single-pose version of Problem (18) admits a convex
semidefinite relaxation that was also explored in [13] and [49]
with the motivation of representing different geometric prim-
itive measurements (lines and planes), rather than anisotropic
noise. It was found that the addition of a few key redundant
constraints made the relaxation tight even when noise levels
were high. We corroborate these results for anisotropic noise
with an extensive analysis in Section VI.

We could potentially introduce the *relative-pose measurements* described in Section III-C2 to Problem (18) by adding the associated cost factors to the objective. The cost function given in (11) is a quadratic function in the optimization variables and can be expressed in the standard homogeneous QCQP form, as shown in Appendix D. However, the addition of such factors to Problem (18) destroys the separability property, meaning that directly solving the SDP relaxation becomes much slower for reasonably sized problems (e.g., $N_p \ge 20$).

In Appendix F, we prove that the SDP relaxation of Problem
(18) is always tight when measurement noise levels are low
and the weighting matrices are non-degenerate.

B. Matrix-Weighted Landmark-based SLAM

In this section, we explore the effect of matrix weighting
when the landmark locations, $\mathbf{m}_0^{k0}$, are not known *a priori*.
The resulting problem, known as landmark-based SLAM, is
given by,

$$
\begin{align}
\min_{C_i, t_i, m_0^{k0} \atop \forall i \in \mathcal{V}_p, \forall k \in \mathcal{V}_m} & \sum_{(i,k) \in \bar{\mathcal{E}}_m} e_{ik}^T \mathbf{W}_{ik} e_{ik} + \sum_{(i,j) \in \bar{\mathcal{E}}_p} e_{ij}^T \mathbf{W}_{ij} e_{ij} \nonumber \\
\text{where} & \quad e_{ik} = \tilde{m}_i^{ki} - C_{i0} m_0^{k0} + t_i^{i0}, \tag{21} \\
& \quad e_{ij} = \text{vec}(\tilde{T}_{ij} T_{j0} - T_{i0}), \nonumber \\
\text{s.t.} & \quad C_i^T C_i = \mathbf{I}, \quad \forall i \in \mathcal{V}_p. \nonumber \\
& \quad (c_i^1)^\times c_i^2 - c_i^3 = \mathbf{0}, \quad \forall i = \mathcal{V}_p, \nonumber
\end{align}
$$

Note that the cost of (21) includes both landmark measure-
ments and relative-pose measurements. In the scalar-weighted
context, the convex relaxation of an equivalent problem has
already been studied and was generally found to be tight
for noise levels well above those found in practical robotics
scenarios [35].

On the other hand, when matrix weights are used (i.e.,
the noise distribution is anisotropic), the landmark-based error
term (8) becomes quadratic in the optimization variables (due
to the $C_i m_0^{k0}$ terms) and the cost function of Problem (21)
becomes quartic. In the scalar-weighted case ($W_{ik} = w_{ik}I$),

<footnote n="12">¹²In the remainder of this paper, we refer to single-pose instances of Problem 18 as Wahba's problem, though it is also known as registration or single-pose localization.</footnote>
this issue is obviated by premultiplying the error terms by the inverse pose rotations to regain linearity of the error terms. However, preforming this operation in the matrix-weighted case will change the cost function, since $C_i^T W_{ik} C_i \neq W_{ik}$ when $W_{ik}$ is an arbitrary positive definite matrix.<fnref n="13" />

In order to cast Problem (21) as a QCQP, we follow a similar strategy to [27] and [14] by introducing substitution variables, $m_i^{ki}$, corresponding to each available measurement, $\tilde{m}_i^{ki}$. These new variables must satisfy the following (quadratic) constraints:

$$m_i^{ki} = C_i m_0^{k0} - t_i, \quad \forall (i, k) \in \tilde{\mathcal{E}}_m. \qquad (22)$$

When the landmarks become unknown, a gauge freedom is also introduced into the problem. This has an important implication for the SDP solution; the well-known SLAM gauge freedom results in solution symmetries that cause the rank of the SDP solution to be higher than one, even when it is numerically tight [14]. To fix this freedom, we assume that a prior factor term is included in the pose-graph terms for at least one pose.<fnref n="14" />

The optimization can now be written as

$$ \begin{array}{ll} \min_{\substack{C_i, t_i, \\ m_0^{k0}, m_i^{ki}}} & \sum_{(i,k) \in \tilde{\mathcal{E}}_m} e_{ik}^T W_{ik} e_{ik} + \sum_{(i,j) \in \tilde{\mathcal{E}}_p} e_{ij}^T W_{ij} e_{ij} \\ \forall i \in \mathcal{V}_p, \forall k \in \mathcal{V}_m & \text{where} \\ & e_{ik} = \tilde{m}_i^{ki} - m_i^{ki}, \\ & e_{ij} = \text{vec}(\tilde{T}_{ij} T_j - T_i), \\ \text{s.t.} & C_i^T C_i = I, \forall i \in \mathcal{V}_p. \\ & (c_i^1)^T c_i^2 - c_i^3 = 0, \forall i = \mathcal{V}_p, \\ & m_i^{ki} = C_i m_0^{k0} - t_i, \forall (i, k) \in \tilde{\mathcal{E}}_m. \end{array} \qquad (23) $$

The pose-landmark cost elements can now be written in the standard form:

$$J_{ik} = \begin{bmatrix} m_i^{ki} \\ w \end{bmatrix}^T \begin{bmatrix} W_{ik} & -W_{ik}\tilde{m}_i^{ki} \\ -\tilde{m}_i^{ki^T} W_{ik} & \tilde{m}_i^{ki^T} W_{ik} \tilde{m}_i^{ki} \end{bmatrix} \begin{bmatrix} m_i^{ki} \\ w \end{bmatrix}$$

The cost elements can be then permuted and summed according to our variable ordering,

$$z^T = \begin{bmatrix} \text{vec}(C_{10})^T & t_1^{10^T} & \dots & \text{vec}(C_{N_p 0})^T & t_{N_p 0^T} \\ & m_0^{k0^T} & \dots & m_i^{ki^T} & \dots & w \end{bmatrix}, \quad (24)$$

and we can apply the semidefinite relaxation described in III-D. However, we have observed that, contrary to the scalar case, this SDP relaxation is not tight even for low levels of noise. A similar situation occurred in [14] and [70] when substitution variables were introduced (though not when introduced in [27]).

### C. Tightening the Relaxations

One of the key contributions of this paper is a concise set of redundant constraints for the problems presented above that is capable of tightening their respective relaxations. In the next sections, we empirically show that these constraints are capable of restoring relaxation tightness that is otherwise destroyed by the introduction of anisotropic noise.

One approach that can be used to tighten this relaxation is to apply the Lasserre-moment hierarchy [34], [42]. However, this method often introduces a prohibitive number of additional variables and constraints to the problem [69].

In contrast, we have opted to discover a smaller set<fnref n="15" /> of redundant constraints that is sufficient to render each of the problems above tight for reasonable levels of noise. To do this, we leverage a constraint-learning concept from our concurrent paper, which, for a given problem instance, uses samples that are drawn from the feasible set to numerically find all possible constraints via a nullspace argument [26]. Since it involves finding the nullspace of a large data matrix, this constraint-learning process can be computationally intensive and it is not feasible to repeat it for each new problem instance.

Therefore, weapplied the constraint-learning method to small instances of the problems defined above and interpreted the resulting numerical, learned constraints. We then cast them as equations that are commensurate with the properties of SO(3) and could be extended to problems of any size (any number of landmarks and poses).

For example, for Problem (23), we randomly initialized a problem with three poses and seven landmarks and drew samples from the feasible set. Using these samples, the methodology in [26] allowed us to numerically find a set of viable constraint matrices for this problem. An example of one such constraint matrix is shown in Figure 2. The constraint matrix corresponds to the equation $z^T A z = 0$, where $A$ is the matrix and $z$ is a subset of variables affected by this constraint. In this case,

$$z^T = \begin{bmatrix} w & \text{vec}(C_1)^T & m_0^{20^T} & m_0^{40^T} & m_1^{21^T} & m_1^{41^T} \end{bmatrix}.$$

The constraint can be interpreted as the following polynomial equation:

$$ \begin{aligned} & -w[m_1^{21}]_3 + w[m_1^{41}]_3 + [\text{vec}(C_1)]_3[m_0^{20}]_1 \\ & + [\text{vec}(C_1)]_6[m_0^{20}]_2 + [\text{vec}(C_1)]_9[m_0^{20}]_3 \\ & - [\text{vec}(C_1)]_3[m_0^{40}]_1 - [\text{vec}(C_1)]_6[m_0^{40}]_2 \\ & - [\text{vec}(C_1)]_9[m_0^{40}]_3 = 0, \end{aligned} $$

where $[v]_i$ denotes the $i^{th}$ element of vector $v$. Further study of this equation in light of the properties of SO(3) reveals that it can be generalized as the third vector component of the following constraint equation:

$$C_i(m_0^{l0} - m_0^{k0}) - (m_i^{li} - m_i^{ki}) = 0,$$

which describes distances between landmarks in the world and $i^{th}$ frames. The analytical version of this constraint can be applied to other problem instances (i.e., with different numbers

<footnote n="15">¹⁵Smaller in the sense that we do not include all possible constraints or further ascend Lasserre's hierarchy.</footnote>

<footnote n="13">¹³This reflects the fact that the measurement noise has an orientation and depends on the (unknown) observation frame. This would not be an issue if all measurements were defined in a common frame, but, in practice, measurements are typically taken in the robot's frame of reference.</footnote>

<footnote n="14">¹⁴As with SLAM problems in general, the pose variable in the prior, $\tilde{T}_{j0}$, is typically used to 'lock' the solution to a known pose using some exteroceptive measurement, such as GPS. However, for our purposes it can be set arbitrarily without loss of generality.</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 2. Redundant constraint matrix, A, found numerically via the method in [26], with colours indicating the relative values of the matrix (teal: 0, yellow: 1, purple: -1). The constraint matrix corresponds to the equation $x^T A x = 0$, where $x$ is a subset of variables affected by this constraint.</figcaption></figure>

of landmarks and poses). Since it was found to be effective in tightening other instances of Problem (23), we added it to our list of effective constraints, (26). Proceeding in this manner, we selected a set of constraints that were empirically found to lead to tightness across various problem instances.

The concept of SDP stability [21] provides some guarantees that similar problems to the ones that we study will also be tight with these constraints, though we cannot a priori claim that any instance of Problem (23) will be tight.

We found that the following set of redundant constraints could be used to tighten Problem (18):

$$C_i C_i^T - I = 0, \qquad (25a)$$

$$(C_i^j)^T C_i^k - C_i^l = 0, \forall (j, k, l) \in \{(2, 3, 1), (3, 1, 2)\},$$

$$C_i^{j^T} C_i^j - r_i^{k^T} r_i^k = 0, \forall j, k \in \{1, 2, 3\}, \qquad (25b)$$

$$C_i^{j^T} C_i^j - r_i^{k^T} r_i^k = 0, \forall j, k \in \{1, 2, 3\}, \qquad (25c)$$

where $c_i^j$ and $r_i^k$ represent the $j^{th}$ column and $k^{th}$ row of $C_i$, respectively, and cyclic() denotes the cyclic group. Note that the first two redundant constraints were also necessary in [13] and [66].

Moreover, our SLAM problem (Problem (23)) can be tightened by using (25) in conjunction with the following constraints:

$$m_0^{l_0^T} m_0^{k_0} - (m_i^{l_i} - t_i)^T (m_i^{k_i} - t_i) = 0, \qquad (26a)$$

$$C_i (m_0^{l_0} - m_0^{k_0}) - (m_i^{l_i} - m_i^{k_i}) = 0, \qquad (26b)$$

$$(m_0^{l_0} - m_0^{k_0}) - C_i^T (m_i^{l_i} - m_i^{k_i}) = 0, \qquad (26c)$$

$$\|m_i^{l_i} - m_i^{k_i}\|^2 - \|m_j^{l_j} - m_j^{k_j}\|^2 = 0, \qquad (26d)$$

$$(m_0^{l_0} - m_0^{k_0})^T C_i - C_i (m_0^{l_0} - m_0^{k_0})^T = 0, \qquad (26e)$$

$$t_i - \left( \frac{1}{N_i} \sum_{i=1}^{N_i} C_i m_0^{k_0} - m_i^{k_i} \right) = 0, \qquad (26f)$$

$$\forall i, j, l, k \text{ s.t. } \{(i, l), (i, k), (j, l), (j, k)\} \subset \bar{\mathcal{E}}_m.$$

Despite the fact that these constraints successfully tighten our problems for practical noise levels, the presence of redundant constraints prohibits the use of fast certification methods as mentioned above, meaning that interior-point methods must be used to certify or solve the relaxation. Therefore, it is very important to characterize the noise regime for which the convex relaxation of Problem (21) is still tight, which is the subject of Section VI-A.

Due to the large number of variables and constraints that must be introduced, the problem sizes that can be solved using this method are still small. This may be mitigated by exploiting the sparsity of the problem, but this remains as future work. One notable exception is the localization problem without relative-pose measurements, which is still reasonably tractable due to the separability of the problem.

## V. ESTIMATION UNCERTAINTY AND THE DUAL CERTIFICATE

In this section, we present a set of theoretical results that allows us to extend intuitions about uncertainty in state estimation to tightness of SDP relaxations. We establish a connection between the dual solution of the SDP relaxation (specifically the certificate matrix, $H$) and the Fisher Information Matrix.

The following Lemma relates the certificate matrix to the Laplace approximation of an equivalent (local) unconstrained problem of the form (2). Throughout this section, we will assume that we have access to the global minimum and explore properties of its associated certificate matrix.

**Lemma 1.** Suppose a given MAP estimation problem can be equivalently formulated as either a standard-form, homogenized QCQP (13) or as an unconstrained optimization as in (2). Let $z^*$ and $x^*$ be the (global) optima of these formulations, respectively. Given a neighborhood, $U \subseteq \mathbb{R}^p$, containing $x^*$, let $\ell: \mathbb{R}^p \to \mathbb{R}^n$ be a smooth, injective map on $U$, such that its image is in the feasible set of (13).<fnref n="16" /> Moreover, let the respective objective functions be equal under the map in the neighborhood $U$. That is,

$$-\log(p(x|\mathcal{D})) = z^T Q z, \qquad (27)$$

for all $(x, z)$ such that $z = \ell(x)$ and $x \in U$. Then the FIM of $p(x|\mathcal{D})$ is given by

$$\Sigma^{-1} = L^T H L, \qquad (28)$$

where $H$ is the certificate matrix at the solution and $L$ is the Jacobian of $\ell(x)$ at the solution, $L = \frac{d}{dx}\ell(x)|_{x=x^*}$.

We defer the proof of this Lemma to Appendix A. Intuitively, the Lemma uses the fact that the local curvature of the two problems are equal on the feasible set to establish a connection between the FIM and the certificate matrix.

<footnote n="16">¹⁶The existence of the mapping, $\ell$, is not restrictive in our context. In fact, since the feasible set of the problems in this paper are smooth manifolds, this mapping can be interpreted as the inverse coordinate chart from differential geometry.</footnote>
One implication of this Lemma is that the certificate matrix can be interpreted as an information matrix in the higher-dimensional, SDP space. It also provides a method to extract posterior covariance matrices from a given SDP solution.

## A. Application to Localization

To make the Lemma in the preceding section more concrete, we show its application to Problem (18). The equivalent unconstrained parameterization of this problem is the Lie-algebra vector space parameterization (see [6], [24] for more details on Lie groups in robotics).

For the $i^{th}$ pose variable in Problem (18), let $x_i = [x_{i,r}^T \ x_{i,t}^T]^T \in \mathbb{R}^3 \times \mathbb{R}^3$ be the associated variable for the unconstrained problem. We have the following relationships:

$$C_i = \exp(x_{i,r}^{\wedge}) \bar{C}_i, \quad t_i = x_{i,t} + \bar{t}_i, \quad (29)$$

where $\bar{C}_i$ and $\bar{t}_i$ are the optimal values of $C_i$ and $t_i$, respectively, for a given problem instance, $\exp$ is the matrix exponential and $\wedge$ is the skew-symmetric operator (see [6] for details). We can rewrite Problem (18) as the unconstrained problem,

$$\begin{align}
\min_{x_i \in \mathbb{R}^6} \quad & \sum_{(i,k) \in \mathcal{E}_m} e_{ik}^T W_{ik} e_{ik} \nonumber \\
\text{where} \quad & e_{ik} = \tilde{m}_i^{ki} - \exp(x_{i,r}^{\wedge}) \bar{C}_i m_0^{k0} + x_{i,t} + \bar{t}_i, \tag{30}
\end{align}$$

From (20), we see that the solution mapping is given by

$$z^T = [z_1^T \ \dots \ z_n^T \ 1] \quad (31)$$

$$z_i = \ell(x_i) = \begin{bmatrix} \text{vec}(\exp(x_{i,r}^{\wedge})\bar{C}_i) \\ x_{i,t} + \bar{t}_i \end{bmatrix}. \quad (32)$$

By construction, this map is bijective with its image in a neighborhood around $(\bar{C}_i, \bar{t}_i)$ and the cost functions of Problems (18) and (30) are equal under the map. Finally, the map is smooth by the properties of Lie groups and its Jacobian is given by

$$L = \text{diag}(L_1, \dots, L_n, 0), \quad L_i = \begin{bmatrix} (I \otimes \bar{C}_i) \bar{G}_d & 0 \\ 0 & I \end{bmatrix}, \quad (33)$$

where $\bar{G}_d$ is a matrix of the vectorized generators of the SO(3) Lie algebra (see equations 7-10 of [24] for a detailed derivation of the top-left block) and diag() denotes block-diagonal concatenation. In this example, it can be shown that the minimum singular value of $L$ is exactly unity and, if applying Lemma 1, we have that the minimum eigenvalue of the parameterized FIM exactly upper bounds the minimum eigenvalue of the certificate matrix.

We visualize Lemma 1 on a two-pose localization problem with stereo measurements by comparing the numerical covariance matrix (obtained by running 10,000 optimization trials with different sampled noise values) with the theoretical covariance matrix (inverse of FIM of the final sample). The matrices match to numerical precision and can be seen in Figure 3.

<figure><img src="image_4.png" /><figcaption>Fig. 3. Comparison of numerical covariance matrix to the theoretical covariance matrix from Lemma 1, $\Sigma = (L^T H L)^{-1}$, for a two-pose, stereo localization problem, Problem 18. Numerical covariance was found by considering the sample covariance of 10,000 (globally optimal) estimates. Note that the covariance matrix is fully coupled because we have included a relative-pose measurement.</figcaption></figure>

## B. Connection to Relaxation Tightness

In this section, we aim to connect posterior estimation uncertainty to tightness of the SDP relaxation. To this end, we present a Lemma that relates the minimum eigenvalues of the certificate matrix and the FIM.

**Proposition 2.** Assume that the setting of Lemma 1 applies. Without loss of generality, we assume that the homogenizing variable is the last element of $z$. Then the certificate matrix can be partitioned as,

$$H = \begin{bmatrix} \bar{H} & h \\ h^T & h \end{bmatrix},$$

and we have the following relation:

$$\sigma_{\min}(\bar{H}) \le \frac{\sigma_{\min}(\Sigma^{-1})}{s_{\min}(L)^2}, \quad (34)$$

where $\sigma_{\min}(\cdot)$ denotes the minimum eigenvalue and $s_{\min}(\cdot)$ denotes the smallest singular value.

The proof of this lemma is given in Appendix B. It is known that high posterior uncertainty along particular dimensions of state-estimation problems leads to degenerate (or nearly degenerate) FIMs [71]. If strict complementarity holds for the SDP,<fnref n="17" /> then as the FIM approaches degeneracy (i.e., its minimum eigenvalue approaches zero) it can be shown (c.f. Appendix C) that either the certificate matrix has a negative eigenvalue at the global solution or the primal SDP solution is not rank-one. In either case, the practical implication in such a scenario is that tightness of the SDP relaxation is lost (regardless of the presence of redundant constraints).

Our investigations in the next section imply that posterior uncertainty plays a role even when the FIM is approximately degenerate (i.e., scenarios with high or concentrated noise), but investigation of the theoretical mechanism explaining this phenomenon is still ongoing. For example, Figure 5 shows

<footnote n="17">¹⁷Strict complementarity is a property of SDPs that has been shown to hold generically [1]. However, this property has been shown to fail in highly structured SDPs, such as relaxations of some combinatorial problems [21], [58]. Nevertheless, we have empirically observed that this property holds for the problems we consider here.</footnote>
that the minimum eigenvalue of the FIM generally upper bounds the second eigenvalue of the certificate matrix across parameters for Wahba's problem.

We will see in next section that the tightness of the semidefinite relaxation degrades as the uncertainty of the optimal state estimate increases (i.e., as the FIM becomes more degenerate). In particular, the introduction of anisotropic noise can cause accentuated uncertainty in a particular direction and erode the tightness of the semidefinite relaxation.

# VI. SIMULATED EXPERIMENTS

It is known that the tightness of SDP relaxations of least-squares perception problems depends on the level of noise in the measurements [13], [21], [54]. For the case of rotational averaging, tightness of the relaxation has been linked to the magnitude of *residual uncertainty* of pose estimates [29].

In this section, we empirically explore the effect that introducing anisotropic noise and matrix weights have on localization and SLAM. Our study is strongly motivated by stereo-camera noise models, but not limited thereto. All of the results in this and the next section were generated using MOSEK's interior-point, SDP solver [2].

It is common in the literature report average rank when assessing tightness of the SDP relaxation, but we find that the eigenvalue ratio (ER) of the optimal solution – that is, the ratio between the first and second eigenvalues – is a more informative metric for tightness. Generally, $ER \ge 10^6$ is an appropriate indicator that an SDP relaxation is rank-one and therefore tight. In the subsequent analysis, we use this metric as the main criterion for tightness.

As mentioned above, when measurements are based on a stereo-camera model, the uncertainty ellipsoids in Euclidean space become elongated. This elongation occurs along rays extending from the camera focal point and depends quadratically on the distance to the measured point. To capture this elongation, we define the *anisotropicity* of a measurement as the square root of the conditioning number of the noise covariance matrix (i.e., square root of ratio of maximum to minimum eigenvalues). Figure 4(a) presents a visualization of the shape of an uncertainty ellipsoid as anisotropicity changes.

All of the studies in this section have the same general format. Landmark locations were randomly generated from a uniform distribution within a bounding cube of a given size and distance relative to the pose/camera frame. Unless otherwise specified, the default values for distance and bounding cube length are 3 m and 1 m, respectively.

We searched for sets of parameters that have tight relaxations through a variety of analyses. For each analysis, the parameter space was sampled using a 30-by-30 grid in logspace across the ranges shown in the figures. For each point in the parameter space, the SDP was solved for 100 random landmark and pose geometries generated using random seeds that were consistent across parameter points. For each point in parameter space, the minimum ER was found across all

trials and boundaries were plotted along the parameter values at which the minimum ER dropped below $1 \times 10^{-6}$.<fnref n="18" />

## A. Anisotropic Noise in Wahba's Problem

In this section, we directly control the level and alignment of measurement noise anisotropicity (i.e., uncertainty ellipsoid size and shape) and observe their effect on the tightness of Wahba's problem.

### 1) Aligned Uncertainty Ellipsoids

We first study the effect of anisotropic noise on Wahba's problem and consider the simplified case in which all of the error ellipsoids are aligned. The alignment of the ellipsoids causes the uncertainty to be concentrated in a single direction leading to high posterior uncertainty in that direction [71]. This case is of interest since our analysis in Section V suggests that there is a connection between high posterior uncertainty and loss of tightness.

Figure 4 shows the boundary between tight and non-tight SDP relaxations for this problem as the standard deviation (STD) of the noise, anisotropicity of the noise and number of landmarks are varied. The problem setup is shown in Figure 4(b), while Figure 4(a) demonstrates the effect of increasing anisotropicity on the uncertainty ellipsoid.

Figure 4 (c) and (d) show the parameter-space regions that have tight relaxations without and with (respectively) the redundant constraints in (25). For the case without redundant constraints, we see that when anisotropicity is close to one (i.e., nearly isotropic), the noise level that yields tight relaxations is high even for low numbers of landmarks. These results are consistent with previous results for isotropic noise [35]. Increasing the number of landmarks generally improves tightness for a given noise level. We also see that for any given level of noise, increasing anisotropicity eventually results in a loss of tightness.

We observe that the redundant constraints effectively make the problem tight across almost all parameters studied. The effect of the redundant constraints as well as the relationship between tightness, the certificate matrix and the FIM are explored in greater depth in Figure 5. This figure shows heatmaps for the 50 landmark case of Figure 4, without and with redundant constraints, across three metrics: the solution ER (subplots (a) and (b)), second smallest eigenvalue of the certificate matrix (subplots (c) and (d)), and the minimum eigenvalue of the FIM (subplots (e) and (f)). The FIM was calculated using (28) with the mapping $L$ as defined in Section V-A.

A number of key observations can be drawn. First, we note that the tightness boundary in (a) (magenta line)<fnref n="19" /> coincides almost exactly with the drop in the certificate matrix eigenvalue in (c), as expected.<fnref n="20" />

<footnote n="18">¹⁸Note that in reality, the raw contours are quite noisy. For convenience to the reader, we first smooth the minimum ER values with a median filter, then plot the contours. The smoothing method is reviewed Appendix H.</footnote>

<footnote n="19">¹⁹Note that this line is the same as the same boundary as shown in Figure 4(c).</footnote>

<footnote n="20">²⁰This actually demonstrates that strict complementarity mostly holds for this problem, since a drop in rank of the certificate is exactly complemented by an increase in rank of the solution matrix.</footnote>
<figure><img src="image_5.png" /><figcaption>Fig. 4. Investigation of the effect of anisotropicity on tightness of semidefinite relaxations for Wahba's problem. Subplot (a) shows how varying anisotropicity affects the uncertainty ellipsoid. Subplot (b) shows the problem setup, with ellipsoids aligned to the z-axis of the pose. Subplot (c) shows the tightness boundary for varying numbers of landmarks. Anisotropicity decreases the tightness boundary while increasing number of observed landmarks increases the boundary.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 5. Extended study of the results in Figure 4 for the 50 landmark case. Heatmaps represent the value of each metric specified in the left-most column, both without (left column) and with (right column) redundant constraints. The presence of the redundant constraints pushes the second certificate eigenvalue closer to the minimum eigenvalue of the FIM, leading to a relaxation that is tight over a larger region of the parameter space. Note that there is a significant heatmap scale difference between (a) and (b) and between (c) and (d).</figcaption></figure>

Second, by comparing (c), (d) and (f), we see that the introduction of redundant constraints increases the certificate eigenvalue to close to the FIM upper bound (though it does not quite attain it). It is interesting to note that the bound in Proposition 2 is quite loose without redundant constraints.

Finally, we note that the FIM changes very little when redundant constraints are introduced. This is expected, since the FIM for a given problem should not depend on the redundant constraints. We posit that slight differences that we observe between plots (e) and (f) are numerical in nature.

## 2) Misaligned Uncertainty Ellipsoids

We now consider the effect of varying the orientation of the uncertainty ellipsoids. In classical state estimation, it is known that high-quality estimates can still be obtained when measurements have high uncertainty in a particular direction,

as long as these directions are not aligned. Leveraging our insight from Section V, we expect to see a similar trend with the tightness of the SDP relaxation.

Figure 6 demonstrates two experiments that vary uncertainty ellipsoid orientation in the same setting as for Figure 4. Figure 6(a) shows results for random rotational perturbation of ellipsoid alignment while Figure 6(b) shows the effect of the ‘fanning out’ of ray-aligned ellipsoids as the size of the landmark bounding cube is increased.

In both cases, the trends coincide with our expectation: when the ellipsoids are *aligned*, uncertainty is concentrated on a single axis and tightness is lost at a lower noise level. On the other hand, when the ellipsoids are highly *misaligned*, uncertainty is not concentrated and the problems are tighter for much higher noise levels.

Another interesting trend that we observe is that in most cases the tightness boundary begins to increase along the noise-level axis as anisotropicity increases. Because the axes are misaligned, high uncertainty of one measurement along a given direction, can be complemented by low certainty of a different measurement along the same direction. We posit that this causes the posterior uncertainty (and hence tightness) to be dominated by ellipsoid minor-axis uncertainty, which decreases as anisotropicity increases.

We did not include the results for Figure 6 with redundant constraints because all of the studied parameter values were tight.

## B. Wahba's Problem with Stereo-Camera Noise Model

Leveraging our understanding from the previous section, we now turn our attention to Wahba's problem with measurements obtained from a less idealized, stereo-camera-based noise model. Both the measurement noise level and anisotropicity depend strongly on the distance between a pose and the observed landmarks (see (45) at the end of Appendix E). Additionally, posterior uncertainty in stereo-camera-based localization depends on the ‘spread’ of the landmarks being measured.

<figure><img src="image_7.png" /><figcaption>Fig. 6. Tightness boundary for Wahba's problem instance with 20 landmarks observed. In (a), axes of maximum uncertainty are aligned to pose z-axis then perturbed by random angles with different standard deviations (see legend). In (b), maximum uncertainty axis is aligned to pose-landmark ray and size of the bounding box on the landmarks is varied (size in legend). In both cases, increased variation of uncertainty ellipsoid axes yields tighter relaxations as anisotropicity increases. Additionally, in both cases, the entire parameter space was tight when redundant constraints were used.</figcaption></figure>
<figure><img src="image_8.png" /><figcaption>Fig. 7. Investigation of tightness boundaries for Wahba's problem with a stereo-camera model without and with redundant constraints. A diagram of experiment setup is shown in (a). (b) shows the effect of increasing the number of landmarks without redundant constraints. (c) shows the boundaries for the same model and scenario, but with redundant constraints included. Note that the sampled distance parameter ranges are different between (a) and (b).</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>Fig. 8. Investigation of tightness for a single-pose, stereo-camera, SLAM problem with and without redundant constraints (orange and blue, respectively). The dashed red line indicates the ER at which we consider a problem to be tight ($1 \times 10^6$). Each box represents 100 trials set up according to the diagram shown in Figure 7 (a), with 20 landmarks uniformly sampled from a 1 m bounding box centered at different distances from the pose (x-axis). The redundant constraints significantly improve the tightness (ER) across trials.</figcaption></figure>

As such, our experiments in this section explore tightness as a function of the distance to and size of the landmark bounding cube (see Figure 7(a) for problem setup). The measurements and their associated covariances are drawn from the stereo-camera model with parameters given in Table II.

Figure 7 (b) and (c) show the tightness boundaries for this setup without and with the redundant constraints given in (25). The figure shows results for Wahba's problem with different numbers of landmarks. As expected, the level of noise for which the relaxation is tight is inversely proportional to the average distance between pose and landmarks. On the other hand, increasing the spread (bounding cube) of the landmarks increases the tightness boundary, which is consistent with our previous observations for the ellipsoid model.

Note that for this camera model, Wahba's problem is not very tight without redundant constraints: the tightness boundary when 50 landmarks are constrained to a unit cube is a range of approximately 2 m for a 0.24 m baseline camera. In contrast, the problem becomes considerably tighter when the redundant constraints are employed, with the maximum range of 100 m. This indicates that redundant constraints are required for practical robotics applications. As we will see in Section VII-A, it is feasible to use redundant constraints in this problem, while still maintaining computational efficiency.

### C. SLAM with Stereo-Camera Noise Model

In Figure 8, we show the effect of applying redundant constraints to a simplified SLAM problem using the same simulation setup as in Figure 7, with 20 landmarks and the landmark bounding box set to 1 m. We see that the application of the redundant constraints causes a considerable increase in the tightness (ER) of the problem in many instances. As in the localization results, tightness is eroded as the landmarks move further from the robot pose. We also observed that tightness is proportional to the number of landmarks observed.

We note that although the redundant constraints are capable of tightening the problem, the runtime of these problems is prohibitive for real-time application. This is due to the size of the problem: for a single pose problem with 20 landmarks

the dimension of the SDP variable is 133 and the number of constraints required is 3343. Even for a small SLAM problem such as this, the SDP runtime is approximately five seconds and these numbers also increase quite rapidly as the number of poses increases.

## VII. REAL-WORLD EXPERIMENTS

### A. Outdoor Stereo Localization

In the preceding sections, we have mentioned the fact that since matrix-weighted Wahba's problem has a small number of variables and constraints (13 and 31, respectively), it is still feasible to use the SDP relaxation for post processing or even real-time applications. In this section, we apply our relaxation of matrix-weighted Wahba's problem (Problem (18)) in the stereo-localization pipeline introduced in [33], which uses a neural network to detect a set of features that are robust to seasonal and lighting conditions.

#### 1) Stereo-Localization Pipeline

The full pipeline can be seen in Figure 9(d). A neural network is used to detect features and provide descriptors for subsequent data association. The features are converted from 2D stereo keypoints to 3D keypoints, which are then used to estimate relative poses between keyframe stereo images in a stored map and corresponding stereo images from a 'live run'. In [33], the pose is estimated via random sample consensus followed by pose refinement with a scalar-weighted Singular Value Decomposition (SVD)<fnref n="21" /> approach.

#### 2) Modifications to Pose Estimation

We replace the pose refinement block with a matrix-weighted optimization (i.e., Wahba's problem). A stereo-camera model is used to compute the inverse covariances of the 3D keypoint measurements (see Section E for details), which are then used as the matrix weights. Since the pipeline does not make use of relative-pose measurements (i.e., IMU data), each pose can be solved separately.

We solved the matrix-weighted pose-refinement optimization with both a local solver and global solver. The local optimization was performed using a Gauss-Newton solver over the SE(3) Lie group in an off-the-shelf framework called

<footnote n="21">²¹See [62] for more details on this method.</footnote>
<figure><img src="image_10.png" /><figcaption>Fig. 9. Overview of the setup and results of the outdoor stereo localization experiment introduced in Section VII-A (map run: 2, live run: 16). (a) shows the estimated trajectory for all three methods and highlights the local minima that appear when using the Local method. (b) shows the robot (Grizzly RUV) and camera (Point Grey XB3) that were used. (c) shows an aerial view of the localization track and (d) shows the stereo-localization pipeline (with modified blocks in red).</figcaption></figure>

Theseus [53]. Tolerances (relative and absolute) were set to $1 \times 10^{-10}$ with 200 as the maximum number of iterations. The maximum number of iterations recorded was 160. Theseus was initialized using the best pose estimate from RANSAC, as is common in practice.

The problem was solved globally via the SDP relaxation of Problem (18) with the redundant constraints given in (25). The cost matrix was computed as shown in Section D. For each pose, we used CVXPY [25] with Mosek [2] to solve the SDP. The solution was extracted by selecting the column corresponding to the homogenizing variable in the solution matrix. The relevant interior-point tolerances for Mosek were also set to $1 \times 10^{-10}$ with the maximum iterations set to 1000 (though the number of iterations did not exceed 30).

All approaches were implemented in PyTorch. Note that the entire pipeline up to the pose refinement step (including RANSAC with SVD to find inliers) was identical for both solvers pipelines. On average, approximately 542 inlier 3D measurements were returned by RANSAC for the refinement step.

### 3) Dataset and Results

We tested the pipeline on runs 2, 11, 16, 17, 23, 28, and 35 from the 'In-the-Dark' dataset, which was used for training and testing in [33].<fnref n="22" /> The number of poses in each run varies, ranging between 886 and 3634, but the path taken in all runs is shown in Figure 9 (c).

We localize between all pairs of the seven runs (21 localizations total). Each run can be used as either the 'map' or the 'live' run since all are equipped with ground truth. Localization was performed for the Baseline (SVD), Local (Theseus), and

<table><caption>TABLE I
AGGREGATE RESULTS ACROSS RUNS FOR IN-THE-DARK DATASET</caption><thead><tr><th>Pipeline</th><th>Weights</th><th>Avg. Time Per Pose</th><th>Long. RMSE (m)</th><th>Lat. RMSE (m)</th><th>Head. RMSE (deg)</th></tr></thead><tbody><tr><td>Baseline</td><td>scalar</td><td>0.055</td><td>0.025</td><td>0.013</td><td>0.249</td></tr><tr><td>Global</td><td>matrix</td><td>0.121</td><td>0.033</td><td>0.021</td><td>0.335</td></tr><tr><td>Local</td><td>matrix</td><td>0.118</td><td>0.175</td><td>0.153</td><td>5.415</td></tr><tr><td>Local (filtered)</td><td>matrix</td><td>0.118</td><td>0.037</td><td>0.035</td><td>0.598</td></tr></tbody></table>

Global (SDP) solvers by running the pipeline with each on an NVIDIA Tesla V100 DGXS GPU with an Intel Xeon 2.20GHz CPU. We present the aggregate results of the analysis in terms of average time per pose, longitudinal root-mean-square error (RMSE), latitudinal RMSE, and heading RMSE for each of the pipelines in Table VII-A3.

We observe that, in terms of accuracy, the Local solver performs significantly worse than the Baseline and Global methods. Further investigation revealed that this was because, for several poses throughout the runs, the Local solver converged to egregious local minima. An example of such a minimum can be seen in Figure 9(a). As such, we have added a 'filtered' row to Table VII-A3, which provides the Local solver results when pose estimates with heading error larger than 3 deg are removed. Even when filtered, the Local solver still has the largest RMSE values, possibly due to less salient local minima.

It is interesting that the (scalar-weighted) Baseline approach outperforms the (matrix-weighted) approaches in terms of RMSE and compute time. However, we note that the 'ground truth' trajectories for each run were computed by minimizing a reprojection-based cost (see [52]) and are subject to additional error, which is likely on the order of the difference between the Baseline and Global solvers ($\le 0.005$ m and $\le 0.05$ deg). Further, the fact that the neural network was trained using the Baseline approach likely contributes to the performance difference.

Finally, we call attention to the fact that, in terms of speed, the Global SDP approach ran only about 2.3 times slower than the (closed-form) Baseline solution. Given the speed (8.19 Hz) and accuracy of the SDP solver, the authors argue that it could be used for online estimation, especially if implemented in a more performant language than Python (such as C++).

## B. Stereo SLAM in a Controlled Environment

In this section, we test matrix-weighted SLAM on the 'Starry Night' dataset [6], which provides stereo-camera measurements of a set of known landmark locations (with known data association). The parameters of the stereo-camera model are shown in Table II, with parameter symbols consistent with those described in Appendix E.

<footnote n="22">²²Dataset is available at http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/. The selected runs correspond to the hold-out test runs from [33].</footnote>
<figure><img src="image_11.png" /><figcaption>Fig. 10. ER results with and without redundant constraints for SLAM on random selections of 10 poses from the “Starry Night” dataset with relative-pose measurements between subsequent poses. 10 trials (random selection of poses) were performed at each noise level. Without redundant constraints (blue), the SLAM problem is not tight for any of the tested noise levels. With redundant constraints (orange) the problem is tight for reasonable noise levels.</figcaption></figure>

<table><caption>TABLE II
GROUND-TRUTH CAMERA PARAMETERS</caption><thead><tr><th>Parameter</th><th><i>b</i></th><th><i>f</i><sub>u</sub></th><th><i>f</i><sub>v</sub></th><th><i>c</i><sub>u</sub></th><th><i>c</i><sub>v</sub></th><th><i>σ</i><sub>u</sub></th><th><i>σ</i><sub>v</sub></th></tr></thead><tbody><tr><td>Units</td><td>m</td><td>pix/m</td><td>pix/m</td><td>pix</td><td>pix</td><td>pix</td><td>pix</td></tr><tr><td>Values</td><td>0.24</td><td>484.5</td><td>484.5</td><td>0.0</td><td>0.0</td><td>6.32</td><td>11.45</td></tr></tbody></table>

To assess tightness of matrix-weighted SLAM with stereo-image measurements, we randomly select 10 poses from the dataset and solve the SDP relaxation with and without redundant constraints. We add relative-pose measurements between subsequent poses based on the ground-truth data and perturb these measurements by a controlled amount of (isotropic) noise allowing to assess the effect of these additional measurements. All measurements and matrix-weights were defined as explained in Section III and the related sections of the Appendix.

The results are shown in Figure 10. At all times, the distance between the pose and the landmarks is within 1.75 m. Without redundant constraints, the problem is not tight across the noise levels considered. On the other hand, with redundant constraints, the problem remains tight until the relative-pose measurement noise exceeds approximately 0.06 m and 0.06 rad, reflecting tightness up to a reasonable level of noise.

Figure 1 shows an example of a local and global minimum for a 10-pose SLAM problem using measurements from the “Starry Night” dataset. Relative-pose measurements between subsequent poses were perturbed by Gaussian noise with standard deviation of $5.0 \times 10^{-2}$ m and $5.0 \times 10^{-2}$ rad in both rotation and translation. The local minimum was the result of running a Gauss-Newton (GN) solver from a randomly selected, poor initialization point (gradient and cost converged to $8.87 \times 10^{-12}$ and $1.13 \times 10^3$, respectively) while the global solution was found by solving the matrix-weighted SLAM SDP relaxation with redundant constraints (ER and cost converged to $2.43 \times 10^6$ and 9.36, respectively).$^{23}$

It is important to note that we have restricted these experiments to a low number of poses because of the necessary addition of variables and constraints, which makes the matrix-

weighted SLAM problem intractable for medium-to-large-scale problems. However, we believe that the analysis and results presented herein are valuable to the robotics community and demonstrate that certifiable methods can be extended to this problem.

## VIII. CONCLUSIONS

### A. Discussion

We have shown that inclusion of matrix weights in state-estimation problems can have profound implications on the tightness of their semidefinite relaxations. In the case of Wahba’s problem, matrix weights can decrease the noise level for which a given problem instance is tight to well below those found in practice. For SLAM, the introduction of matrix weights leads to a fundamental change in the formulation of the problem and the resulting formulation is not tight even for very low noise levels (without redundant constraints).

We have established a key connection between the posterior distribution of a state estimate and the dual or certificate matrix. We have also explored the relationship between the noise distribution of measurements, the posterior distribution, and the tightness of the semidefinite relaxation. Namely, anisotropicity in the underlying noise model results in non-tight relaxations when uncertainty is aligned in a given direction. This effect can be counteracted (to some extent) by increasing the number and variety of measurements in the problem.

One of the goals of our analysis was to determine whether redundant constraints are necessary for the problems considered herein. Based on the results in Sections VI-B and VI-C, redundant constraints seem to be necessary to achieve reliable robustness to noise in stereo-camera-based applications. In the case of Wahba’s problem, our results in Section VII-A show that, even with redundant constraints, directly solving the SDP can be fast enough to be used in real applications. Moreover, we have shown that even when initialized well, local methods can converge to egregious local minima. On the other hand, for matrix-weighted SLAM, the addition of even a sparse set of redundant constraints seems to be prohibitive for online use, as shown in Sections VI-C and VII-B.

### B. Future Work

The state-of-the-art, large-scale, certifiable perception methods in robotics concentrate on cases that do not require redundant constraints [12], [54]. However, even a relatively small change to the noise model (e.g., introducing anisotropicity) seems to necessitate such constraints. Therefore, we posit that a crucial area of development for certifiable algorithms in robotics involves maintaining computational speed even with these additional constraints.

The exploration of amendments to the *Riemannian Staircase* and *Burer-Monteiro* techniques to accommodate redundant constraints is one potential avenue of future work. This would involve addressing potential bottlenecks during the certification step due to a costly search for optimal Lagrange multipliers.

<footnote>$^{23}$Note that for good initializations, it was confirmed that the GN solver converged to the same cost and solution as the SDP relaxation.</footnote>
Another key avenue, which has proved essential in the past, is to exploit the sparsity of the SDP with redundant constraints as shown in [73]. In conjunction with the design of sparsity-promoting redundant constraints, this approach may yield the speed that is required for more general, online, large-scale certifiable solvers.

## IX. ACKNOWLEDGMENTS

The authors would like to thank Natural Sciences and Engineering Research Council of Canada (NSERC) for their generous support of this work. The authors would also like to thank Matt Giamou and Dave Rosen for their useful insights and suggestions.

## REFERENCES

[1] F. Alizadeh, J.-P. A. Haeberly, and M. L. Overton, "Complementarity and nondegeneracy in semidefinite programming," *Mathematical Programming*, vol. 77, no. 1, pp. 111–128, Apr. 1997.

[2] M. ApS, MOSEK Optimizer API for Python: Release 10.1.8, 2022. [Online]. Available: https://docs.mosek.com/latest/pythonapi/index.html#

[3] K. S. Arun, T. S. Huang, and S. D. Blostein, "Least-Squares Fitting of Two 3-D Point Sets," *IEEE Transactions on Pattern Analysis and Machine Intelligence*, vol. PAMI-9, no. 5, pp. 698–700, Sep. 1987.

[4] T. Bailey and H. Durrant-Whyte, "Simultaneous localization and mapping (SLAM): Part II," *IEEE Robotics & Automation Magazine*, vol. 13, no. 3, pp. 108–117, Sep. 2006.

[5] A. S. Bandeira, N. Boumal, and A. Singer, "Tightness of the maximum likelihood semidefinite relaxation for angular synchronization," *Math. Program.*, vol. 163, no. 1-2, pp. 145–167, May 2017.

[6] T. Barfoot, "State Estimation for Aerospace Vehicles - AER1513 Course Assignments," *University of Toronto Institute for Aerospace Studies, Tech. Rep.*, 2011.

[7] T. Barfoot, J. R. Forbes, and P. T. Furgale, "Pose estimation using linearized rotations and quaternion algebra," *Acta Astronautica*, vol. 68, no. 1, pp. 101–112, 2011.

[8] T. D. Barfoot, *State Estimation for Robotics*, 1st ed. Cambridge University Press, Jul. 2017.

[9] N. Boumal, V. Voroninski, and A. S. Bandeira, "The non-convex Burer-Monteiro approach works on smooth semidefinite programs," in *Proceedings of the 30th International Conference on Neural Information Processing Systems*, ser. NIPS'16. Red Hook, NY, USA: Curran Associates Inc., Dec. 2016, pp. 2765–2773.

[10] ——, "Deterministic Guarantees for Burer-Monteiro Factorizations of Smooth Semidefinite Programs," *Communications on Pure and Applied Mathematics*, vol. 73, no. 3, pp. 581–608, Mar. 2020.

[11] S. P. Boyd and L. Vandenberghe, *Convex Optimization*. Cambridge, UK; New York: Cambridge University Press, 2004.

[12] J. Briales and J. Gonzalez-Jimenez, "Cartan-Sync: Fast and Global SE(d)-Synchronization," *IEEE Robotics and Automation Letters*, vol. 2, no. 4, pp. 2127–2134, Oct. 2017.

[13] ——, "Convex Global 3D Registration with Lagrangian Duality," in *2017 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)*. Honolulu, HI: IEEE, Jul. 2017, pp. 5612–5621.

[14] J. Briales, L. Kneip, and J. Gonzalez-Jimenez, "A Certifiably Globally Optimal Solution to the Non-minimal Relative Pose Problem," in *2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition*. Salt Lake City, UT: IEEE, Jun. 2018, pp. 145–154.

[15] L. Brynte, V. Larsson, J. P. Iglesias, C. Olsson, and F. Kahl, "On the Tightness of Semidefinite Relaxations for Rotation Estimation," *J Math Imaging Vis*, vol. 64, no. 1, pp. 57–67, Jan. 2022.

[16] S. Burer and R. D. Monteiro, "A nonlinear programming algorithm for solving semidefinite programs via low-rank factorization," *Mathematical Programming*, vol. 95, no. 2, pp. 329–357, Feb. 2003.

[17] L. Carlone, D. M. Rosen, G. Calafiore, J. J. Leonard, and F. Dellaert, "Lagrangian duality in 3D SLAM: Verification techniques and optimal solutions," in *2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. Hamburg, Germany: IEEE, Sep. 2015, pp. 125–132.

[18] A. Censi, "On achievable accuracy for pose tracking," in *2009 IEEE International Conference on Robotics and Automation*. Kobe: IEEE, May 2009, pp. 1–7.

[19] K. N. Chaudhury, Y. Khoo, and A. Singer, "Global Registration of Multiple Point Clouds Using Semidefinite Programming," *SIAM J. Optim.*, vol. 25, no. 1, pp. 468–501, Jan. 2015.

[20] Y. Cheng and J. L. Crassidis, "A Total Least-Squares Estimate for Attitude Determination," in *AIAA Scitech 2019 Forum*, ser. AIAA SciTech Forum. American Institute of Aeronautics and Astronautics, Jan. 2019.

[21] D. Cifuentes, S. Agarwal, P. A. Parrilo, and R. R. Thomas, "On the local stability of semidefinite relaxations," *Math. Program.*, vol. 193, no. 2, pp. 629–663, Jun. 2022.

[22] J. L. Crassidis, F. L. Markley, and Y. Cheng, "Survey of Nonlinear Attitude Estimation Methods," *Journal of Guidance, Control, and Dynamics*, vol. 30, no. 1, pp. 12–28, Jan. 2007.

[23] F. Dellaert and G. Contributors, "borglab/gtsam," May 2022. [Online]. Available: https://github.com/borglab/gtsam

[24] F. Dellaert, D. M. Rosen, J. Wu, R. Mahony, and L. Carlone, "Shonan Rotation Averaging: Global Optimality by Surfing $SO(p)^n$," in *Computer Vision - ECCV 2020*, A. Vedaldi, H. Bischof, T. Brox, and J.-M. Frahm, Eds. Cham: Springer International Publishing, 2020, vol. 12351, pp. 292–308.

[25] S. Diamond and S. Boyd, "CVXPY: A Python-Embedded Modeling Language for Convex Optimization."

[26] F. Dümbgen, C. Holmes, B. Agro, and T. D. Barfoot, "Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations," *arXiv:2308.05783*, Aug. 2023.

[27] F. Dümbgen, C. Holmes, and T. D. Barfoot, "Safe and Smooth: Certified Continuous-Time Range-Only Localization," *IEEE Robotics and Automation Letters*, vol. 8, no. 2, pp. 1117–1124, Feb. 2023.

[28] H. Durrant-Whyte and T. Bailey, "Simultaneous localization and mapping: Part I," *IEEE Robotics & Automation Magazine*, vol. 13, no. 2, pp. 99–110, Jun. 2006.

[29] A. Eriksson, C. Olsson, F. Kahl, and T.-J. Chin, "Rotation Averaging and Strong Duality," in *2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition*. Salt Lake City, UT: IEEE, Jun. 2018, pp. 127–135.

[30] M. Garcia-Salguero, J. Briales, and J. Gonzalez-Jimenez, "A Tighter Relaxation for the Relative Pose Problem Between Cameras," *J Math Imaging Vis*, Apr. 2022.

[31] M. Giamou, F. Marić, D. M. Rosen, V. Peretroukhin, N. Roy, I. Petrović, and J. Kelly, "Convex Iteration for Distance-Geometric Inverse Kinematics," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 1952–1959, Apr. 2022.

[32] G. H. Golub and C. F. Van Loan, *Matrix Computations*, fourth edition, ser. Johns Hopkins Studies in the Mathematical Sciences. Baltimore: The Johns Hopkins University Press, 2013.

[33] M. Gridseth and T. D. Barfoot, "Keeping an Eye on Things: Deep Learned Features for Long-Term Visual Localization," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 1016–1023, Apr. 2022.

[34] D. Henrion, M. Korda, and J.-B. Lasserre, *The Moment-SOS Hierarchy: Lectures in Probability, Statistics, Computational Geometry, Control and Nonlinear PDEs*, ser. Series on Optimization and Its Applications. New Jersey: World Scientific, 2021, no. vol. 4.

[35] C. Holmes and T. D. Barfoot, "An Efficient Global Optimality Certificate for Landmark-Based SLAM," *IEEE Robot. Autom. Lett.*, vol. 8, no. 3, pp. 1539–1546, Mar. 2023.

[36] B. K. P. Horn, "Closed-form solution of absolute orientation using unit quaternions," *J. Opt. Soc. Am. A*, vol. 4, no. 4, p. 629, Apr. 1987.

[37] B. K. P. Horn, H. M. Hilden, and S. Negahdaripour, "Closed-form solution of absolute orientation using orthonormal matrices," *J. Opt. Soc. Am. A*, *JOSAA*, vol. 5, no. 7, pp. 1127–1135, Jul. 1988.

[38] S. Hu and L. Carlone, "Accelerated Inference in Markov Random Fields via Smooth Riemannian Optimization," *IEEE Robotics and Automation Letters*, vol. 4, no. 2, pp. 1295–1302, Apr. 2019.

[39] J. P. Iglesias, C. Olsson, and F. Kahl, "Global Optimality for Point Set Registration Using Semidefinite Programming," in *2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)*. Seattle, WA, USA: IEEE, Jun. 2020, pp. 8284–8292.

[40] A. Juric, F. Kendes, I. Markovic, and I. Petrovic, "A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM," in *2021 44th International Convention on Information, Communication and Electronic Technology (MIPRO)*. Opatija, Croatia: IEEE, Sep. 2021, pp. 1113–1118.

[41] K. Khosoussi, S. Huang, and G. Dissanayake, "A Sparse Separable SLAM Back-End," *IEEE Trans. Robot.*, vol. 32, no. 6, pp. 1536–1549, Dec. 2016.

[42] J. B. Lasserre, "Global Optimization with Polynomials and the Problem of Moments," *SIAM J. Optim.*, vol. 11, no. 3, pp. 796–817, Jan. 2001.
[43] M. Liu, S. Huang, G. Dissanayake, and H. Wang, “A convex optimization based approach for pose SLAM problems,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Oct. 2012, pp. 1898–1903.

[44] J. R. Magnus and H. Neudecker, Matrix Differential Calculus with Applications in Statistics and Econometrics, third edition ed., ser. Wiley Series in Probability and Statistics. Hoboken, NJ: Wiley, 2019.

[45] M. Maimone, Y. Cheng, and L. Matthies, “Two years of Visual Odometry on the Mars Exploration Rovers,” Journal of Field Robotics, vol. 24, no. 3, pp. 169–186, Mar. 2007.

[46] L. Matthies and S. A. Shafer, “Error Modeling in Stereo Navigation,” IEEE Journal of Robotics and Automation, vol. RA-3, no. 3, p. 12, Jun. 1987.

[47] Y. Nesterov, H. Wolkowicz, and Y. Ye, “Semidefinite Programming Relaxations of Nonconvex Quadratic Optimization,” in Handbook of Semidefinite Programming, F. S. Hillier, H. Wolkowicz, R. Saigal, and L. Vandenberghe, Eds. Boston, MA: Springer US, 2000, vol. 27, pp. 361–419.

[48] J. Nocedal and S. J. Wright, Numerical Optimization, 2nd ed., ser. Springer Series in Operations Research. New York: Springer, 2006.

[49] C. Olsson and A. Eriksson, “Solving quadratically constrained geometrical problems using lagrangian duality,” in 2008 19th International Conference on Pattern Recognition, Dec. 2008, pp. 1–5.

[50] A. Papalia, A. Fishberg, B. W. O'Neill, J. P. How, D. M. Rosen, and J. J. Leonard, “Certifiably Correct Range-Aided SLAM,” arXiv:2302.11614, Feb. 2023.

[51] P. A. Parrilo and S. Lall, “Semidefinite Programming Relaxations and Algebraic Optimization in Control,” European Journal of Control, vol. 9, no. 2-3, pp. 307–321, Jan. 2003.

[52] M. Paton, K. MacTavish, M. Warren, and T. D. Barfoot, “Bridging the appearance gap: Multi-experience localization for long-term visual teach and repeat,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Daejeon, South Korea: IEEE, Oct. 2016, pp. 1918–1925.

[53] L. Pineda, T. Fan, M. Monge, S. Venkataraman, P. Sodhi, R. Chen, J. Ortiz, D. DeTone, A. Wang, S. Anderson, J. Dong, B. Amos, and M. Mukadam, “Theseus: A Library for Differentiable Nonlinear Optimization,” arXiv:2207.09442, Jul. 2022.

[54] D. M. Rosen, L. Carlone, A. S. Bandeira, and J. J. Leonard, “SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group,” The International Journal of Robotics Research, vol. 38, no. 2-3, pp. 95–125, Mar. 2019.

[55] D. M. Rosen, K. J. Doherty, A. Terán Espinoza, and J. J. Leonard, “Advances in Inference and Representation for Simultaneous Localization and Mapping,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 4, no. 1, pp. 215–242, 2021.

[56] J. P. Ruiz and I. E. Grossmann, “Using redundancy to strengthen the relaxation for the global optimization of MINLP problems,” Computers & Chemical Engineering, vol. 35, no. 12, pp. 2729–2740, Dec. 2011.

[57] N. Shor, “Quadratic optimization problems,” Soviet Journal of Computer and Systems Sciences, vol. 25, pp. 1–11, 1987.

[58] M. K. d. C. Silva and L. Tunçel, “Strict Complementarity in MaxCut SDP,” arXiv:1806.01173, Jun. 2018.

[59] Y. Tian, Y. Chang, F. Herrera Arias, C. Nieto-Granda, J. P. How, and L. Carlone, “Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems,” IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2022–2038, Aug. 2022.

[60] Y. Tian, K. Khosoussi, D. M. Rosen, and J. P. How, “Distributed Certifiably Correct Pose-Graph Optimization,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 2137–2156, Dec. 2021.

[61] R. Tron, D. M. Rosen, and L. Carlone, “On the Inclusion of Determinant Constraints in Lagrangian Duality for 3D SLAM.”

[62] S. Umeyama, “Least-Squares Estimation of Transformation Parameters Between Two Point Patterns,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 13, no. 04, pp. 376–380, Apr. 1991.

[63] L. Vandenberghe and S. Boyd, “Semidefinite Programming,” SIAM Rev., vol. 38, no. 1, pp. 49–95, Mar. 1996.

[64] G. Wahba, “A Least Squares Estimate of Satellite Attitude,” SIAM Rev., vol. 7, no. 3, pp. 409–409, Jul. 1965.

[65] K. Wilson, D. Bindel, and N. Snavely, “When is Rotations Averaging Hard?” in Computer Vision – ECCV 2016, B. Leibe, J. Matas, N. Sebe, and M. Welling, Eds. Cham: Springer International Publishing, 2016, vol. 9911, pp. 255–270.

[66] E. Wise, M. Giamou, S. Khoubyarian, A. Grover, and J. Kelly, “Certifiably Optimal Monocular Hand-Eye Calibration,” in 2020 IEEE International Conference on Multisensor Fusion and Integration for

Intelligent Systems (MFI). Karlsruhe, Germany: IEEE, Sep. 2020, pp. 271–278.

[67] H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection,” IEEE Robot. Autom. Lett., vol. 5, no. 2, pp. 1127–1134, Apr. 2020.

[68] H. Yang and L. Carlone, “One Ring to Rule Them All: Certifiably Robust Geometric Perception with Outliers,” in Advances in Neural Information Processing Systems, vol. 33, 2020, pp. 18 846–18 859.

[69] ——, “Certifiably Optimal Outlier-Robust Geometric Perception: Semidefinite Relaxations and Scalable Global Optimization,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 45, no. 3, pp. 2816–2834, Mar. 2023.

[70] H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration,” IEEE Transactions on Robotics, vol. 37, no. 2, pp. 314–333, Apr. 2021.

[71] J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-based state estimation problems,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). Stockholm, Sweden: IEEE, May 2016, pp. 809–816.

[72] J. Zhao, W. Xu, and L. Kneip, “A Certifiably Globally Optimal Solution to Generalized Essential Matrix Estimation,” in 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). Seattle, WA, USA: IEEE, Jun. 2020, pp. 12 031–12 040.

[73] Y. Zheng, G. Fantuzzi, and A. Papachristodoulou, “Chordal and factor-width decompositions for scalable semidefinite and polynomial optimization,” Annual Reviews in Control, vol. 52, pp. 243–279, 2021.

# APPENDIX

## A. Proof of Lemma 1

*Proof.* Taking inspiration from [48], we use a second-order Taylor approximation of the Lagrangian of the QCQP (13) to prove our result. This Lagrangian can be expressed as follows [11]:

$$ \mathcal{L}(z, \lambda, \rho) = z^T \left( Q + \rho A_0 + \sum_{i=1}^{N_c} \lambda_i A_i \right) z - \rho. $$

Recalling the definition of the certificate matrix from the dual problem, (16), we have

$$ \mathcal{L}(z, \lambda, \rho) = z^T \mathbf{H}(\lambda, \rho) z - \rho. $$

Let $x = x^* + \delta x \in \mathcal{U}$ be a perturbation of the optimal solution, $x^*$, of Problem (2). Since, by assumption, the objective functions are locally equal, we have $z^* = \ell(x^*)$. Let $z = \ell(x)$. Since $\ell$ is smooth, we have the following perturbation to $z^*$:

$$ \delta z = z - z^* = \mathbf{L}\delta x + O(\delta x), \quad (35) $$

where $\delta x = x - x^*$ and $O(\cdot)$ denotes Bachmann-Landau (“big-o”) notation. Since $z$ is in the feasible set of Problem (13), the Lagrangian at $z$ is equal to the objective of Problem (13) and, by assumption, we have

$$ \mathcal{L}(z, \lambda, \rho) = \mathcal{L}(\ell(x), \lambda, \rho) = \ell(x)^T Q \ell(x) = - \log(p(x|D)). $$

We now proceed by considering second-order Taylor expansions of the Lagrangian about $z^*$.<fnref n="24" /> Letting $\mathbf{H} = \mathbf{H}(\lambda^*, \rho^*)$ to simplify notation, we have

$$ \begin{aligned} \mathcal{L}(z, \lambda^*, \rho^*) &= z^T \mathbf{H} z - \rho^* \\ &= (z^* + \delta z)^T \mathbf{H} (z^* + \delta z) - \rho^* \\ &= -\rho^* + \delta z^T \mathbf{H} \delta z \end{aligned} $$

<footnote n="24">²⁴As in [48, Theorem 12.5] we leave the Lagrange multipliers fixed</footnote>
where the third line follows from the first-order necessary optimality conditions ($Hz^* = 0$). Applying (35), we get

$$-\log(p(x|\mathcal{D})) = -\rho^* + \delta x^T L^T H L \delta x + O(\delta x^2),$$

where c is a constant. Since the FIM is exactly the Hessian of $-\log(p(x|\mathcal{D}))$ evaluated at $\delta x = 0$, we have the result:

$$\Sigma^{-1} = L^T H L. \quad (36)$$

<figure><img src="image_12.png" /></figure>

### B. Proof of Proposition 2

*Proof.* By assumption, we have $\Sigma^{-1} = L^T H L$. Without loss of generality, assume that the homogenizing variable of $z$ is the last element in the vector. Since this element is always equal to one, the Jacobian of the feasible set mapping is given by $L^T = [\bar{L}^T \ 0^T]$. It follows that $\Sigma^{-1} = \bar{L}^T \bar{H} \bar{L}$.

Since $\ell$ is injective, $\bar{L}$ has full column rank. It follows that$^{25}$

$$s_{\min}(\bar{L})\|v\| \le \|\bar{L}v\| \le s_{\max}(\bar{L})\|v\|, \quad (37)$$

where $\| \cdot \|$ denotes the Euclidean norm, $s_{\min}(\bar{L})$ is the minimum singular value of $\bar{L}$, and $s_{\max}(\bar{L})$ is the maximum singular value of $\bar{L}$. We proceed using the Raleigh quotient characterization of eigenvalues, noting that both $\Sigma^{-1}$ and $\bar{H}$ are real, symmetric matrices.

$$\begin{aligned} \sigma_{\min}(\bar{H}) &= \min_{y \in \mathbb{R}^n} \frac{y^T \bar{H} y}{\|y\|^2} \le \min_{\substack{y=\bar{L}v \\ v \in \mathbb{R}^p}} \frac{y^T \bar{H} y}{\|y\|^2} \\ &= \min_{v \in \mathbb{R}^p} \frac{v^T \bar{L}^T \bar{H} \bar{L} v}{\|\bar{L}v\|^2} \le \min_{v \in \mathbb{R}^p} \frac{v^T \bar{L}^T \bar{H} \bar{L} v}{s_{\min}(\bar{L})^2 \|v\|^2} \\ &= \frac{\sigma_{\min}(\Sigma^{-1})}{s_{\min}(\bar{L})^2}, \end{aligned}$$

where the first inequality follows from the fact that we are restricting the feasible set of the optimization and the second follows from (37). Finally, note that $\sigma_{\min}(\bar{L}) = \sigma_{\min}(L)$.

<figure><img src="image_13.png" /></figure>

### C. Degeneracy of the FIM and Tightness

**Proposition 3.** Assume that the setting of Proposition 2 holds and strict complementarity holds for Problem (15). If the FIM is degenerate (i.e., $\sigma_{\min}(\Sigma^{-1}) = 0$), then the SDP relaxation becomes non-tight. That is, either $H$ has negative eigenvalues or the optimal primal SDP solution has rank higher than one.

*Proof.* Let $z_0 = [y_0^T \ 1]^T$ be the globally optimal solution to the QCQP, with associated certificate matrix $H$. By definition, we have $Hz_0 = 0$. It can be shown that this implies that $h = -\bar{H}y_0$.

By assumption, we have that $\sigma_{\min}(\Sigma^{-1}) = 0$. Thus, Proposition 2 implies that $\sigma_{\min}(\bar{H}) \le 0$. If $\sigma_{\min}(\bar{H}) < 0$ then $\sigma_{\min}(H) < 0$ and the relaxation cannot be tight (since $z_0$ is globally optimal).

On the other hand, if $\sigma_{\min}(H) \ge 0$ then $\sigma_{\min}(\bar{H}) \ge 0$ and, by Proposition 2, we have $\sigma_{\min}(\bar{H}) = 0$. It follows that there

exists a vector $y_1$ such that $\bar{H}y_1 = 0$. Let $z_1 = [y_1^T \ 0]^T$ and note that,

$$Hz_1 = \begin{bmatrix} \bar{H}y_1 \\ h^T y_1 \end{bmatrix} = \begin{bmatrix} \bar{H}y_1 \\ -y_0^T \bar{H}y_1 \end{bmatrix} = 0.$$

Since $z_0$ and $z_1$ are linearly independent, $H$ has two minimum eigenvalues at zero. By strict complementarity, the corresponding primal SDP solution will have rank equal to two and is therefore not tight. $\square$

**Remark 1.** From the proof, we see that degeneracy of the FIM implies that the Lagrangian has the same value (to first order) for any vector, $z_0 + \mu z_1$, as long as $\mu \in \mathbb{R}$ is small. A further implication is that at the global solution, $z_0$, there is a feasible direction, $z_1$, which does not lead to an increase in the objective. This implies that the original QCQP has symmetries in its solution (i.e., its solution is not unique). It has been shown that such symmetries in the original QCQP solution cause higher rank SDP relaxation solutions, corresponding to the convex hull of the lifted rank-one solutions [14].

### D. Cost Function Elements

In this section, we show how to determine the QCQP cost matrices for the localization and SLAM problems given in Sections IV-A and IV-B. Throughout this section, we use the facts that $\text{tr}(ABC) = (C^T \otimes A)\text{vec}(B)$, $A = 1 \otimes A$, and $(A \otimes B)(C \otimes D) = (AC \otimes BD)$ [44].

#### 1) Landmark Measurements

Consider a single cost element corresponding to an edge in $\bar{\mathcal{E}}_m$:

$$\begin{aligned} J_{ik} &= (\tilde{m}_i^{ki} - C_i m_0^{k0} + t_i)^T W_k (\tilde{m}_i^{ki} - C_i m_0^{k0} + t_i) \\ &= \tilde{m}_i^{kiT} W_k \tilde{m}_i^{ki} w^2 - 2w \tilde{m}_i^{kiT} W_k C_i m_0^{k0} \\ &\quad + 2w \tilde{m}_i^{kiT} W_k t_i - 2m_0^{k0T} C_i^T W_k t_i \\ &\quad + m_0^{k0T} C_i^T W_k C_i m_0^{k0} + t_i^T W_k t_i, \end{aligned}$$

Re-organizing terms, we see that this cost element can be written as $J_{ik} = x^T Q_{ik} x_i$, where $x_i^T = [c_i^T \ t_i^T \ w]$ and $c_i = \text{vec}(C_i)$.

$$Q_{ik} = \begin{bmatrix} m_0^{k0} m_0^{k0T} \otimes W_k & -m_0^{k0} \otimes W_k & -m_0^{k0} \otimes W_k \tilde{m}_i^{ki} \\ -m_0^{k0T} \otimes W_k & W_k & W_k \tilde{m}_i^{ki} \\ -m_0^{k0T} \otimes \tilde{m}_i^{kiT} W_k & \tilde{m}_i^{kiT} W_k & \tilde{m}_i^{kiT} W_k \tilde{m}_i^{ki} \end{bmatrix}.$$

#### 2) Relative-Pose Measurements

Similarly, each edge in $\bar{\mathcal{E}}_p$ represents a cost element of the following form:

$$J_{ij} = \frac{1}{\sigma_{ij}^2} \left\| \tilde{C}_{ij} C_j - C_i \right\|_F^2 + \frac{1}{\tau_{ij}^2} \left\| \tilde{t}_i^{ji} - \tilde{C}_{ij} t_j + t_i \right\|_2^2. \quad (38)$$

As before, we collect the relevant variables,

$$x_{ij}^T = [c_i^T \ c_j^T \ t_i^T \ t_j^T \ w],$$

allowing us to write the cost element as

$$\begin{aligned} J_{ij} &= x_{ij}^T Q_{ij} x_{ij}, \\ Q_{ij} &= \begin{bmatrix} \frac{1}{\sigma_{ij}^2} Q_{r,ij} & 0 \\ 0 & \frac{1}{\tau_{ij}^2} Q_{t,ij} \end{bmatrix}, \end{aligned}$$

<footnote>$^{25}$See proof of Corollary 2.4.4 in [32].</footnote>
with

$$
\begin{align*}
Q_{r,ij} &= \begin{bmatrix} \mathbf{I} & -\mathbf{I} \otimes \tilde{C}_{ij} \\ -\mathbf{I} \otimes \tilde{C}_{ij} & \mathbf{I} \end{bmatrix}, \\
Q_{t,ij} &= \begin{bmatrix} \mathbf{I} & -\tilde{C}_{ij} & \tilde{t}_i^{ji} \\ -\tilde{C}_{ij}^T & \mathbf{I} & -\tilde{C}_{ij} \tilde{t}_i^{ji} \\ \tilde{t}_i^{ji}^T & -\tilde{t}_i^{ji} \tilde{C}_{ij}^T & \tilde{t}_i^{ji} \tilde{t}_i^{ji} \end{bmatrix}.
\end{align*}
$$

The blocks of these cost elements can be permuted accord-
ing to the variable ordering defined for a given problem and
the final cost matrix is obtained by summing all cost elements.

E. Stereo-Camera Model

In this section, we seek to convert pixel measurements
from left- and right-rectified stereo images to Euclidean point
measurements. We also seek to determine an appropriate
model of the uncertainty of the measurement in the Euclidean
space. We assume that left and right stereo frames have their
z-axes aligned with the viewing direction and their other axes
coincident. We also assume that they are separated by baseline
distance b ∈ ℝ and that the camera frame is coincident with
the left camera.

Consider a 3D point expressed in the world frame, $\mathbf{x}_w = [x_w \ y_w \ z_w]^T$ representing a feature that we wish to track. This point can be expressed in the left camera frame as $\mathbf{x}_c = \mathbf{C}_{cw}\mathbf{x}_w + \mathbf{t}_c^{wc}$, where $\mathbf{C}_{cw} \in \text{SO}(3)$ is the rotation matrix from the world to the camera frame and $\mathbf{t}_c^{wc} \in \mathbb{R}^3$ is vector from the world frame origin to the camera frame origin expressed in the camera frame.

Assuming a pinhole stereo-camera, the resulting pixel mea-
surements can be expressed as follows:

$$
\begin{bmatrix} p_{ul} \\ p_{vl} \\ p_{ur} \\ p_{vr} \end{bmatrix} = \frac{1}{z_c} \begin{bmatrix} f_u & 0 & c_u & 0 \\ 0 & f_v & c_v & 0 \\ f_u & 0 & c_u & -b f_u \\ 0 & f_v & c_v & 0 \end{bmatrix} \begin{bmatrix} \mathbf{x}_c \\ 1 \end{bmatrix} + \boldsymbol{\epsilon}_p, \quad (39)
$$

where *u* and *v* subscripts represent horizontal and vertical
pixel directions, respectively, *l* and *r* subscripts represent
left and right cameras, respectively, *p*<sub>*ij*</sub> represents the pixel
measurement, *f*<sub>*i*</sub> represents the focal length parameter, and
*c*<sub>*i*</sub> represents the camera centre parameter for direction *i*
of camera *j*. The variable *ε*<sub>*p*</sub> represents noise on the pixel
measurements and is assumed to have zero-mean, Gaus-
sian distribution, *ε*<sub>*p*</sub> ~ N(0, Σ<sub>*p*</sub>), with covariance matrix
Σ<sub>*p*</sub> = diag (σ<sub>*u*</sub><sup>2</sup>, σ<sub>*v*</sub><sup>2</sup>, σ<sub>*u*</sub><sup>2</sup>, σ<sub>*v*</sub><sup>2</sup>), where σ<sub>*i*</sub> represents the standard
deviation of the noise.

We define the *disparity*, *d*, of a given feature as the
horizontal difference between the feature's position in the right
and left images in terms of pixels: *d* = *p*<sub>ul</sub> − *p*<sub>ur</sub>. We define
the intermediate, Gaussian-distributed measurement,

$$
\mathbf{y} = [p_{ul} \ p_{vl} \ d]^T \sim \mathcal{N}(\boldsymbol{\mu}_y, \boldsymbol{\Sigma}_y), \quad (40)
$$

where the mean and covariance matrices are given by

$$
\boldsymbol{\mu}_y = \begin{bmatrix} \frac{1}{z_c} f_u x_c + c_u \\ \frac{1}{z_c} f_v y_c + c_v \\ \frac{1}{z_c} f_u b \end{bmatrix}, \quad \boldsymbol{\Sigma}_y = \begin{bmatrix} \sigma_u^2 & 0 & \sigma_u^2 \\ 0 & \sigma_v^2 & 0 \\ \sigma_u^2 & 0 & 2\sigma_u^2 \end{bmatrix}. \quad (41)
$$

Given this measurement, we can use the (known) in-
trinsic camera parameters to generate Euclidean pseudo-
measurements, $\hat{x} = [\hat{x}_c \ \hat{y}_c \ \hat{z}_c]^T$, of the (unknown) feature
locations via the following mapping:

$$
g^{-1} : (\mathbb{R}^2 \times \mathbb{R}_+) \rightarrow \mathbb{R}^3, \quad \text{s.t.} \ \hat{x} = g^{-1}(y) = b \begin{bmatrix} \frac{p_{ul} - c_u}{d} \\ \frac{p_{vl} - c_v}{d} \\ \frac{f_u}{d} \end{bmatrix}. \qquad (42)
$$

Although this transformation is non-linear, we make the as-
sumption that the distribution of $\hat{x}$ remains Gaussian. To
approximate the covariance of the pseudo-measurement, we
map the intermediate measurement covariance through the
linearized Jacobian of this transformation:

$$
\mathbf{G} = \frac{\partial \mathbf{g}^{-1}(\mathbf{y})}{\partial \mathbf{y}} = b \begin{bmatrix} \frac{1}{d} & 0 & -\frac{p_{ul}-c_u}{d^2} \\ 0 & \frac{f_u}{f_v d} & -\frac{f_u}{f_v} \frac{p_{vl}-c_v}{d^2} \\ 0 & 0 & -\frac{f_u}{d^2} \end{bmatrix} . \quad (43)
$$

Typically, such a linearization is performed about a prior belief
– as in a Kalman filter – or previous iterate – as in iteratively
re-weighted least squares – of the variables involved. However,
in the global-optimization context, neither of these options are
available and we choose to linearize about the measurement
itself.

Therefore, the noise model of the pseudo-measurement can
be expressed as

$$
\hat{x} = x_c + \epsilon_x, \quad \epsilon_x \sim \mathcal{N}(0, \Sigma_x), \quad (44)
$$

where $\epsilon_x$ represents an approximately Gaussian-distributed
variable with zero mean and covariance given by $\Sigma_x =
G\Sigma_y G^T$.

It is instructive to consider the following alternate formula-
tion of the Jacobian:

$$
\mathbf{G} = \begin{bmatrix} \frac{\hat{z}_c}{f_u} & 0 & -\frac{\hat{x}_c}{f_u} \frac{\hat{z}_c}{b} \\ 0 & \frac{\hat{z}_c}{f_v} & -\frac{\hat{y}_c}{f_u} \frac{\hat{z}_c}{b} \\ 0 & 0 & -\frac{\hat{z}_c}{f_u} \frac{\hat{z}_c}{b} \end{bmatrix}. \quad (45)
$$

We see that the Jacobian matrix scales linearly with the z-
axis coordinate, $\hat{z}_c$, meaning that the variance of the Euclidean
measurement scales quadratically with this variable. Ignoring
the off-diagonal terms, we note that the anistropicity of the
measurement is approximately proportional to $\frac{\hat{z}_c}{b}$.

F. SDP Stability of Matrix-Weighted Localization

In this section, we prove that Problem (18) enjoys the
property of ‘SDP stability’ established in [21]. That is, the
convex relaxation of Problem (18) is tight whenever the set
of measurements has sufficiently low noise. In particular, we
connect the SDP stability to the well-known condition that the
observed landmarks are not coplanar, which has been shown to
lead to a unique solution for point-set regression [3]. Such re-
sults have been given for other state-estimation problems [54],
[60] and our proof closely follows the development given
in [66].

**Definition 4 (Noise-free Measurements).** Given the setting of Problem (18), we define the set of noise-free measurements for a given set of poses, {($\bar{\mathbf{C}}_i$, $\bar{\mathbf{t}}_i$) ∀i ∈ $\mathcal{V}_p$}, as

$$
\{ \bar{\boldsymbol{m}}_i^{ki} = \bar{\mathbf{C}}_i \boldsymbol{m}_0^{k0} - \bar{\mathbf{t}}_i, \forall (i, k) \in \bar{\mathcal{E}}_m \}, \quad (46)
$$
and collect all such measurements in a vector, $\tilde{m}$.

**Theorem 5 (SDP-Stability of Matrix-Weighted Localization).**
Consider the setting of Problem (18) with positive definite weighting matrices ($W_{ik} \succ 0$) and suppose the set of landmarks observed from a given pose are not coplanar. Let $\tilde{m}$ be a vector containing all of the pose-landmark measurements for the problem and let $\tilde{m}$ be the set of noise-free measurements associated with the globally optimal solution of the problem with the same ordering as $\tilde{m}$. Then, there exists some $\epsilon > 0$ such that if $\|\tilde{m}-\tilde{m}\|^2 < \epsilon$, the SDP relaxation of the problem is tight (strong duality holds) and the global minimizer can be recovered from the SDP solution.

*Proof.* To prove the theorem, we show that the conditions of Theorem 3.9 from [21] are satisfied when the assumptions of our theorem hold. In the following, we treat the measurements, $\tilde{m}$, as a set of parameters on which the cost of the problem depends. Since there are no relative-pose measurements in Problem (18), it is separable and we can prove the result for a single-pose problem (fixed pose $i$) without loss of generality. The conditions of Theorem 3.9 are as follows:

1) The cost and constraints are quadratic in the variables.

2) The cost depends continuously on the set of parameters, $\tilde{m}$.

3) When constructed with $\tilde{m}$ rather than $\tilde{m}$, the cost becomes strictly convex, with the same unconstrained minimum as the original problem.

4) The Abadie Constraint Qualification (ACQ) holds for the feasible set at the global solution.

Let $\mathbf{y} = [\text{vec}(C_i)^T \ t_i^T]^T \in \mathbb{R}^{12}$ be the vectorized form of the variables. The cost function for pose $i$ in Problem (18) is given by

$$f_i(\tilde{m}, \mathbf{y}) = \sum_{(i,k) \in \tilde{\mathcal{E}}_m} e_{ik}^T W_{ik} e_{ik}. \quad (47)$$

We can write $e_{ik}$ as an affine function of $\mathbf{y}$

$$e_{ik} = \tilde{m}_{i}^{ki} - A_{k} \mathbf{y}, \quad \forall (i, k) \in \tilde{\mathcal{E}}_{m}, \quad (48)$$

where $A_k = [m_0^{k0} \otimes I -I] \in \mathbb{R}^{3 \times 12}$. The cost function becomes

$$f_i(\tilde{m}, \mathbf{y}) = \mathbf{y}^T Q \mathbf{y} + b^T \mathbf{y} + c, \quad (49)$$

where

$$Q = \sum_{(i,k) \in \tilde{\mathcal{E}}_m} A_k^T W_{ik} A_k, \quad (50)$$

$$b = -2 \sum_{(i,k) \in \tilde{\mathcal{E}}_m} W_{ik} \tilde{m}_i^{ik}, \quad (51)$$

$$c = \sum_{(i,k) \in \tilde{\mathcal{E}}_m} \tilde{m}_i^{ik^T} W_{ik} \tilde{m}_i^{ik}. \quad (52)$$

We see immediately that the cost function depends quadratically on the variables, $\mathbf{y}$, and quadratically (thus continuously) on the parameters, $\tilde{m}$. Together with the fact that the O(3) constraints are quadratic, conditions 1) and 2) are satisfied.

Now, let $\tilde{\mathbf{y}}$ represent the vectorized form of the global solution and consider the cost function, $f_i(\tilde{m}, \mathbf{y})$, constructed using the noise-free measurements, $\tilde{m}$, associated with the

global solution. Since $f_i(\tilde{m}, \mathbf{y})$ is a sum of quadratic forms, we have that $f_i(\tilde{m}, \mathbf{y}) \ge 0$. Moreover, its global minimizer is $\tilde{\mathbf{y}}$ since $f_i(\tilde{m}, \tilde{\mathbf{y}}) = 0$.

To conclude that this minimizer is unique, we must show strict convexity of $f_i(\tilde{m}, \mathbf{y})$, which is implied if $\nabla_y^2 f_i(\tilde{m}, \mathbf{y}) = Q$ is strictly positive definite [11]. Note that since the weights are positive definite ($W_{ik} \succ 0$) we already have that $Q \succeq 0$. To show positive definiteness, it remains to show that $Q$ is full-rank. To this end, note that we can write

$$Q = A^T W A, \quad (53)$$

where

$$W = \operatorname{diag}(W_{i1}, \dots, W_{iN}), \quad (54)$$

$$A = \begin{bmatrix} A_1 \\ \vdots \\ A_N \end{bmatrix} = \begin{bmatrix} m_0^{10^T} & 1 \\ \vdots & \vdots \\ m_0^{N0^T} & 1 \end{bmatrix} \otimes I_3, \quad (55)$$

and we have re-indexed the landmarks from 1 to $N$ for convenience. We also define

$$B = \begin{bmatrix} m_0^{10^T} & 1 \\ \vdots & \vdots \\ m_0^{N0^T} & 1 \end{bmatrix}.$$

Since $W$ is full rank, we are required to show that rank($A$) = 12. Recall that the singular values of a Kronecker product, $A \otimes B$, is given by $\{\mu\lambda, \forall \mu \in \sigma(A), \lambda \in \sigma(B)\}$, where $\sigma(\cdot)$ denotes the set of singular values. Therefore, rank($A$) = 12 is equivalent to the fact that rank($B$) = 4. We can subtract the first row from the remaining rows of $B$ without changing its row rank. Thus,

$$\operatorname{rank}(B) = \operatorname{rank}\left(\begin{bmatrix} m_0^{10^T} & 1 \\ D & 0 \end{bmatrix}\right), \quad D = \begin{bmatrix} (m_0^{20} - m_0^{10})^T \\ \vdots \\ (m_0^{N0} - m_0^{10})^T \end{bmatrix}.$$

Since the landmarks are not coplanar by assumption, rank($D$) = 3 and, since rank($B$) = rank($D$)+1, condition 3 holds.

Finally, the ACQ holds at any feasible point by the argument given in [21], giving the final condition of Theorem 3.9. $\square$

We conclude this section with two remarks. First, the assumption that the weight matrices are positive definite (i.e., not degenerate) is not strictly required as long as rank($A^T W A$) = 12 is guaranteed by sufficient measurements. As mentioned above, degenerate weight matrices were encountered in [13] when measurements of lines and planes were considered. Second, the extension of this proof to include relative-pose measurements should be possible since it has already been proved when only relative-pose measurements are considered (at least for the case of a weakly connected measurement graph) [54].
<figure><img src="image_14.png" /><figcaption>Fig. 11. Comparison of ER and duality gap tightness boundary contours for the results in Figure 4 (without redundant constraints). The ER boundary corresponds to an ER of $1 \times 10^{-6}$ whereas the duality gap boundary corresponds to a relative gap of $1 \times 10^{-10}$.</figcaption></figure>

<figure><img src="image_15.png" /><figcaption>Fig. 12. Comparison of tightness boundary contours with and without median filter for the results in Figure 4. Darker contours (marked “filt.” in legend) represent the smoothed contours that were used in the body text.</figcaption></figure>

## H. Tightness Boundary Smoothing

As mentioned in the main body, we plot tightness boundaries based on the parameters for which the minimum ER across all trials passes a threshold. These contours were plotted using the ‘contour’ function from PyPlot. However, it was initially found that the resulting contour plots were quite noisy and difficult to interpret even when the number of trials was increased considerably. To filter the data, the median of minimum ER values was taken over an N-by-N block in the parameter space (centered on a given parameter value) and the result was used to generate the smoothed contours. It was found that $N = 7$ was sufficient to ensure adequate smoothing. A comparison of the contours on the raw minimum ER data versus the filtered data is shown in Figure 12, in which the data corresponds to the data in Figure 4 in the main text.

<figure><img src="image_16.png" /></figure>

**Connor Holmes** received a B.A.Sc. degree in Engineering Science and M.A.Sc. degree in Electrical and Computer Engineering from the University of Toronto in 2014 and 2016, respectively. From 2016 until 2021, he worked as an Guidance Navigation and Controls Engineer at MDA Space. Since 2021, he has been pursuing a Ph.D. at the University of Toronto Robotics Institute. Connor’s research interests include the application of convex optimization in robotics, particularly for certification of state-estimation algorithms.

<figure><img src="image_17.png" /></figure>

**Frederike Dümbgen** received the B.Sc. and M.Sc. degrees in Mechanical Engineering, with minor in Computational Science and Engineering, from École Polytechnique Fédérale de Lausanne (EPFL) in 2013 and 2016, respectively, conducting her Master’s thesis at ETH Zürich. She obtained her Ph.D. degree in computer and communication sciences from EPFL in 2021 and worked as a post-doctoral researcher at the Robotics Institute of University of Toronto, Canada, from 2022 to 2024. As of May 2024, she is a researcher in the WILLOW team, affiliated with

Inria and ENS, PSL University, Paris. Her research interests lie in the areas of estimation and advanced optimization for robotics.

## G. Other Metrics of Tightness

In other works, it is sometimes the case that a ‘relative gap’ is used as a metric for evaluating tightness of a convex relaxation. This gap is typically defined as follows:

$$ \text{gap} = \frac{p(\mathbf{x}_r) - d^*}{1 + d^*}, $$

where $p(\mathbf{x}_r)$ represents the primal cost of the closest feasible, rank-1 solution (rounded solution) and $d^*$ represents the optimal SDP cost (equivalently, the dual cost). The one in the denominator keeps the metric stable when $d^*$ is low.

In Figure 11, we compare this metric with the ER, our metric of choice throughout the paper. We note that the corresponding boundaries are very close, except when anisotropy is large. When redundant constraints were used, both metrics showed that the relaxation was tight for all parameter values.

<figure><img src="image_18.png" /></figure>

**Timothy D. Barfoot** received the B.A.Sc. degree in Engineering Science from University of Toronto, Toronto, ON, Canada, in 1997 and the Ph.D. degree in aerospace science and engineering from University of Toronto, in 2002. He is a Professor with the University of Toronto Robotics Institute, Toronto, ON, Canada. He works in the areas of guidance, navigation, and control of autonomous systems for a variety of applications. He is interested in developing methods to allow robotic systems to operate over long periods of time in large-scale, unstructured,

three-dimensional environments, using rich onboard sensing (e.g., cameras and laser rangefinders) and computation.