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

HAL
open science

# Exploiting Chordal Sparsity for Fast Global Optimality with Application to Localization

Frederike Dümbgen, Connor Holmes, Timothy D. Barfoot

► To cite this version:

Frederike Dümbgen, Connor Holmes, Timothy D. Barfoot. Exploiting Chordal Sparsity for Fast Global Optimality with Application to Localization. WAFR 2024 - 16th International Workshop on the Algorithmic Foundations of Robotics, Oct 2024, Chicago, United States. hal-04884970

HAL Id: hal-04884970

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

Submitted on 13 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_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Exploiting Chordal Sparsity for Fast Global Optimality with Application to Localization

Frederike Dümbsgen¹[0000-0002-7258-9753], Connor Holmes²[0000-0002-8314-3677], and Timothy D. Barfoot²[0000-0003-3899-631X]

¹ Inria, Ecole Normale Supérieure, PSL University, Paris, France
https://di.ens.fr/willow/
frederike.dumbgen@inria.fr

² University of Toronto, Robotics Institute, Toronto ON M3H5T6, Canada
https://robotics.utoronto.ca

**Abstract.** In recent years, many estimation problems in robotics have been shown to be solvable to global optimality using their semidefinite relaxations. However, the runtime complexity of off-the-shelf semidefinite programming (SDP) solvers is up to cubic in problem size, which inhibits real-time solutions of problems involving large state dimensions. We show that for a large class of problems, namely those with chordal sparsity, we can reduce the complexity of these solvers to linear in problem size. In particular, we show how to replace the large positive-semidefinite variable with a number of smaller interconnected ones using the well-known chordal decomposition. This formulation also allows for the straightforward application of the alternating direction method of multipliers (ADMM), which can exploit parallelism for increased scalability. We show for two example problems in simulation that the chordal solvers provide a significant speed-up over standard SDP solvers, and that global optimality is crucial in the absence of good initializations.

**Keywords:** Control Theory and Optimization · Localization and Mapping · Multi-Agent Systems and Distributed Robotics.

## 1 Introduction

To this date, optimization problems in robotics are predominantly solved with fast local methods. Popular choices in state estimation, for example, are Gauss-Newton (GN) [4], or incremental smoothing and mapping (iSAM) [29, 30]; planning and control problems are commonly solved with sequential quadratic programming algorithms and variants thereof [48]. Because the cost can become non-convex as soon as non-linear measurement models, dynamic models, or other non-convex constraints are employed, these methods may converge to poor local minima in the absence of well-informed initializations.

In recent years, important advances have been made towards globally optimal solvers. In computer vision, rotation averaging and registration problems [31, 23, 21] have been globally solved first, followed more recently by methods for optimal outlier-robust estimation [60]. Classical robotics problems such
<figure><img src="image_3.png" /><figcaption>**Fig. 1.** By the nature of variable dependencies, the core matrices of many robotics optimization problems will be chordally sparse. Examples are arrowhead (left) and band-sparse (middle) matrices, or combinations thereof (right). In batch state estimation, local solvers can exploit such sparsity patterns (of the inverse covariance matrix) to yield linear complexity. In this paper, we explain how certifiably optimal SDP solvers can do the same, following problem decomposition and distribution [61].</figcaption></figure>

as range-only localization [20, 26] and range-aided simultaneous localization and mapping (SLAM) [46], pose-graph optimization [50, 11], and isotropic [27] or non-isotropic [28] SLAM have also seen adaptations of global solvers. We are even seeing a surge of globally optimal solvers for non-smooth planning problems [37, 15].

In many of the aforementioned global methods, the computational bottleneck lies in solving a high-dimensional semidefinite program (SDP). Off-the-shelf solvers for solving SDPs commonly use interior-point methods [41], which are of approximately cubic complexity in the number of unknowns and constraints [2]. This compares favorably with worst-case exponential-time methods such as branch and bound [43]. However, to be competitive with local solvers [4, 30], more efficient SDP solvers are desired. Indeed, by exploiting sparsity, state-of-the-art local solvers can run in linear time in either the number of unknown landmarks or poses and, thanks to sophisticated handling of computation graphs, can update solutions efficiently in an online manner [30].

In this paper, we use well-established tools from the optimization community [7, 57] to show how SDP solvers can also be significantly sped up by exploiting sparsity. For a particular class of problems, namely problems with chordal sparsity (see Figure 1), the SDP can be decomposed into interconnected smaller problems. This reduces the solver complexity and allows the problem to be distributed across computational cores using, for example, the alternating direction method of multipliers (ADMM). We show that such decompositions reduce the solver complexity to approximately linear in the number of variables, which is a significant speedup and comparable to efficient but initialization-dependent local solvers. In summary, our contributions are as follows.

1. We adopt the decomposed SDP (dSDP) solver and its distributed version from [61], and show how they apply to robotics problems, using an intuitive factor-graph language [18] for modeling.

2. We showcase the performance of the methods on two localization problems, which both satisfy the chordal sparsity assumptions, and empirically show that they have the same complexity as efficient *local* solvers.

We emphasize that the proposed methods have been considered in the control community [62], and applied, for example, for optimal power flow [17]. Our contribution lies in adopting them for state estimation problems, characterizing their behavior, and, by using the factor-graph language, opening the door to
their broader application in robotics. We also pay particular attention to redundant constraints, which are commonly required for tightness in state estimation problems and may significantly impact the effectiveness of the approach.

This paper is structured as follows. We give an overview of related work and important graph-theoretic identities in Sections 2 and 3, respectively. In Section 4, we present the methodology to pass from the original sparse non-convex problem to the convex relaxation, design the factor-graph framework to describe and decompose such problems, and finally discuss the sparsity-exploiting solvers. We demonstrate the performance of the two proposed methods on two localization problems in Section 5 and conclude in Section 6.

## 2 Related Work

The proposed methods lie at the intersection of two active research areas in robotics: efficient certifiably optimal solvers and distributed algorithms.

Methods to speed up the certifiably optimal solvers based on SDP relaxations can be broadly categorized into methods that exploit the low-rank property of the sought solution, based on the Burer-Monteiro factorization [10], and methods that exploit sparsity of the involved matrices [57, 61]. The former have been successfully applied to pose-graph optimization, using in particular the Riemanian staircase method [50], and distributed versions of the method have been derived [56, 55]. The practicality of the approach has been demonstrated by its adoption in a distributed semantic SLAM framework [54]. However, to this date, the Burer-Monteiro methods have not been successfully applied to problems that require redundant constraints, since they break common constraint qualifications.

Since for many instances in robotics [60, 59, 28, 19, 6, 26], redundant constraints are paramount to achieve tight semidefinite relaxations and thus globally optimal solutions, we explore sparsity-exploiting methods as an alternative. On one hand, sparsity can be used to speed up canonical operations within a solver such as solving linear systems of equations: in [42], for example, sparsity of the constraint matrices is used to speed up the projection step of the ADMM solver for the homogeneous self-dual embedding problem. Different in nature are so-called decomposition methods for SDPs, which are the subject of this paper. Here, the sparsity is exploited to reduce the problem to many smaller-sized instances that can either be solved in one shot [24, 34] or in an alternating way [32, 53]. We show in this paper that many state estimation problems are suitable candidates for these methods, provided that redundant constraints are such that chordal sparsity is not significantly affected.

The aforementioned decomposition methods lend themselves to distributed implementations. Distribution allows for increased efficiency thanks to parallelism and load sharing, and it has seen a growing interest in robotics in recent years, where real-time requirements and high dimensionality prevail. In control, for example, there is growing interest in ADMM-based quadratic-program solvers [3, 52], which lend themselves to distribution [51]. In parallel work, a cer-
tifiably optimal distributed solver for trajectory optimization, exploiting chain-like sparsity as a special case of correlative sparsity [35], was developed [33]. In estimation, for high-dimensional problems such as structure from motion and bundle adjustment, distributed methods based on majorization minimization techniques [22] or Gaussian belief propagation [45] have shown impressive scalability improvements. Distributed algorithms can also be shared across multiple robots, allowing for collaborative solutions to, for example, localization and mapping [12, 13, 38], with dedicated measures to address limited communication bandwidth and proximity constraints [16, 14, 40]. Since they distribute a local solver, and often solve non-convex problems, the above methods may converge to poor local minima. The ADMM algorithm described herein can be seen as a first step towards a distributed and certifiably optimal alternative for robotics.

# 3 Background

## 3.1 Notation and definitions

We denote scalars, vectors, matrices, and sets, respectively, by the fonts $x$, $x'$, $X$, and $\mathcal{X}$. The $j$-th element of a vector, the $j$-th column of a matrix, and the element $(i, j)$ of a matrix are denoted, respectively, by $x[j]$, $X[j]$, and $X[i, j]$. We write $X \succeq 0$ to mean $X$ is positive semidefinite. $I_d$ and $0_d$ are the identity matrix and vector of all zeros of size $d$, respectively, and $d$ is dropped when clear from context. The operator vech($\cdot$) returns the upper-diagonal elements of a symmetric matrix $X$, where off-diagonal elements are multiplied by $\sqrt{2}$, ensuring $\text{tr}(Q^\top X) = \text{vech}(Q)^\top \text{vech}(X)$. The inverse operation is mat($\cdot$): $X = \text{mat}(y)$ and we write $y \succeq 0$ for mat($y$) $\succeq 0$. An undirected graph is defined as $\mathcal{G} = (\mathcal{V}, \mathcal{E})$, where $\mathcal{V}$ is the set of vertices and $\mathcal{E}$ is the set of edges. We use $(i, j) \in \mathcal{E}$ and $(i, j) \in \mathcal{G}$ interchangeably, as well as $i \in \mathcal{V}$ and $i \in \mathcal{G}$. A subgraph induced by a set of vertices $W \subset \mathcal{V}$ is the graph with vertices $W$ and edges $\mathcal{E} \cap (W \times W)$. A clique is a subgraph that is fully connected. A cycle is a set of pairwise distinct vertices $v_1, \dots, v_k \subset \mathcal{V}$ such that $(v_1, v_k) \in \mathcal{E}$ and $(v_i, v_{i+1}) \in \mathcal{E}$. A chord in a cycle is an edge connecting two non-consecutive vertices.

## 3.2 Chordal sparsity in SDPs

An extensive treatment of sparsity in SDPs is given in [61]. Here, we introduce some lighter notation and highlight the concepts that will be the most useful to us in the remainder of the paper. Given a set of symmetric matrices $\mathcal{A} = \{\mathcal{A}_1, \dots, \mathcal{A}_K\}$, we define the aggregate sparsity pattern of $\mathcal{A}$ as the following set of pairwise indices:

$$ \mathcal{E} = \{(i, j) \mid \exists \mathcal{A}_k \in \mathcal{A} \text{ s.t. } \mathcal{A}_k[i, j] \neq 0\}. \quad (1) $$

In other words, this is the set of all index pairs at which at least one of the matrices in the set has a non-zero element. We define the sparsity graph of $\mathcal{A}$ as $\mathcal{G} = (\{1, \dots, N\}, \mathcal{E})$. Next, we reiterate a few well-known properties of graphs.
**Definition 1 (Chordal graph).** A graph $\mathcal{G}$ is chordal if every cycle of length $\ge 4$ contains at least one chord.

A graph can be rendered chordal by adding edges until it is chordal. Finding the minimum chordal extension, i.e., the minimum set of chords to render the graph chordal, is an NP-hard problem, but efficient heuristics exist [1]. In this paper, we will use specific sparsity patterns that typically arise in robotics applications and use deterministic chordal extensions.

**Definition 2 (Maximal clique).** $\mathcal{C}$ is a maximal clique of $\mathcal{G}$ if it is not contained in any other clique of $\mathcal{G}$.

Finally, we say that a matrix is semidefinite-completable in $\mathcal{E}$, written as $\mathbf{X}_{\mathcal{E}} \succeq 0$, if $\mathbf{X}$ can be made positive semidefinite by changing only the elements corresponding to indices not in $\mathcal{E}$.

Now, we can state the important property that enables us to replace the large SDP variable with smaller ones. We consider SDPs of the general form

$$ \min\{\mathrm{tr}(\mathbf{Q}\mathbf{X}) | \mathbf{X} \succeq 0, \mathrm{tr}(\mathbf{A}_i\mathbf{X}) = b_i, i = 1 \dots K\}, \quad (2) $$

where $\mathbf{Q}$ is a symmetric cost matrix. Then we have the following property:

**Theorem 1.** [61, Theorem 2.2] Let $\mathcal{E}_{SDP}$ be the aggregate sparsity pattern of the set of problem matrices, $\{\mathbf{Q}, \mathbf{A}_1, \dots, \mathbf{A}_K\}$, and let $\mathcal{G}_{SDP}$ be the sparsity graph corresponding to its chordal extension. Let the maximal cliques of $\mathcal{G}_{SDP}$ be $C_1, \dots, C_T$ and let $C_1, \dots, C_T$ be the submatrices of $\mathbf{X}$ at the indices of the cliques. Then we have: $\mathbf{X}_{\mathcal{E}} \succeq 0 \iff (\forall t) C_t \succeq 0$.

# 4 Method

In this section, we present two scalable methods for solving localization problems in robotics to global optimality, based on [61], which rely on the graph-theoretic concepts outlined in the previous section. We formally define the state estimation problem and its reformulation to a quadratically-constrained quadratic program (QCQP) in Sections 4.1 and 4.2, respectively. We then derive the monolithic SDP in 4.3 and show how it can be decomposed and distributed in Sections 4.4 and 4.5. A summarizing diagram of the different reformulations is given below.

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

## 4.1 Nonlinear least squares (NLS) formulation

Many classic state estimation problems in robotics, including localization and SLAM, take the form of a nonlinear least squares (NLS) problem when we seek the maximum a-posteriori estimator [4]:

$$ \hat{\boldsymbol{\xi}} = \arg \min_{\boldsymbol{\xi} \in \mathcal{X}} \sum_{k \in \mathcal{Q}} \underbrace{e_k(\boldsymbol{\xi}_k)^\top \mathbf{W}_k e_k(\boldsymbol{\xi}_k)}_{a_k(\boldsymbol{\xi}_k)} + \sum_{(i,j) \in \mathcal{R}} \underbrace{e_{ij}(\boldsymbol{\xi}_i, \boldsymbol{\xi}_j)^\top \mathbf{W}_{ij} e_{ij}(\boldsymbol{\xi}_i, \boldsymbol{\xi}_j)}_{r_{ij}(\boldsymbol{\xi}_i, \boldsymbol{\xi}_j)}, \quad (3) $$
where $\xi = \{\xi_1, \dots, \xi_N\}$ represents the state variables to be estimated. We denote by $\mathcal{Q}(\mathcal{R})$ the set of indices (pairs) at which we have absolute (relative) measurements or prior terms. The absolute and relative cost terms are denoted $a_k(\xi_k)$ and $r_{ij}(\xi_i, \xi_j)$, respectively, and defined by their respective error terms $e(\cdot)$ and corresponding inverse covariance matrix $W$.

Note that, for non-linear $e_k$, $e_{ij}$, or non-convex $\mathcal{X}$, Problem (3) may be non-convex and thus its globally optimal solutions may be hard to find efficiently. Practitioners commonly resort to local solvers such as GN [4] or iSAM [29], which are efficient but have the important caveat that they may converge to poor local minima in the absence of a good initialization.

**Factor-graph representation.** Problem (3) can be represented by a factor graph $\mathcal{G}_f = (\mathcal{V}, \mathcal{M}, \mathcal{E})$ [18], where $\mathcal{V}$ contains all states, $\mathcal{M}$ contains the measurements or priors, and $\mathcal{E}$ defines the (bipartite) connectivity between $\mathcal{V}$ and $\mathcal{M}$. In this paper, we are interested in factor graphs that are chordally sparse – in other words, graphs that have small maximum cliques. All examples given in Figure 1 fall into this category. Throughout this paper, we will consider two localization problems as examples where $\mathcal{E}$ is a chain. Their sparsity patterns are given in Figure 2.<fnref n="3" /> We define these problems next, represented by the respective error terms entering (3).

**Example CT-RO.** We adopt the continuous-time range-only localization (CT-RO) problem from [20], where our goal is to find the position of a moving device over time, given distance measurements to $N_m$ fixed and known landmarks. Because distance measurements may be sparse, we add a Gaussian process (GP) motion prior to encourage smoothness and to ensure that recovery is possible even in underdetermined cases [5]. It was found in [20] that local solvers are prone to converge to local minima for this application, particularly when the landmarks are in degenerate configurations. The state is given by $\xi_i = x_i = [t_i^\top v_i^\top]^\top$, where $t_i$ and $v_i$ are the position and velocity at time index $i$, respectively.<fnref n="4" /> The motion-prior errors are given by $e_{ij} = x_i - \Phi_{ij}x_j$, where $\Phi_{ij}$ is the so-called transition matrix, and $W_{ij}$ is calculated using the GP motion-prior covariance [5]. Measurement error terms are given by $e_k \in \mathbb{R}^{N_k}$ with $N_k$ the number of landmarks seen at time index $k$, and the $i$-th element given by $e_k[i] = \tilde{d}_{ki}^2 - \|t_k - m_i\|^2$, where $m_i$ is the $i$-th known landmark position, and $\tilde{d}_{ki}$ is the corresponding distance measurement. The matrix $W_k$ is set to the assumed inverse covariance matrix of the squared distance measurements.

**Example MW.** In matrix-weighted localization (MW), adopted from [28], we seek to estimate the pose of a moving device based on Euclidean observations of known landmarks. Because such observations typically come from sensors

<footnote n="3">³ Note that we are not limited to chains, but chains are a particularly well-behaved structure with uniformly sized, small maximum cliques.</footnote>

<footnote n="4">⁴ For conciseness, we focus on the constant-velocity motion prior in this paper, but the extension to other priors, such as constant-acceleration or zero-velocity, is straightforward [20].</footnote>
with varying accuracy in the radial and lateral directions, it is often crucial to allow for matrix-weighted, i.e., anisotropic, measurement noise. It was found in [28] that this problem exhibits local minima, particularly as measurement anisotropicity increases. We define the state by $\xi_i : T_{i0} \in SE(3)$, composed of rotation $C_{i0} \in SO(3)$, and translation $t_{0i}^k \in \mathbb{R}^3$, as explained in more detail in [28]. Given this state definition, the relative error terms can be expressed as $e_{ij} = \text{vec}(T_{i0} - \tilde{T}_{ij}T_{j0})$, where $\tilde{T}_{ij}$ is a measurement of the relative transform from pose $i$ to pose $j$. For relative measurements, we assume isotropic noise, thus $W_{ij} = \sigma_{ij}^{-2}I$. For absolute state measurements, we define $e_{k,l} = \tilde{m}_{lk}^k - T_{k0}m_{l0}^0$, where $\tilde{m}_{lk}^k$ is the Euclidean measurement of landmark $l$ as seen from pose $k$, and $m_{l0}^0$ are the known landmark positions, expressed in world frame, and $W_k$ is a typically anisotropic inverse covariance matrix.

## 4.2 NLS to Quadratically Constrained Quadratic Program

In many applications of interest, Problem (3) can be written as a polynomial optimization problem, which in turn can be formulated as an equivalent QCQP. The intuition is that we introduce a 'lifted' vector $x$ that contains $\xi$ and potentially some higher-order 'lifting functions' thereof – enough so that both the cost and the constraints stemming from $\mathcal{X}$ become quadratic in $x$.

More specifically, we call the lifting functions $\ell_k(\xi_k)$ and introduce $h = 1$, the so-called 'homogenization' variable, which ensures that linear and constant cost terms can also be written in quadratic form. Introducing the vectors<fnref n="5" />

$$x_k := [h \xi_k^\top \ell_k(\xi_k)^\top]^\top, \quad x_{ij} := [h \xi_i^\top \ell_i(\xi_i)^\top \xi_j^\top \ell_j(\xi_j)^\top]^\top, \quad (4)$$

the QCQP reformulation of (3) takes the form:

$$\begin{align}
\hat{x} &= \arg \min_{x} \sum_{k \in \mathcal{Q}} x_k^\top Q_k x_k + \sum_{(i,j) \in \mathcal{R}} x_{ij}^\top R_{ij} x_{ij} \tag{5} \\
\text{s.t.} \quad &x_k^\top A_{jk}^s x_k = b_{jk}, \quad k \in \mathcal{V}, j_k = 1 \dots N_k^s, \nonumber \\
&x_k^\top A_{jk}^p x_k = 0, \quad k \in \mathcal{V}, j_k = 1 \dots N_k^p \nonumber
\end{align}$$

where $N_k^p$ is the number of primary constraints of node $k$ (from $\mathcal{X}$) and $N_k^s$ is the number of substitution constraints. The link between the cost matrices $Q_k$ and $R_{ij}$ and the original error terms in (3), as well as the form of the substitution and primary constraint matrices $A_{jk}^s, A_{ik}^p$, is given in Appendix A.1.<fnref n="6" />

**Factor-graph representation.** We represent the reformulated Problem (5) as a variant of factor graphs that includes constraints on variables, which has

<footnote n="5">⁵ We include one homogenization variable per variable group for simplicity of exposition. In practice, the homogenization variable can be shared among different groups.</footnote>

<footnote n="6">⁶ Note that each primary constraint is only a function of one variable group, akin to the separability assumption assumed in related works [61], which is crucial for efficient problem distribution.</footnote>
<figure><img src="image_5.png" /><figcaption>**Fig. 2.** Factor graphs [18] for the localization problems treated in this paper. On the left is the factor graph of the original NLS problem (3). On the right is the variant of factor graphs proposed to describe the QCQP reformulation (5). This graph is formulated regarding the lifted states $x_k$ and includes constraint and cost factors. Round nodes are variables, square nodes are cost terms, and diamonds are constraints.</figcaption></figure>

also been considered in [16, 47]. We call this factor graph $\mathcal{G}_q$, shown in Figure 2, where constraint factors are represented with diamonds. The factor graph can be seen as the generalization of $\mathcal{G}_f$ (which represents the cost of (3)) to the Lagrangian of (5). While there is a one-to-one mapping from the connectedness of the factor graph $\mathcal{G}_f$ to the sparsity of the information matrix of the original problem (3) [18], the connectedness of the factor graph $\mathcal{G}_q$ reflects the aggregate sparsity of the matrices $Q$, $A_{jk}^s$, and $A_{jk}^p$ in (5).

**Examples.** For brevity, we provide the details of the introduced lifting functions for the CT-RO and MW examples in Appendix A.2. The sparsity patterns of the problem matrices for the two examples are provided in Figure 3.

## 4.3 QCQP to Semidefinite Program

We first derive the monolithic semidefinite relaxation of (5), which is our centralized baseline. To this end, we introduce the combined vector $x \in \mathbb{R}^{N_x}$,

$$x := [h \xi_1^\top \ell_1(\xi_1)^\top \cdots \xi_N^\top \ell_N(\xi_N)^\top]^\top, \quad (6)$$

and the projection matrices $E^k$, $E^{ij}$, defined implicitly through $x_k = E^k x$, and $x_{ij} = E^{ij} x$. Then, we rewrite (5) as

$$ \begin{gathered} \hat{x} = \arg \min_{x} x^{\top} Q x + x^{\top} R x \\ \text{s.t.} \quad x^{\top} \bar{A}_{jk}^{s} x = b_{jk}, \quad k \in \mathcal{V}, j_k = 1 \dots N_k^{s}, \quad (7) \\ x^{\top} \bar{A}_{jk}^{p} x = 0, \quad k \in \mathcal{V}, j_k = 1 \dots N_k^{p} \end{gathered} $$

with $Q = \sum_k \bar{Q}_k$ and $R = \sum_{ij} \bar{R}_{ij}$. The constraint matrices, $\bar{A}_{jk}^p$, $\bar{A}_{jk}^s$, and the cost matrix $\bar{Q}_k$ in (7), are obtained using $\bar{Y}_k = E^k Y_k E^{k\top}$. Similarly, we have defined $\bar{R}_{ij} = E^{ij} \bar{R}_{ij} E^{ij\top}$.

Every QCQP can be relaxed to an SDP by using the succession of identities $x^\top Q x = \text{tr}(x^\top Q x) = \text{tr}(Q x x^\top) = \{\text{tr}(Q X)|X = xx^\top\} = \{\text{tr}(Q X)|X \succeq$
<figure><img src="image_6.png" /><figcaption>Fig. 3. Sparsity patterns for CT-RO and MW. We show the aggregate sparsity of the cost matrix $Q+R$, and the primary, substitution, and redundant constraints: $A^p, A^s$, and $A^r$. The overlaid colored boxes on the cost matrix mark our used chordal decomposition of the aggregate sparsity patterns. The right-most plots show automatically generated chordal decompositions (different colors mark different cliques).</figcaption></figure>

0, rank $\mathbf{X} = 1\}$. By relaxing the rank-one constraint, we obtain the following primal relaxation of (7):

$$ 
\begin{aligned}
\hat{\mathbf{X}} &= \arg \min_{\mathbf{X} \succeq 0} \operatorname{tr}(\mathbf{Q}\mathbf{X}) + \operatorname{tr}(\mathbf{R}\mathbf{X}) \\
\text{s.t.} \quad & \operatorname{tr}(\bar{\mathbf{A}}_{jk}^s \mathbf{X}) = b_{jk}, \quad k \in \mathcal{V}, j_k = 1 \dots N_k^s, \qquad (8) \\
& \operatorname{tr}(\bar{\mathbf{A}}_{jk}^p \mathbf{X}) = 0, \quad k \in \mathcal{V}, j_k = 1 \dots N_k^p \\
& \operatorname{tr}(\bar{\mathbf{A}}_i^r \mathbf{X}) = 0, \quad i = 1 \dots N_r
\end{aligned}
$$

where we have introduced $\bar{\mathbf{A}}_i^r$, so-called redundant constraints. Redundant constraints are trivially satisfied constraints of (7), i.e., they are linearly dependent in (7) but are independent in (8). Their role is to help enforce the structure that may be lost when dropping the rank constraint, thus encouraging the solution $\hat{\mathbf{X}}$ to be of low rank. This is important because only if the optimal solution $\hat{\mathbf{X}}$ of (8) is of rank one can we obtain the globally optimal solution $\hat{\mathbf{x}}$ of (7) from the factorization $\hat{\mathbf{X}} = \hat{\mathbf{x}}\hat{\mathbf{x}}^\top$ (in which case we say that the relaxation is *tight*).

We end this section by making a few observations about Problem (8). The constraint $\mathbf{X} \succeq 0$ links variables that otherwise would not interact through the cost or constraint matrices. However, Theorem 1 suggests that if the measurement graph $\mathcal{G}_p$ is chordally sparse, we can regain efficiency by splitting the problem into its maximal cliques. Note that the redundant constraints $\mathbf{A}_i^r$, which may be required for tightness, can however induce new dependencies in $\mathcal{G}_p$ and reduce sparsity (i.e., increase the size of the maximal cliques). Thus, we deal with opposing forces: without redundant constraints, the SDP relaxation may not be tight, preventing us from getting a globally optimal estimate. However, adding redundant constraints may reduce sparsity and prevent us from effectively solving the SDP. As we see next, the two example problems have tight semidefinite relaxations with essentially unchanged sparsity patterns.

**Example CT-RO.** For this problem, the relaxation is already tight without the need for redundant constraints, as observed in [20] and [19].

**Example MW.** For this problem, we can add the following well-known redundant constraints [9]: $\text{vech}(\mathbf{C}_{i0})^\top \text{vech}(\mathbf{C}_{i0}^\top) = \text{vech}(\mathbf{I}_3)^7$ and the redundant

<footnote n="7">⁷ We use only 5 of these 6 constraints since one is linearly dependent.</footnote>
handedness constraints $C_{i0}[2] \times C_{i0}[3] = C_{i0}[1]$ and $C_{i0}[3] \times C_{i0}[1] = C_{i0}[2]$. It was shown in [28] that these redundant constraints may significantly improve tightness as the noise level increases and the number of measurements decreases. Since the constraints depend only on one pose at a time, they do not change the structure of the measurement graph, which can also be observed from the sparsity patterns in Figure 3.

## 4.4 Decomposed solution: dSDP

We can now describe our monolithic solver, which decomposes the problem into smaller parts but runs in a centralized way. We adopt a vectorized SDP formulation from here on for notational convenience. In particular, we introduce variable $\mathbf{y} = \text{vech}(\mathbf{X})$ and $\mathbf{y}_k = \text{vech}(\mathbf{X}_k)$. Using this notation, we can write (8) in the standard dual form for conic programs [8]:

$$ \hat{\mathbf{y}} = \arg \min_{\mathbf{y} \succeq 0} \sum_{k \in \mathcal{Q}} \tilde{q}_k^\top \mathbf{y}_k + \sum_{i,j \in \mathcal{R}} \tilde{r}_{ij}^\top \mathbf{y}_{ij}, \qquad (9) \\ \text{s.t. } \mathbf{A}_n \mathbf{y}_n = \mathbf{b}_n, \quad n \in \mathcal{V} $$

where we have introduced $\tilde{q}_k := \text{vech}(Q_k)$, $\tilde{r}_{ij} := \text{vech}(R_{ij})$, and we have grouped all primary, substitution, and redundant constraints into $\mathbf{A}_n$:

$$ \mathbf{A}_n^\top := [\cdots \text{vech}(\mathbf{A}_{j_k}^p) \cdots \text{vech}(\mathbf{A}_{j_k}^s) \cdots \text{vech}(\mathbf{A}_{j_k}^r) \cdots], \qquad (10) $$

and $b_n$ is defined accordingly. Typically, the lack of speed in SDPs is due to the semidefinite constraint on $\mathbf{y}$. However, we can now apply Theorem 1, allowing us to decompose this constraint into a sequence of smaller positive-definiteness constraints. Indeed, if the aggregate sparsity pattern of the SDP is chordally sparse, we can replace the condition $\mathbf{y} \succeq 0$ with $c_t \succeq 0, t = 1 \dots T$ where $c_t$ correspond to the variables from the clique $t$.

Given the aggregate sparsity pattern of Problem (8), we could automatically compute the maximal cliques using efficient heuristics [1]. Figure 3, on the right, gives example decompositions using the approximate minimum degree ordering. We observe that the method has generated some fill-in between variables groups that do not have dependencies in the original graph $G_q$. This reduces the sparsity, which would affect our solver's computational cost. Instead, we suggest using a manual decomposition, which, although potentially less minimal, avoids such dependencies and generates fewer cliques. Indeed, a straightforward decomposition of the problem is given by $c_i = (h, y_i, y_{i+1})$, also shown in Figure 3 for the same example. For simplicity, we use the trivial maximal chordal extension within each clique. Exploiting chordal sparsity within each clique is expected to only lead to a minor improvement.<fnref n="8" />

<footnote n="8">⁸ This manual decomposition also works for graphs that are not simple chains: we can first find a chordal decomposition at the variable block level and then use the maximal chordal extension within each block.</footnote>
<figure><img src="image_7.png" /><figcaption>**Fig. 4.** Visualization of the decomposition of the chordal localization problems. On the left is the factor graph split into maximal cliques, with the shared cost terms $Q_i, i = 1...N-1$ split into half, and constraints added to enforce the overlap of neighboring cliques. On the right is the corresponding clique tree, used for dSDP (constraints in blue) and dSDP-admm (consensus variable $z$ and constraints shown in green).</figcaption></figure>

More formally, we define the chordal clique tree $T_c := (\mathcal{V}_c, \mathcal{E}_c)$, whose vertices index the cliques, and edges between two cliques indicate that they share variables. This is necessarily a tree because any loop closure in this graph would violate the definition of maximal cliques. Using this notion and Theorem 1, we can define the (dSDP) problem:

$$ \begin{array}{l} \{\hat{c}_1, \dots, \hat{c}_T\} = \arg \min_{c_1, \dots, c_T} \sum_{t \in \mathcal{V}_c} q_t^\top c_t \\ \text{s.t.} \quad A_t c_t = b_t, \quad t \in \mathcal{V}_c, \\ \qquad S_t^{t'} c_t = S_{t'}^t c_{t'}, \quad t, t' \in \mathcal{E}_c \\ \qquad c_t \succeq 0, \quad t \in \mathcal{V}_c \end{array} \qquad (11) $$

where $S_t^{t'}$ is the indexing matrix selecting the shared variables with $t'$ from clique $t$, and $q_t$ is the clique-wise cost vector, obtained from splitting the cost variables equally across overlapping nodes. Problem (11) can be solved by any off-the-shelf SDP solver.

**Factor-graph representation.** The process of splitting variables and associated cost terms is visualized, for our example problems, in the factor graph in Figure 4(a). We also provide a sketch of the associated clique tree, which includes the introduced overlap constraints in (11). Note that the proposed decomposition has the advantage of having a clean factor-graph representation since it keeps the original variable groups intact.

Problem (11) has the potential of being significantly faster to solve because the large SDP variable is split into $T := |\mathcal{V}_c|$ smaller ones. As the complexity of interior-point solvers depends on both the problem dimension and the number of constraints [2], the speedup is a function of the number of cliques, their size, and their connectivity. We will see that this speed-up is significant for the studied localization problems and our manual decomposition. Note that Problem (11) is still solved in a centralized manner as the overlapping constraints introduce dependencies between the cliques. Next, we formulate the distributed algorithm.
## 4.5 Distributed solution: dSDP-admm

In this section, we state a distributed version of dSDP using ADMM, which we call dSDP-admm. Our method is an adaptation of the so-called consensus ADMM [7, Chapter 7], where one variable group contains local but overlapping variables, and the other variable group contains the ‘consensus’ among overlapping variables. We create the feasible set $\mathcal{F}_t := \{c_t|c_t \succeq 0, A_t c_t = b_t\}$ and define the indicator function $\mathcal{I}_\mathcal{X}(x)$ which maps to 0 when $x \in \mathcal{X}$ and to $\infty$ otherwise. Then, we can reformulate (11) as follows:

$$ \begin{array}{ll} \min_{c_1, \dots, c_T} & \sum_{t \in \mathcal{V}_c} q_t^\top c_t + \mathcal{I}_{\mathcal{F}_t}(c_t) \\ \text{s.t.} & S^t z = c_t, \quad t \in \mathcal{V}_c \end{array} \qquad (12) $$

where we have introduced the consensus variable $z$, similar to $x$ in the original problem. Importantly, the semi-definite constraint is now on the smaller variables $c_t$. The clique tree corresponding to this problem is shown in Figure 4(b).

Applying the standard ADMM iterations to (12), we get:

$$ (\forall t) : c_t^i = \arg \min_{c_t} \mathcal{L}_{\rho}(c_t, z^i, \lambda_t^i), \qquad (13a) $$

$$ z^i = \arg \min_{z} \mathcal{L}_{\rho}(c^i, z, \lambda^i), \qquad (13b) $$

$$ (\forall t) : \lambda_t^{i+1} = \lambda_t^i + \rho (S^t z^i - c_t^i), \qquad (13c) $$

where the augmented Lagrangian is given by $\mathcal{L}_{\rho}(c, z, \lambda) = f(c) + g(z) + \sum_t \lambda_t^\top (S^t z - c_t) + \frac{\rho}{2} \|S^t z - c_t\|^2$. Thanks to the introduction of $z$, step (13a) is decomposed into $T$ optimization problems, each of which is an SDP with quadratic cost that can reformulated to a linear SDP using a linear matrix inequality, see e.g., [36]. The second step (13b) is an unconstrained problem with a closed-form solution: each element of $z$, consists of the average the cliques which depend on it. Therefore, $z$ can also be updated in a distributed manner, where current values of $c^i$ need to be shared only between overlapping cliques. The entire algorithm can thus be run in a distributed fashion.

# 5 Localization Results

We test the two methods in simulation on the two example localization problems, evaluating their performance in terms of efficiency, optimality, and accuracy, compared with local and standard SDP solvers.

## 5.1 Simulation Setup

**Example CT-RO.** We draw $N_m$ landmarks uniformly at random from a cube of dimension $10m$. Then, within the same cube, we generate a random trajectory of $N$ states at times sampled uniformly at random and sorted from
<figure><img src="image_8.png" /><figcaption>**Fig. 5.** Solver times for increasing problem sizes for range-only localization (left) and matrix-weighted localization (right). The dashed gray lines correspond to $O(N)$ and $O(N^3)$. The presented sparsity-exploiting algorithms have a complexity comparable with the local solver and with little variance. On average, the local solver is faster than the decomposed methods but has a high variance (vertical bars) depending on the initialization.</figcaption></figure>

0 to $N-1$. We use the constant-velocity motion prior [5, Section IV.A] with random initial velocity of $0.1 \text{ ms}^{-1}$ and standard deviation (STD) $\sigma_a = 0.2 \text{ ms}^{-2}$, making sure the trajectory stays within the cube. Then, we simulate distance measurements by adding i.i.d Gaussian noise of STD $\sigma_d = 10 \text{ cm}$, unless stated otherwise, to each squared distance. We set the prior distance measurement noise to $\tilde{\sigma}_d = 1 \text{ mm}$ and $\tilde{\sigma}_a = 0.2 \text{ ms}^{-2}$, set $\mathbf{W}_k = \tilde{\sigma}_d^{-2} \mathbf{I}_{N_k}$ and form $\mathbf{W}_{ij}$ according to the GP prior, as explained in [5].

**Example MW.** We generate $N_m$ landmarks uniformly at random from a cube of dimension $1 \text{ m}$ centered at the origin. Then, we sample poses uniformly at random with coordinates $t_{i0}^0$ in $(-0.1 \text{ m}, -0.1 \text{ m}, 2.9 \text{ m})$, $(0.1 \text{ m}, 0.1 \text{ m}, 3.1 \text{ m})$, with the $z$-axis pointing downwards with randomly generated yaw angles between $0$ and $2\pi$. We use camera parameters $f = 1077$, $c = 0$, $b = 0.12 \text{ mm}$ and pixel standard deviation $\sigma_u = \sigma_v = 1.0$, unless stated otherwise. We generate pixel and non-isotropic $\mathbf{W}_k$ from these parameters by linearizing the pinhole measurement model as explained in [28]. Similarly, we generate relative motion measurements and $\mathbf{W}_{ij}$ with translation STD $\sigma_t = 1 \text{ cm}$ and rotation STD $\sigma_r = 10 \text{ deg}$.

We solve one example per problem size for the timing studies, while for the noise study, we average over ten random problem instances. For all SDP solvers, we use the MOSEK Fusion API for Python [39], and we set one global tolerance parameter $\tau$ per algorithm, for primal/dual feasibility, and relative duality gap parameters. For dSDP and SDP, we use $\tau^{\text{dSDP}} = \tau^{\text{SDP}} = 10^{-10}$. For the local solvers, we use our custom GN implementations with the stopping criterion that the element-wise maximum of the gradient is below $\tau^{\text{GN}} = 10^{-7}$ or at a number of iterations above $N_{\text{it}}^{\text{GN}} = 100$. We consider random initializations of GN, drawn randomly from a Gaussian of standard deviation $0.5$ around the ground truth (local), and initializing at ground truth (local-gt).

For dSDP-admm, we use the penalty adaptation rule described in [7, eq. (3.13)], with $\mu = 10$ and $\tau = 2.0$. For the convergence criteria, we use thresh-
<figure><img src="image_9.png" /><figcaption>**Fig. 6.** Tightness and accuracy studies for increasing noise levels for range-only localization (left) and matrix-weighted localization (right). Noise is added to the squared distance measurements (in meters) and pixel observations. All results are with $N = 100$ and $N_m = 8$.</figcaption></figure>

olds on the two-norms of the primal and dual residuals as suggested in [7, eq. (3.12)], where we use only the relative criterion to calculate the threshold with $\epsilon^{rel} = 10^{-10}$ or maximum number iterations $N_{it}^{dSDP-admm} = 10$. Because dSDP-admm does not require its inner iterations to be very precise, we set the tolerance $\tau^{dSDP-admm}$ to $10^{-3}$. We run all algorithms on a server with 2 Intel Xeon processors, resulting in 24 physical cores and 48 threads, and with 320 GB of RAM.

## 5.2 Results

**Efficiency.** First, we investigate the speed of the proposed methods as a function of the problem size. Figure 5 shows the performance of the two proposed algorithms as a function of the number of poses or positions. The decomposed methods are significantly faster than the full SDP method. In fact, these decomposed methods are competitive with the local GN solver (same runtime complexity), yet can still provide a certified globally optimal solution.

Due to hardware limitations and communication overhead, the optimal number of threads for dSDP-admm is fixed to 10, and is run for a fixed number of iterations 3. We expect the performance of dSDP-admm, currently on par with dSDP, to improve as more computational nodes are available.

**Optimality.** Next, we investigate the tightness of our relaxations with respect to the noise level. To this end, we compute the Eigenvalue ratio (EVR) $\lambda_0/\lambda_1$ with $\lambda_0, \lambda_1$ the largest and second-largest eigenvalues of the solution of the relaxation $\mathbf{X}^*$, respectively. This ratio measures how close the solution is to being rank one. Looking at the EVR in Figure 6, we observe that the relaxation becomes less tight with increasing noise levels, as expected. For CT-RO in particular, the fact that we terminate dSDP-admm after a fixed number of iterations means that the solution quality deteriorates as the noise increases. We argue that this is due to a looser connection between nodes; consisting only of a motion prior as opposed to pose-to-pose measurements. It may thus be harder for the alternating algorithm to reach 'consensus'; for higher noise levels, the
different subproblems pull in different directions at each iteration. Here, we see
the fundamental speed vs. accuracy trade when using the ADMM algorithm. We
have opted for speed here, and the ADMM algorithm struggles to yield accurate
results in the given fixed time. The SDP and dSDP algorithms, on the other
hand, reach consistently high EVR for all noise levels for CT-RO.

For MW, we observe that adding redundant constraints significantly affects
the tightness of the relaxation and renders the EVR almost constant for all con-
sidered noise levels. The added computational cost for achieving higher tightness
is minimal, as shown in Figure 5. We observe again that dSDP-admm is signif-
icantly less tight than the other algorithms but still achieves high enough EVR
to suggest a rank-one solution.

**Accuracy.** Finally, the accuracy of the algorithms is measured in terms of the distance between ground truth values and estimates. We choose the Euclidean distance for vector-valued functions and the chordal distance for rotation estimates. The accuracy obtained by the different solvers is reported in Figure 6. The randomly initialized local solver (**local**) has a high variance because it converges to local minima. The **local-gt** solver, SDP, and dSDP, on the other hand, solve the problem to almost identical accuracy. Only dSDP-admm has a significantly worse accuracy, particularly for the CT-RO example, because it would require more than the imposed three iterations to converge. This emphasizes the typical trade-off between accuracy and runtime present for dSDP-admm. With an improved implementation of dSDP-admm<fnref n="9" />, the method could be distributed more effectively and run with higher accuracy without compromising speed.

# 6 Conclusion and Future Work

We have shown that a large class of optimization problems that occur in robotics
allow for linear-time SDP solvers using the chordal decomposition of the under-
lying aggregate sparsity graph. This decomposition can be used to speed up
centralized computation but also lends itself to an ADMM-based distributed
implementation, which we also tested. The methods are on par with efficient
local solvers in terms of computational complexity, but with the important ad-
vantage that they are certifiably optimal for reasonable noise levels. Possible
improvements include the adoption of known heuristics for speeding up our
vanilla ADMM, such as diagonal rescaling for better conditioning [25], or the
use of an approximate interior-point solver [49]. The behavior of the algorithms
when the problem is not chordally sparse presents another interesting direction
of research, using, for example, factor-width decomposition [61], loopy belief
propagation [44], or using the chordal reduction as an initialization technique
for more exact solvers.

<footnote n="9">⁹ The current implementation uses Python, which does not allow for effective multi-threading.</footnote>
**Acknowledgments.** We would like to thank Matt Giamou (McMaster) and Alexander Amice (MIT) for introducing us to the idea of exploiting chordal sparsity in SDPs. FD is funded by the ANR JCJC project NIMBLE (ANR-22-CE33-0008), and most of this work was conducted while at the University of Toronto, funded by the Swiss National Science Foundation, Postdoc Mobility under Grant 206954. The Natural Sciences and Engineering Research Council of Canada also partly funded this work.

**Disclosure of Interests.** The authors have no competing interests to declare that are relevant to the content of this article.

# A Appendix

## A.1 General formulation

We assume that the functions $\ell_k(\cdot)$ in (4) include enough higher-order terms in $\xi_i$ so that we can write $e_k(\xi_k) = B_k x_k(\xi_k)$ and $e_{ij}(\xi_i, \xi_j) = B_{ij} x_{ij}(\xi_i, \xi_j)$. This allows us to rewrite the cost terms in the required form by introducing $Q_k := B_k^\top W_k B_k$ and $R_{ij} := B_{ij}^\top W_{ij} B_{ij}$. We further assume that each term of the lifted vectors can be enforced with a quadratic constraint, which is always possible when the substitutions are polynomial or rational functions of any order.<fnref n="10" /> Finally, assuming that the feasible sets $\mathcal{X}_n$ can also be characterized by quadratic constraints, which we denote by $A_i^p$, we obtain the generic equivalent QCQP formulation (5).

## A.2 Example reformulations

**Example CT-RO.** By using just one substitution: $\ell_k(\xi_k) := \|t_k\|^2$, we get the lifted vector $x_k(\xi_k)^\top := [1 \ t_k^\top \ v_k^\top \ \|t_k\|^2]$ and we can write each absolute error term as

$$e_k(\xi_k)[i] = [\tilde{d}_{ki}^2 - \|m_i\|^2 \ 2m_i^\top \ 0_3^\top - 1] x_k(\xi_k) = b_{ki}^\top x_k(\xi_k), \quad (14)$$

where $b_{ki}$ corresponds to one row of $B_k$ from Section A.1. Similarly, we have:

$$e_{ij}(\xi_i, \xi_j) = [0_6 \ I_6 \ 0_6 \ \Phi \ 0_6] x_{ij}(\xi_i, \xi_j). \quad (15)$$

Since the substitution is the squared norm of the position, it is a polynomial and can thus be trivially written in quadratic form.

**Example MW.** For this example, we note that the cost is already quadratic in the constraints, so no substitutions are necessary. Furthermore, the feasible set is given by $\mathcal{X}_n := SE(3)$, which can be enforced by the quadratic constraints $C_{i0}^\top C_{i0} = I_3$ and $C_{i0}[1] \times C_{i0}[2] = C_{i0}[3]$ (with $C_{i0}[j]$ the j-th column of $C_{0i}$), which enforces handedness and thus substitutes the (cubic) determinant constraint in the definition of $SO(3)$ [9].

<footnote n="10">¹⁰ To see this, note that for an original cost of degree $k$, we can add all monomials up to degree $k$, in which case each term can be recursively constrained using its preceding terms. In practice it is often desirable to use as few terms as possible [58].</footnote>
References

1. Amestoy, P.R., Davis, T.A., Duff, I.S.: An Approximate Minimum Degree Ordering Algorithm. SIAM Journal on Matrix Analysis and Applications 17(4), 886–905 (1996), https://epubs.siam.org/doi/10.1137/S0895479894278952

2. Andersen, E., Roos, C., Terlaky, T.: On implementing a primal-dual interior-point method for conic quadratic optimization. Mathematical Programming 95(2), 249–277 (2003), http://link.springer.com/10.1007/s10107-002-0349-3

3. Bambade, A., El-Kazdadi, S., Taylor, A., Carpentier, J.: ProxQP: Yet another Quadratic Programming Solver for Robotics and beyond. In: Robotics: Science and Systems (2022), http://www.roboticsproceedings.org/rss18/p040.pdf

4. Barfoot, T.D.: State Estimation for Robotics. Cambridge University Press (2017), https://www.cambridge.org/core/product/identifier/9781316671528/type/book

5. Barfoot, T.D., Hay Tong, C., Särkkä, S.: Batch continuous-time trajectory estimation as exactly sparse Gaussian process regression. In: Robotics: Science and Systems (2014), http://www.roboticsproceedings.org/rss10/p01.pdf

6. Barfoot, T.D., Holmes, C., Dümbgen, F.: Certifiably Optimal Rotation and Pose Estimation Based on the Cayley Map. International Journal of Robotics Research (2024), https://doi.org/10.1177/02783649241269337

7. Boyd, S.: Distributed Optimization and Statistical Learning via the Alternating Direction Method of Multipliers. Foundations and Trends in Machine Learning 3(1), 1–122 (2010), http://www.nowpublishers.com/article/Details/MAL-016

8. Boyd, S., Vandenberghe, L.: Convex Optimization. Cambridge University Press (2004), https://web.stanford.edu/~boyd/cvxbook/

9. Briales, J., Gonzalez-Jimenez, J.: Convex Global 3D Registration with Lagrangian Duality. In: IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). pp. 5612–5621 (2017), http://ieeexplore.ieee.org/document/8100078/

10. Burer, S., Monteiro, R.D.: Local Minima and Convergence in Low-Rank Semidefinite Programming. Mathematical Programming 103(3), 427–444 (2005), https://doi.org/10.1007/s10107-004-0564-1

11. Carlone, L., Calafiore, G.C., Tommolillo, C., Dellaert, F.: Planar Pose Graph Optimization: Duality, Optimal Solutions, and Verification. IEEE Transactions on Robotics 32(3), 545–565 (2016), https://doi.org/10.1109/TR0.2016.2544304

12. Carlone, L., Kaouk Ng, M., Du, J., Bona, B., Indri, M.: Simultaneous Localization and Mapping Using Rao-Blackwellized Particle Filters in Multi Robot Systems. Journal of Intelligent & Robotic Systems 63(2), 283–307 (2011), http://link.springer.com/10.1007/s10846-010-9457-0

13. Choudhary, S., Carlone, L., Nieto, C., Rogers, J., Christensen, H.I., Dellaert, F.: Distributed mapping with privacy and communication constraints: Lightweight algorithms and object-based models. The International Journal of Robotics Research 36(12), 1286–1311 (2017), http://journals.sagepub.com/doi/10.1177/0278364917732640

14. Cieslewski, T., Choudhary, S., Scaramuzza, D.: Data-Efficient Decentralized Visual SLAM. In: IEEE International Conference on Robotics and Automation (ICRA). pp. 2466–2473 (2018), https://ieeexplore.ieee.org/document/8461155/

15. Cohn, T., Petersen, M., Simchowitz, M., Tedrake, R.: Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets. In: Robotics: Science and Systems (2023), http://www.roboticsproceedings.org/rss19/p057.pdf
16. Cunningham, A., Paluri, M., Dellaert, F.: DDF-SAM: Fully distributed SLAM using Constrained Factor Graphs. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. pp. 3025–3030 (2010), https://ieeexplore.ieee.org/document/5652875

17. Dall'Anese, E., Zhu, H., Giannakis, G.B.: Distributed Optimal Power Flow for Smart Microgrids. IEEE Transactions on Smart Grid 4(3), 1464–1475 (2013), https://ieeexplore.ieee.org/document/6502290/

18. Dellaert, F.: Factor graphs: Exploiting structure in robotics. Annual Review of Control, Robotics and Autonomous Systems 4, 141–166 (2021), https://www.annualreviews.org/doi/10.1146/annurev-control-061520-010504

19. Dümbgen, F., Holmes, C., Agro, B., Barfoot, T.D.: Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations. IEEE Transactions on Robotics 40, 4338–4358 (2024), http://arxiv.org/abs/2308.05783

20. Dümbgen, F., Holmes, C., Barfoot, T.D.: Safe and Smooth: Certified Continuous-Time Range-Only Localization. IEEE Robotics and Automation Letters 8(2), 1117–1124 (2023), https://doi.org/10.1109/LRA.2022.3233232

21. Eriksson, A., Olsson, C., Kahl, F., Chin, T.J.: Rotation Averaging and Strong Duality. In: IEEE/CVF Conference on Computer Vision and Pattern Recognition. pp. 127–135 (2018), https://ieeexplore.ieee.org/document/8578119/?arnumber=8578119

22. Fan, T., Ortiz, J., Hsiao, M., Monge, M., Dong, J., Murphey, T., Mukadam, M.: Decentralization and Acceleration Enables Large-Scale Bundle Adjustment. In: Robotics: Science and Systems (2022), http://arxiv.org/abs/2305.07026

23. Fredriksson, J., Olsson, C.: Simultaneous Multiple Rotation Averaging using Lagrangian Duality. In: Asian Conference on Computer Vision. pp. 245–258 (2013), http://link.springer.com/10.1007/978-3-642-37431-9_19

24. Fukuda, M., Kojima, M., Murota, K., Nakata, K.: Exploiting Sparsity in Semidefinite Programming via Matrix Completion I: General Framework. SIAM Journal on Optimization 11(3), 647–674 (2001), https://epubs.siam.org/doi/10.1137/S1052623400366218

25. Giselsson, P., Boyd, S.: Diagonal scaling in Douglas-Rachford splitting and ADMM. In: IEEE Conference on Decision and Control (CDC). pp. 5033–5039 (2014), https://ieeexplore.ieee.org/abstract/document/7040175

26. Goudar, A., Dümbgen, F., Barfoot, T.D., Schoellig, A.P.: Optimal Initialization Strategies for Range-Only Trajectory Estimation. IEEE Robotics and Automation Letters 9(3), 2160–2167 (2024), https://ieeexplore.ieee.org/document/10400824

27. Holmes, C., Barfoot, T.D.: An Efficient Global Optimality Certificate for Landmark-Based SLAM. IEEE Robotics and Automation Letters 8(3), 1539–1546 (2023), https://ieeexplore.ieee.org/document/10023976

28. Holmes, C., Dümbgen, F., Barfoot, T.D.: On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics. IEEE Transactions on Robotics 40, 4805–4824 (2024), https://doi.org/10.1109/TR0.2024.3475220

29. Kaess, M., Ranganathan, A., Dellaert, F.: iSAM: Incremental Smoothing and Mapping. IEEE Transactions on Robotics 24(6), 1365–1378 (2008), http://ieeexplore.ieee.org/document/4682731/

30. Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J.J., Dellaert, F.: iSAM2: Incremental Smoothing and Mapping using the Bayes Tree. International Journal of Robotics Research 31(2), 216–235 (2012), https://doi.org/10.1177/0278364911430419
31. Kahl, F., Henrion, D.: Globally Optimal Estimates for Geometric Reconstruction Problems. International Journal of Computer Vision **74**(1), 3–15 (2007), https://link.springer.com/10.1007/s11263-006-0015-y

32. Kalbat, A., Lavaei, J.: A Fast Distributed Algorithm for Decomposable Semidefinite Programs. In: IEEE Conference on Decision and Control (CDC). pp. 1742–1749 (2015), http://ieeexplore.ieee.org/document/7402462/

33. Kang, S., Xu, X., Sarva, J., Liang, L., Yang, H.: Fast and Certifiable Trajectory Optimization. In: Workshop on Algorithmic Foundations for Robotics (2024), http://arxiv.org/abs/2406.05846

34. Kim, S., Kojima, M., Mevissen, M., Yamashita, M.: Exploiting Sparsity in Linear and Nonlinear Matrix Inequalities via Positive Semidefinite Matrix Completion. Mathematical Programming **129**(1), 33–68 (2011), http://link.springer.com/10.1007/s10107-010-0402-6

35. Lasserre, J.B.: Convergent SDP-Relaxations for Polynomial Optimization with Sparsity. In: Mathematical Software (ICMS). pp. 263–272. Springer, Berlin, Heidelberg (2006)

36. Madani, R., Kalbat, A., Lavaei, J.: ADMM for sparse semidefinite programming with applications to optimal power flow problem. In: IEEE Conference on Decision and Control (CDC). pp. 5932–5939 (2015), http://ieeexplore.ieee.org/document/7403152/

37. Marcucci, T., Petersen, M., von Wrangel, D., Tedrake, R.: Motion planning around obstacles with convex optimization. Science Robotics **8**(84) (2023), https://www.science.org/doi/10.1126/scirobotics.adf7843

38. McGann, D., Lassak, K., Kaess, M.: Asynchronous Distributed Smoothing and Mapping via On-Manifold Consensus ADMM. In: IEEE International Conference on Robotics and Automation (ICRA). pp. 4577–4583 (2024), https://ieeexplore.ieee.org/abstract/document/10611193

39. MOSEK Aps: The MOSEK Fusion API for Python 10.1.22. Tech. rep. (2024), https://docs.mosek.com/latest/pythonfusion/index.html

40. Murai, R., Alzugaray, I., Kelly, P.H., Davison, A.J.: Distributed Simultaneous Localisation and Auto-Calibration Using Gaussian Belief Propagation. IEEE Robotics and Automation Letters **9**(3), 2136–2143 (2024), https://ieeexplore.ieee.org/document/10387679/

41. Nocedal, J., Wright, S.J.: Numerical Optimization. Springer Series in Operation Research, Springer-Verlag, New York, NY, 2nd edn. (2006), https://link.springer.com/book/10.1007/978-0-387-40065-5

42. O'Donoghue, B., Chu, E., Parikh, N., Boyd, S.: Conic Optimization via Operator Splitting and Homogeneous Self-Dual Embedding. Journal of Optimization Theory and Applications **169**(3), 1042–1068 (2016), https://doi.org/10.1007/s10957-016-0892-3

43. Olsson, C., Kahl, F., Oskarsson, M.: Branch-and-Bound Methods for Euclidean Registration Problems. IEEE Transactions on Pattern Analysis and Machine Intelligence **31**(5), 783–794 (2009), https://doi.org/10.1109/TPAMI.2008.131

44. Ortiz, J., Evans, T., Davison, A.J.: A visual introduction to Gaussian Belief Propagation. arXiv:2107.02308 [cs] (2021), http://arxiv.org/abs/2107.02308

45. Ortiz, J., Pupilli, M., Leutenegger, S., Davison, A.J.: Bundle Adjustment on a Graph Processor. In: IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). pp. 2413–2422 (2020), https://ieeexplore.ieee.org/document/9157499/
46. Papalia, A., Fishberg, A., O'Neill, B.W., How, J.P., Rosen, D.M., Leonard, J.J.: Certifiably Correct Range-Aided SLAM. IEEE Transactions on Robotics (2024), https://doi.org/10.48550/arXiv.2302.11614

47. Qadri, M., Sodhi, P., Mangelson, J.G., Dellaert, F., Kaess, M.: InCOpt: Incremental Constrained Optimization using the Bayes Tree. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 6381–6388 (2022), https://ieeexplore.ieee.org/document/9982178/

48. Rawlings, J.B., Mayne, D.Q., Diehl, M.: Model Predictive Control: Theory, Computation, and Design. Nob Hill Publishing, Santa Barbara, California, 2nd edn. (2020)

49. Rontsis, N., Goulart, P., Nakatsukasa, Y.: Efficient Semidefinite Programming with Approximate ADMM. Journal of Optimization Theory and Applications 192(1), 292–320 (2022), https://doi.org/10.1007/s10957-021-01971-3

50. Rosen, D.M., Carlone, L., Bandeira, A.S., Leonard, J.J.: SE-sync: A certifiably correct algorithm for synchronization over the special Euclidean group. International Journal of Robotics Research 38(2-3), 95–125 (2019), http://journals.sagepub.com/doi/10.1177/0278364918784361

51. Schubiger, M., Banjac, G., Lygeros, J.: GPU acceleration of ADMM for large-scale quadratic programming. Journal of Parallel and Distributed Computing 144, 55–67 (2020), https://www.sciencedirect.com/science/article/pii/S0743731520303063

52. Stellato, B., Banjac, G., Goulart, P., Bemporad, A., Boyd, S.: OSQP: An operator splitting solver for quadratic programs. Mathematical Programming Computation 12(4), 637–672 (2020), http://link.springer.com/10.1007/s12532-020-00179-2

53. Sun, Y., Andersen, M.S., Vandenberghe, L.: Decomposition in Conic Optimization with Partially Separable Structure. SIAM Journal on Optimization 24(2), 873–897 (2014), https://epubs.siam.org/doi/abs/10.1137/130926924

54. Tian, Y., Chang, Y., Arias, F.H., Nieto-Granda, C., How, J.P., Carlone, L.: Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems. IEEE Transactions on Robotics 38(4), 2022–2038 (2022), http://arxiv.org/abs/2106.14386

55. Tian, Y., Khosoussi, K., How, J.P.: Block-Coordinate Minimization for Large SDPs with Block-Diagonal Constraints. arXiv:1903.00597 [cs, math] (2019), http://arxiv.org/abs/1903.00597

56. Tian, Y., Khosoussi, K., Rosen, D.M., How, J.P.: Distributed Certifiably Correct Pose-Graph Optimization. IEEE Transactions on Robotics 37(6), 2137–2156 (2021), http://arxiv.org/abs/1911.03721

57. Vandenberghe, L.: Chordal graphs and semidefinite optimization. Foundations and Trends in Optimization 1(4), 241–433 (2015), http://dx.doi.org/10.1561/2400000006

58. Wang, J., Magron, V., Lasserre, J.B.: TSSOS: A Moment-SOS Hierarchy That Exploits Term Sparsity. SIAM Journal on Optimization 31(1), 30–58 (2021), https://epubs.siam.org/doi/10.1137/19M1307871

59. Wise, E., Giamou, M., Khoubyarian, S., Grover, A., Kelly, J.: Certifiably Optimal Monocular Hand-Eye Calibration. In: IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI). pp. 271–278 (2020)

60. Yang, H., Carlone, L.: Certifiably Optimal Outlier-Robust Geometric Perception: Semidefinite Relaxations and Scalable Global Optimization. IEEE Transactions on Pattern Analysis and Machine Intelligence 45(3), 2816–2834 (2023), http://
arxiv.org/abs/2109.03349

61. Zheng, Y., Fantuzzi, G., Papachristodoulou, A.: Chordal and factor-width decompositions for scalable semidefinite and polynomial optimization. Annual Reviews in Control 52, 243–279 (2021), https://www.sciencedirect.com/science/article/pii/S1367578821000717

62. Zheng, Y., Fantuzzi, G., Papachristodoulou, A., Goulart, P., Wynn, A.: Chordal decomposition in operator-splitting methods for sparse semidefinite programs. Mathematical Programming 180(1-2), 489–532 (2020), http://link.springer.com/10.1007/s10107-019-01366-3