# Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations

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

► To cite this version:

Frederike Dümbgen, Connor Holmes, Ben Agro, Timothy D. Barfoot. Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations. IEEE Transactions on Robotics, 2024, 40, pp.4338-4358. 10.1109/TRO.2024.3454570. hal-04884959

HAL Id: hal-04884959

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

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_1.png" /></figure>
# Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations

Frederike Dümben Member, IEEE, Connor Holmes Student Member, IEEE, Ben Agro, Timothy D. Barfoot Fellow, IEEE

**Abstract**—In recent years, semidefinite relaxations of common optimization problems in robotics have attracted growing attention due to their ability to provide globally optimal solutions. In many cases, it was shown that specific handcrafted redundant constraints are required to obtain tight relaxations and thus global optimality. These constraints are formulation-dependent and typically identified through a lengthy manual process. Instead, the present paper suggests an automatic method to find a set of sufficient redundant constraints to obtain tightness, if they exist. We first propose an efficient feasibility check to determine if a given set of variables can lead to a tight formulation. Secondly, we show how to scale the method to problems of bigger size. At no point of the process do we have to find redundant constraints manually. We showcase the effectiveness of the approach, in simulation and on real datasets, for range-based localization and stereo-based pose estimation. We also reproduce semidefinite relaxations presented in recent literature and show that our automatic method always finds a smaller set of constraints sufficient for tightness than previously considered.

**Index Terms**—Optimization and optimal control, Localization, Robot Safety, Global Optimality

## I. INTRODUCTION

MANY problems encountered in robotic state estimation, such as calibration and simultaneous localization and mapping (SLAM), are typically posed as nonlinear least-squares (NLS) optimization problems [1, 2]. Widely adopted solvers for these problems, such as Gauss-Newton (GN) and Levenberg-Marquardt (LM), have only local, if any, convergence guarantees and may return suboptimal solutions [3].

Over the past years, there has been a growing effort to exploit semidefinite relaxations of these optimization problems. They open the door to global optimality in at least two different ways: in certain cases, a (convex) semidefinite program (SDP) (or a sequence thereof) may be solved instead of the original nonconvex problem to find the globally optimal solution [4, 5, 6, 7]. In other cases, the Lagrangian dual of the SDP offers the possibility to construct so-called optimality certificates [8, 9] to determine the global optimality of the solutions obtained by local solvers.

FD is with Inria, École Normale Supérieure, PSL University, Paris, France. CH, BA and TDB are with the University of Toronto Robotics Institute, University of Toronto, Canada. Corresponding author: frederike.dumbgen@inria.fr

Received 3 April 2024; accepted 19 July 2024. Date of publication 4 September 2024;

The majority of this work was conducted while FD was at University of Toronto. The work of FD was supported in part by the ANR JCJC through Project NIMBLE under Grant ANR-22-CE33-0008 and in part by the SNF, Switzerland, Postdoc Mobility Grant under Grant 206954. The work of CH and TDB was supported by NSERC, Canada.

<figure><img src="image_2.png" /><figcaption>Fig. 1: The proposed method in a nutshell: we circumvent the lengthy manual process of finding redundant constraints to tighten a given semidefinite relaxation, using instead a sampling-based approach to automatically find them. This allows for the quick evaluation of different formulations and substitutions of a given optimization problem, enabling SDPs to be more widely adopted for finding globally optimal solutions to state estimation problems in robotics.</figcaption></figure>

The performance and feasibility of the aforementioned methods greatly depends on whether the SDP relaxation is *tight*. For example, for some problems, the globally optimal solution to the original problem can only be extracted from the SDP solution when the latter has rank one, in which case the relaxation is called tight [8, 10]. Similarly, common certifiable algorithms work only when strong duality holds [11], i.e., when the cost of the relaxed problem equals the cost of the original problem, which is also sometimes referred to as tightness [8, 5].<fnref n="1" />

One important enabler for tight relaxations has been a mathematical framework called *Lasserre's hierarchy* [12]. Put simply, the hierarchy consists of a sequence of semidefinite relaxations where polynomial terms of increasing order are added to the original problem. Calling the original variable dimension *d* and the hierarchy order *k*, each level consists of a $N_k$-dimensional SDP, with $N_k := \binom{d+k}{k}$. Under weak technical assumptions, any problem that can be written as a polynomial optimization problem (POP) can be lifted to a high enough order *k* to allow for a tight relaxation. The required order may be infinite, but many follow-up works have shown that tightness is obtained with finite *k* [13, 8, 14, 15]. More recently, it has been shown that many problems admit a *sparse Lasserre's hierarchy*, meaning that only some of the $N_k$ terms may be required at each level [16, 7].

As SDPs scale poorly with problem dimension, it is desirable to achieve tightness with as few additional higher-order substitutions as possible (ideally, with none). For this matter, it has been shown that so-called redundant constraints are paramount [8, 10, 13, 17]. However, to date, these constraints

<footnote n="1">¹To disambiguate between these two cases, we will use the terms *rank tightness* and *cost tightness* in this paper.</footnote>
are usually the result of a lengthy manual search process and it is often hard to retrace how the constraints were discovered. In [7], a method to find all “trivially satisfied” constraints is provided, but this process is costly and not all of these constraints may be necessary. Furthermore, using different formulations may lead to entirely different forms and numbers of required redundant constraints. Due to the lack of a systematic method of finding the right formulation and sufficient redundant constraints, practitioners often have to spend great effort in trial-and-error reformulations. This adds significant overhead as opposed to easy-to-use local solvers, and thus may hinder the wide adoption of SDP methods in robotics.

In this paper, we provide tools that help automate the search for redundant constraints required for tightness. We study in particular two classical state estimation problems: range-only (RO) localization and stereo-camera localization, which are nonconvex and exhibit local minima in which standard solvers may get stuck [18]. The proposed methods enable the globally optimal solution or certification of these and many other problems presented in Section VII. More concretely, we present two methods:

1) AUTOTIGHT determines if a problem in a given form can be tightened by adding enough automatically found redundant constraints.

2) AUTOTemplate generates a set of constraint templates that can be applied to new problems of any numbers of variables.

The focus of AUTOTIGHT is feasibility. It is purposefully kept simple (typically only a few lines of code) and should be performed on a small example problem. The focus of AUTOTemplate is scalability, enabling the automatic tightening of problems of any size, which is a strict requirement for typically high-dimensional problems encountered in robotics. The only prerequisite for using the provided tools is a method for randomly generating many problem setups (also called a “sampling oracle” in the literature [19]). For many use cases, creating such a method is part of the standard development process, and if not, it can usually be obtained fairly easily. Implementations of AUTOTIGHT and AUTOTemplate are publicly available.<fnref n="2" />

This paper is structured as follows. We put the proposed method in context with related work in Section II. Then, we introduce mathematical preliminaries for relaxing a NLS problem to an SDP in Section III. In Section IV, we present AUTOTIGHT and in Section V we propose AUTOTemplate, its scalable extension. We define the two example state-estimation problems in Section VI, and we provide novel insights on the tightness of their relaxations, as well as other problems, in Section VII. Finally, we test the method on real-world datasets for the example applications in Section VIII and conclude in Section IX.

## II. RELATED WORK

The list of problems in robotics and computer vision that have been solved using semidefinite relaxations is long and continues to grow. In vision-based state estimation, semidefinite relaxations have been widely explored, for example to solve rotation synchronization [20, 21, 22] or to perform camera pose estimation from pixel measurements [10, 17]. The first theoretical guarantees on tightness of these and other problems were given in [23, 9]. A set of analytical redundant constraints that successfully tightens many problem instances involving rotations has been proposed in [24, 10] and used successfully in follow-up works to certify, for instance, hand-eye calibration [25] and generalized-essential-matrix estimation [26]. Follow-up works have shown that tight relaxations can be achieved for robust cost functions, too [15, 8], of which a comprehensive overview, and a recipe for constructing trivially satisfied redundant constraints, is given in [7]. Robotics planning and control problems have recently also seen a surge of relaxation-based methods [14, 27, 27, 28]. Notably, specific redundant constraints (again, analytically specified) were found to be paramount for tightness in [27].

For some problems, no redundant constraints are required for tightness. For these problems, methods based on the *Burer Monteiro* approach [29] and the *Riemannian staircase* [30] have been shown to be very effective at finding the optimal solution with speeds competitive with efficient local solvers [4, 5, 31, 32]. Other methods have explored fast global optimality certificates of solutions of local solvers [33, 18]. To date, whenever redundant constraints are required for tightness, SDP solvers are generally too slow for real-time performance [7]. However, recent advances have shown that solvers can be significantly sped up when the optimal solution is of low rank [34, 6, 7]. More progress in developing fast SDP solvers is a requirement to enable their large-scale adoption for robotics; another requirement is finding the necessary redundant constraints for a larger class of problems. The method proposed in this paper contributes to the latter requirement.

Recently, a sampling paradigm has been explored in the sums-of-squares (SOS) literature to overcome some limitations of SDP solvers [19].<fnref n="3" /> The authors suggest to solve an SDP based on only a small number of feasible samples of the corresponding SOS program. The method thus exploits the geometry of the variety without the use of advanced concepts such as Gröbner bases [35]. This solution has shown great promise on small problems in control [36]. We use a similar paradigm in this paper, but instead of solving a sampling-based SDP, we use the samples to find generalizable constraints. This allows us to generate tight SDP relaxations for a wide range of problems, and by learning templates, allows us to generalize to new and higher-dimensional problems.

The present paper complements our prior and parallel work [37, 18, 38, 39] as follows. We show in [37] that the semidefinite relaxation commonly used in pose graph optimization and SLAM [33, 5] requires redundant con-

<footnote n="2">²The code is available as an open-source Python package at https://github.com/utiasASRL/constraint-learning.</footnote>

<footnote n="3">³There is a direct connection between the SOS relaxation and Lasserre's hierarchy (also called moment relaxation in this context); a clear description of this connection is given in [22].</footnote>
straints when using non-isotropic measurement-noise models. A preliminary form of the presented methods were used to automatically find these redundant constraints. The optimality certificate derived in [18] for RO localization does not require redundant constraints, but we show in the present paper that a different formulation does, and we find these constraints automatically. For RO pose estimation [38] and rotation estimation with the Cayley map [39], the presented methods (in simplified form) were also used to find the redundant constraints required for tightness.

### III. PRELIMINARIES

#### A. Notation

We denote vectors and matrices by bold-face lowercase and uppercase letters, respectively. The transpose of matrix $A$ is written as $A^\top$. The identity matrix in $d$ dimensions is $I_d$. A positive-semidefinite (PSD) matrix is written as $X \succeq 0$, and we denote the space of $N \times N$ PSD matrices by $\mathbb{S}_+^N$. The operator $\otimes$ is the Kronecker product and the operator $[\cdot]$ is the ceiling function. The inner product is denoted by $\langle \cdot, \cdot \rangle$, and the matrix inner product is defined as $\langle A, B \rangle = \text{tr}(A^\top B)$ where $\text{tr}(\cdot)$ is the trace operator. We introduce vec$\langle \cdot \rangle$, which extracts the elements of the upper-triangular part of a matrix, and multiplies the off-diagonal elements by $\sqrt{2}$. This ensures that $\langle A, B \rangle = \text{vech}(A)^\top \text{vech}(B)$, and is commonly used in SDP solvers [40]. We denote the inverse operation by $\text{vech}^{-1}(\cdot)$. $x[k]$ denotes the $k$-th element of vector $x$, starting at 1. For shorter notation, we use $[N]$ for the index set $\{1, \dots, N\}$.

#### B. Semidefinite Relaxations

In the remainder of this section, we provide theoretical background on semidefinite relaxations and duality theory necessary to understand this paper for the nonexpert reader. For an in-depth introduction to these topics we refer to [11, 3].

Most generally speaking, the subject of this paper is optimization problems of the form

$$ \min_{\theta \in \mathbb{R}^d} \{c(\theta) \mid e_j(\theta) = 0, j \in [N_e]\}, \quad (1) $$

where $\theta$ contains the variables, $c(\cdot)$ is the cost, and $e_i(\cdot)$ are equality constraints.<fnref n="4" /> In robotics, the cost is most commonly a (robust) NLS cost function, and the constraints may enforce the variables to lie, for example, in $SO(3)$ or $SE(3)$ [2]. The following is a simple (unconstrained) NLS problem that we will use throughout this paper to demonstrate the theoretical concepts. More realistic cost functions, which are also of the form (1), can be found in Section VI.

**Example** (stereo-1D, NLS). Inspired by camera localization, we propose the following pedagogical example problem:

$$ \min_{\theta} \sum_{i=1}^{N} \left( u_i - \frac{1}{(\theta - m_i)} \right)^2 =: c(\theta), \quad (2) $$

where $\theta \in \mathbb{R}$ is the decision variable, $u_i \in \mathbb{R}$ are measurements and $m_i \in \mathbb{R}$ are known landmarks. The problem is of the form (1), with $N_e = 0$, $d = 1$, and $c(\theta)$ a NLS function.

The problems in which we are interested can be lifted to a quadratically constrained quadratic program (QCQP), which is true for any polynomial optimization problem. In other words, we assume that we can rewrite (1) as

$$ \min_{x \in \mathbb{R}^N} \{ f(x) \mid g_j(x) = 0, l_i(x) = b_i, j \in [N_e], i \in [N_l] \cup \{0\} \}, \quad (3) $$

where $f$, $g_j$, and $l_i$ are quadratic in the lifted vector $x$, and $b_0 = 1, b_i = 0, i \in [N_l]$. The lifted vector is given by

$$ x^\top = [h \quad \theta^\top \quad z_1 \quad \cdots \quad z_{N_l}], \quad (4) $$

where we have introduced $z_i := l_i(\theta)$, higher-order lifting functions of $\theta$. By choosing enough of these substitutions, we can enforce that each substitution can itself be written as a quadratic constraint: $l_i(x) = 0$. We have added $h$ in (4) as a homogenization variable, enforced by $l_0(x) = x[1]^2 = 1$, which is common practice for allowing constant and linear functions to be written as quadratic functions (see, e.g., [41]).<fnref n="5" /> We illustrate these concepts in our pedagogical example in what follows, and refer the reader to Section VI for examples for writing more complex problems as QCQPs.

**Example** (stereo-1D, QCQP). Using the lifted vector

$$ x^\top = [h \quad \theta \quad z_1 \quad \cdots \quad z_N], \quad z_i = l_i(\theta) := \frac{1}{\theta - m_i}, \quad (5) $$

we can rewrite (2) in the form (3), with $f(x) = \sum_{i=1}^N (x[1]u_i - x[2+i])^2$, and $l_i(x) = z_i(\theta - m_i) = x[2+i]x[2] - x[2+i]x[1]m_i, i \in [N]$, which are all quadratic functions in $x$.

Since all functions in (3) are quadratic in $x$, we can write

$$ \min_{x \in \mathbb{R}^N} \{ x^\top Q x \mid x^\top A_0 x = 1, x^\top A_i x = 0, i \in [N_A] \}, \quad (Q) $$

where $Q$ and $A_i$, $i \in [N_A]$ are the cost and constraint matrices, respectively, and $N_A = N_e + N_l$. The matrix $A_0$ enforces the homogenization variable through the constraint $x[1]^2 = 1$. We call the constraints in (Q) the primary constraints.

**Example** (stereo-1D, known matrices). The cost and constraints matrices for the toy stereo problem are zero except for $Q[2+i, 2+i] = 1$ and $Q[1, 2+i] = Q[2+i, 1] = -u_i$ for $i \in [N]$, $Q[1, 1] = \sum_i u_i^2$, $A_i[1, 2+i] = A_i[2+i, 1] = -m_i$, $A_i[2, 2+i] = A_i[2+i, 2] = 1$, and $A_i[1, 1] = -2$, for $i > 0$.

Problem (Q) is a QCQP. Its solution space, defined by a set of polynomial equality constraints, defines a real algebraic variety, which is a central object of the field of algebraic geometry. This is by itself an active area of research, with methods existing for finding, for example, the minimal set of constraints to uniquely define a variety [35]. For the proposed paper, no knowledge of these advanced concepts is required as we take a numerical approach rather than an algebraic approach to describe the varieties. For the interested reader,

<footnote n="4">⁴We focus on equality constraints here for the sake of clarity. Note that inequality constraints can be added as long as they can also be written as quadratic constraints in the lifted vector and thus carried forward as quadratic inequality constraints in the relaxations. We include one example of inequality constraints in Section VII-D.</footnote>

<footnote n="5">⁵Technically, the first element of $x$ may thus take the value $-1$, but this does not pose a problem as the whole vector can then be simply negated.</footnote>
we do include some references to the algebraic geometry perspective in footnotes.

Because (Q) is, in general, NP-hard to solve, a common strategy is to relax the problem to a SDP by introducing $\mathbf{X} := \mathbf{x}\mathbf{x}^\top$, which can be enforced using $\mathbf{X} \ge 0$ and rank ($\mathbf{X}$) = 1, where the semidefinite constraint is convex while the rank constraint is not. We can drop the rank constraint and solve the following standard SDP:

$$ \min_{\mathbf{X} \in \mathbb{S}_+^N} \{ \langle \mathbf{Q}, \mathbf{X} \rangle \mid \langle \mathbf{A}_0, \mathbf{X} \rangle = 1, \langle \mathbf{A}_i, \mathbf{X} \rangle = 0, i \in [N_A] \}, \quad (P) $$

which is also called the primal or rank relaxation of (Q).

### C. Global Optimality and Duality Theory

The SDP problem can be used in several ways to make claims about the global optimality of candidate solutions. Let us denote by $\mathbf{X}^*$ the solution of (P) and its associated cost by $p^* := \langle \mathbf{Q}, \mathbf{X}^* \rangle$. If $\mathbf{X}^*$ has rank one, then it can be factored as $\mathbf{X}^* = \mathbf{x}^*\mathbf{x}^*^\top$ and $\mathbf{x}^*$ is the optimal solution to (Q) with $q^* := f(\mathbf{x}^*) = p^*$. This leads us to the first form of tightness used in this paper.

**Definition (Rank-tightness of the SDP relaxation).** We call the SDP relaxation (P) rank tight if its optimal solution $\mathbf{X}^*$ has rank one.

SDPs also enjoy a well-understood duality theory, which makes them great candidates for optimality certificates. The Lagrangian dual problem of (P) is given by

$$ d^* = \max_{\rho, \mathbf{\lambda}} \{-\rho \mid \mathbf{H}(\rho, \mathbf{\lambda}) := \mathbf{Q} + \rho \mathbf{A}_0 + \sum_{i=1}^{N_A} \lambda_i \mathbf{A}_i \ge 0\}, \quad (D) $$

where $\rho, \mathbf{\lambda} = [\lambda_1, \dots, \lambda_{N_A}]^\top \in \mathbb{R}^{N_A}$ are the Lagrangian dual variables corresponding to $\mathbf{A}_0$ and $\mathbf{A}_i, i \in [N_A]$, respectively. It is well known that we always have $d^* \le p^* \le q^*$. In what follows, we will also make the assumption that $d^* = p^*$, which holds under common constraint qualifications such as Slater's condition [11] (see Figure 1).

We can use the dual problem to, instead of solving the primal SDP and checking the rank of the solution, certify a local candidate solution $\hat{\mathbf{x}}$. Indeed, using the Karush-Kuhn-Tucker (KKT) conditions of (D), it is well-known (see e.g., [41]) that a solution candidate $\hat{\mathbf{x}}$ is globally optimal if there exist $\hat{\rho}, \hat{\mathbf{\lambda}}$ such that

$$ \left\{ \begin{array}{l} \mathbf{H}(\hat{\rho}, \hat{\mathbf{\lambda}})\hat{\mathbf{x}} = 0, \\ \mathbf{H}(\hat{\rho}, \hat{\mathbf{\lambda}}) \succeq 0. \end{array} \right. \qquad (6a) \qquad (6b) $$

If these two conditions hold, we have strong duality, meaning that $d^* = p^* = q^*$ (right plot of Figure 1). If we do not have strong duality, the above conditions cannot be jointly satisfied and we cannot use them to claim global optimality of a candidate solution. Therefore, we introduce the notion of cost tightness, a weaker form of tightness than rank tightness,<fnref n="6" /> which allows for candidate solutions to be certified:

**Definition (Cost-tightness of the SDP relaxation).** We call the SDP relaxation (P) cost tight if $d^* = p^* = q^*$.

Both forms of tightness may be useful in practice: when we have rank tightness, we can solve the SDP and derive the optimal value of the QCQP from it. When the SDP is prohibitively large, or when only cost tightness is attained, one may instead resort to a local solver and certify the solution candidate using Lagrangian duality. For completeness, we also mention that in some cases, one may extract a solution estimate from a higher-rank solution of the SDP in a procedure called “rounding”, see e.g., [5]. This typically consists of extracting the dominant eigenvector from $\mathbf{X}^*$, and projecting it to the feasible set of (1). Note that in this case there are not usually guarantees on the quality of the solution and cases have been reported where the obtained estimate may be far from optimal [42].

We have seen that either rank or cost tightness are necessary for efficiently obtaining or certifying globally optimal solutions, respectively. The remaining question is how one may increase the tightness of a given problem. This leads to the notion of redundant constraints, as explained next.

### D. Redundant Constraints

Redundant constraints can be added to (3) and, equivalently, (Q), without changing its feasible set.<fnref n="7" /> While the constraints are redundant for the QCQP, they may, however, change the feasible region of the SDP. In particular, redundant constraints typically reimpose structure on $\mathbf{X}$ that is lost when relaxing the rank-one constraint. For example, if the lifted vector is $\mathbf{x}^\top = [1 \quad \theta \quad \theta^2 \quad \theta^3]$, then

$$ \mathbf{X} = \mathbf{x}\mathbf{x}^\top = \begin{bmatrix} 1 & \theta & \theta^2 & \theta^3 \\ \star & \theta^2 & \theta^3 & \theta^4 \\ \star & \star & \theta^4 & \theta^5 \\ \star & \star & \star & \theta^6 \end{bmatrix}, \quad (7) $$

which has a very clear structure (it is a Hankel matrix [43]) that might be lost in the relaxation. The lifting constraints (in this case, $\mathbf{x}[3] = \mathbf{x}[2]^2$ and $\mathbf{x}[4] = \mathbf{x}[3]\mathbf{x}[2]$) and symmetry of the solution take care of constraining all terms $\theta, \theta^2, \theta^3$, and $\theta^5$, but nothing directly enforces that the elements corresponding to $\theta^4$ in the variable $\mathbf{X}$ are equal. We can add the redundant constraint corresponding to $(\mathbf{x}[3]^2 = \mathbf{x}[2]\mathbf{x}[4])$ to enforce this. Redundant constraints can often be hard to find — as the next example illustrates.

**Example (stereo-1D, redundant constraints).** A simple computation shows that

$$ z_i - z_j = \frac{1}{\theta - m_i} - \frac{1}{\theta - m_j} = (m_i - m_j)z_i z_j, \quad (8) $$

which holds for any $i, j$ and $z_i, z_j$ constructed using the lifting functions $l_i(\theta)$ introduced in (5). This shows that equation (8), which is quadratic in the elements of $\mathbf{x}$, is redundant in (3), but non-redundant in the SDP. It can be added with matrices $\mathbf{A}_{ij}$ with only non-zero entries $\mathbf{A}_{ij}[1, i+2] = \mathbf{A}_{ij}[i+2, 1] = 1$, $\mathbf{A}_{ij}[1, j+2] = \mathbf{A}_{ij}[j+2, 1] = -1$, $\mathbf{A}_{ij}[i+2, j+2] = \mathbf{A}_{ij}[j+2, i+2] = (m_j - m_i)$, for all $i, j \in [N], i \neq j$.

<footnote n="6">⁶It is straightforward to see that rank tightness implies cost tightness.</footnote>

<footnote n="7">⁷Speaking in terms of algebraic geometry, the redundant constraints do not change the algebraic variety that is defined by the feasible set.</footnote>
Because they impose more structure on $\mathbf{X}$, redundant constraints may have the effect of reducing the rank of $\mathbf{X}$, and thus improve the tightness of the relaxation. However, finding the right form and number of redundant constraints can be a tedious process, especially as the dimension of the problem increases. The present paper circumvents this process by proposing a numerical method to find all available redundant constraints, as we explain next.

## IV. AUTOIGHT

In this section, we present our method to determine whether a given semidefinite relaxation can be tightened. We summarize the algorithm AUTOIGHT in Figure 2. In the formulation phase, run only once for each new problem type using a representative example problem, all possible redundant constraints are automatically found and the tightness of the relaxation is evaluated. In the application phase, the learned constraints are used to either solve or certify new problems created using incoming measurement data.<fnref n="8" />

### A. Setting up the Nullspace Problem

At the core of the presented method is the insight that all of the possible constraint matrices lie in the nullspace of the linear subspace spanned by the feasible points of problem (Q). We can thus determine constraints automatically by numerically retrieving the nullspace basis of a particular matrix. We will sometimes call these the *learned constraints*. The fact that feasible points do, indeed, form a linear space once lifted into a higher-dimensional space is proven in Appendix C.

Assume we can generate feasible samples $\theta^{(s)}$, $s \in [N_s]$, of (1), and therefore also a set of lifted samples $\mathbf{x}^{(s)}$, constructed using the *known* substitutions $z_i^{(s)} = \ell_i(\theta^{(s)})$:<fnref n="9" /> $\mathbf{x}^{(s)\top} = [h \ \theta^{(s)\top} \ z_1^{(s)} \ \dots \ z_{N_l}^{(s)}]$. Then, for any valid constraint matrix $\mathbf{A}_i$ ($i > 0$) of (Q) (whether primary or redundant), we must have (by definition):

$$ \langle \mathbf{A}_i, \mathbf{X}^{(s)} \rangle = \mathrm{vech}(\mathbf{A}_i)^{\top} \mathrm{vech}(\mathbf{X}^{(s)}) = 0, \quad (9) $$

with $\mathbf{X}^{(s)} := \mathbf{x}^{(s)}\mathbf{x}^{(s)\top}$. This must hold for all samples $\mathbf{x}^{(s)}$. Defining the data matrix $\mathbf{Y} = [\mathrm{vech}(\mathbf{X}^{(1)}) \ \dots \ \mathrm{vech}(\mathbf{X}^{(N_s)})] \in \mathbb{R}^{n \times N_s}$, an admissible set of learned constraints, $\mathcal{A}_l$, is found in the left nullspace basis of $\mathbf{Y}$:

$$ \mathcal{A}_l = \{\mathbf{A}_1, \dots, \mathbf{A}_{N_n}\} = \{\mathrm{vech}^{-1}(\mathbf{a}_i) \mid \mathbf{a}_i^{\top} \mathbf{Y} = \mathbf{0}\}. \quad (10) $$

In other words, each nullspace basis vector corresponds to one (vectorized) constraint matrix, and finding all possible constraints is a standard nullspace problem. The dimension of the nullspace, $N_n$, corresponds to the total number of constraints. Note that we have exploited the fact that $\mathbf{X}^{(s)}$ and $\mathbf{A}_i$ are symmetric by using the half-vectorization operator, which reduces the problem size to $n := \frac{N(N+1)}{2}$.

By definition, all the constraints found through (10) are linearly independent when operating in matrix form. When reformulating the constraints as a function of $\theta$ and adding them to (1), however, some constraints may become dependent; in other words, the method finds both primary and redundant constraints. We explain this process in our pedagogical example at the end of this section, and in real-world applications in Section VI.

Sometimes, it may be desirable to enforce some of the basis vectors to be known, for example to enforce that the original constraints from (Q) appear in the final formulation. We denote the set of known constraints to be enforced by $\mathcal{A}_k = \{\tilde{\mathbf{A}}_1, \dots, \tilde{\mathbf{A}}_{N_k}\}$. Completing the nullspace basis is as simple as appending the known constraints to the data matrix:

$$ \mathbf{Y} = [\mathrm{vech}(\mathbf{X}^{(1)}) \ \dots \ \mathrm{vech}(\mathbf{X}^{(N_s)}) \ \tilde{\mathbf{a}}_1 \ \dots \ \tilde{\mathbf{a}}_{N_k}], \quad (11) $$

with $\tilde{\mathbf{a}}_i = \mathrm{vech}(\tilde{\mathbf{A}}_i)$. By definition, the left nullspace vectors of $\mathbf{Y}$ will then be orthogonal to the known constraints.

To find a valid nullspace basis, we need to have at least $r = n - N_n$ samples, with $n$ the number of rows of $\mathbf{Y}$, $N_n$ the nullspace dimension, and $r$ the rank of $\mathbf{Y}$. However, since $r$ is not known a priori, a viable strategy is to randomly generate $N_s > n$ samples. This ensures that the data matrix is rank-deficient with probability one (because it has more columns than rows), The nullspace basis can then be reliably calculated using the permuted QR decomposition, as we explain next.<fnref n="10" />

### B. Sparse Basis Retrieval

Constraint matrices are expected to be sparse, since they usually involve only a subset of variables. Sparsity is good not only for lower runtime and memory consumption of SDP solvers, but also for interpretability of the matrices. Finding the sparsest nullspace basis is a NP-hard problem [44]. However, when using a pivoted, or rank-revealing, QR decomposition [45] to find the left nullspace of the data matrix, we found the resulting constraints to be highly sparse. We show in Section VII that sometimes basis vectors are even as sparse as analytically identified constraints. Other matrix decomposition methods, such as the singular value decomposition (SVD), were empirically found to exhibit less sparsity.

The pivoted QR decomposition returns a decomposition of the form [45]

$$ \mathbf{Y}^{\top} \mathbf{P} = \mathbf{S} \mathbf{R} = \mathbf{S} \begin{bmatrix} \mathbf{R}_1 & \mathbf{R}_2 \\ 0 & 0 \end{bmatrix}, \quad (12) $$

where $\mathbf{P}$ is a $n \times n$ permutation matrix ensuring that the diagonal of $\mathbf{R}$ is non-increasing, $\mathbf{S}$ is $N_s \times N_s$ and orthogonal, $\mathbf{R}_1$ is upper-diagonal with dimensions $r \times r$, and $\mathbf{R}_2$ is of size $r \times N_n$. The nullspace basis vectors $\mathbf{a}_i$ are then given by

$$ [\mathbf{a}_1 \ \dots \ \mathbf{a}_{N_n}] = \mathbf{P} \begin{bmatrix} \mathbf{R}_1^{-1} \mathbf{R}_2 \\ -\mathbf{I}_{N_n} \end{bmatrix}. \quad (13) $$

Note that when using the permuted QR decomposition, the obtained basis vectors are linearly independent, but not necessarily orthogonal to each other, as would be the case with

<footnote n="8">⁸During the formulation, ground truth information is usually available and can optionally be used to find the globally optimal solution efficiently. In the application phase, however, no ground truth information is required.</footnote>

<footnote n="9">⁹Note that we can also allow for unknown lifting functions, as long as a sampler of $\mathbf{x}$ is available.</footnote>

<footnote n="10">¹⁰The procedure is equivalent to Algorithm 3 in [19] and ensures poisedness, as defined in Appendix C. Intuitively speaking, poisedness ensures that properties derived for the samples hold for the entire variety.</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 2: Overview of AUTOTIGHT and AUTO TEMPLATE. Using randomly generated training data, we find a set of sufficient redundant constraints or templates to tighten the relaxation, if it exists. We call this the formulation phase, and it only has to be performed once for each problem type. The found templates (AUTO TEMPLATE) or interpreted constraints (AUTOTIGHT) can be then used in the application phase to solve or certify new test problems to global optimality, as long as the domain gap from training to test data is sufficiently small.</figcaption></figure>

an SVD, for example. However, we found that the increased sparsity was of higher importance, both for computational speed and interpretability, than orthogonality.<fnref n="11" />

### C. Determining Tightness

The method so far is independent of the cost function and only depends on the substitutions and primary constraints. To determine if the relaxation is tight, we consider a randomly generated problem setup, which defines the cost matrix $Q$ in (Q). We determine tightness for this example setup in the formulation phase, as we explain next. Thanks to so-called SDP stability, we know that if the problem is tight, it will also be tight for similar problems [41] in the application phase, for example for similar noise and sparsity patterns.<fnref n="12" />

We determine cost tightness by comparing the optimal dual cost with the cost of a candidate global solution. The optimal dual cost is obtained by solving (D) after adding all redundant constraints. The candidate global solution can be found by running an off-the-shelf local solver initialized at the ground-truth state, which we expect to be close to the optimal solution for low-enough noise. Indeed, this strategy allowed us to find the global minimum almost always for the noise regimes considered in Section VII.<fnref n="13" /> We compute the relative duality gap (RDG) between the cost of this local solution $\hat{q}$ and the optimal dual cost $d^*$ through $(\hat{q}-d^*)/\hat{q}$, and report cost tightness if the RDG is below a fixed threshold (see Section VII-A). To determine rank tightness, we calculate the eigenvalues of the solution $X$, and take the ratio of the first to the second-largest eigenvalue, called the eigenvalue ratio (ER) in what follows. If the ratio is larger than a fixed value (see Section VII-A) we report that the solution is rank one.

### D. Possible Outcomes

There are three possible outcomes of AUTOTIGHT:

1. The problem cannot be tightened. Either a new formulation can be tried — adding for instance (a subset of) higher-order monomial (Lasserre) terms [12] — or the non-tight SDP can be used in conjunction with rounding, for example as an initialization for a local solver.

2. The problem is already tight or can be made so with few interpretable redundant constraints. We will see such a case in our pedagogical example and RO localization. In this case, constraints matrices can be efficiently created analytically and AUTO TEMPLATE is not necessary.

3. The problem can be tightened, but only with non-interpretable redundant constraints. In this case, AUTOTIGHT would have to be reapplied to every new problem instance, which does not scale to problem sizes typically encountered in robotics. We later present AUTO TEMPLATE, which finds constraint templates — patterns that can be applied to problems of any size.

We conclude this section by studying the outcome of AUTOTIGHT for our example problem.

**Example (stereo-1D, AUTOTIGHT).** We test tightness with $N = 2$ landmarks. The randomly generated setup is $m_1 = 0.5488$ and $m_2 = 0.7152$, $u_1 = 18.14$, $u_2 = -8.719$, and ground truth $\theta_{gt} = 0.6028$.

Since (2) is unconstrained, it is straightforward to construct feasible samples $\theta^{(s)}$ by uniform sampling, for example from the bounding interval of the anchor locations $m_i$. For each sample $\theta^{(s)}$, we create $x^{(s)}$ using (5), and vech($X^{(s)}$) which is of size $n = 10$. We choose $N_s \ge 12$ samples, which is 10% more than necessary, to create $Y$. We perform a QR decomposition and observe that the nullspace is of dimension $N_n = 3$. Converting the three basis vectors to matrices, we get as elements of $A_\ell$:

$$ \begin{bmatrix} 0 & 0 & -\alpha & 0 \\ \star & 0 & -1 & 1 \\ \star & \star & 0 & -2\gamma \\ \star & \star & \star & 0 \end{bmatrix}, \begin{bmatrix} 0 & 0 & 1 & -1 \\ \star & 0 & 0 & 0 \\ \star & \star & 0 & \alpha \\ \star & \star & \star & 0 \end{bmatrix}, \begin{bmatrix} 1 & 0 & \beta & 0 \\ \star & 0 & 0 & -0.5 \\ \star & \star & 0 & \gamma \\ \star & \star & \star & 0 \end{bmatrix} \quad (14) $$

where $\alpha, \beta$ and $\gamma$ are numeric values — we find by inspection that $\alpha = m_2 - m_1$, $\beta = \frac{m_2}{2}$, and $2\gamma = m_2(m_2 - m_1)$.

We evaluate the tightness by solving (2) using GN initialized at ground truth on one hand (yielding $\hat{\theta}$ and $\hat{q}$), and the SDP relaxation (P) on the other hand (yielding $X^*$ and $p^*$). We find $\hat{\theta} = 0.6038$, $\hat{q} = 0.06857$, and an RDG of $1.6 \times 10^{-6}$. The rank of $X^*$ is two, and the problem is thus cost tight.

<footnote n="11">¹¹By construction, each basis vector has at least $N_n - 1$ zeros because of choosing the identity matrix in (13). The enforced sparsity in a few elements was empirically found to also induce sparsity in the other elements.</footnote>

<footnote n="12">¹²Note that whenever we find a certifiably optimal solution for in the application phase, we know that our test problem is 'similar enough'. In other words, we have an a posteriori generalization guarantee.</footnote>

<footnote n="13">¹³If this fails or no ground truth information is available, we can regenerate random setups and initializations until we find that the cost of the candidate solution is equal to the dual cost (up to numerical tolerance). Then we know that it corresponds to the global minimum, because of duality theory [11].</footnote>
but not rank tight. However, when imposing only the known substitution constraints and no redundant constraints, we have an RDG of 1.0 and thus neither cost nor rank tightness. We conclude that this problem formulation has a tight relaxation with redundant constraints. For this example, in order to scale to larger problems, we can interpret the constraints (14) or use directly the analytic form (8) (we are thus in the second outcome in Section IV-D). Note, however, that even for this simple problem, interpreting (14) was not trivial. For more complex problems, we thus propose to use AUTOTEMPLATE, which we introduce in the next section.

### E. Theoretical Guarantee

The success of the AUTOTIGHT algorithm provides a trivial sufficient condition that a given problem formulation can be tightened with redundant constraints. In this section, we show the converse of this statement: the formulation can be tightened only if AUTOTIGHT is successful. That is, the AUTOTIGHT method is complete, as formalized in Theorem 1.

**Theorem 1 (Completeness of AUTOTIGHT).** Let $A_\ell$ be the set of learned redundant constraints using AUTOTIGHT, and let $A_k$ be any other set of redundant constraints (perhaps found via a different method). Let $(P_\ell)$ and $(P_k)$ be the SDP (P) constructed from $A_\ell$ or $A_k$, respectively. Denote by $(\mathbf{X}_\ell^*, p_\ell^*)$ and $(\mathbf{X}_k^*, p_k^*)$ the optimal values and costs of $(P_\ell)$ and $(P_k)$, respectively, and by $q^*$ the optimal cost of (1). Then we have:

$$ 
\begin{aligned}
q^* &= p_k^* \implies q^* = p_\ell^*, \\
\operatorname{rank}(\mathbf{X}_k^*) &= 1 \implies \operatorname{rank}(\mathbf{X}_\ell^*) = 1.
\end{aligned} 
\quad (15) 
$$

The proof of Theorem 1 uses results from Cifuentes et al. [19] and is provided in Appendix C. The implication of Theorem 1 is that if AUTOTIGHT does not lead to a tight relaxation of a given problem formulation, then there is no other set of constraints that can tighten the problem, and one needs to instead reformulate the problem, e.g., ascend in Lasserre's hierarchy [12].

## V. AUTOTEMPLATE

The method AUTOTIGHT determines whether a given problem can be tightened, but unless the learned constraints are interpretable, we need to repeatably apply AUTOTIGHT as we change the problem formulation. This induces unnecessary computational overhead and may be prohibitively expensive for large problem sizes. We thus present AUTOTEMPLATE, a scalable extension of AUTOTIGHT.

### A. Setting up the Nullspace Problems

While problems in robotics tend to be high-dimensional, they usually only exhibit a few different variable types. For example, in SLAM, the only variable types are robot poses and landmark positions [2]. We would expect the constraints relating instances from the same variable types to be repeatable; for example, all constraints that involve a single pose should hold for all other poses too. AUTOTEMPLATE uses this insight to learn templates that act on variable sets and can thus be more easily scaled.

To give a simple example, when learning the constraints for the RO localization problem (see for example the first row of Figure 5 in Section VII-B) using AUTOTIGHT, the same constraint $(z_i = \|\theta_i\|^2)$ is found three times: once for each position. Using AUTOTEMPLATE, we learn only one template of this form, and can then apply it to as many positions as necessary in the application phase.

Given a problem formulation, we first identify the different variable types and a sequence of variable sets. Variable types can be landmarks, poses, substitutions, etc. Each variable set represents one group of variables for which we learn constraints. A simple recipe for creating variable sets goes as follows. We start by creating one variable set per variable type, including one instance and the homogenization variable per set. Then, we incrementally add one more variable at a time, using always all possible permutations of variable types. The resulting variable sets, for the applications considered in this paper, are given in Table 1 in Section VII. We call each variable set $G_k$ and write $(\cdot)_{G_k}$ to reduce a variable or function to the elements included in $G_k$.

**Example (stereo-1D, variable sets).** For our pedagogical example, the variable types are the position $\theta$, the substitutions $z_i$, and $h$. Note that the landmarks are known and not considered as variables. The above recipe then leads to the following list of variable sets: $\{\{h, \theta\}\}, \{h, z_1\}, \{h, \theta, z_1\}, \{h, z_1, z_2\}, \{h, \theta, z_1, z_2\}, \{h, z_1, z_2, z_3\}, \{h, \theta, z_1, z_2, z_3\}, \dots$.

### B. Factoring Out Parameters

For some problems, constraints may depend on parameters that are known a priori. In the stereo-1D example and in the stereo localization problem presented in Section VI-B, for example, the constraints depend on the known landmark coordinates. Because of this, the learned templates may be harder to interpret, and not applicable to other random setups. To overcome this problem, we treat such quantities as parameters and append them to the samples of feasible points. Let $\mathbf{p} \in \mathbb{R}^{N_p}$ be the vector of $N_p$ parameters, chosen rich enough so that all constraints are linear functions of $\mathrm{vec}(\mathbf{X}^{(s)}) \otimes \mathbf{p}$. We always set $\mathbf{p}[1] = 1$ to include the case without added parameter dependencies.<fnref n="14" />

For scalability, we do not add all elements of $\mathbf{p}$ for each considered variable set, but only the ones that depend on the variables in $G_k$, which we denote by $p_{G_k}$. For a visualization of these concepts, we return to the pedagogical example.

**Example (stereo-1D, augmented variable sets).** This problem has parameters $m_i$. Because the lifting constraints $l_i(\mathbf{x})$ are linear in $\theta$ and $m_i$, respectively, we use $\mathbf{p} = [1 \ m_1 \ \cdots \ m_N]^\top$. For this particular example, we know the redundant constraints are of the form (8), which confirms that this choice of parameters is rich enough: each redundant

<footnote n="14">¹⁴Note that defining the parameters requires user input: for example, for a problem with landmarks, the user needs to decide if $\mathbf{p}$ contains only the landmark coordinates or also high-order monomials thereof (see the stereo-1D example or Section VI-B2 for more concrete examples). This is still significantly simpler than manually finding all required redundant constraints, and comparable to the choice of a sparse Lasserre's hierarchy, for which systematic methods exist that could likely be employed here as well [16].</footnote>
constraint can be written as a linear combination of the elements of $\mathrm{vech}(\mathbf{X}^{(s)}) \otimes \mathbf{p}$.

We modify each feasible sample to include its parameter dependencies, leading to the augmented feasible sample

$$ \tilde{\mathbf{z}}_{G_k}^{(s)} := \mathrm{vech}(\mathbf{X}_{G_k}^{(s)}) \otimes \mathbf{p}_{G_k} \in \mathbb{R}^{\bar{n}_k}, \quad (16) $$

with $\bar{n}_k := n_k K_k$, $n_k$ the dimension of $\mathrm{vech}(\mathbf{X}_{G_k})$, and $K_k$ is the dimension of $\mathbf{p}_{G_k}$. We get the augmented data matrix

$$ \bar{\mathbf{Y}}_{G_k} = [\tilde{\mathbf{z}}_{G_k}^{(1)} \quad \dots \quad \tilde{\mathbf{z}}_{G_k}^{(\bar{N}_s)}] \in \mathbb{R}^{\bar{n}_k \times \bar{N}_s}, \quad (17) $$

where the number of samples $\bar{N}_s$ has to be chosen to ensure that $\bar{\mathbf{Y}}_{G_k}$ is rank-deficient, as in Section IV-A. We denote the left nullspace basis vectors of (17) by $\bar{\mathbf{a}}_l \in \mathbb{R}^{n_k K_k}$, with $l \in [\bar{N}_n]$. We call these basis vectors templates and we will apply them to new problems as explained next.

### C. Applying Templates

Conceptually speaking, applying the templates means repeating each constraint for each possible combination of the variables that it involves. For example, if one constraint matrix involves one position and two different landmarks, then we repeat the constraint for each position and each possible pair of landmarks per position. To facilitate this operation programmatically, we have created a tool to generate sparse matrices using variable names for indexing.<fnref n="15" /> That way, applying constraints to all possible variables of a given type simply means creating duplicates of a given constraint, and then renaming the variables that it involves.

If parameters were factored out as explained in Section V-B, then they need to be factored back in before solving the SDP, using the current parameter realization. We introduce the operator $\mathrm{mat}(\cdot)$, which folds the augmented basis vector $\bar{\mathbf{a}}_l$ (which we recall has $n_k K_k$ dimensions, with $K_k$ the number of lifted parameters) column-wise into a $n_k \times K_k$ matrix. Then, factoring in a parameter realization $\mathbf{p}^{(s)}$ can be written as

$$ \mathbf{a}_l = \mathrm{mat}(\bar{\mathbf{a}}_l)\mathbf{p}^{(s)}, \quad (18) $$

where the output $\mathbf{a}_l \in \mathbb{R}^{n_k}$ is now a problem-specific vectorized constraint that can be converted to the corresponding constraint matrix $\mathbf{A}_l = \mathrm{vech}^{-1}(\mathbf{a}_l)$. Zero padding is used to apply constraints learned on subsets of variables for the full variable set. We return to the stereo-1D example to illustrate these concepts:

**Example** (stereo-1D, templates). We apply AUTOTemplate to the stereo-1D example, imposing the known substitution constraints. We do not find any additional constraints for all first variable sets. Only when using the group $G_5 = \{h, \theta, z_1, m_1, z_2, m_2\}$, each augmented sample is of the form:

$$ \bar{\mathbf{z}}_{G_5}^{(s)\top} = [1 \ \theta \ z_1 \ z_2 \ \theta^2 \ \theta z_1 \ \theta z_2 \\ z_1^2 \ z_1 z_2 \ z_2^2]^{(s)} \otimes [1 \ m_1 \ m_2]^{(s)}, \quad (19) $$

The redundant constraints from (8) are in the nullspace of the augmented data matrix and can thus be learned automatically. Indeed, the template corresponding to $\mathbf{A}_{12}$ can be written as

$$ \begin{align} \bar{\mathbf{a}}_{12}^{\top} &= [\alpha_1^{\top} \quad \alpha_2^{\top} \quad \alpha_3^{\top}], \nonumber \\ \alpha_1^{\top} &= [0 \quad 0 \quad 1 \quad -1 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0], \tag{20} \\ \alpha_2^{\top} &= [0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 1 \quad 0], \nonumber \\ \alpha_3^{\top} &= [0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \quad -1 \quad 0]. \nonumber \end{align} $$

and satisfies $\bar{\mathbf{a}}_{12}^{\top} \bar{\mathbf{z}}_{G_5}^{(s)} = 0$ for any sample $\bar{\mathbf{z}}_{G_5}^{(s)}$. Note that the template $\bar{\mathbf{a}}_{12}$ does not depend on the landmarks anymore. Given new realizations of parameters $\mathbf{p}^{(s)}$, we can create $\mathbf{p}_{G_5}^{(s)} = [1 m_1^{(s)} m_2^{(s)}]^{\top}$ and the corresponding constraint matrix $\mathbf{A}_{12}^{(s)} = \mathrm{vech}^{-1}(\mathbf{a}_{12}^{(s)})$, with

$$ \mathbf{a}_{12}^{(s)} = \mathrm{mat}(\bar{\mathbf{a}}_{12}) \mathbf{p}_{G_5}^{(s)} = [\alpha_1 \quad \alpha_2 \quad \alpha_3] \mathbf{p}_{G_5}^{(s)}. \quad (21) $$

### D. Reducing the Number of Constraints

In practice, not all of the found templates are actually necessary for tightness. Therefore, we suggest to prune the found templates before applying them to large problem sizes. Assume we have found a set of learned constraints $\mathcal{A}_l$ for which the problem is (at least) cost tight. Then, we can solve the following optimization problem in an attempt to sort the constraints by their importance for tightness:

$$ \begin{align} \min_{\lambda, \rho} & \|\lambda\|_1 \nonumber \\ \text{s.t.} \quad & \mathbf{H}(\rho, \lambda) \succeq 0 \tag{22} \\ & \mathbf{H}(\rho, \lambda)\hat{\mathbf{x}} = \mathbf{0}, \nonumber \end{align} $$

where $\|\cdot\|_1$ denotes the $L_1$-norm, $\mathbf{H}$ is defined as in (D) (with the learned matrices substituted for $\mathbf{A}_k$) and $\hat{\mathbf{x}}$ is the optimal solution of (Q). Intuitively, Problem (22) finds a sparse set of dual variables required for cost tightness, as the $l^1$-norm promotes sparsity. By ordering the learned constraints by decreasing magnitude of $\lambda$ and adding them one by one, we find a smaller subset of constraints that is also sufficient for cost tightness. This problem naturally lends itself to a bisection-like algorithm, where we try using all and no redundant constraints at first, and then continue to try cutting the interval in half. We terminate when the considered interval is of size one. Using only these constraints as templates significantly reduces the computational cost of all downstream operations, as shown in Section VII. We call this smaller set of constraints, empirically found to be also sufficient for tightness, the reduced constraints.

As another pruning step, we also make sure that all constraints are linearly independent after applying templates to other variables. For this purpose, we use the same rank-revealing QR decomposition as in Section IV-B but keep only the valid range-space basis vectors. Because of the sparsity of the constraints, this adds no significant cost.

### E. Summary

To summarize, AUTOTemplate consists of the following steps, as displayed in Figure 2. During the formulation phase, we set up the nullspace problem for one variable set at a

<footnote n="15">¹⁵The code is available as a stand-alone open-source package at https://github.com/utiasASRL/poly_matrix.</footnote>
<table><caption>TABLE I: Overview of the tightened problems, including the variable sets, problem dimensions, and noise parameters. For simplicity, all substitutions are called $z_i$. $N_{out}$ is the number of outliers, and noise corresponds to the standard deviation of zero-mean Gaussian noise.</caption><thead><tr><th>Problem (Parameters; Inlier noise / Outlier noise):</th><th>Variables</th></tr></thead><tbody><tr><td>RO loc. (d = 3, N<sub>m</sub> = 10, N = 3; 10<sup>-2</sup>):</td><td>[{h, θ<sub>2</sub>}, {h, z<sub>1</sub>}, {h, θ<sub>1</sub>, z<sub>1</sub>}, ...]</td></tr><tr><td>stereo loc. (d ∈ {2, 3}, N = d + 1; 10<sup>0</sup>):</td><td>[{h, θ}, {h, z<sub>1</sub>}, {h, θ, z<sub>1</sub>}, {h, z<sub>1</sub>, z<sub>2</sub>}, ...]</td></tr><tr><td>PPR [10] (d = 3, N = 3; 10<sup>-2</sup>), PLR [10] (d = 3, N = 5; 10<sup>-3</sup>):</td><td>[{h, θ}]</td></tr><tr><td>rPPR [7] (d = 3, N = 4, N<sub>out</sub> = 1; 10<sup>-2</sup>/10<sup>0</sup>),</td><td></td></tr><tr><td>rPLR [7] (d = 3, N = 6, N<sub>out</sub> = 1; 10<sup>-3</sup>/10<sup>-1</sup>):</td><td>[{h, θ}, {h, θ, w<sub>1</sub>}, {h, θ, z<sub>1</sub>}, {h, θ, w<sub>1</sub>, w<sub>2</sub>}, {h, θ, w<sub>1</sub>, z<sub>1</sub>}, {h, θ, z<sub>1</sub>, z<sub>2</sub>}, ...]</td></tr></tbody></table>

<figure><img src="image_4.png" /><figcaption>Fig. 3: Sketches of the two example applications and their corresponding factor graphs: (a) RO localization, (b) stereo camera localization and (c) the factor graph for both problems, where the filled circles are unknown states, the squares are landmark measurement factors, and the empty circles are known landmarks. For completeness, we plot in orange the motion-prior factors, which could be added as in [18] but are omitted here for simplicity.</figcaption></figure>

time, including corresponding parameters, as explained in Sections V-A and V-B. This allows us to learn templates instead of constraints. Before checking tightness with the same procedure as for AUTOTIGHT (Section IV-C), we apply these templates to all other variables of the same variable group as outlined in Section V-C. If the problem is tight, we return the learned templates, after optionally extracting a smaller sufficient subset as explained in Section V-D. If the relaxation is not tight, we either restart the process, using the next variable set if available, or go up in Lasserre's hierarchy if all groups at this level have been exhausted. During the application phase, we apply the templates to new problem setups of any size.

In terms of complexity, one important computational load of both methods, AUTOTIGHT and AUTOTTEMPLATE, is the nullspace calculation, which involves a permuted QR decomposition and is cubic in complexity. The crucial dimensions are thus $n$ and $\max_k \bar{n}_k$, respectively, which correspond to the dimensions of the feasible samples in the respective methods. For AUTOTIGHT, if higher-order substitutions are required for tightness, this $n$ grows quickly (see, for example, the results in Figure 12). For AUTOTTEMPLATE, the dimensions $\bar{n}_k$ depend on the chosen variable sets and can be kept as small as possible by growing the size of the groups incrementally (using for example our proposed recipe), until tightness is achieved. This is the main reason for better scalability of AUTOTTEMPLATE. The application of the learned templates to new problem setups in AUTOTTEMPLATE may add to the computational load when the number of learned templates is large, as we will see in the stereo-localization application.

## VI. APPLICATIONS

We study two real-world applications of the proposed method in depth: RO and stereo localization. Their sketches and the corresponding factor graph are provided in Figure 3. We outline the problem formulations in this section, and report simulated and experimental results in the next two sections. Formulations from the literature that we also study are provided in Appendix B.

### A. Range-Only Localization

The first application is RO localization with fixed and known landmarks, as encountered in ultra-wideband (UWB)-

based localization [46, 47] or WiFi- or Bluetooth-based indoor localization [48]. This problem has been widely studied, for example, in [42, 49] and, more recently, in [50] and [18]. We reproduce the tightness results from [18] using our new method, and then study a different formulation, which we automatically tighten using AUTOTIGHT and AUTOTTEMPLATE.

#### 1) Problem Statement

The goal of RO localization is to estimate the position of a moving device over time, given range measurements to fixed and known landmarks, also called anchors in this context. We call the anchors $m_k \in \mathbb{R}^d$ with $k \in [N_m]$ and the position at time $t_n$ is denoted $\theta_n \in \mathbb{R}^d$, with $n \in [N]$. We use $d = 3$ in all of the experiments, and the following common formulation of the problem [42]:

$$ \min_{\theta} c(\theta), \quad c(\theta) = \sum_{n,k \in \mathcal{E}} \left( d_{nk}^2 - \|m_k - \theta_n\|^2 \right)^2, \quad (23) $$

where $\mathcal{E}$ is the edge set of the measurement graph, with an edge between position $n$ and anchor $k$ if their distance $d_{nk}$ is measured, and $\theta$ is the concatenation of unknowns $\{\theta_n\}_{n=1}^N$.<fnref n="16" />

#### 2) Problem Reformulation

Problem (23) is quartic in the unknowns, and thus may contain multiple local minima [18]. However, by introducing lifting functions that are quadratic in $\theta_n$, it can be written as a QCQP, making it a candidate for SDP relaxation. We study two such lifting functions. First, it is instructive to expand all elements of the cost:

$$ c(\theta) = \sum_{n,k \in \mathcal{E}} \left( d_{nk}^2 - \|m_k\|^2 + 2m_k^\top \theta_n - \|\theta_n\|^2 \right)^2 . \quad (24) $$

Looking at (24), we see that the substitution

$$ z_n := \|\theta_n\|^2 \in \mathbb{R} \quad (25) $$

is enough to make the problem quadratic in the lifted vector $x^\top = [h \ \theta^\top \ z_1 \ \cdots \ z_N]$. The same substitution was used in [18] and was shown to require no redundant constraints for tightness. This substitution (26) is also called a sparse Lasserre substitution [16]. We also study the more methodological (dense) substitution that introduces all quadratic terms of the elements of $\theta_n$:

$$ y_n := \text{vech}(\theta_n \theta_n^\top) \in \mathbb{R}^{d(d+1)/2}. \quad (26) $$

<footnote n="16">¹⁶Note that it is straightforward to include a motion prior in (23), such as a constant-velocity prior, as shown in [18] and visualized in Figure 3. Such priors are typically up to quadratic in the unknowns, thus not requiring any special treatment when it comes to constraints, and are omitted for simplicity.</footnote>
In general, given polynomial cost and constraints functions of degree up to $k$, it suffices to introduce lifting functions up to degree $\lfloor \frac{k}{2} \rfloor$. Out of all possible lifting functions, some may not require redundant constraints (for example $z_n$ above), while others do (for example $y_n$ above), which we will study in more detail in Section VII-B.

As in the pedagogical example, sampling feasible points is straightforward as the problem is initially unconstrained. We sample points uniformly at random from the convex hull of the anchors, yielding $\theta^{(s)}$, and create $x^{(s)}$ and $Y$ as explained in Section IV-A.

## B. Stereo Localization

The second application is the estimation of the pose of a stereo camera by minimizing the reprojection error of known landmarks, which we refer to as stereo localization. The reprojection error can be used to model Gaussian noise on pixel measurements [51]. To the best of our knowledge, this problem has not been successfully relaxed to a tight SDP before, with common solutions typically resorting to the back-projection error [52, 15] (i.e., the error is assumed Gaussian in Euclidean space). Closest to our solution is [20], where a branch-and-bound method in combination with a (non-tight) semidefinite relaxation is used to minimize the reprojection cost. Instead, we use the proposed methods to 1) find a new formulation of the problem that can be tightened using AUTOTIGHT, and to 2) generate templates that can be scaled to new problem instances using AUTOTemplate.

### 1) Problem Statement

Our goal is to estimate the pose of a stereo camera given the measured image coordinates, in both left and right frames, of a number of known landmarks. We call the known landmarks $\mathbf{m}_k$ and the homogenized form $\tilde{\mathbf{m}}_k = [\mathbf{m}_k^\top \ 1]^\top$, with $k \in [N]$.[17] For simplicity, we focus on one measurement time only, and call the unknown pose at that time $\mathbf{T} \in SE(d)$, which contains both the rotation matrix from world to camera frame, $\mathbf{C} \in SO(d)$, and the associated translation $\mathbf{t} \in \mathbb{R}^d$. We collect the pixel measurements of landmark $k$ in $\mathbf{y}_k^\top := [u_k^\ell \ v_k^\ell \ u_k^r \ v_k^r]$, where $u$ and $v$ denote the $x$ and $y$ coordinates in pixel space, and superscripts $\ell$ and $r$ correspond to the left and right frame, respectively. We call the intrinsic stereo camera matrix in $d$ dimensions $\mathbf{M}_d$, with for example:

$$ \mathbf{M}_2 = \begin{bmatrix} f_u & c_u & b_u \\ f_u & c_u & -b_u \end{bmatrix}, \quad \mathbf{M}_3 = \begin{bmatrix} f_u & 0 & c_u & b_u \\ 0 & f_v & c_v & 0 \\ f_u & 0 & c_u & -b_u \\ 0 & f_v & c_v & 0 \end{bmatrix}, \quad (27) $$

where $b_u = f_u \frac{b}{2}$, and $f_u$, $f_v$, and $b$ are the focal lengths and baseline, respectively. Given pixel measurements from $N$ landmarks, the pose can be estimated as the solution of the optimization problem

$$ \min_{\mathbf{T} \in SE(d)} c(\mathbf{T}), c(\mathbf{T}) = \sum_{k \in [N]} \|\mathbf{y}_k - \mathbf{M}_d (e_d^\top \mathbf{T} \tilde{\mathbf{m}}_k)^{-1} \mathbf{T} \tilde{\mathbf{m}}_k\|^2, \quad (28) $$

where $e_d$ is the $d$-th standard basis vector.

### 2) Problem Reformulation

Due to the $SE(d)$ constraint and the rational cost function, Problem (28) is hard to solve globally. However, the problem can again be lifted to a QCQP by introducing a series of relaxations and substitutions. First, we relax the $SO(d)$ to a $O(d)$ constraint, which essentially drops the $\det(\mathbf{C}) = 1$ constraint. As discussed in [5], this relaxation is often tight without additional constraints, and if not, handedness constraints can be added [10]. As we are automatically finding all redundant constraints, these constraints will be added later if required. Secondly, by inspecting the cost in (28), we note that the following substitution makes the cost quadratic:

$$ v_k = (e_d^\top \mathbf{T} \tilde{\mathbf{m}}_k)^{-1} \mathbf{T} \tilde{\mathbf{m}}_k, \quad (29) $$

and it can be enforced as quadratic constraints $l_k(\mathbf{T}) = 0$ by multiplying both sides by the denominator. We obtain the following QCQP:

$$ \begin{gathered} \min_{\mathbf{C},t} \sum_{k \in [N]} \|\mathbf{y}_k - \mathbf{M}_d \mathbf{v}_k\|^2 \\ \text{s.t.} \quad (\mathbf{I}_d - \mathbf{v}_k e_d^\top) \mathbf{T} \tilde{\mathbf{m}}_k = \mathbf{0}, \quad k \in [N] \\ \mathbf{C}^\top \mathbf{C} = \mathbf{I}_d. \end{gathered} \quad (30) $$

Because the $d$-th element of $v_k$ is always one by definition, we introduce

$$ \mathbf{u}_k^\top := [\mathbf{v}_k[1] \ \dots \ \mathbf{v}_k[d-1] \ \mathbf{v}_k[d+1]], \quad (31) $$

and we write (30) as a QCQP in the lifted vector

$$ \mathbf{x}^\top = [h \ t^\top \ \text{vec}(\mathbf{C})^\top \ \mathbf{u}_1^\top \ \dots \ \mathbf{u}_N^\top]. \quad (32) $$

For sampling feasible points, we randomly generate positions $t^{(s)}$ uniformly from a unit cube, and orientations $\mathbf{C}^{(s)}$ from uniformly sampled quaternions. We construct $\mathbf{Y}$ from these samples as outlined in Section IV-A.

When using AUTOTemplate, we first add parameters that are linear polynomials of each landmark's coordinates, because the substitutions result in constraints that are linear in both the landmark coordinates and the camera pose. However, we will see in Section VII-C that we need to add additional quadratic substitutions to achieve tightness. We therefore include all up to quadratic monomials in the parameters $\mathbf{p}$.

## VII. SIMULATION RESULTS

We are now equipped to apply AUTOTIGHT and AUTOTemplate to the two example problems and problems from the literature. In this section, we demonstrate the performance on simulated data, while in the next section, we provide results on real-world datasets for RO and stereo localization.

### A. Hyperparameters

Throughout the experiments, we keep the following parameters fixed. When learning the constraints, we oversample the data matrix $\mathbf{Y}$ by 20% to improve conditioning of the nullspace problem. In practice, we found this to be sufficient to ensure linearly independent constraints without significantly increasing the cost of the nullspace decomposition. For the SDP solver, we use MOSEK [40] interfaced through

<footnote n="17">¹⁷We use $N$ and not $N_m$ because in stereo localization, the number of landmarks determines the problem size $N$ (since the number of poses is fixed to one).</footnote>
<table><caption>TABLE II: Overview of the considered problems, their tightness and whether there are redundant constraints. Highlighted in red are formulations that were found to be non-tight.</caption><thead><tr><th>Problem</th><th>lifting</th><th>redundant constr.</th><th>cost tight</th><th>rank tight</th></tr></thead><tbody><tr><td>RO loc.</td><td>z (25)</td><td>no</td><td>yes</td><td>yes</td></tr><tr><td rowspan="2">stereo loc.</td><td>y (26)</td><td>yes</td><td>yes</td><td>yes</td></tr><tr><td>u (31)</td><td>yes</td><td>no</td><td>no</td></tr><tr><td></td><td>u, u &otimes; t</td><td>yes</td><td>yes</td><td>no</td></tr><tr><td>PPR [10]</td><td>none</td><td>no</td><td>yes</td><td>yes</td></tr><tr><td>PLR [10]</td><td>none</td><td>yes</td><td>yes</td><td>yes</td></tr><tr><td>rPPR [7]</td><td>&theta; &otimes; &theta;</td><td>yes</td><td>no</td><td>no</td></tr><tr><td rowspan="2">rPLR [7]</td><td>&theta; &otimes; w</td><td>yes</td><td>yes</td><td>no</td></tr><tr><td>&theta; &otimes; w</td><td>yes</td><td>no</td><td>no</td></tr></tbody></table>

cvxpy [53, 54], fixing the tolerances of primal and dual feasibility, as well as the relative complementary gap to $10^{-10}$ and the tolerance of infeasibility to $10^{-12}$. These tolerances were found to give satisfactory solution accuracy without impeding convergence. For finding the minimal set of constraints (Section V-D), we set the relative gap termination to $10^{-1}$ to allow even for inaccurate solutions to be returned (as the output is only used for ordering the constraints).

In terms of local solvers, we use the off-the-shelf pymanopt [55] solver for all problems in Section VII-D, using the conjugate gradient optimizer and for stopping criteria $10^{-6}$ in gradient norm and $10^{-10}$ in step size. When inequality constraints are present in the QCQP, we use the log-sum-exp function described in [56, §4.1] with $\rho = 10$ and $u = 10^{-3}$. For RO localization, we use the scipy implementation of the BFGS solver, and our custom GN implementation, respectively, with the same stopping criteria as for pymanopt. The above criteria were chosen to achieve good performance while attempting to yield similar accuracy as the SDP solver.

In order to ensure the accuracy of the constraints, we compute the nullspace of $Y$ twice, removing the sample from $Y$ with highest nullspace errors after the first pass. This ensured that the maximal error was below $10^{-10}$ after the second pass for all problems.

As in [7], a problem is considered cost tight when its RDG is below 0.1%. It is considered rank tight when the ER is above $10^7$. Parameters that change for each problem, such as the considered noise levels, variable sets, and example problem sizes, are summarized in Table I. The chosen noise levels and problem sizes ensure unique and reasonably accurate solutions. We use fully connected measurement graphs for all considered problems, as visualized in Figure 3 (c).

## B. Range-Only Localization

**AUTOTIGHT:** We start by using AUTOTIGHT to evaluate the two different substitutions of RO localization, on a small example problem as defined in Table I.

The data matrix $Y$ exhibits a well-separated nullspace for both substitutions, as can be seen in Figure 4. We can see immediately that the $z_n$ substitution leads to a small nullspace ($N_n = 3 = N$), corresponding exactly to the number of substitution variables. The substitution $y_n$, on the other hand, leads to a nullspace that includes more than just the substitution variables ($N_n = 60 = 20N$), which shows the existence of redundant constraints.

<figure><img src="image_5.png" /><figcaption>Fig. 4: Singular value spectrum of the data matrix for RO localization. The singular values below the threshold (in orange) correspond to the nullspace basis vectors. For the substitution $z_n$ (25) (left plot), we find 3 basis vectors, however, for the substitution $y_n$ (26) (right plot) we find 20 basis vectors.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 5: Examples of learned constraint matrices for $z_n$ substitution (top) and the $y_n$ substitution (bottom) of RO localization. Shown below each matrix are the algebraic identities that the matrices enforce. For simplicity, we call $\theta_1^1 = [a \ b \ c]$.</figcaption></figure>

We show the three identified constraint matrices for the $z_n$ substitution in the first row of Figure 5. Interestingly, the three automatically found matrices correspond exactly to the three substitution formulas (shown below each matrix).<fnref n="18" /> The second row of Figure 5 shows three example matrices for the $y_n$ substitution. The first one is an example of a substitution constraint found by the algorithm, while the other two matrices are examples of discovered redundant constraints. Our method finds the $d(d + 1)/2 = 6$ substitution constraints, and 14 redundant constraints similar to the two shown examples.

We find that both substitutions lead to cost tight and rank tight relaxations when all constraints are imposed (including the redundant constraints for $y_n$ substitution), with ER above $10^9$ and RDG below $10^{-4}$.

**AUTOTemplate:** We use AUTOTemplate to find scalable templates for the $y_n$ substitution, which requires a significant number of redundant constraints. In this particular example, the learned constraints are interpretable, as shown in Figure 5, and we could infer the mathematical expression of all constraints (second outcome of Section IV-D). Instead, we show here that AUTOTemplate is a tractable alternative

<footnote n="18">¹⁸Here, we chose not to enforce the known constraint matrices using (11), to highlight the interpretability of the found constraints.</footnote>
<table><caption>TABLE III: Breakdown of characteristics for all tightened problems for the formulation phase of AUTOTEMPLATE. This phase has to be run only once, and the output are reduced (red.) constraint (constr.) templates. All times are in seconds, with $t_n$ the total time to compute the nullspaces, $t_a$ the time to apply templates to all variables, $t_s$ the time to check for tightness, and $t_r$ the time required to reduce the constraints using (22).</caption><thead><tr><th>Problem</th><th>Dimension n per iteration</th><th># Constraints</th><th># Reduced</th><th>t<sub>n</sub> [s]</th><th>t<sub>a</sub> [s]</th><th>t<sub>s</sub> [s]</th><th>t<sub>r</sub> [s]</th><th>total [s]</th><th>RDG</th><th>ER</th></tr></thead><tbody><tr><td>RO (z<sub>n</sub>)</td><td>[10 15]</td><td>4</td><td>4</td><td>0.01</td><td>0.00</td><td>0.17</td><td>0.12</td><td>0.29</td><td>4.41e-05</td><td><strong>1.76e+09</strong></td></tr><tr><td>RO (y<sub>n</sub>)</td><td>[10 55]</td><td>61</td><td>33</td><td>0.08</td><td>0.09</td><td>0.30</td><td>9.25</td><td>9.71</td><td>4.69e-05</td><td><strong>2.27e+09</strong></td></tr><tr><td>stereo (2d)</td><td>[28 168 546 1365]</td><td>171</td><td>61</td><td>5.58</td><td>0.92</td><td>0.70</td><td>13.32</td><td>20.52</td><td><strong>3.59e-06</strong></td><td>4.39e+00</td></tr><tr><td>stereo (3d)</td><td>[91 910 3250 9100]</td><td>639</td><td>225</td><td>299.35</td><td>3.55</td><td>3.30</td><td>80.03</td><td>386.23</td><td><strong>2.86e-07</strong></td><td>2.01e+01</td></tr><tr><td>PPR</td><td>[91]</td><td>21</td><td>7</td><td>0.21</td><td>0.03</td><td>0.17</td><td>0.15</td><td>0.56</td><td>3.31e-06</td><td><strong>8.74e+09</strong></td></tr><tr><td>PPL</td><td>[91]</td><td>21</td><td>10</td><td>0.21</td><td>0.03</td><td>0.40</td><td>0.77</td><td>1.40</td><td>1.85e-04</td><td><strong>2.90e+08</strong></td></tr><tr><td>rPPR</td><td>[91 105 325 120 351 703]</td><td>1771</td><td>1003</td><td>6.75</td><td>12.94</td><td>18.45</td><td>548.16</td><td>586.29</td><td><strong>1.08e-06</strong></td><td>3.44e+01</td></tr><tr><td>rPLR</td><td>[91 105 325 120 351 703]</td><td>2349</td><td>2049</td><td>6.79</td><td>17.45</td><td>22.14</td><td>944.40</td><td>990.78</td><td><strong>4.05e-05</strong></td><td>2.15e+01</td></tr></tbody></table>

<figure><img src="image_7.png" /><figcaption>Fig. 6: Rank-tightness study for RO localization, using $z_n$ substitution (left) vs. $y_n$ substitution (right). We compare the spectra with different numbers of added constraints (gray lines), highlighting the points where cost tightness (C) and rank tightness (R) are obtained in red and black, respectively.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 7: Timing study for RO localization, using the $y_n$ substitution. We compare using only the reduced (solid line) or all templates (dashed line) output by AUTOTEMPLATE, which are very close for this particular problem. They compare favorably to learning constraints from scratch for each problem using AUTOTIGHT.</figcaption></figure>

that does not require any manual interpretation of constraint matrices.

The employed variable sets are given in Table I. The algorithm terminates after using group $\{h, \theta_1, y_1\}$, at which point the identified templates lead to a tight relaxation (in both cost and rank) when applied to all $N = 3$ positions.<fnref n="19" />

Before applying the templates to new problems of increasing size, we reduce them to a smaller sufficient subset of constraints using (22). Figure 6 visualizes this process, showing rank- and cost tightness for different subsets of constraints used. First, we confirm that the substitution $z_n$ leads to rank- and cost tightness after adding the substitution constraints only. For $y_n$, when adding constraints one-by-one in the order dictated by (22), we find that 33 out of the 61 constraints are enough for rank tightness. Cost-tightness, on the other hand, is achieved after adding 19 constraints only.

We apply the templates for $y_n$ to problems with up to 30 positions. Figure 7 shows the time required for creating the constraints and solving the SDP for each problem size. We also report the results for $z_n$ in Appendix A. We compare the processing times of using AUTOTIGHT from scratch vs. applying the templates from AUTOTEMPLATE, using either all or only the reduced constraint set sufficient for rank tightness.

For the substitution $y_n$, AUTOTIGHT becomes prohibitively expensive beyond $N = 15$ positions. When using AUTOTEMPLATE, the cost of generating the constraints becomes

tractable, staying close to the cost of solving the SDP for all problem sizes. Ordering the constraints according to (22) did not have a significant effect in this case, and there is little difference between using the full vs. only the reduced constraint set. Note that learning the templates and determining the reduced constraints are part of the formulation phase and thus constitute a fixed cost, listed separately in Table III.

### C. Stereo Localization

**AUTOTIGHT:** We first use AUTOTIGHT to investigate whether the stereo localization problem (30) can be tightened, using the example problem defined in Table I.

The left plots of Figure 8 show the cost tightness study. Even when adding all 45 identified constraints, the problem cannot be tightened in the present form. Note how quickly we came to this conclusion: no manual search for redundant constraints had to be performed, a process that can be very time consuming.

We resort to (sparse) Lasserre hierarchy [12] to tighten the problem. We try different higher-order lifting functions and retest for tightness after adding all possible redundant constraints. We individually test additions such as $u_k \otimes u_k$, $t \otimes t$, etc. and find that by adding $(u_k \otimes t)$ for each landmark, we achieve tightness. For simplicity, we call the combined substitution $z_k^\top := [u_k^\top (u_k \otimes t)^\top] \in \mathbb{R}^{d+d^2}$. Figure 8 on the right shows the cost tightness test in 3D, which now passes. Since cost tightness is achieved, we can solve (22) to determine a significantly smaller subset of sufficient constraints: we reduce the number from 639 to 144 constraints, as shown in Figure 8. In all considered cases, rank tightness is not attained,

<footnote n="19">¹⁹Note that we do not need to consider any combinations of positions (or substitutions), which is a consequence of the problem being separable. This could have been observed from (23), but we did not exploit this structure here to facilitate the extension to regularized problems (i.e., with motion prior).</footnote>
<figure><img src="image_9.png" /><figcaption>Fig. 8: Tightness study for stereo localization, using the original substitutions (left) vs. the higher-order substitutions (right). The bisection algorithm for finding the number of required constraints, terminates immediately for the original substitutions as even all constraints are not sufficient for cost tightness. When adding higher-order substitutions, tightness is achieved after a few steps, using only 144 constraints when sorting constraints using (22) (sort.), and 590 when using the original order (orig.).</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>Fig. 9: Study of the eigenvalue spectra of stereo localization using original order of constraints (left) and the sorted order using (22) (right). Even after adding the higher-order substitutions and all redundant constraints, a significant number of eigenvalues are nonzero. More higher-order Lasserre variables may be required to achieve rank tightness. See Figure 6 for a detailed description of the labels.</figcaption></figure>

which is shown in Figure 9. Rank tightness may require lifting functions of even higher order. As we are already approaching what is computationally feasible for the SDP solver, we settle for cost tightness.

**AUTOTemplate:** To tighten new problems, it is crucial to use AUTOTemplate, for two reasons. Firstly, the problem dimension is large, in particular after adding the additional lifting functions required for tightness. Secondly, an investigation of the learned constraints, shown in Figure 10, suggests that many matrices actually depend on the (known) landmark coordinates and are therefore not easily interpretable.

Using the succession of variable sets listed in Table I, we achieve tightness after including all groups up to {$h, z_1, z_2$}. Figure 11 shows the output of the method, for a 2D example: a set of templates over not only the original variables, but also their products with the parameters. Most importantly, note that the matrix is now more quantized, with all nonzero elements in {$2, \sqrt{2}, \pm 1, \pm \frac{1}{\sqrt{2}}, \pm \frac{1}{2}$}. We have thus factored out all landmark dependencies and the obtained templates can be applied to any setup. The amount of templates may seem unmanageable at first; but the templates can be significantly reduced by solving (22): only 65 of the 171 templates (highlighted in dark in Figure 11) are sufficient for tightness.

We successfully apply the templates for up to 30 landmarks.

<figure><img src="image_11.png" /><figcaption>Fig. 10: Three learned constraint matrices for a 2D stereo localization problem. Many of the matrices are less sparse than in the RO localization example and contain non-quantized numbers which suggests a dependency on landmark coordinates. Only few matrices, such as the one shown on the left, are interpretable (the identity is shown below the plot, where for simplicity, we call $\mathbf{t} = (t_x, t_y)^\top$, $\mathbf{T}\mathbf{m}_2 = (x, y, 1)^\top$, thus $\mathbf{u} = \frac{1}{y}(x, 1)^\top$ and $\mathbf{z}_1 = \frac{1}{y}(x, 1, t_x x, t_y x, t_x, t_y)^\top$).</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>Fig. 11: Subset of the constraint templates learned for stereo-localization in 2D after factoring out parameters. The red bars delimit different parameter dependencies, with the left-most block corresponding to the original variables. Highlighted in dark is the reduced set of templates sufficient for tightness (65 out of 171).</figcaption></figure>

Figure 12 shows how the times for applying the templates and solving the SDP (i.e., for the application phase) scale with $N$. The one-time cost for finding the reduced set of templates (i.e., for the formulation phase) is reported in Table III. As for RO localization, learning templates from scratch for each new setup does not scale beyond $N = 15$ landmarks, while applying the reduced templates comes at a reasonable cost, comparable to the cost of solving the SDP itself. This is a considerable improvement compared to existing approaches: inputting the same problem formulation to the (sparse) Lasserre hierarchy tool provided by [7] leads to unmanageable numbers of variables and constraints, even for small problem sizes. For $d = 3$ and only $N = 3$ landmarks, a total of 27,692 trivially satisfied constraints are generated, which is far beyond what SDP solvers can currently handle in reasonable time. In contrast, we can go to as many as $N = 30$ landmarks, at which point we compute less than 5000 sufficient constraints for tightness.

## D. Other Problems

We conclude the simulation study by applying the proposed method to a number of problems from the literature whose semidefinite relaxations have been shown to be tight using certain redundant constraints. We first consider two multimodal registration problems that have been treated by Briales et al. [10]: point-point registration (PPR) and point-
<figure><img src="image_13.png" /><figcaption>Fig. 12: Timing study of the stereo-localization problem in 3D as we increase the number of landmarks N. The labels are the same as in Figure 7. Learning constraints from scratch using AUTO TIGHT is prohibitively expensive even for N = 10. On the other hand, AUTO-TEMPLATE scales reasonably up to N = 30.</figcaption></figure>

line registration (PLR), before studying their robust version, provided by Yang et al. [7], in the next section.

### 1) PPR and PLR [10]

In multimodal registration, the goal is to find an object's translation $t \in \mathbb{R}^d$ and orientation $C \in SO(d)$ w.r.t. a world frame, given measurements of points lying on the object. The object is assumed to be represented by a set of known geometric primitives of either points (PPR), lines (PLR), or planes (not considered for brevity). The problem is posed as the following minimization problem [10]:

$$ \min_{C \in SO(d), t \in \mathbb{R}^d} \sum_{i=1}^{N} \|Cp_i + t - y_i\|_{W_i}^2, \quad (33) $$

with $p_i \in \mathbb{R}^d$ the measured point and $y_i$ an arbitrary point on the associated primitive $P_i$ (note that data association is assumed known). The matrix $W_i \succ 0 \in \mathbb{R}^{d \times d}$ is chosen depending on the type of primitive $P_i$, see Appendix B.

Problem (33) can be relaxed to a QCQP by dropping the determinant constraint from $SO(d)$ as explained in Section VII-C, and introducing $x^\top = [h \ \theta^\top]$, with $\theta^\top = [t^\top \ \text{vec}(C)^\top]$.

**Manual method [10]:** The primal relaxation of problem (33) was shown to be always tight when using a specific set of redundant constraints, that enforce, for example, the handedness of the C matrix that may have been lost because of dropping the determinant constraint [10]. The formulas of these 22 redundant constraints are given in Appendix B.

**Proposed method:** AUTO TIGHT finds the required redundant constraints outlined above automatically. As shown in Appendix A, we find a total of 21 independent constraints, including the homogenization, suggesting that at least one of the 22 constraints presented by [10] is linearly dependent. Indeed let $e_i(C) = 1$ be the orthonormality constraints involving the diagonal with $i \in \{1, 2, 3\}$ for (39b) and $i \in \{4, 5, 6\}$ for (39c). Then, it is easy to see that $\sum_{i=1}^3 e_i(C) = \sum_{i=4}^6 e_i(C)$, so any of these six constraints can be written as a linear combination of the five others.

While the constraints by [10] have been shown to be sufficient for tightness, they have not been shown to be necessary. In fact, we found that, for the considered noise level, none of the redundant constraints are required for PPR to be both cost and rank tight, as shown in Figure 13. For PLR, Figure 13

<figure><img src="image_14.png" /><figcaption>Fig. 13: Rank-tightness study for PPR (left) and PLR (right) problems [10]. PPR is cost tight without redundant constraints, and for PLR, only one and three redundant constraints are required for cost and rank tightness, respectively; a small subset of the 12 available redundant constraints [10].</figcaption></figure>

shows that the solution becomes rank one after adding as few as three of the 12 available redundant constraints.

### 2) rPPR and rPLR [7]

Next, we consider the robust versions of the two registration problems: rPPR and rPLR. These problems are called robust pointcloud registration and robust absolute-pose estimation, respectively, in [7], and they are two (of many) robust estimation problems that can be formulated as a QCQP. Using the truncated least-squares (TLS) cost function as an example, rPPR and rPLR can be formulated as

$$ \min_{\theta \in \mathcal{D}, w \in \{\pm 1\}^N} \frac{1}{2} \sum_{i=1}^{N} \frac{1+w_i}{\beta_i^2} r^2(\theta, y_i) + 1 - w_i, \quad (34) $$

where $y_i$ are measurements, $w$ is the vector of decision variables (for outliers, $w_i = -1$ and for inliers $w_i = 1$) and $\beta_i > 0$ are user-defined parameters determining the truncation threshold. The feasible domain $\mathcal{D}$ and a sketch of the derivation of (34) are given in Appendix B. The residual functions are given by

$$ \text{rPPR: } r(\theta, y_i) = \|Cp_i + t - y_i\|^2, \quad (35) $$

$$ \text{rPLR: } r(\theta, y_i) = \|Cp_i + t\|_{I_d - y_i y_i^\top}^2, \quad (36) $$

where $y_i$ are pointcloud and unit direction measurements, respectively, in rPPR and rPLR. Problem (34) can be written as a QCQP in the lifted vector

$$ x^\top = [h \ \theta^\top \ w^\top \ z^\top], \quad (37) $$

with $\theta^\top = [t^\top \ \text{vec}(C)^\top]$. The variable $z$ contains additional substitutions that are required to make problem (34) quadratic in $x$ (the cost is cubic because the residual functions $r$ are linear in $\theta$), as discussed next.

**Manual method [7]:** In [7], the authors propose to add the (sparse) Lasserre lifting function $z = \theta \otimes w$, which leads to a tight relaxation after adding a list of (trivially satisfied) constraints. The authors also mention in passing that other lifting functions, such as $z = \theta \otimes \theta$, which also allow to write (34) as a QCQP, do not lead to a tight relaxation.

**Proposed method:** We study both lifting functions and come to the same conclusions as in [7]: both formulations allow for a large number of redundant constraints (which we find automatically), but only the first formulation becomes
<figure><img src="image_15.png" /><figcaption>Fig. 14: Timing results of scaling to N landmarks for rPPR. Thanks to AUTO TEMPLATE, we can automatically create the constraints of problems up to N = 15 landmarks.</figcaption></figure>

tight. Because of the large number of variables in the lifted state vector, we resort directly to AUTO TEMPLATE. The variable ordering used (for both problems) can be found in Table I, where we drop some of the trivial first variable sets to save time. When using the lifting function $z := \theta \otimes w$, the method terminates with cost tightness after considering variables $\{l, \theta, z_1, z_2\}$. For $z := \theta \otimes \theta$, the method returns that no tightness can be achieved.

The number of constraint templates before and after reduction can be found in Table III. The number of required constraints is already very high (more than 1000) when considering only $N = 4$ and $N = 6$ for rPPR and rPLR, respectively. Nevertheless, we can apply the templates to problems up to size $N = 15$, as shown in Figure 14, for rPPR. We report the timing results for rPLR, and the eigenvalue spectra for rPPR and rPLR, in Appendix A. For both problems, learning constraints from scratch is prohibitively expensive. With AUTO TEMPLATE, we obtain cost tightness for all considered problems. Just as in stereo localization, rank tightness is not achieved, as shown in Figure 22 in the Appendix, and is not computationally tractable since we already need many constraints for cost tightness.

As a final study, we compare the number of constraints we find with the number of constraints found in [7] in Table IV. The results suggest that we find a significantly smaller subset of constraints, but without compromising tightness. One possible explanation is that we find more than only the “trivially satisfied” redundant constraints at each level, and thus we can chose from a larger pool when tightening the problem. We plan to further investigate this phenomenon in future research.

<table><caption>TABLE IV: The number of constraints for cost tightness found for rPPR and rPLR, respectively, using our method and the method proposed by [7], as a function of the number of measurements N.</caption><thead><tr><th rowspan="2">N</th><th colspan="2">rPPR</th><th colspan="2">rPLR</th></tr><tr><th>our method</th><th>[7]</th><th>our method</th><th>[7]</th></tr></thead><tbody><tr><td>10</td><td>4,508</td><td>6,257</td><td>5,330</td><td>7,379</td></tr><tr><td>11</td><td>5,293</td><td>7,398</td><td>6,279</td><td>8,724</td></tr><tr><td>12</td><td>6,139</td><td>8,633</td><td>7,304</td><td>10,180</td></tr><tr><td>13</td><td>7,046</td><td>9,962</td><td>8,405</td><td>11,747</td></tr><tr><td>14</td><td>8,014</td><td>11,385</td><td>9,582</td><td>13,425</td></tr><tr><td>15</td><td>9,043</td><td>12,902</td><td>10,835</td><td>15,214</td></tr></tbody></table>

VIII. REAL-WORLD EXPERIMENTS

To conclude, we showcase the performance of the proposed framework on real-world datasets for RO localization and stereo localization. The purpose of these experiments is to 1) give an example of the full pipeline in action and to 2) investigate how the constraints, determined using a specific example problem, generalize to real data with different characteristics. In the formulation phase, we run AUTO TEMPLATE with random landmarks to generate constraint templates. No knowledge of the actual measurement setup is required at this point. In the application phase, we apply the templates to generate constraints, using the actual landmark locations at each considered pose.

A. Experimental Setups

We test our methods on two real-world datasets, visualized in Figure 15. The starrynight dataset [57] includes stereo-camera images of Vicon markers scattered on the floor. The STAR-loc dataset [58] includes stereo-camera images of April-tag [59] landmarks scattered around a room at different heights and orientations. The STAR-loc dataset also includes UWB-based distance measurements to eight fixed anchors.

For RO localization, we always randomly select 4 out of the 8 available anchors to investigate the local minima that typically arise when anchors are almost co-planar. We report results on three example runs: zigzag_s3, loop-2d_s4, and eight_s3. For stereo localization, we only consider poses where more than 4 landmarks are observed, and we cap at maximum 8 landmarks, to limit the computation time. We initialize the local solver with normally distributed positions centered at the ground truth and with 1 m standard deviation in all axes.

B. Results

First, we investigate the tightness of the relaxations when evaluated on real data. Figure 16 shows the ER and RDG for both RO and stereo localization, for randomly picked poses from both datasets (see Figure 15 for plots of the selected poses). We plot the respective tightness measures against the maximum residual error, which is a good proxy for the noise level and has been shown to affect the tightness of semidefinite relaxations [9, 41]. As expected, the relaxation of RO localization is mostly rank tight across all considered datasets and poses, with an ER of more than $10^6$ for most poses. On the other hand, the stereo-localization relaxation is only reliably cost tight for poses with a sub-pixel maximum residual error, which is a characteristic found in the starrynight dataset but in none of the runs from the STAR-loc dataset.

Next, we study the occurrence of local vs. global minima found in both problems. We certify a local solution by trying to find dual variables that satisfy (6) via a feasibility SDP. To account for numerical errors, we change (6a) to $|H(\rho, \lambda)| \le \epsilon 1$ and minimize $\epsilon$ as objective function. We claim that a candidate solution $\hat{x}$ is certified if we find a feasible solution with $\epsilon \le 10^{-3}$.

Figure 17 shows the cost of certified and uncertified solutions of RO and stereo localization, as a function of the
<figure><img src="image_16.png" /><figcaption>Fig. 15: Experimental setups of real-world datasets: starrynight (left) [57], and STAR-loc (middle & right plots) [58]. Apriltags The plots show the ground-truth poses at which stereo (top) and UWB (bottom) measurements from fixed landmarks (black crosses) are obtained.</figcaption></figure>

<figure><img src="image_17.png" /><figcaption>Fig. 16: Real-world tightness study for RO (left plots) and stereo localization (right plots). Each data point corresponds to one estimated pose. Cost-tightness (top) and rank tightness (bottom) are compared with the maximum residual error. We see that RO localization is mostly cost- and rank tight, while stereo localization is only cost tight for the lowest residual error levels in the starrynight dataset.</figcaption></figure>

<caption>Fig. 17: Real-world comparison of local and global solutions as a function of the maximum residual error. Each data point corresponds to one pose estimate, and we distinguish between true positives (tp, certified global minimum), true negatives (tn, uncertified local minimum), false negatives (fn, uncertified global minimum). Crucially, no false positives occurred. Note that all local solutions for stereo localization exhibit a high cost, and as the maximum residual error increases, false negatives occur, which is a consequence of reduced tightness (compare with Figure 16).</caption>

maximum residual error. For RO localization, we note that local minima are ubiquitous for both problems, across all noise levels. Second, it can be observed that, as the noise increases, the global solution candidates from the stereo-localization problem are not all certified anymore, because of the lack of tightness at higher noise levels. However, local minima occur even at lower levels, and the relaxation can correctly identify them. For RO localization, all global solutions are correctly certified. All local solutions, which, compared to stereo localization, are harder to classify based on only their cost, are also detected correctly. Note that since this relaxation is rank tight, solving the primal SDP and extracting $x^*$ from the rank-1 $X^*$ would also be a viable solution method.

We evaluate the performance of the locally optimal vs. certifiably optimal solver qualitatively in Figure 18. The local solver has an overall high success rate for the two datasets eight_s3 and zigzag_s3, but for loop-2d_s4, it is prone to

converge to local minima as the number of anchors drops below $N_m = 6$. Intuitively, the cost landscape is more likely to exhibit an approximate symmetry when all anchors are close to co-planar, which is more likely with fewer anchors, and the local solver gets stuck in the wrong half when initialized there. Note that due to high noise, even the globally optimal position may be far from the ground truth. For stereo localization, the local minima occurring in the starrynight dataset are in fact invalid camera poses since the landmarks are not in the estimated field of view. While such faulty estimates could be identified with a manually found heuristic, the local minima occurring in other datasets, for example in loop-2d_s4 shown in Figure 18, are harder to detect with heuristics. We also recall that for the latter dataset, the relaxation is not always tight at the present noise levels, and therefore only few estimates can be certified (only the plotted ones). Those few optimal estimates would however be enough to provide an accurate overall trajectory estimate, significantly better than if the poor local minima were used.

Quantitative results are shown in Table V. We report the average translation and orientation errors, defined as

$$e_t = \|\hat{t} - t\|_2, \quad e_C = \|\hat{C}^\top C - I\|_F, \qquad (38)$$

where $t, \hat{t}$ and $C, \hat{C}$ are the ground truth and estimated trans-
<figure><img src="image_18.png" /><figcaption>Fig. 18: Real-world qualitative study of RO (top row) and stereo localization (bottom row). The left plots show the success rate of the local solver (proportion of convergence to global minima), as a function of the number of landmarks $N_m$. For RO, the plots in the middle and right show results for the loop-2d_s4 dataset, where the success rate depends highly on the number of considered anchors. Global minima are marked with green dots, and (poor, local) minima with red dots. For stereo localization, there is a weaker dependence of the success rate on the number of landmarks, but local minima, shown by small poses with red center, appear less frequently for the starrynight dataset, which exhibits less noise. Ground truth trajectories are shown with gray lines, landmarks with black crosses.</figcaption></figure>

lation and orientation, respectively. The average and standard deviation (STD) are taken over all considered datasets. We observe that for both applications, local minima returned by the standard local solver are very poor in accuracy, with errors more than double those of global optima.

To summarize, in both applications, random initializations and landmark placements are prone to yield bad, locally optimal solutions. For low-enough noise levels, we can certify globally optimal solutions since our formulation is tight when the automatically identified redundant constraints are used.

# IX. CONCLUSION AND FUTURE WORK

We have presented new tools to find all possible redundant constraints for a given QCQP, and applied them to tighten the semidefinite relaxations of many state estimation problems encountered in robotics. The first tool, AUTOIGHT, allows for the fast evaluation of different problem formulations. We have successfully used this tool to evaluate different substitutions for RO localization and found a novel tight formulation for stereo localization. The second tool, AUTOTemplate, can be employed to create scalable templates to tighten new setups and larger problem sizes. To show the wide applicability of both tools, we have also evaluated their performance on example problems from the literature [10, 7], showing that we find tight relaxations with fewer redundant constraints than

<table><caption>TABLE V: Real-world localization errors of RO and stereo localization. The errors $e_t$ and $e_C$ are calculated as in (38).</caption><thead><tr><th rowspan="2"></th><th colspan="2">RO localization</th><th colspan="4">stereo localization</th></tr><tr><th>global e<sub>t</sub></th><th>local e<sub>t</sub></th><th>global e<sub>t</sub></th><th>global e<sub>C</sub></th><th>local e<sub>t</sub></th><th>local e<sub>C</sub></th></tr></thead><tbody><tr><td>mean</td><td>0.335296</td><td>0.945848</td><td>0.093914</td><td>0.044693</td><td>7.676829</td><td>2.778475</td></tr><tr><td>STD</td><td>0.367695</td><td>0.506339</td><td>0.058125</td><td>0.121845</td><td>1.681287</td><td>0.224380</td></tr></tbody></table>

<figure><img src="image_19.png" /><figcaption>Fig. 19: Timing study for RO localization, using the $z_n$ substitution. The same explanations as for Figure 7 apply.</figcaption></figure>

previously considered. As SDPs scale poorly with the number of constraints, this is an important step to make semidefinite relaxations scale to problems encountered in robotics.

A number of follow-up questions deserve further attention. First, it has been shown that both the measurement graph and the noise level can have an effect on tightness [41, 33, 4]. In future work, we plan to further investigate these characteristics using the new tools, in particular in order to understand to what level the additional redundant constraints may push the boundaries of tightness. Along the same lines, a given measurement graph may in fact help in finding the variable substitutions and parameters that are most likely to succeed, a component of the proposed method that currently requires user input.

Finally, the full potential of the proposed method will be unlocked when faster SDP solvers are developed for problems that require redundant constraints. First steps into this direction have shown promising results [6, 7], but more work remains to be done. In parallel, there lies potential in further pushing the efficiency of optimality certificates of fast local solvers, for example using sampling-based approaches as in [19] or sparsity-exploiting approaches as in [33, 18].

# ACKNOWLEDGMENTS

We would like to thank David Rosen, Alan Papalia, Luca Carlone, Heng Yang, and Nicolas Boumal for helpful discussions around the topic of this paper. We would also like to thank Daniel Frisch for pointing out typos.

# APPENDIX

## A. Additional simulation results

Figure 19 shows the time required for AUTOIGHT and AUTOTemplate, applied to RO localization with $z_n$ substitution. Applying templates and checking for tightness remain relatively cheap as the problem size grows, because the number of total constraints grows only linearly in the number of variables. Even learning the constraints from scratch is reasonably fast for this case. Figure 20 shows the discovered constraint matrices (in compressed form) for PLR and PPR. Figure 21 shows the timing study for rPLR, and Figure 22 shows the obtained eigenvalue spectra for both rPPR and rPLR.
<figure><img src="image_20.png" /><figcaption>Fig. 20: Learned constraint templates for PPR and PLR [10]. The labels h and cᵢ correspond to the homogenization variable and the i-th element of vec (C), respectively. The constraints highlighted in dark are sufficient for rank tightness of PLR, PPR is rank tight without any redundant constraints.</figcaption></figure>

<figure><img src="image_21.png" /><figcaption>Fig. 21: Timing results of scaling to N landmarks for rPLR. Thanks to AUTO TEMPLATE, we can automatically create the constraints of problems up to N = 15 landmarks.</figcaption></figure>

## B. Mathematical formulations of PPR, PLR, rPPR, and rPLR

The PPR and PLR problems are derived from (33) by using $W_i = I_d$ for PPR and $W_i = I_d - v_i v_i^\top$ for PLR, where $v_i$ is the unit direction of the line. Using $W_i = n_i n_i^\top$, measurements to planes with normal vectors $n_i$ could also be modeled with the same framework. The redundant constraints for the relaxation of (33) are given by [10]

$$h^2 = 1 \quad (\text{prim., homogenization}), \qquad (39a)$$

$$I_d = C^\top C \quad (\text{prim., orthonormal rows}), \qquad (39b)$$

$$I_d = CC^\top \quad (\text{red., orthonormal columns}), \qquad (39c)$$

$$c_i \times c_{i+1|3} = c_{i+2|3}, i \in [3] \quad (\text{red., handedness}), \qquad (39d)$$

where prim. and red. are short for primary and redundant, | is the modulo operator and $c_i$ is the i-th column of C. This leads to a total of $1 + 2 \cdot 6 + 3 \cdot 3 = 22$ constraints in 3D, accounting for the symmetry of the optimization variable.

rPPR and rPLR can be written in the form [7]

$$\min_{\theta \in \mathcal{D}} \sum_{i=1}^{N} \rho(r(\theta, y_i)), \qquad (40)$$

where $\mathcal{D}$ is the domain of $\theta$, $\rho$ is a robust cost function and $r$ the residual function. In order to satisfy the Archimedean condition, the authors further restrict the domain $\mathcal{D}$ to the domain with $t \in \mathbb{R}^d$ contained in the ball of radius $T$.<fnref n="20" /> For rPLR, $t$ is additionally chosen so that the landmarks are in the field of view of the camera, characterized by aperture angle $\alpha$. These two problems are thus examples with primary inequality constraints in (1). For TLS cost, it has been shown that

<figure><img src="image_22.png" /><figcaption>Fig. 22: Rank-tightness study for rPPR (left) and rPLR (right). We obtain cost tightness, but not rank tightness, for both problems.</figcaption></figure>

solving (40) is equivalent to solving the QCQP (34). This is also true for many other robust cost functions [7], and enabled by applying Black-Rangarajan duality [60].

## C. Proof of Theorem 1

In this proof, We aim to introduce minimal additional notation, and refer the interested reader to [19] for a more in-depth treatment of the following identities. We introduce the variety $\mathcal{V} := \{\theta | e_i(\theta) = 0, i \in [N_e]\}$, where $e_i$ are polynomial functions, which corresponds to the feasible set of (1). We define the linear subspace $\mathcal{L} := \{p(\theta) | p(\theta) = \beta^\top \text{vech}(x(\theta)x(\theta)^\top)\}$, where $\beta$ is any vector and $x(\theta)$ is the lifted vector as defined in (4), with the dependency on $\theta$ made explicit. We will need the property of poisedness, which, loosely speaking, determines whether characteristics derived for samples of a variety hold for any element of it.

**Definition (poisedness).** Let $\mathcal{R} = \mathbb{R}[\mathcal{V}]$ be the coordinate ring of $\mathcal{V}$, let $\mathcal{L} \subset \mathcal{R}$ be a linear subspace, and let $\mathbf{\Theta} \subset \mathcal{V}$ be a set of samples. We say that $(\mathcal{L}, \mathbf{\Theta})$ is poised if the only polynomial $q \in \mathcal{L}$ such that $q(\theta^{(s)}) = 0, \forall \theta^{(s)} \in \mathbf{\Theta}$, is the zero polynomial, i.e., $q(\theta) = 0, \forall \theta \in \mathcal{V}$.

As mentioned in Section IV-A, our sampling method ensures poisedness. For ease of notation, we use $y(\theta) := \text{vech}(x(\theta)x(\theta)^\top)$ in what follows. We create $\mathbf{Y}$ from the samples in $\mathbf{\Theta}$ as described in Section IV-A: $\mathbf{Y} = [y(\theta^{(1)}) \cdots y(\theta^{(S)})]$. The next lemma uses poisedness to ensure that the output of AUTOTIGHT is guaranteed to encompass all valid constraints.

**Lemma 1.** Let $(\mathcal{L}, \mathbf{\Theta})$ be poised, and let some polynomial $g(\theta) \in \mathcal{L}$. Let $\mathbf{a}$ be such that $g(\theta) = \mathbf{a}^\top y(\theta)$. Then we have:

$$g(\theta) = \mathbf{a}^\top y(\theta) = 0, \forall \theta \in \mathcal{V} \iff \mathbf{a} \in \text{null}(\mathbf{Y}^\top). \quad (41)$$

*Proof: (⇒)* Let $g(\theta) = 0, \forall \theta \in \mathcal{V}$. Then we also have $g(\theta^{(s)}) = \mathbf{a}^\top y(\theta^{(s)}) = 0, \forall \theta^{(s)} \in \mathbf{\Theta}$ because all samples are feasible by construction. Since $\mathbf{Y} = [y(\theta^{(1)}) \cdots y(\theta^{(S)})]$ this implies that $\mathbf{a}$ is in the nullspace of $\mathbf{Y}^\top$.

(←) Let $\mathbf{a} \in \text{null}(\mathbf{Y}^\top)$, or in other words $\mathbf{a}^\top \mathbf{Y} = [g(\theta^{(1)}) \cdots g(\theta^{(S)})] = \mathbf{0}^\top$. Since $(\mathcal{L}, \mathbf{\Theta})$ is poised and $g$ is zero for all samples $\theta^{(s)} \in \mathbf{\Theta}$, we also have $g(\theta) = 0, \forall \theta \in \mathcal{V}$. □

Consequently, the elements of the learned matrices by AUTOTIGHT, $\mathcal{A}_\ell$, span the nullspace of $\mathbf{Y}^\top$. Therefore, any

<footnote n="20">²⁰The Archimedean condition is a stronger form of compactness [43].</footnote>
matrix B of the set $\mathcal{A}_k$ can be written as a linear combination of the elements of $\mathcal{A}_\ell$. This is the main ingredient for proving Theorem 1.

*Proof of Theorem 1:* In Theorem 1, $(P_k)$ is given by

$$ (P_k) \qquad p_k^* = \min_{\mathbf{X} \succeq 0} \langle \mathbf{Q}, \mathbf{X} \rangle \qquad (42) \\ \text{s.t.} \quad \langle \mathbf{B}_j, \mathbf{X} \rangle = 0, \langle \mathbf{A}_0, \mathbf{X} \rangle = 1, $$

where $\mathbf{B}_j$ are the elements of $\mathcal{A}_k$ and equivalently, we have

$$ (P_\ell) \qquad p_\ell^* = \min_{\mathbf{X} \succeq 0} \langle \mathbf{Q}, \mathbf{X} \rangle \qquad (43) \\ \text{s.t.} \quad \langle \mathbf{A}_i, \mathbf{X} \rangle = 0, \langle \mathbf{A}_0, \mathbf{X} \rangle = 1, $$

where $\mathbf{A}_i$ are the elements of $\mathcal{A}_\ell$. Calling $q^*$ the optimal value of $(\mathbf{Q})$, we recall that $p_k^* \le q^*$ and $p_\ell^* \le q^*$, which is true by duality theory, or equivalently because $(P_k)$ and $(P_\ell)$ are rank relaxations of $(\mathbf{Q})$. Because of Lemma 1, $(P_k)$ is a relaxation of $(P_\ell)$ and we have $p_k^* \le p_\ell^*$. Therefore, if $p_k^* = q^*$, then $q^* = p_k^* \le p_\ell^* \le q^*$ and all inequalities are equalities.

Similarly, because $(P_k)$ is a relaxation of $(P_\ell)$ their respective optimal values respect rank $\mathbf{X}_k^* \ge \text{rank} \mathbf{X}_\ell^*$. Furthermore, note that the rank of any optimizer $\mathbf{X}^*$ of $(P_k)$ or $(P_\ell)$ is at least one, because the homogenization constraint prevents it from being zero. Therefore, if $\text{rank} \mathbf{X}_k^* = 1$ then $1 \le \text{rank} \mathbf{X}_\ell^* \le \text{rank} \mathbf{X}_k^* = 1$, which concludes the proof. $\square$

## REFERENCES

[1] J. Rehder, J. Nikolic, T. Schneider, T. Hinzmann, and R. Siegwart, "Extending kalibr: Calibrating the extrinsics of multiple IMUs and of individual axes," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2016, pp. 4304–4311.

[2] T. D. Barfoot, *State Estimation for Robotics*. Cambridge University Press, 2017.

[3] J. Nocedal and S. J. Wright, *Numerical Optimization*, 2nd ed., ser. Springer Series in Operation Research. New York, NY: Springer-Verlag, 2006.

[4] A. Papalia, A. Fishberg, B. W. O'Neill, J. P. How, D. M. Rosen, and J. J. Leonard, "Certifiably Correct Range-Aided SLAM," *IEEE Transactions on Robotics*, 2004.

[5] 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," *International Journal of Robotics Research*, vol. 38, no. 2-3, pp. 95–125, 2019.

[6] H. Yang, L. Liang, L. Carlone, and K.-C. Toh, "An inexact projected gradient method with rounding and lifting by nonlinear programming for solving rank-one semidefinite relaxation of polynomial optimization," *Mathematical Programming*, vol. 201, pp. 409–472, 2023.

[7] H. Yang and L. Carlone, "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, 2023.

[8] H. Yang, J. Shi, and L. Carlone, "TEASER: Fast and certifiable point cloud registration," *IEEE Transactions on Robotics*, vol. 32, no. 2, pp. 314–333, 2020.

[9] A. Eriksson, C. Olsson, F. Kahl, and T.-J. Chin, "Rotation averaging and strong duality," in *IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)*, 2018, pp. 127–135.

[10] J. Briales and J. Gonzalez-Jimenez, "Convex global 3d registration with Lagrangian duality," in *IEEE Conference on Computer Vision and Pattern Recognition (CVPR)*, 2017, pp. 5612–5621.

[11] S. Boyd and L. Vandenberghe, *Convex Optimization*. Cambridge University Press, 2004.

[12] J. B. Lasserre, "Global optimization with polynomials and the problem of moments," *SIAM Journal on Optimization*, vol. 11, no. 3, pp. 796–817, 2001.

[13] 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, 2011.

[14] A. Majumdar, R. Vasudevan, M. M. Tobenkin, and R. Tedrake, "Convex optimization of nonlinear feedback controllers via occupation measures," *International Journal of Robotics Research*, vol. 33, no. 9, pp. 1209–1230, 2014.

[15] L. Sun and Z. Deng, "Certifiably optimal and robust camera pose estimation from points and lines," *IEEE Access*, vol. 8, pp. 124032–124054, 2020.

[16] J. Wang, V. Magron, and J.-B. Lasserre, "TSSOS: A moment-SOS hierarchy that exploits term sparsity," *SIAM Journal on Optimization*, vol. 31, no. 1, pp. 30–58, 2021.

[17] J. Briales, L. Kneip, and J. Gonzalez-Jimenez, "A certifiably globally optimal solution to the non-minimal relative pose problem," in *IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)*, 2018, pp. 145–154.

[18] 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, 2023.

[19] D. Cifuentes and P. A. Parrilo, "Sampling algebraic varieties for sum of squares programs," *SIAM Journal on Optimization*, vol. 27, no. 4, pp. 2381–2404, 2017.

[20] C. Olsson, F. Kahl, and M. Oskarsson, "Branch-and-bound methods for Euclidean registration problems," *IEEE Transactions on Pattern Analysis and Machine Intelligence*, vol. 31, no. 5, pp. 783–794, 2009.

[21] R. Hartley, J. Trumpf, and D. H. Li, "Rotation averaging," *International Journal of Computer Vision*, vol. 103, no. 3, pp. 267–305, 2013.

[22] L. Brynte, V. Larsson, J. P. Iglesias, C. Olsson, and F. Kahl, "On the tightness of semidefinite relaxations for rotation estimation," *Journal of Mathematical Imaging and Vision*, vol. 64, no. 1, pp. 57–67, 2022.

[23] D. Cifuentes, "A convex relaxation to compute the nearest structured rank deficient matrix," *SIAM Journal on Matrix Analysis and Applications*, vol. 42, no. 2, pp. 708–729, 2021.

[24] K. Anstreicher and H. Wolkowicz, "On Lagrangian relaxation of quadratic matrix constraints," *SIAM Journal on Matrix Analysis and Applications*, vol. 22, no. 1, pp. 41–55, 2000.

[25] E. Wise, M. Giamou, S. Khoubyarian, A. Grover, and J. Kelly, "Certifiably optimal monocular hand-eye calibration," in *IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)*, 2020, pp. 271–278.

[26] J. Zhao, W. Xu, and L. Kneip, "A certifiably globally optimal solution to generalized essential matrix estimation," in *IEEE/CVF Conference on Computer Vision and Pattern Analysis (CVPR)*, 2020, pp. 12034–12043.

[27] T. Marcucci, J. Umenberger, P. A. Parrilo, and R. Tedrake, "Shortest paths in graphs of convex sets," *SIAM Journal on Optimization*, vol. 34, no. 1, pp. 507–532, 2024.

[28] B. P. Graesdal, S. Y. C. Chia, T. Marcucci, S. Morozov, A. Amice, P. A. Parrilo, and R. Tedrake, "Towards tight convex relaxations for contact-rich manipulation," in *Robotics: Science and Systems*, 2024.

[29] S. Burer and R. D. Monteiro, "Local minima and convergence in low-rank semidefinite programming," *Mathematical Programming*, vol. 103, no. 3, pp. 427–444, 2005.

[30] N. Boumal, "A Riemannian low-rank method for optimization over semidefinite matrices with block-diagonal constraints," *arXiv:1506.00575 [cs, math, stat]*, 2016.

[31] K. J. Doherty, D. M. Rosen, and J. J. Leonard, "Performance Guarantees for Spectral Initialization in Rotation Averaging and Pose-Graph SLAM," in *IEEE International Conference on Robotics and Automation*, 2022, pp. 5608–5614.

[32] F. Dellaert, D. M. Rosen, J. Wu, R. Mahony, and L. Carlone, "Shonan rotation averaging: Global optimality by surfing SO(p)," in *European Conference on Computer Vision*, 2020, pp. 292–308.

[33] C. Holmes and T. D. Barfoot, "An efficient global optimality certificate for landmark-based SLAM," *IEEE Robotics and Automation Letters*, vol. 8, no. 3, pp. 1539–1546, 2023.

[34] Á. Parra, S.-F. Chng, T.-J. Chin, A. Eriksson, and I. Reid, "Rotation coordinate descent for fast globally optimal rotation averaging," in *IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)*, 2021, pp. 4296–4305.

[35] B. Buchberger, "An algorithm for finding the basis elements of the residue class ring of a zero dimensional polynomial ideal," Ph.D. dissertation, Johannes Kepler University of Linz, 1965.

[36] S. Shen and R. Tedrake, "Sampling quotient-ring sum-of-squares programs for scalable verification of nonlinear systems," in *IEEE Conference on Decision and Control (CDC)*, 2020, pp. 2535–2542.

[37] C. Holmes, F. Dümbgen, and T. D. Barfoot, "On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics," *IEEE Transactions on Robotics* (to appear), 2024.
[38] A. Goudar, F. Dümbgen, T. D. Barfoot, and A. P. Schoellig, “Optimal initialization strategies for range-only trajectory estimation,” *IEEE Robotics and Automation Letters*, vol. 9, no. 3, pp. 2160–2167, 2024.

[39] T. D. Barfoot, C. Holmes, and F. Dümbgen, “Certifiably Optimal Rotation and Pose Estimation Based on the Cayley Map,” *International Journal of Robotics Research*, 2024.

[40] MOSEK. ApS. MOSEK optimization suite. Available: http://docs.mosek.com/10.0/toolbox/index.html. Accessed: Sept. 17, 2024.

[41] D. Cifuentes, S. Agarwal, P. A. Parrilo, and R. R. Thomas, “On the local stability of semidefinite relaxations,” *Mathematical Programming*, no. 193, pp. 629–663, 2022.

[42] A. Beck, P. Stoica, and J. Li, “Exact and approximate solutions of source localization problems,” *IEEE Transactions on Signal Processing*, vol. 56, no. 5, pp. 1770–1778, 2008.

[43] G. Blekherman, P. A. Parillo, and R. Thomas, *Semidefinite Optimization and Convex Algebraic Geometry*. MOS-SIAM Series on Optimization, 2012, vol. 13.

[44] T. F. Coleman and A. Pothen, “The null space problem i. complexity,” *SIAM Journal on Algebraic Discrete Methods*, vol. 7, no. 4, pp. 527–537, 1986.

[45] G. H. Golub and C. F. Van Loan, *Matrix Computations*, 4th ed. The John Hopkins University Press, 2003.

[46] M. W. Mueller, M. Hamer, and R. D'Andrea, “Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation,” in *IEEE International Conference on Robotics and Automation (ICRA)*, 2015, pp. 1730–1736.

[47] A. Goudar, W. Zhao, T. D. Barfoot, and A. P. Schoellig, “Gaussian variational inference with covariance constraints applied to range-only localization,” in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2022, pp. 2872–2879.

[48] F. Zafari, A. Gkelias, and K. K. Leung, “A survey of indoor localization systems and technologies,” *IEEE Communications Surveys & Tutorials*, vol. 21, no. 3, pp. 2568–2599, 2019.

[49] M. Larsson, V. Larsson, K. Astrom, and M. Oskarsson, “Optimal trilateration is an eigenvalue problem,” in *IEEE ICASSP*, 2019, pp. 5586–5590.

[50] M. Pacholska, F. Dümbgen, and A. Scholefield, “Relax and recover: Guaranteed range-only continuous localization,” *IEEE Robotics and Automation Letters*, vol. 5, no. 2, pp. 2248–2255, 2020.

[51] L. Matthies and S. Shafer, “Error modeling in stereo navigation,” *IEEE Journal on Robotics and Automation*, vol. 3, no. 3, pp. 239–248, 1987.

[52] G. Terzakis and M. Lourakis, “A consistently fast and globally optimal solution to the perspective-n-point problem,” in *European Conference on Computer Vision*, 2020, pp. 478–494.

[53] S. Diamond and S. Boyd, “CVXPY: A Python-embedded modeling language for convex optimization,” *Journal of Machine Learning Research*, vol. 17, pp. 1–5, 2016.

[54] A. Agrawal, R. Verschueren, S. Diamond, and S. Boyd, “A rewriting system for convex optimization problems,” *Journal of Control and Decision*, vol. 5, no. 1, pp. 42–60, 2018.

[55] J. Townsend, N. Koep, and S. Weichwald, “Pymanopt: A Python toolbox for optimization on manifolds using automatic differentiation,” *Journal of Machine Learning Research*, vol. 17, no. 137, pp. 1–5, 2016.

[56] C. Liu and N. Boumal, “Simple algorithms for optimization on Riemannian manifolds with constraints,” *Applied Mathematics & Optimization*, vol. 82, no. 3, pp. 949–981, 2020.

[57] T. D. Barfoot. State estimation for aerospace vehicles – AER1514 course assignments. Available: https://github.com/tuiasASRL/aer1513. Accessed: Sept. 17, 2024.

[58] F. Dümbgen, M. A. Shalaby, C. Holmes, C. C. Cossette, J. R. Forbes, J. L. Ny, and T. D. Barfoot, “STAR-loc: Dataset for STereo And Range-based localization,” *arXiv:2309.05518 [cs.RO]*, 2023.

[59] J. Wang and E. Olson, “Apriltag 2: Efficient and robust fiducial detection,” in *IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2016, pp. 4193–4198.

[60] M. J. Black and A. Rangarajan, “On the unification of line processes, outlier rejection, and robust statistics with applications in early vision,” *International Journal of Computer Vision*, vol. 19, no. 1, pp. 57–91, 1996.