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

HAL
open science

# Certifiably optimal rotation and pose estimation based on the Cayley map

Timothy D Barfoot, Connor Holmes, Frederike Dümbgen

► To cite this version:

Timothy D Barfoot, Connor Holmes, Frederike Dümbgen. Certifiably optimal rotation and pose estimation based on the Cayley map. The International Journal of Robotics Research, In press, 10.1177/02783649241269337. hal-04901876

HAL Id: hal-04901876

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

Submitted on 20 Jan 2025

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

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

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

Distributed under a Creative Commons Attribution 4.0 International License
# Certifiably Optimal Rotation and Pose Estimation Based on the Cayley Map

Timothy D. Barfoot<fnref n="1" />, Connor Holmes<fnref n="1" />, and Frederike Dümbgen<fnref n="1" />

## Abstract

We present novel, convex relaxations for rotation and pose estimation problems that can a posteriori guarantee global optimality for practical measurement noise levels. Some such relaxations exist in the literature for specific problem setups that assume the matrix von Mises-Fisher distribution (a.k.a., matrix Langevin distribution or chordal distance) for isotropic rotational uncertainty. However, another common way to represent uncertainty for rotations and poses is to define anisotropic noise in the associated Lie algebra. Starting from a noise model based on the Cayley map, we define our estimation problems, convert them to Quadratically Constrained Quadratic Programs (QCQPs), then relax them to Semidefinite Programs (SDPs), which can be solved using standard interior-point optimization methods; global optimality follows from Lagrangian strong duality. We first show how to carry out basic rotation and pose averaging. We then turn to the more complex problem of trajectory estimation, which involves many pose variables with both individual and inter-pose measurements (or motion priors). Our contribution is to formulate SDP relaxations for all these problems based on the Cayley map (including the identification of redundant constraints) and to show them working in practical settings. We hope our results can add to the catalogue of useful estimation problems whose solutions can be a posteriori guaranteed to be globally optimal.

## Keywords

Rotation Estimation; Pose Estimation; Quadratically Constrained Quadratic Program; Semi-Definite Program; Lagrangian Duality; Cayley map

## 1 Introduction

State estimation is concerned with fusing several noisy measurements (and possibly a prior model) into a less noisy estimate of the state (e.g., position, velocity, orientation) of a vehicle, robot, or other object of interest. Real-world state estimation problems often involve measurement functions and motion models that are nonlinear with respect to the state. Alternatively, the state itself may not be an element of a vector space, such as the rotation of a rigid body. These challenging aspects typically mean that when we set up our estimator as an optimization, it is a nonconvex problem; the cost function, the feasible set, or both are not convex. Nonconvex optimization problems are in general much harder to solve than convex ones because they can have local minima and common gradient-based optimizers can easily become trapped therein.

For example, we might have a generic nonlinear least-squares problem such as

$$ \min_{x} \sum_{m=1}^{M} (y_m - g(x))^T (y_m - g(x)), \quad (1) $$

where $x$ is the unknown state, $y_m$ are noisy measurements, and $g(\cdot)$ is a measurement function. If $g(x)$ is linear in $x$, then this problem is convex, but otherwise it often is not. Using gradient descent or Gauss-Newton to solve this problem means we usually require a good initial guess for $x$ to arrive at the global minimum. What if such an initial guess is not available? Could we solve a problem such as this one globally without such a guess? It turns out the answer may be yes, depending on the specific problem to be solved.

There has been quite a bit of work in robotics and computer vision aimed at the idea of solving estimation problems globally. Most of these works employ sophisticated tools from the optimization literature to achieve this. In particular, *Lagrangian duality* is used to derive convex relaxations, which can be solved globally.

<footnote n="1">¹Robotics Institute, University of Toronto</footnote>

<footnote>**Corresponding author:**</footnote>

<footnote>Timothy D. Barfoot, Institute for Aerospace Studies, University of Toronto, Toronto, Ontario, M3H 5T6, Canada.</footnote>

<footnote>Email: tim.barfoot@utoronto.ca</footnote>
Boyd and Vandenberghe (2004, §5) provide the necessary background on duality theory. We will be following a common pathway where we first convert our nonconvex optimization problem into another nonconvex form called a Quadratically Constrained Quadratic Program (QCQP); from here we relax this to a (convex) Semidefinite Program (SDP), amenable to off-the-shelf solvers (e.g., the interior-point-based SDP solver in mosek (ApS 2019)). This last step is known as *Shor's relaxation* (Shor 1987). Theoretically, SDPs admit polynomial-time solutions because they are convex; in practice, modern SDP problems with a few thousand variables can be solved in reasonable time but struggle to scale up beyond this. Our contribution in this paper is to show that we can solve a set of estimation problems involving rotations and poses globally, using these convex relaxation tools; the novelty lies in the fact that our particular problems (formulated using the Cayley map) have not been examined in this globally optimal framework before.

As mentioned, the convex relaxation procedure we employ has been well known in the optimization community for some time. In particular, it has been used for polynomial optimization (Parrilo 2003) and in various combinatorial optimization problems, such as quadratic assignment (Nesterov et al. 2000) and max-cut graph partitioning (Anjos and Wolkowicz 2002). To the authors' knowledge, the first use of SDP relaxations in the robotics community was by Liu et al. (2012) for planar Simultaneous Localization and Mapping (SLAM), though their application in computer vision (Kahl and Henrion 2005) and signal processing (Luo et al. 2010; Krislock and Wolkowicz 2010) occurred earlier. More generally, Cifuentes et al. (2022) provides a nice overview of some common problems in computer vision and robotics where these tools have been applied before, as well as providing a rationale for why they are so effective. One of the most commonly investigated relaxations in the robotics and vision communities is rotation synchronization* (Bandeira et al. 2017; Eriksson et al. 2018, 2019; Dellaert et al. 2020); here several rotations are linked through noisy relative rotation measurements. Rotation synchronization turns out to be the nucleus of several of the other problems under study including pose-graph optimization (Briales and Gonzalez-Jimenez 2017a; Carlone and Calafiore 2018; Rosen et al. 2019; Tian et al. 2021; Carlone et al. 2016), point-set registration (Chaudhury et al. 2015; Iglesias et al. 2020; Yang et al. 2020), calibration (Giamou et al. 2019; Wise et al. 2020), mutual localization (Wang et al. 2022b), and landmark-based SLAM (Holmes and Barfoot 2023). Rotation synchronization and its cousins have been shown to admit fast solutions through low-rank factorizations (Rosen et al. 2019). More recently, other measurement models such as range sensing have also been incorporated

into globally certifiable problems (Papalia et al. 2022; Dümbgen et al. 2022).

It is worth also mentioning the works of (Horowitz et al. 2014; Forbes and de Ruiter 2015) on globally optimal pointcloud alignment, which also employ convex relaxation, but the route to the SDP is not Shor's relaxation; instead, Lie group optimization variables are relaxed to live in a convex set. On the surface, this approach is not applicable to the problems considered herein; for example, our cost functions are not always initially convex.

A common thread that ties most of the existing literature together is that the chordal distance is used to construct the terms of the cost function that involve rotation variables. Viewed through a probabilistic lens, the chordal distance is related to the matrix Langevin or matrix von Mises-Fisher distribution, whose density can be written in the form

$$p(\mathbf{C}) \propto \exp \left( -\frac{1}{2}\sigma^{-1} \operatorname{tr} \left( (\mathbf{C} - \bar{\mathbf{C}})(\mathbf{C} - \bar{\mathbf{C}})^T \right) \right), \quad (2)$$

where C is a rotation matrix, $\bar{\mathbf{C}}$ is the mode, and $\sigma > 0$ is a scalar concentration parameter; this distribution is isotropic, which is one limitation we aim to overcome in this work. This is not the only way to represent rotational uncertainty. Another common way is to use exponential coordinates (e.g., (Long et al. 2013; Barfoot and Furgale 2014)), where a rotational distribution can have a density of the form

$$p(\mathbf{C}) \propto \exp \left( -\frac{1}{2} \ln(\mathbf{C}\bar{\mathbf{C}}^T)^{\vee T} \mathbf{\Sigma}^{-1} \ln(\mathbf{C}\bar{\mathbf{C}}^T)^{\vee} \right), \quad (3)$$

where C is a member of the matrix Lie group $SO(3)$, $\bar{\mathbf{C}}$ is also a member of $SO(3)$ and represents the mean, and $\mathbf{\Sigma}$ is an anisotropic matrix covariance. We also have $\exp(\cdot)$ as the matrix exponential, $\ln(\cdot)$ the matrix logarithm, and $\vee$ a Lie algebra operator detailed a bit later in the paper. Here we are essentially defining a Gaussian distribution in the vector space of the Lie algebra associated with $SO(3)$ and then mapping the uncertainty to the Lie group through the matrix exponential. This allows for anisotropic distributions and the same approach can be easily extended to any matrix Lie group, such as the special Euclidean group $SE(3)$ that represents poses (see, for example, (Long et al. 2013; Barfoot and Furgale 2014)). Our aim in this paper is to present some novel convex relaxations where rotational uncertainty is defined closer to this exponential coordinate model; to achieve this, we use the Cayley map, which is very close to the exponential map for small-to-moderate rotational uncertainty. Our Cayley distributions will have

<footnote>*Some authors instead use the term 'rotation averaging', whereas we use rotation averaging to mean the process of fusing several noisy measurements of a single rotation into an estimate.</footnote>
the form

$$p(\mathbf{C}) \propto \operatorname{cay}\left(-\frac{1}{2}\operatorname{cay}^{-1}(\mathbf{C}\bar{\mathbf{C}}^{T})^{\vee T} \mathbf{\Sigma}^{-1}\operatorname{cay}^{-1}(\mathbf{C}\bar{\mathbf{C}}^{T})^{\vee}\right), \quad (4)$$

where $\operatorname{cay}(\cdot)$ is the Cayley map. This also allows us to define our optimization problems directly on $SE(3)$ rather than $SO(3) \times \mathbb{R}^3$ when poses are involved.

To our knowledge, the examination of global optimality for state estimation problems where rotational (and pose) uncertainty is defined in this way has not been explored previously in the literature. Our novel contribution is therefore a family of specific convex relaxations of rotation and pose estimation problems formulated using the Cayley map (including redundant constraints needed to make them work in practice); this is important as it opens the door to providing certification for a broad class of state estimation problems used in practice.

This paper is organized as follows. In Section 2 we review the relevant mathematical background including Lie groups, the Cayley map, and the convex relaxation procedure that we will employ. Section 3 presents the basic problems of averaging a number of noisy rotation or pose measurements. In Section 4, we expand the method to include discrete-time trajectory estimation of several poses based on individual and inter-pose measurements. Section 5 expands this to include so-called continuous-time trajectory estimation where we have a smoothing assumption on the trajectory and estimate both pose and twist at each state. In each of Sections 3 to 5, we provide experimental results that demonstrate the viability of our convex relaxations to find globally optimal solutions. Section 6 concludes the paper. Appendix A discusses the similarity between distributions defined using the exponential and Cayley maps while Appendix B presents the baseline local solvers to which we compare our new global estimates.

# 2 Mathematical Background

We begin by reviewing the relevant background concepts for the paper including Lie groups, the Cayley map, and convex relaxations of nonconvex optimization problems via Lagrangian duality.

## 2.1 Lie Groups for Rotations and Poses

The special orthogonal group, representing rotations, is the set of valid rotation matrices:

$$SO(3) = \{ \mathbf{C} \in \mathbb{R}^{3 \times 3} \mid \mathbf{C}\mathbf{C}^{T} = \mathbf{I}, \det(\mathbf{C}) = 1 \}, \quad (5)$$

where $\mathbf{I}$ is the identity matrix. It is common to map a vector, $\phi \in \mathbb{R}^3$, to a rotation matrix, $\mathbf{C}$, through the matrix exponential,

$$\mathbf{C}(\phi) = \exp(\phi^{\wedge}), \quad (6)$$

where $(\cdot)^{\wedge}$ is the skew-symmetric operator,

$$\phi^{\wedge} = \begin{bmatrix} \phi_1 \\ \phi_2 \\ \phi_3 \end{bmatrix}^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}, \quad (7)$$

and $\phi = \varphi\mathbf{a} \in \mathbb{R}^3$, the product of the angle and unit axis of rotation. The mapping is surjective-only, meaning every $\mathbf{C}$ can be produced by many different values for $\phi$.

The special Euclidean group, representing poses (i.e., translation and rotation), is the set of valid transformation matrices:

$$SE(3) = \left\{ \mathbf{T} = \begin{bmatrix} \mathbf{C} & \mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \mathbf{C} \in SO(3), \mathbf{r} \in \mathbb{R}^3 \right\}. \quad (8)$$

It is again common to map a vector, $\boldsymbol{\xi} \in \mathbb{R}^6$, to a transformation matrix, $\mathbf{T} \in SE(3)$, through the matrix exponential,

$$\mathbf{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^{\wedge}), \quad (9)$$

where we have overloaded the $\wedge$ operator as

$$\boldsymbol{\xi}^{\wedge} = \begin{bmatrix} \boldsymbol{\rho} \\ \phi \end{bmatrix}^{\wedge} = \begin{bmatrix} \phi^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{bmatrix}. \quad (10)$$

Notationally, we will use $\vee$ to mean the inverse operation of $\wedge$. As is common practice (Barfoot 2024), we have broken the pose vector, $\boldsymbol{\xi}$, into a translational component, $\boldsymbol{\rho}$, and a rotational component, $\phi$. The mapping is also surjective-only, meaning every $\mathbf{T}$ can be produced by many different values for $\boldsymbol{\xi}$.

Finally, the adjoint of a pose is given by

$$\mathcal{T}(\boldsymbol{\xi}) = \operatorname{Ad}(\mathbf{T}) = \begin{bmatrix} \mathbf{C} & \mathbf{r}^{\wedge} \mathbf{C} \\ \mathbf{0} & \mathbf{C} \end{bmatrix}, \quad (11)$$

which is now $6 \times 6$. We will refer to the set of adjoints as $\operatorname{Ad}(SE(3))$. We can map a vector, $\boldsymbol{\xi} \in \mathbb{R}^6$, to an adjoint transformation matrix again through the matrix exponential map:

$$\mathcal{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^{\wedge}), \quad (12)$$

where

$$\boldsymbol{\xi}^{\wedge} = \begin{bmatrix} \boldsymbol{\rho} \\ \phi \end{bmatrix}^{\wedge} = \begin{bmatrix} \phi^{\wedge} & \boldsymbol{\rho}^{\wedge} \\ \mathbf{0} & \phi^{\wedge} \end{bmatrix}. \quad (13)$$

Notationally, we will use $\Upsilon$ to mean the inverse operation of $\wedge$. The mapping is again surjective-only, meaning every $\mathcal{T}$ can be produced by many different values for $\boldsymbol{\xi}$.

## 2.2 Cayley Map

While the exponential map is the canonical way to map from a Lie algebra (vector space) to a Lie group, it is not the only possibility. There are in fact infinitely many such vectorial mappings for $SO(3)$ (Bauchau and Trainelli
<figure><img src="image_3.png" /><figcaption>**Figure 1.** Comparison of uncertainty on rotation angle, $\phi$, for the exponential and Cayley maps, where the variances have been approximately matched (see Appendix A for further discussion of how this was done). (left) Standard deviation of rotational uncertainty is $\sigma = 0.2$ [rad]. (right) $\sigma = 0.5$ [rad]. The match is good in both cases with more divergence as rotational uncertainty increases.</figcaption></figure>

2003), SE(3) (Barfoot et al. 2022), and Ad(SE(3)) (Bauchau and Choi 2003; Bauchau 2011).

In particular, it is well known that for the Cayley-Gibbs-Rodrigues parameterization of rotation we can write the rotation matrix in terms of the Cayley map, $cay(\mathbf{A}) = (\mathbf{I} - \frac{1}{2}\mathbf{A})^{-1}(\mathbf{I} + \frac{1}{2}\mathbf{A})$, according to (Borri et al. 2000; Bauchau and Trainelli 2003):

$$C(\phi) = cay(\phi^{\wedge}) = \left(\mathbf{I} - \frac{1}{2}\phi^{\wedge}\right)^{-1} \left(\mathbf{I} + \frac{1}{2}\phi^{\wedge}\right), \quad (14a)$$

$$\phi = cay^{-1}(C)^{\vee} = (2(C - \mathbf{I})(C + \mathbf{I})^{-1})^{\vee}, (14b)$$

for some $\phi = 2 \tan \frac{\phi}{2} \mathbf{a} \in \mathbb{R}^3$ with $\phi$ the rotation angle and $\mathbf{a}$ the unit axis. Borri et al. (2000) and later Selig (2007) demonstrated that the Cayley map can also be used to map pose vectors to SE(3) according to

$$\mathbf{T}(\xi) = cay(\xi^{\wedge}) = \left(\mathbf{I} - \frac{1}{2}\xi^{\wedge}\right)^{-1} \left(\mathbf{I} + \frac{1}{2}\xi^{\wedge}\right), \quad (15a)$$

$$\xi = cay^{-1}(\mathbf{T})^{\vee} = (2(\mathbf{T} - \mathbf{I})(\mathbf{T} + \mathbf{I})^{-1})^{\vee}, (15b)$$

for some $\xi \in \mathbb{R}^6$. Although we will not need it, the Cayley map can be used to map pose vectors to Ad(SE(3)) according to

$$\mathcal{T}(\xi) = cay(\xi^{\wedge}) = \left(\mathbf{I} - \frac{1}{2}\xi^{\wedge}\right)^{-1} \left(\mathbf{I} + \frac{1}{2}\xi^{\wedge}\right), \quad (16a)$$

$$\xi = cay^{-1}(\mathcal{T})^{\vee} = (2(\mathcal{T} - \mathbf{I})(\mathcal{T} + \mathbf{I})^{-1})^{\vee}. (16b)$$

However, Selig (2007) demonstrates that starting from the same $\xi$ and applying (15) and (16) does not result in an equivalent transformation, i.e., $\mathcal{T}(\xi) \neq \text{Ad}(\mathbf{T}(\xi))$; the commutative property for adjoints does not hold. Nevertheless, we shall not require this property here.

Figure 1 provides examples comparing two rotational distributions derived from the exponential and Cayley maps

that have approximately the same variance; we can see that even with quite large rotational uncertainty they match quite closely. Appendix A provides some further discussion on how closely these distributions can be made to match. The key idea of the paper will be to replace instances of the exponential map with the Cayley map, which we will see is more amenable to producing polynomial optimization problems, a key prerequisite to our route to global optimality.

The Cayley map has been used in the past for rotation, pose, and trajectory estimation (Mortari et al. 2007; Majji et al. 2011; Junkins et al. 2011; Alismail et al. 2014; Wong and Majji 2016; Wong et al. 2018; Qian et al. 2020; Barfoot et al. 2022), typically to parameterize rotations or poses thereby creating a simpler unconstrained quadratic optimization problem. The drawback of these approaches is that they are still subject to singularities and local minima. The Cayley map has also been used to achieve global optimality in the perspective-$n$-point (PnP) problem (Nakano 2015; Wang et al. 2022a); we take a quite different approach, however, through the use of convex relaxations. Additionally, the Cayley map has found application in parametrizing lines in structure from motion, as an unconstrained alternative to parametrizations such as Plücker coordinates (Zhang and Koch 2014).

The Cayley map has also been employed in areas of robotics other than estimation. In (Kobilarov and Marsden 2011; Kobilarov 2014), for example, the authors suggest using the Cayley map for rotation parametrization in the context of optimal control of mechanical systems on Lie groups. The authors observe that, compared with the exponential map, the Cayley map is computationally more efficient because of its simpler structure, in particular as it circumvents trigonometric functions. It is also noted that the Cayley map has no singularities in its gradients, which is of advantage for the numerical stability of commonly used solvers (Kobilarov and Marsden 2011). In (Solo and Wang 2019), the Cayley map is employed for simulating stochastic differential equations that evolve on Stiefel
manifolds, which is subsequently used in (Wang and Solo 2020) for a novel particle filter variant. It is possible that our global optimality approach to using the Cayley map could be employed within some of these applications, but we leave this investigation for future work.

## 2.3 Convex Relaxations

We next summarize the key optimization tools that we will use. Boyd and Vandenberghe (2004) provide the appropriate background. Suppose that we have a nonconvex optimization problem of the form

$$ 
\begin{array}{ll}
\min & f(\mathbf{z}) \\
\text{w.r.t.} & \mathbf{z} \\
\text{s.t.} & g_i(\mathbf{z}) = 0 \quad (\forall i)
\end{array} 
\quad (17) 
$$

We attempt to introduce appropriate nonlinear substitution variables, $\mathbf{x}$, to replace $\mathbf{z}$ so that both the objective and the constraints can be written in a standard, homogeneous, quadratic form:

$$ 
\begin{array}{ll}
\min & \mathbf{x}^T \mathbf{Q} \mathbf{x} \\
\text{w.r.t.} & \mathbf{x} \\
\text{s.t.} & \mathbf{x}^T \mathbf{A}_0 \mathbf{x} = 1 \\
& \mathbf{x}^T \mathbf{A}_i \mathbf{x} = 0 \quad (\forall i \neq 0)
\end{array} 
\quad (18) 
$$

This problem is a Quadratically Constrained Quadratic Program (QCQP), which is still nonconvex and typically of higher dimension than the original problem, but possesses more exploitable structure. Next, by defining $\mathbf{X} = \mathbf{x}\mathbf{x}^T$, we rewrite this problem exactly as

$$ 
\begin{array}{ll}
\min & \operatorname{tr}(\mathbf{Q}\mathbf{X}) \\
\text{w.r.t.} & \mathbf{X} \\
\text{s.t.} & \mathbf{X} \succeq 0 \\
& \operatorname{rank}(\mathbf{X}) = 1 \\
& \operatorname{tr}(\mathbf{A}_0\mathbf{X}) = 1 \\
& \operatorname{tr}(\mathbf{A}_i\mathbf{X}) = 0 \quad (\forall i \neq 0)
\end{array} 
\quad (19) 
$$

Finally, if we drop the $\operatorname{rank}(\mathbf{X}) = 1$ constraint, we have a convex relaxation of the problem in the form of a Semidefinite Program (SDP):

$$ 
\begin{array}{ll}
\min & \operatorname{tr}(\mathbf{Q}\mathbf{X}) \\
\text{w.r.t.} & \mathbf{X} \\
\text{s.t.} & \mathbf{X} \succeq 0 \\
& \operatorname{tr}(\mathbf{A}_0\mathbf{X}) = 1 \\
& \operatorname{tr}(\mathbf{A}_i\mathbf{X}) = 0 \quad (\forall i \neq 0)
\end{array} 
\quad (20) 
$$

This is known as *Shor's relaxation* (Shor 1987). If the solution to this problem happens to result in $\operatorname{rank}(\mathbf{X}) = 1$, then we have an a posteriori† guarantee that we also have a global solution to the original problem, $\mathbf{x}$. Since SDPs are convex problems, we can attempt to use standard solvers, such as interior-point methods, to solve them numerically.

While there are tractability issues to be addressed to scale up to very large problem instances, we will see that for the problems in this paper, this approach is viable for nontrivial problem sizes.

Unfortunately, for most problems in this paper, Shor's relaxation is not tight out of the box. In this paper, tightness means $\operatorname{rank}(\mathbf{X}) = 1$ (and therefore that the optimal cost matches that of the original problem). However, there is still a way forward. We can attempt to introduce additional so-called redundant constraints to tighten up the relaxation. These constraints do not affect the feasible set of the original optimization problem, but they do reduce the feasible set of the SDP in order to tighten it.

The technique of adding redundant constraints to improve the tightness of a given SDP relaxation has been known for some time in the optimization literature (Anstreicher and Wolkowicz 2000; Nesterov et al. 2000). More recently, there have been several cases in which redundant constraints have been used in the robotics (Yang et al. 2020; Yang and Carlone 2022; Giamou et al. 2019; Wang et al. 2022b) and machine vision (Briales and Gonzalez-Jimenez 2017b; Briales et al. 2018; Garcia-Salguero et al. 2022; Kezurer et al. 2015) literature. With redundant constraints, our problem becomes

$$ 
\begin{array}{ll}
\min & \operatorname{tr}(\mathbf{Q}\mathbf{X}) \\
\text{w.r.t.} & \mathbf{X} \\
\text{s.t.} & \mathbf{X} \succeq 0 \\
& \operatorname{tr}(\mathbf{A}_0\mathbf{X}) = 1 \\
& \operatorname{tr}(\mathbf{A}_i\mathbf{X}) = 0 \quad (\forall i \neq 0) \\
& \operatorname{(redundant)} \operatorname{tr}(\mathbf{B}_j\mathbf{X}) = 0 \quad (\forall j \neq 0)
\end{array} 
\quad (21) 
$$

where the $\mathbf{B}_j$ encapsulate these additional redundant constraints. In theory, Lasserre (2001) tells us how to tighten our SDP, if possible, by adding a progression of variables and constraints, but adding too many constraints can be computationally expensive and in practice not necessary for tightness.§ On the other hand, devising a sufficient set of constraints can be challenging by trial and error. In our concurrent work, we have been developing a tool to automatically find such constraints (Duembgen et al. 2023), which we used to help identify some of the constraints reported in this paper. In all of the pose estimation problems to follow, we do require redundant constraints and we will be explicit in enumerating ones that

<footnote>†While $\operatorname{rank}(\mathbf{X}) = 1$ provides an a posteriori guarantee that our solution is globally optimal, we cannot a priori guarantee that we will get a rank-one solution for a given set of measurements. Measurement noise, for example, can have a big impact on tightness and so we will later use empirical studies to a priori gauge how likely we are to find rank-one solutions at different measurement noise levels.</footnote>

<footnote>§The interested reader is directed to (Henrion et al. 2020) and (Lasserre 2010) for a more in depth treatment on the moment/sum-of-squares hierarchy.</footnote>
<figure><img src="image_4.png" /><figcaption>**Figure 2. Rotation Averaging:** A quantitative evaluation of the tightness of the rotation averaging problem with increasing measurement noise level, $\sigma$. At each noise level, we conducted 1000 trials of averaging 10 noisy rotations. (left) We see that the local solver (randomly initialized) finds the global minimum with decreasing frequency (green) as the measurement noise is increased, while the SDP solver (blue) successfully produces rank-1 solutions (we consider log Singular Value Ratio (SVR) of at least 5 to be rank 1) to much higher noise levels. For completeness, we also show how frequently the local solver converges to any minimum (red). (right) Boxplots of the log SVR of the SDP solution show that the global solution remains highly rank 1 over a wide range of measurement noise values.</figcaption></figure>

in practice result in tight SDP relaxations of our problems. This could be viewed as the core contribution of the paper.

## 3 Averaging

We will build up our optimization problems gradually starting with simply ‘averaging’ several noisy estimates of rotation or pose.

<figure><img src="image_5.png" /><figcaption>**Figure 3. Rotation Averaging:** An example of noisy rotation averaging where the randomly initialized local solver (dotted) becomes trapped in a poor local minimum while the global solver (dashed) finds the correct global solution, which is closer to the groundtruth rotation (solid).</figcaption></figure>

### 3.1 Rotation Averaging

In order to average $M$ rotations, we could set up an optimization problem as

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \ln (C \tilde{C}_{m}^{T})^{\vee T} W_{m} \ln (C \tilde{C}_{m}^{T})^{\vee} \\ \text{w.r.t.} & C \\ \text{s.t.} & C \in SO(3) \end{array} \quad (22) $$

where $\tilde{C}_m \in SO(3)$ are the noisy rotations to be averaged and $W_m$ is a matrix weight. This type of cost function is used frequently in rotational estimation problems (Barfoot 2024) and can represent a maximum-likelihood problem when the generative model for the noisy measurements is of the form

$$ \tilde{C}_m = \exp(\phi_m^\wedge) C, \quad \phi_m \sim N(0, W_m^{-1}). \quad (23) $$

Alternatively, we can view our cost as the negative log-likelihood of the joint distribution of the measurements if each obeys (3). The trouble is that the matrix exponential and logarithm are difficult expressions to manipulate into the QCQP form we seek.

This is where the main insight of the paper comes in. We can substitute the Cayley map for the exponential

<footnote>§On each box, the central mark is the median, the edges of the box are the 25th and 75th percentiles, the whiskers extend to the most extreme datapoints the algorithm considers to be not outliers, and the outliers are plotted individually (as red + symbols).</footnote>
<figure><img src="image_6.png" /><figcaption>**Figure 4.** Pose Averaging: Four examples of noisy pose averaging where the randomly initialized local solver (dotted) becomes trapped in a poor local minimum while the global solver (dashed) finds the correct solution, which is closer to the groundtruth pose (solid). The noisy pose measurements being averaged are shown in grey.</figcaption></figure>

map without too much effect on the stated problem (see Figure 1). With this substitution, our generative model for noisy rotations becomes

$$ \tilde{C}_m = \operatorname{cay}(\phi_m^{\wedge}) C, \quad \phi_m \sim \mathcal{N}(0, W_m^{-1}), \quad (24) $$

and so our optimization problem can be restated as

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \operatorname{cay}^{-1} (C \tilde{C}_m^T)^{\vee T} W_m \operatorname{cay}^{-1} (C \tilde{C}_m^T)^{\vee} \\ \text{w.r.t.} & C \\ \text{s.t.} & C \in \mathcal{SO}(3) \end{array} \quad (25) $$

Now our cost represents the negative log-likelihood of the joint distribution of the measurements, assuming each obeys (4). Turning this into a QCQP is then fairly easy:

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \phi_m^T W_m \phi_m \\ \text{w.r.t.} & c_1, c_2, c_3, \phi_m \quad (\forall m) \\ \text{s.t.} & c_i^T c_j = \delta_{ij} \quad (\forall i, j) \\ & (\mathbf{I} - \frac{1}{2}\phi_m^\wedge) c_i = (\mathbf{I} + \frac{1}{2}\phi_m^\wedge) \tilde{c}_{m,i} \quad (\forall i, m) \end{array} \quad (26) $$

where $\delta_{ij}$ is the Kronecker delta and

$$ C = [c_1 \ c_2 \ c_3], \quad \tilde{C}_m = [\tilde{c}_{m,1} \ \tilde{c}_{m,2} \ \tilde{c}_{m,3}]. \quad (27) $$

We have essentially introduced variables, $\phi_m$, for the residual errors of each term in the cost and used these to connect $C$ to each $\tilde{C}_m$ through the Cayley map; by bringing the inverse factor of the Cayley map to the other side, this becomes a quadratic constraint. Thus we have both a quadratic cost and quadratic constraints and hence a QCQP. The dimension of the problem is now higher since we must now optimize over $C$ and all the $\phi_m$; however, we can follow the approach of Section 2.3 to produce a SDP relaxation of the problem. Note, we have quietly dropped the $\det(C) = 1$ constraint on the rotation and will simply check it at the end$^\dagger$; our optimization then only guarantees $C \in O(3)$ not $SO(3)$. We leave the details of manipulating (26) into the standard form of (20)

to the reader. We did not find any redundant constraints were necessary to tighten this relaxation; the SDP solution produced remains rank 1 in practice for reasonably high amounts of noise.

The details of a baseline local solver can be found in the appendix. For the global (SDP) solver we used `cvx` in Matlab with `mosek` (ApS 2019). The solution costs of the global and local solvers agree to high precision$^\dagger$ if a good initial guess is given to the local solver. Figure 3 provides an example where the local solver converges from a poor initial guess to a local minimum, while the global solver finds the optimal solution near the groundtruth. Figure 2 provides a quantitative study of the tightness of the SDP solution with increasing measurement noise; we selected the measurement covariance as $W_m^{-1} = \sigma^2 \mathbf{I}$, with $\sigma$ increasing. To gauge numerically whether the SDP solution, $X$, is rank 1, we define the logarithmic Singular Value Ratio (SVR) as the base-10 logarithm of the ratio of the largest to second-largest singular values of $X$; we consider a log SVR of at least 5 to represent rank 1. We see there is a large range for the noise over which the local solver can become trapped in a local minimum while the global solver remains rank 1. With $M = 10$ rotations to be averaged, the local solver took on average 0.0012s while the SDP solver took on average 0.3486s.

## 3.2 Pose Averaging

Pose averaging follows a very similar approach to the previous section. An optimization problem based on the

<footnote>$^\dagger$This is also a relaxation of the problem, but one that has been shown to often be tight in practice (Rosen et al. 2019).</footnote>

<footnote>$^\ddagger$For this and all subsequent experiments, the local solver was verified to be converged in each case to a step size change of less than $10^{-6}$ while the global solver reported its own solution quality and used the `cvx_precision` high setting (see the cvx documentation).</footnote>
<figure><img src="image_7.png" /><figcaption>**Figure 5.** Pose Averaging: A quantitative evaluation of the tightness of the pose averaging problem with increasing measurement noise level, $\sigma$. At each noise, we conducted 1000 trials of averaging 10 noisy poses. (left) We see that the local solver (randomly initialized) finds the global minimum with decreasing frequency (green) as the measurement noise is increased, while the SDP solver (blue) successfully produces rank-1 solutions (we consider log SVR of at least 5 to be rank 1) to much higher noise levels. For completeness, we also show how frequently the local solver converges to any minimum (red). (right) Boxplots of the log SVR of the SDP solution show that the global solution remains highly rank 1 over a wide range of measurement noise values.</figcaption></figure>

Cayley map can be stated as

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \operatorname{cay}^{-1} (\mathbf{T} \tilde{\mathbf{T}}_{m}^{-1})^{\vee T} \mathbf{W}_{m} \operatorname{cay}^{-1} (\mathbf{T} \tilde{\mathbf{T}}_{m}^{-1})^{\vee} \\ \text { w.r.t. } & \mathbf{T} \\ \text { s.t. } & \mathbf{T} \in \mathbf{SE}(3) \end{array} \qquad (28) $$

where $\tilde{\mathbf{T}}_m$ are noisy pose measurements with matrix weights, $\mathbf{W}_m$.

We convert the residual pose errors, $\xi_m = \operatorname{cay}^{-1}(\mathbf{T}\tilde{\mathbf{T}}_m^{-1})^{\vee}$, to variables and now our optimization can be stated as

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \xi_m^T \mathbf{W}_m \xi_m \\ \text { w.r.t. } & \mathbf{T}, \xi_m \quad (\forall m) \\ \text { s.t. } & \mathbf{C}^T \mathbf{C} = \mathbf{I} \\ & (\mathbf{I} - \frac{1}{2} \xi_m^\wedge) \mathbf{T} = (\mathbf{I} + \frac{1}{2} \xi_m^\wedge) \tilde{\mathbf{T}}_m \quad (\forall m) \end{array} \qquad (29) $$

where we have again dropped the det($\mathbf{C}$) = 1 constraint. Since the bottom row of a transformation matrix is constant, we can parameterize it in the following way

$$ \begin{align} \mathbf{T} &= \begin{bmatrix} \mathbf{C} & \mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{c}_1 & \mathbf{c}_2 & \mathbf{c}_3 & \mathbf{r} \\ 0 & 0 & 0 & 1 \end{bmatrix}, \tag{30a} \\ \tilde{\mathbf{T}}_m &= \begin{bmatrix} \tilde{\mathbf{c}}_m & \mathbf{r}_m \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} \tilde{\mathbf{c}}_{m,1} & \tilde{\mathbf{c}}_{m,2} & \tilde{\mathbf{c}}_{m,2} & \tilde{\mathbf{r}}_m \\ 0 & 0 & 0 & 1 \end{bmatrix}, \tag{30b} \end{align} $$

$$ \xi_m = \begin{bmatrix} \rho_m \\ \phi_m \end{bmatrix}, \qquad (30c) $$

and then rewrite the optimization problem using the reduced set of variables as

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \xi_m^T \mathbf{W}_m \xi_m \\ \text { w.r.t. } & \mathbf{c}_i, \mathbf{r}, \rho_m, \phi_m \quad (\forall i, m) \\ \text { s.t. } & \mathbf{c}_i^T \mathbf{c}_j = \delta_{ij} \quad (\forall i, j) \\ & (\mathbf{I} - \frac{1}{2} \phi_m^\wedge) \mathbf{c}_i = (\mathbf{I} + \frac{1}{2} \phi_m^\wedge) \tilde{\mathbf{c}}_{m,i} \quad (\forall i, m) \\ & (\mathbf{I} - \frac{1}{2} \phi_m^\wedge) \mathbf{r} = (\mathbf{I} + \frac{1}{2} \phi_m^\wedge) \tilde{\mathbf{r}}_m + \rho_m \quad (\forall m) \end{array} \qquad (31) $$

This is now a QCQP, but unfortunately when we relax to a SDP, it is not always tight even for low noise levels. We found that introducing specific redundant constraints for each $m$ tightens the problem nicely for practical noise levels. One such useful constraint can be found by premultiplying the last constraint in (31) by $\mathbf{r}^T$ whereupon

$$ \mathbf{r}^T \mathbf{r} - \frac{1}{2} \underbrace{\mathbf{r}^T \phi_m^\wedge}_{0} \mathbf{r} = \mathbf{r}^T \tilde{\mathbf{r}}_m - \frac{1}{2} \mathbf{r}^T \tilde{\mathbf{r}}_m^\wedge \phi_m + \mathbf{r}^T \rho_m. \qquad (32) $$

The key is that the second cubic term vanishes, leaving a new quadratic constraint that it is not simply a trivial linear combination of the existing constraints (Yang and Carlone 2022). However, this constraint is redundant because it does not restrict the original feasible set at all. In the lifted SDP space it serves to restrict the feasible set and ultimately tighten the relaxation. Another useful redundant constraint can be formed by combining the last two of (31); the second last can be written as

$$ \frac{1}{2} (\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T \phi_m^\wedge = -(\mathbf{c}_i - \tilde{\mathbf{c}}_{m,i})^T, \qquad (33) $$
<figure><img src="image_8.png" /><figcaption>**Figure 6.** Pose Averaging Ablation Study: Here we show the effect on SDP tightness of varying the number of redundant constraints in the pose averaging problem. The rightmost column shows our full set of recommended redundant constraints with the light-grey box indicating the region of measurement noise for which our problem can be deemed tight. The same grey box is shown in the other columns for reference, indicating that including fewer redundant constraints results in a lower level of noise for which we can keep the solution tight.</figcaption></figure>

while the last can be premultiplied by $(\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T$ and written as

$$ (\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T (\mathbf{r} - \tilde{\mathbf{r}}_m) = \underbrace{\frac{1}{2}(\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T \phi_m^\wedge (\mathbf{r} + \tilde{\mathbf{r}}_m)}_{-(\mathbf{c}_i - \tilde{\mathbf{c}}_{m,i})^T} + (\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T \rho_m. \quad (34) $$

After performing the indicated substitution, this becomes

$$ \frac{1}{2}(\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T \rho_m = \mathbf{c}_i^T \mathbf{r} - \tilde{\mathbf{c}}_{m,i}^T \tilde{\mathbf{r}}_m, \quad (35) $$

which is once again a quadratic constraint.

Summarizing, the following QCQP offers a reasonably tight SDP relaxation in practice:

$$ \begin{array}{ll} \min & \sum_{m=1}^{M} \xi_m^T \mathbf{W}_m \xi_m \\ \text{w.r.t.} & \mathbf{c}_i, \mathbf{r}, \rho_m, \phi_m \quad (\forall i, m) \\ \text{s.t.} & \mathbf{c}_i^T \mathbf{c}_j = \delta_{ij} \quad (\forall i, j) \\ & (\mathbf{I} - \frac{1}{2}\phi_m^\wedge)\mathbf{c}_i = (\mathbf{I} - \frac{1}{2}\phi_m^\wedge)\tilde{\mathbf{c}}_{m,i} \quad (\forall i, m) \\ & (\mathbf{I} - \frac{1}{2}\phi_m^\wedge)\mathbf{r} = (\mathbf{I} + \frac{1}{2}\phi_m^\wedge)\tilde{\mathbf{r}}_m + \rho_m \quad (\forall m) \\ \text{(red.)} & \frac{1}{2}(\mathbf{c}_i + \tilde{\mathbf{c}}_{m,i})^T \rho_m = \mathbf{c}_i^T \mathbf{r} - \tilde{\mathbf{c}}_{m,i}^T \tilde{\mathbf{r}}_m \quad (\forall i, m) \\ & \mathbf{r}^T \mathbf{r} = \mathbf{r}^T \tilde{\mathbf{r}}_m - \frac{1}{2} \mathbf{r}^T \tilde{\mathbf{r}}_m^\wedge \phi_m + \mathbf{r}^T \rho_m \quad (\forall m) \end{array} \quad (36) $$

We leave it to the reader to manipulate this into the standard form of (21).

The appendix again provides a baseline local solver for this problem. For the global (SDP) solver we used `cvx` in Matlab with `mosek` (ApS 2019). The solution costs of the global and local solvers agree to high precision if a good initial guess is given to the local solver.

Figure 4 provides examples of the local solver becoming trapped in poor local minima while the global solver converges to the correct minima near the groundtruth. Figure 5 provides a quantitative study of the tightness of the SDP solution with increasing measurement noise; we selected the measurement covariance as $\mathbf{W}_m^{-1} = \sigma^2 \mathbf{I}$, with $\sigma$ increasing. We again see there is a large range for the noise over which the local solver can become trapped in a local minimum while the global solver remains tight. With $M=10$ poses to be averaged, the local solver took on average 0.0064s while the SDP solver took on average 0.5944s.

To justify the need for the redundant constraints, we conducted an ablation study (see Figure 6) wherein we varied the number of redundant constraints. The study shows that with more redundant constraints, we can tolerate a higher level of measurement noise while keeping the SDP tight. We always included the last redundant constraint in (36) as this enforces that the search space for the SDP remains compact** and it is therefore well posed. In the interest of space, we forgo similar ablation studies for the subsequent problems (discrete-time and continuous-time trajectory estimation), which reuse the pose averaging redundant constraints and then build on top of them. The studies are similar in that the more redundant constraints

<footnote>**Compactness in this context is related to the Archimedean property of the feasible set, which is a required condition when tightening a relaxation using Lasserre’s hierarchy (Lasserre 2010).</footnote>
<figure><img src="image_9.png" /><figcaption>**Figure 7.** Discrete-time Trajectory Estimation: Factor graph representation of the discrete-time estimation problem. Each black dot represents one of the error terms in the cost function of (37).</figcaption></figure>

we add, the larger the noise region for which we can a priori predict that we will achieve rank-1 SDP solutions.

## 4 Discrete-Time Trajectory Estimation

Our next problem is to consider estimation of a trajectory of K poses, $T_k$, where we have noisy measurements of each pose, $\tilde{T}_k$, as well as noisy relative measurements, $\tilde{T}_{k+1,k}$, from one pose to the next. The optimization problem that we want to solve is

$$ \begin{array}{ll} \min & \sum_{k=1}^{K} \operatorname{cay}^{-1} \left(\mathbf{T}_k \tilde{\mathbf{T}}_k^{-1}\right)^{\mathrm{T}} \mathbf{W}_k \operatorname{cay}^{-1} \left(\mathbf{T}_k \tilde{\mathbf{T}}_k^{-1}\right)^{\mathrm{V}} \\ & + \sum_{k=1}^{K-1} \operatorname{cay}^{-1} \left(\mathbf{T}_{k+1} \mathbf{T}_k^{-1} \tilde{\mathbf{T}}_{k+1,k}^{-1}\right)^{\mathrm{V}^{\mathrm{T}}} \\ & \times \mathbf{W}_{k+1,k} \operatorname{cay}^{-1} \left(\mathbf{T}_{k+1} \mathbf{T}_k^{-1} \tilde{\mathbf{T}}_{k+1,k}^{-1}\right)^{\mathrm{V}}, \\ \text { w.r.t. } & \mathbf{T}_k \quad (\forall k) \\ \text { s.t. } & \mathbf{T}_k \in \mathbf{SE}(3) \quad (\forall k) \end{array} \tag{37} $$

for some weight matrices, $W_k$ and $W_{k+1,k}$. Figure 7 depicts the estimation problem as a factor graph. Similarly to the pose averaging problem, we introduce new optimization variables for the residual errors:

$$ \xi_k = \operatorname{cay}^{-1} \left( \mathbf{T}_k \tilde{\mathbf{T}}_k^{-1} \right)^{\mathrm{V}}, \quad (38a) $$

$$ \xi_{k+1,k} = \operatorname{cay}^{-1} \left( \mathbf{T}_{k+1} \mathbf{T}_k^{-1} \tilde{\mathbf{T}}_{k+1,k}^{-1} \right)^{\mathrm{V}}, \quad (38b) $$

so that the optimization problem can be stated as a QCQP:

$$ \begin{array}{ll} \min & \sum_{k=1}^{K} \xi_k^T \mathbf{W}_k \xi_k + \sum_{k=1}^{K-1} \xi_{k+1,k}^T \mathbf{W}_{k+1,k} \xi_{k+1,k} \\ \text { w.r.t. } & \mathbf{T}_k, \xi_k, \xi_{k+1,k} \quad (\forall k) \\ \text { s.t. } & \mathbf{C}_k^T \mathbf{C}_k = \mathbf{I} \quad (\forall k) \\ & \left(\mathbf{I} - \frac{1}{2} \boldsymbol{\xi}_k^{\wedge}\right) \mathbf{T}_k = \left(\mathbf{I} + \frac{1}{2} \boldsymbol{\xi}_k^{\wedge}\right) \tilde{\mathbf{T}}_k \quad (\forall k) \\ & \left(\mathbf{I} - \frac{1}{2} \boldsymbol{\xi}_{k+1,k}^{\wedge}\right) \mathbf{T}_{k+1} \\ & \qquad = \left(\mathbf{I} + \frac{1}{2} \boldsymbol{\xi}_{k+1,k}^{\wedge}\right) \tilde{\mathbf{T}}_{k+1,k} \mathbf{T}_k \quad (\forall k) \end{array} \tag{39} $$

where the $\det(\mathbf{C}_k) = 1$ constraints have been dropped. Decomposing the matrices according to

$$ \mathbf{T}_k = \begin{bmatrix} \mathbf{C}_k & \mathbf{r}_k \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{c}_{k,1} & \mathbf{c}_{k,2} & \mathbf{c}_{k,3} & \mathbf{r}_k \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad (40a) $$

$$ \tilde{\mathbf{T}}_k = \begin{bmatrix} \tilde{\mathbf{C}}_k & \tilde{\mathbf{r}}_k \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} \tilde{\mathbf{c}}_{k,1} & \tilde{\mathbf{c}}_{k,2} & \tilde{\mathbf{c}}_{k,2} & \tilde{\mathbf{r}}_k \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad (40b) $$

$$ \tilde{\mathbf{T}}_{k+1,k} = \begin{bmatrix} \tilde{\mathbf{C}}_{k+1,k} & \tilde{\mathbf{r}}_{k+1,k} \\ \mathbf{0}^T & 1 \end{bmatrix} \quad (40c) $$

$$ = \begin{bmatrix} \tilde{\mathbf{c}}_{k+1,k,1} & \tilde{\mathbf{c}}_{k+1,k,2} & \tilde{\mathbf{c}}_{k+1,k,2} & \tilde{\mathbf{r}}_{k+1,k} \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad (40d) $$

$$ \boldsymbol{\xi}_k = \begin{bmatrix} \boldsymbol{\rho}_k \\ \boldsymbol{\phi}_k \end{bmatrix}, \quad \boldsymbol{\xi}_{k+1,k} = \begin{bmatrix} \boldsymbol{\rho}_{k+1,k} \\ \boldsymbol{\phi}_{k+1,k} \end{bmatrix}, \quad (40e) $$

the QCQP optimization problem can be rewritten compactly as

$$ \min \sum_{k=1}^{K} \xi_k^T \mathbf{W}_k \xi_k + \sum_{k=1}^{K-1} \xi_{k+1,k}^T \mathbf{W}_{k+1,k} \xi_{k+1,k} $$

$$ \text{w.r.t.} \quad \mathbf{c}_{k,i}, \mathbf{r}_k, \boldsymbol{\rho}_k, \boldsymbol{\phi}_k, \boldsymbol{\rho}_{k+1,k}, \boldsymbol{\phi}_{k+1,k} \quad (\forall i, k) $$

$$ \begin{array}{ll} \text{s.t.} & \mathbf{c}_{k,i}^T \mathbf{c}_{k,j} = \delta_{ij} \quad (\forall i, j, k) \\ & (\mathbf{I} - \frac{1}{2} \boldsymbol{\phi}_k^{\wedge}) \mathbf{c}_{k,i} = (\mathbf{I} - \frac{1}{2} \boldsymbol{\phi}_k^{\wedge}) \tilde{\mathbf{c}}_{k,i} \quad (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2} \boldsymbol{\phi}_k^{\wedge}) \mathbf{r}_k = (\mathbf{I} + \frac{1}{2} \boldsymbol{\phi}_k^{\wedge}) \tilde{\mathbf{r}}_k + \boldsymbol{\rho}_k \quad (\forall k) \\ & (\mathbf{I} - \frac{1}{2} \boldsymbol{\phi}_{k+1,k}^{\wedge}) \mathbf{c}_{k+1,i} \\ & \qquad = (\mathbf{I} - \frac{1}{2} \boldsymbol{\phi}_{k+1,k}^{\wedge}) \tilde{\mathbf{C}}_{k+1,k} \mathbf{c}_{k,i} \quad (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2} \boldsymbol{\phi}_{k+1,k}^{\wedge}) \mathbf{r}_{k+1} \\ & \qquad = (\mathbf{I} + \frac{1}{2} \boldsymbol{\phi}_{k+1,k}^{\wedge}) \left( \tilde{\mathbf{C}}_{k+1,k} \mathbf{r}_k + \tilde{\mathbf{r}}_{k+1,k} \right) \\ & \qquad + \boldsymbol{\rho}_{k+1,k} \quad (\forall k) \end{array} \tag{41} $$

Similarly to the pose averaging problem, if we convert this QCQP to a SDP, it is not always tight even for low noise levels. We need to include some redundant constraints to improve tightness. For each of the $\xi_k$ variables, we can create copies of the redundant constraints required in the pose averaging problem. However, this is still not enough; we require some additional constraints involving the $\xi_{k+1,k}$ variables.

Such additional redundant constraints can be formed by combining the last two of (41); the second last can be written as

$$ \frac{1}{2} (\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k} \mathbf{c}_{k,i})^T \boldsymbol{\phi}_{k+1,k}^{\wedge} \\ = -(\mathbf{c}_{k+1,i} - \tilde{\mathbf{C}}_{k+1,k} \mathbf{c}_{k,i})^T, \quad (42) $$
<figure><img src="image_10.png" /><figcaption>**Figure 8. Discrete-time Trajectory Estimation:** Two examples of discrete-time trajectory estimation where the randomly initialized local solver (red) becomes trapped in a poor local minimum while the global solver (green) finds the correct solution, which is closer to the groundtruth (blue). The noisy pose measurements are also shown (grey). It is interesting to note that the poor local solver solutions are twisted around the groundtruth.</figcaption></figure>

while the last can be premultiplied by $(\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{c}_{k,i})^T$ and written as

$$ (\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{c}_{k,i})^T (\mathbf{r}_{k+1} - \tilde{\mathbf{C}}_{k+1,k}\mathbf{r}_k - \tilde{\mathbf{r}}_{k+1,k}) \\ = \frac{1}{2} (\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{c}_{k,i})^T \phi_{k+1,k}^\wedge \\ \underbrace{-(\mathbf{c}_{k+1,i} - \tilde{\mathbf{C}}_{k+1,k}\mathbf{c}_{k,i})^T}_{\mathbf{r}_{k+1} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{r}_k + \tilde{\mathbf{r}}_{k+1,k}} \\ \times (\mathbf{r}_{k+1} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{r}_k + \tilde{\mathbf{r}}_{k+1,k}) \\ + (\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{c}_{k,i})^T \rho_{k+1,k}. \quad (43) $$

After performing the indicated substitution, this becomes

$$ \frac{1}{2}(\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k}\mathbf{c}_{k,i})^T \rho_{k+1,k} \\ = \mathbf{c}_{k+1,i}^T \mathbf{r}_{k+1} - \mathbf{c}_{k,i}^T (\mathbf{r}_k + \tilde{\mathbf{C}}_{k+1,k}^T \tilde{\mathbf{r}}_{k+1,k}), \quad (44) $$

which is once again a quadratic constraint.

Summarizing, the following QCQP offers a reasonably tight SDP relaxation in practice:

$$ \begin{array}{lll} \min & \sum_{k=1}^{K} \xi_k^T \mathbf{W}_k \xi_k + \sum_{k=1}^{K-1} \xi_{k+1,k}^T \mathbf{W}_{k+1,k} \xi_{k+1,k} & \\ \text{w.r.t.} & \mathbf{c}_{k,i}, \mathbf{r}_k, \rho_k, \phi_k, \rho_{k+1,k}, \phi_{k+1,k} & (\forall i, k) \\ \text{s.t.} & \mathbf{c}_{k,i}^T \mathbf{c}_{k,j} = \delta_{ij} & (\forall i, j, k) \\ & (\mathbf{I} - \frac{1}{2}\phi_k^\wedge) \mathbf{c}_{k,i} = (\mathbf{I} - \frac{1}{2}\phi_k^\wedge) \tilde{\mathbf{c}}_{k,i} & (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2}\phi_k^\wedge) \mathbf{r}_k = (\mathbf{I} + \frac{1}{2}\phi_k^\wedge) \tilde{\mathbf{r}}_k + \rho_k & (\forall k) \\ & (\mathbf{I} - \frac{1}{2}\phi_{k+1,k}^\wedge) \mathbf{c}_{k+1,i} = (\mathbf{I} - \frac{1}{2}\phi_{k+1,k}^\wedge) \tilde{\mathbf{C}}_{k+1,k} \mathbf{c}_{k,i} & (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2}\phi_{k+1,k}^\wedge) \mathbf{r}_{k+1} = (\mathbf{I} + \frac{1}{2}\phi_{k+1,k}^\wedge) (\tilde{\mathbf{C}}_{k+1,k} \mathbf{r}_k + \tilde{\mathbf{r}}_{k+1,k}) & \\ & \qquad + \rho_{k+1,k} & (\forall k) \end{array} $$

$$ \begin{array}{lll} \text{(red.)} & \frac{1}{2}(\mathbf{c}_{k,i} + \tilde{\mathbf{c}}_{k,i})^T \rho_k = \mathbf{c}_{k,i}^T \mathbf{r}_k - \tilde{\mathbf{c}}_{k,i}^T \tilde{\mathbf{r}}_k & (\forall i, k) \\ & \mathbf{r}_k^T \mathbf{r}_k = \mathbf{r}_k^T \tilde{\mathbf{r}}_k - \frac{1}{2} \mathbf{r}_k^T \tilde{\mathbf{r}}_k^\wedge \phi_k + \mathbf{r}_k^T \rho_k & (\forall k) \\ & \frac{1}{2}(\mathbf{c}_{k+1,i} + \tilde{\mathbf{C}}_{k+1,k} \mathbf{c}_{k,i})^T \rho_{k+1,k} \\ & = \mathbf{c}_{k+1,i}^T \mathbf{r}_{k+1} - \mathbf{c}_{k,i}^T (\mathbf{r}_k + \tilde{\mathbf{C}}_{k+1,k}^T \tilde{\mathbf{r}}_{k+1,k}) & (\forall i, k) \end{array} \quad (45) $$

We again leave it to the reader to manipulate this into the standard form of (21).

The appendix provides a baseline local solver for this problem. For the global (SDP) solver we used `cvx` in Matlab with `mosek` (ApS 2019). The solution costs of the global and local solvers agree to high precision if a good initial guess is given to the local solver. Figure 8 provides examples of the local solver becoming trapped in poor local minima while the global solver converges to the correct minima near the groundtruth. Figure 9 provides a quantitative study of the tightness of the SDP solution with increasing measurement noise; we selected the measurement covariances as $\mathbf{W}_k^{-1} = \mathbf{W}_{k+1,k}^{-1} = \sigma^2 \mathbf{I}$, with $\sigma$ increasing. We again see there is a large range for the noise over which the local solver can become trapped in a local minimum while the global solver remains tight; in fact, even at very low noise levels it is quite easy to have the local solver become trapped. With $K = 20$ poses in the trajectory, the local solver took on average 0.1574s while the SDP solver took on average 14.32s.

## 5 Continuous-Time Trajectory Estimation

Finally, we consider so-called continuous-time trajectory estimation. Continuous-time methods come in parametric (Furgale et al. 2012) and nonparametric (Barfoot et al. 2014; Anderson and Barfoot 2015) varieties; here we will discuss the latter. We consider a continuous-time Gaussian Process (GP) prior over the trajectory known as White Noise on Acceleration (WNOA); this serves to smooth the trajectory and is fused with pose measurements provided at discrete times. We will still ultimately have to discretize the trajectory for the purpose of estimation and so will have $K$ states comprising both pose and generalized velocity (a.k.a., twist), $\{\mathbf{T}_k, \boldsymbol{\omega}_k\}$. Figure 10 depicts the situation as a factor graph. In practice, we may not actually have pose
<figure><img src="image_11.png" /><figcaption>**Figure 9. Discrete-time Trajectory Estimation:** A quantitative evaluation of the tightness of the discrete-time trajectory estimation problem with increasing measurement noise, $\sigma$. At each noise level, we conducted 100 trials with the geometry of the trajectory as in the left example of Figure 8. (left) We see that the local solver (randomly initialized) finds the global minimum with decreasing frequency (green) as the measurement noise is increased, while the SDP solver (blue) successfully produces rank-1 solutions (we consider log SVR of at least 5 to be rank 1) to much higher noise levels. For completeness, we also show how frequently the local solver converges to any minimum (red). (right) Boxplots of the log SVR of the SDP solution show that the global solution remains highly rank 1 over a wide range of measurement noise values.</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>**Figure 10.** Factor graph representation of the continuous-time estimation problem. Each block dot represents one of the error terms in the cost function of (46).</figcaption></figure>

measurements at every time at which we introduce a state variable.

The optimization problem that we want to solve in this case is

$$ 
\begin{array}{ll}
\min & \sum_{k=1}^{K} \operatorname{cay}^{-1} \left(\mathbf{T}_k \tilde{\mathbf{T}}_k^{-1}\right)^{\vee T} \mathbf{W}_k \operatorname{cay}^{-1} \left(\mathbf{T}_k \tilde{\mathbf{T}}_k^{-1}\right)^{\vee} \\
& + (\tilde{\boldsymbol{\omega}}_1 - \boldsymbol{\omega}_1)^T \mathbf{Q}_1^{-1} (\tilde{\boldsymbol{\omega}}_1 - \boldsymbol{\omega}_1) \\
& + \sum_{k=1}^{K-1} \mathbf{e}_{k+1,k}^T \mathbf{Q}_{k+1,k}^{-1} \mathbf{e}_{k+1,k} \\
\text{w.r.t.} & \mathbf{T}_k, \boldsymbol{\omega}_k \quad (\forall k) \\
\text{s.t.} & \mathbf{T}_k \in SE(3) \quad (\forall k)
\end{array}
\qquad (46) $$

for some weight matrices, $\mathbf{W}_k$, and

$$ \mathbf{e}_{k+1,k} = \left[ \frac{(t_{k+1} - t_k)\boldsymbol{\omega}_k - \operatorname{cay}^{-1}(\mathbf{T}_{k+1}\mathbf{T}_k^{-1})^{\vee}}{\boldsymbol{\omega}_k - \boldsymbol{\omega}_{k+1}} \right], \qquad (47a) $$

$$ \mathbf{Q}_{k+1,k} = \left[ \frac{\frac{1}{3}(t_{k+1} - t_k)^3 \mathbf{Q}_c}{\frac{1}{2}(t_{k+1} - t_k)^2 \mathbf{Q}_c} \quad \frac{\frac{1}{2}(t_{k+1} - t_k)^2 \mathbf{Q}_c}{(t_{k+1} - t_k) \mathbf{Q}_c} \right]. \qquad (47b) $$

The $t_k$ are known timestamps of the states, $\mathbf{Q}_c$ is a power-spectral density matrix affecting smoothness of the GP prior, and $\tilde{\boldsymbol{\omega}}_1$ together with $\mathbf{Q}_1$ represent a Gaussian prior on the initial generalized velocity. The GP prior defined by (47a) is similar in spirit to the one first defined by Anderson and Barfoot (2015), only now adapted to work with the Cayley map. Looking at $\mathbf{e}_{k+1,k}$, the first row encourages the change in pose from one time to the next to be similar to the generalized velocity multiplied by the change in time; the second row encourages the generalized velocity to remain constant over time (i.e., no acceleration). The process noise covariance, $\mathbf{Q}_{k+1,k}$, comes from integrating the WNOA prior over the same time interval (Barfoot et al. 2014; Barfoot 2024).
<figure><img src="image_13.png" /><figcaption>**Figure 11.** Continuous-time Trajectory Estimation: Four examples of continuous-time trajectory estimation where the randomly initialized local solver (red) becomes trapped in a poor local minimum while the global solver (green) finds the correct solution, which is closer to the groundtruth (blue). The noisy pose measurements (occurring only at the start, middle, and end of each trajectory) are also shown (grey). The local minimum in the leftmost example is very similar to one reported by Lilge et al. (2022).</figcaption></figure>

Similarly to the discrete-time trajectory estimation case, we introduce the following substitution variables††:

$$ \xi_k = \text{cay}^{-1} (\mathbf{T}_k \tilde{\mathbf{T}}_k^{-1})^{\vee}, \quad (48a) $$

$$ \xi_{k+1,k} = \text{cay}^{-1} (\mathbf{T}_{k+1} \mathbf{T}_k^{-1})^{\vee}, \quad (48b) $$

so that the optimization problem can be stated as a QCQP:

$$ \begin{array}{ll} \min & \sum_{k=1}^{K} \xi_k^T \mathbf{W}_k \xi_k + (\tilde{\omega}_1 - \omega_1)^T \mathbf{Q}_1^{-1} (\tilde{\omega}_1 - \omega_1) \\ & + \sum_{k=1}^{K-1} \left[ \frac{(t_{k+1} - t_k)\bar{\omega}_k - \xi_{k+1,k}}{\bar{\omega}_k - \bar{\omega}_{k+1}} \right]^T \\ & \times \mathbf{Q}_{k+1,k}^{-1} \left[ \frac{(t_{k+1} - t_k)\bar{\omega}_k - \xi_{k+1,k}}{\bar{\omega}_k - \bar{\omega}_{k+1}} \right], \end{array} $$

$$ \text{w.r.t.} \quad \mathbf{T}_k, \bar{\omega}_k, \xi_k, \xi_{k+1,k} \quad (\forall k) $$

$$ \begin{array}{ll} \text{s.t.} & \mathbf{C}_k^T \mathbf{C}_k = \mathbf{I} \quad (\forall k) \\ & (\mathbf{I} - \frac{1}{2}\xi_k^\wedge) \mathbf{T}_k = (\mathbf{I} + \frac{1}{2}\xi_k^\wedge) \tilde{\mathbf{T}}_k \quad (\forall k) \\ & (\mathbf{I} - \frac{1}{2}\xi_{k+1,k}^\wedge) \mathbf{T}_{k+1} = (\mathbf{I} + \frac{1}{2}\xi_{k+1,k}^\wedge) \mathbf{T}_k \quad (\forall k) \end{array} \quad (49) $$

where the det($\mathbf{C}_k$) = 1 constraints have been dropped. Decomposing the matrices according to

$$ \mathbf{T}_k = \begin{bmatrix} \mathbf{C}_k & \mathbf{r}_k \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{c}_{k,1} & \mathbf{c}_{k,2} & \mathbf{c}_{k,3} & \mathbf{r}_k \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad (50a) $$

$$ \tilde{\mathbf{T}}_k = \begin{bmatrix} \tilde{\mathbf{C}}_k & \tilde{\mathbf{r}}_k \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} \tilde{\mathbf{c}}_{k,1} & \tilde{\mathbf{c}}_{k,2} & \tilde{\mathbf{c}}_{k,2} & \tilde{\mathbf{r}}_k \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad (50b) $$

$$ \xi_k = \begin{bmatrix} \rho_k \\ \phi_k \end{bmatrix}, \quad \xi_{k+1,k} = \begin{bmatrix} \rho_{k+1,k} \\ \phi_{k+1,k} \end{bmatrix}, \quad (50c) $$

the QCQP optimization problem can be rewritten compactly as

$$ \begin{array}{ll} \min & \sum_{k=1}^{K} \xi_k^T \mathbf{W}_k \xi_k + (\tilde{\omega}_1 - \omega_1)^T \mathbf{Q}_1^{-1} (\tilde{\omega}_1 - \omega_1) \\ & + \sum_{k=1}^{K-1} \left[ \frac{(t_{k+1} - t_k)\bar{\omega}_k - \xi_{k+1,k}}{\bar{\omega}_k - \bar{\omega}_{k+1}} \right]^T \\ & \times \mathbf{Q}_{k+1,k}^{-1} \left[ \frac{(t_{k+1} - t_k)\bar{\omega}_k - \xi_{k+1,k}}{\bar{\omega}_k - \bar{\omega}_{k+1}} \right] \end{array} $$

$$ \text{w.r.t.} \quad \mathbf{c}_{k,i}, \mathbf{r}_k, \rho_k, \phi_k, \rho_{k+1,k}, \phi_{k+1,k}, \bar{\omega}_k \quad (\forall i, k) $$

$$ \begin{array}{ll} \text{s.t.} & \mathbf{c}_{k,i}^T \mathbf{c}_{k,j} = \delta_{ij} \quad (\forall i, j, k) \\ & (\mathbf{I} - \frac{1}{2}\phi_k^\wedge) \mathbf{c}_{k,i} = (\mathbf{I} - \frac{1}{2}\phi_k^\wedge) \tilde{\mathbf{c}}_{k,i} \quad (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2}\phi_k^\wedge) \mathbf{r}_k = (\mathbf{I} + \frac{1}{2}\phi_k^\wedge) \tilde{\mathbf{r}}_k + \rho_k \quad (\forall k) \\ & (\mathbf{I} - \frac{1}{2}\phi_{k+1,k}^\wedge) \mathbf{c}_{k+1,i} \\ & \qquad = (\mathbf{I} - \frac{1}{2}\phi_{k+1,k}^\wedge) \mathbf{c}_{k+1,i} \quad (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2}\phi_{k+1,k}^\wedge) \mathbf{r}_{k+1} \\ & \qquad = (\mathbf{I} + \frac{1}{2}\phi_{k+1,k}^\wedge) \mathbf{r}_{k+1} \quad (\forall k) \end{array} \quad (51) $$

Similarly to the problems above, if we convert this QCQP to a SDP, it is not tight even for low noise levels. We need to include some redundant constraints to improve tightness. For each of the $\xi_k$ and $\xi_{k+1,k}$ variables, we can create copies of the redundant constraints required in the discrete-time trajectory estimation problem. However, this is still not always enough to tighten the problem, particularly when we

<footnote>††Importantly, we were forced to deviate from our program of substitution variables being the residual errors of the cost function in our choice of $\xi_{k+1,k}$; this could be problematic for very-high-velocity trajectories as $\xi_{k+1,k}$ could become large and approach the singularity of the Cayley map. We justify our choice as providing the necessary pathway to a QCQP, but this could be revisited in future work.</footnote>
do not have a pose measurement at every state time (see below comment regarding sparser measurement graphs).

We can generate some additional redundant constraints fairly easily. First, we can premultiply the second constraint of (51) by $\phi_k^T$ so that

$$ \phi_k^T(\mathbf{I} - \frac{1}{2}\phi_k^\wedge)\mathbf{c}_{k,i} = \phi_k^T(\mathbf{I} - \frac{1}{2}\phi_k^\wedge)\tilde{\mathbf{c}}_{k,i} \\ \Rightarrow \quad \phi_k^T \mathbf{c}_{k,i} = \phi_k^T \tilde{\mathbf{c}}_{k,i}, \quad (52) $$

where we have used that $\phi_k^T \phi_k^\wedge = \mathbf{0}^T$. Similarly, premultiplying the third constraint in (51) by $\phi_k^T$ results in

$$ \phi_k^T \mathbf{r}_k = \phi_k^T \tilde{\mathbf{r}}_k + \phi_k^T \rho_k. \quad (53) $$

Premultiplying the fourth and fifth constraints in (51) by $\phi_{k+1,k}^T$ results in

$$ \phi_{k+1,k}^T \mathbf{c}_{k+1,i} = \phi_{k+1,k}^T \mathbf{c}_{k,i}, \quad (54a) $$

$$ \phi_{k+1,k}^T \mathbf{r}_{k+1} = \phi_{k+1,k}^T \mathbf{r}_k + \phi_{k+1,k}^T \rho_{k+1,k}. \quad (54b) $$

Next, we can exploit the fact that columns of a rotation matrix satisfy $\mathbf{c}_\ell^\wedge \mathbf{c}_m = \mathbf{c}_n$ where $\ell mn \in \{123, 231, 312\}$. If we premultiply the second constraint of (51) by $\mathbf{c}_{k,m}^T$ we have

$$ \begin{aligned} \mathbf{c}_{k,m}^T (\mathbf{I} - \frac{1}{2} \phi_k^\wedge) \mathbf{c}_{k,\ell} &= \mathbf{c}_{k,m}^T (\mathbf{I} - \frac{1}{2} \phi_k^\wedge) \tilde{\mathbf{c}}_{k,\ell} \\ \Rightarrow \quad &\mathbf{c}_{k,m}^T \mathbf{c}_{k,\ell} - \frac{1}{2} \phi_k^T \mathbf{c}_{k,n} = \mathbf{c}_{k,m}^T \tilde{\mathbf{c}}_{k,\ell} - \frac{1}{2} \mathbf{c}_{k,m}^T \tilde{\mathbf{c}}_{k,\ell}^\wedge \phi_k, \end{aligned} \quad (55) $$

which is still a quadratic constraint. Finally, if we premultiply the last constraint of (51) by $(\mathbf{r}_{k+1} + \mathbf{r}_k)^T$, this results in

$$ \mathbf{r}_{k+1}^T \mathbf{r}_{k+1} = \mathbf{r}_{k+1}^T \rho_{k+1,k} + \mathbf{r}_k^T \rho_{k+1,k} + \mathbf{r}_k^T \mathbf{r}_k, \quad (56) $$

which is once again a quadratic constraint.

Summarizing, the following QCQP offers a reasonably tight SDP relaxation in practice:

$$ \begin{aligned} \min \quad & \sum_{k=1}^K \xi_k^T \mathbf{W}_k \xi_k + (\tilde{\omega}_1 - \tilde{\omega}_1)^T \mathbf{Q}_1^{-1} (\tilde{\omega}_1 - \tilde{\omega}_1) \\ & + \sum_{k=1}^{K-1} \left[ (t_{k+1} - t_k) \tilde{\omega}_k - \xi_{k+1,k} \right]^T \\ & \times \mathbf{Q}_{k+1,k}^{-1} \left[ (t_{k+1} - t_k) \tilde{\omega}_k - \xi_{k+1,k} \right] \end{aligned} $$

$$ \text{w.r.t.} \quad \mathbf{c}_{k,i}, \mathbf{r}_k, \rho_k, \phi_k, \rho_{k+1,k}, \phi_{k+1,k}, \tilde{\omega}_k \quad (\forall i, k) $$

$$ \begin{aligned} \text{s.t.} \quad & \mathbf{c}_{k,i}^T \mathbf{c}_{k,j} = \delta_{ij} \quad (\forall i, j, k) \\ & (\mathbf{I} - \frac{1}{2} \phi_k^\wedge) \mathbf{c}_{k,i} = (\mathbf{I} - \frac{1}{2} \phi_k^\wedge) \tilde{\mathbf{c}}_{k,i} \quad (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2} \phi_k^\wedge) \mathbf{r}_k = (\mathbf{I} + \frac{1}{2} \phi_k^\wedge) \tilde{\mathbf{r}}_k + \rho_k \quad (\forall k) \\ & (\mathbf{I} - \frac{1}{2} \phi_{k+1,k}^\wedge) \mathbf{c}_{k+1,i} \\ & \quad = (\mathbf{I} - \frac{1}{2} \phi_{k+1,k}^\wedge) \mathbf{c}_{k,i} \quad (\forall i, k) \\ & (\mathbf{I} - \frac{1}{2} \phi_{k+1,k}^\wedge) \mathbf{r}_{k+1} \\ & \quad = (\mathbf{I} + \frac{1}{2} \phi_{k+1,k}^\wedge) \tilde{\mathbf{r}}_k + \rho_{k+1,k} \quad (\forall k) \end{aligned} $$

$$ \begin{aligned} \text{(red.)} \quad & \frac{1}{2}(\mathbf{c}_{k,i} + \tilde{\mathbf{c}}_{k,i})^T \rho_k = \mathbf{c}_{k,i}^T \mathbf{r}_k - \tilde{\mathbf{c}}_{k,i}^T \tilde{\mathbf{r}}_k \quad (\forall i, k) \\ & \mathbf{r}_k^T \mathbf{r}_k = \mathbf{r}_k^T \tilde{\mathbf{r}}_k - \frac{1}{2} \mathbf{r}_k^T \tilde{\mathbf{r}}_k^\wedge \phi_k + \mathbf{r}_k^T \rho_k \quad (\forall k) \end{aligned} $$

$$ \begin{aligned} \phi_k^T \mathbf{c}_{k,i} &= \phi_k^T \tilde{\mathbf{c}}_{k,i} \quad (\forall i, k) \\ \phi_k^T \mathbf{r}_k &= \phi_k^T \tilde{\mathbf{r}}_k + \phi_k^T \rho_k \quad (\forall k) \\ \mathbf{c}_{k,m}^T \mathbf{c}_{k,\ell} - \frac{1}{2} \phi_k^T \mathbf{c}_{k,n} \\ &= \mathbf{c}_{k,m}^T \tilde{\mathbf{c}}_{k,\ell} - \frac{1}{2} \mathbf{c}_{k,m}^T \tilde{\mathbf{c}}_{k,\ell}^\wedge \phi_k \quad (\forall \ell mn \in \{123, 231, 312\}, k) \end{aligned} $$

$$ \begin{aligned} & \frac{1}{2}(\mathbf{c}_{k+1,i} + \mathbf{c}_{k,i})^T \rho_{k+1,k} \\ & \quad = \mathbf{c}_{k+1,i}^T \mathbf{r}_{k+1} - \mathbf{c}_{k,i}^T \mathbf{r}_k \quad (\forall i, k) \\ & \mathbf{r}_{k+1}^T \mathbf{r}_{k+1} \\ & \quad = \mathbf{r}_{k+1}^T \rho_{k+1,k} + \mathbf{r}_k^T \rho_{k+1,k} + \mathbf{r}_k^T \mathbf{r}_k \quad (\forall k) \end{aligned} $$

$$ \phi_{k+1,k}^T \mathbf{c}_{k+1,i} = \phi_{k+1,k}^T \mathbf{c}_{k,i} \quad (\forall i, k) $$

$$ \phi_{k+1,k}^T \mathbf{r}_{k+1} = \phi_{k+1,k}^T \mathbf{r}_k + \phi_{k+1,k}^T \rho_{k+1,k} \quad (\forall k) \quad (57) $$

We again leave it to the reader to manipulate this into the standard form of (21). We can also notice that the $\tilde{\omega}_k$ variables are not involved in any of the constraints and thus remain unconstrained variables. Similar to Rosen et al. (2019); Holmes and Barfoot (2023), at implementation we use the Schur complement to marginalize these variables out of the problem, thereby keeping the size of the SDP as small as possible; we can easily compute them after the main solve.

The appendix provides a baseline local solver for this problem. For the global (SDP) solver we used `cvx` in Matlab with `mosek` (ApS 2019). The solution costs of the global and local solvers agree to high precision if a good initial guess is given to the local solver. Figure 11 provides examples of the local solver becoming trapped in poor local minima while the global solver converges to the correct minimum near the groundtruth. Figure 12 provides a quantitative study of the tightness of the SDP solution with increasing measurement noise; we selected the measurement covariances as $\mathbf{W}_k^{-1} = \sigma^2 \mathbf{I}$, with $\sigma$ increasing. We again see there is a large range for the noise over which the local solver can become trapped in a local minimum while the global solver remains tight; in fact,
<figure><img src="image_14.png" /><figcaption>**Figure 12.** *Continuous-time Trajectory Estimation:* A quantitative evaluation of the tightness of the continuous-time trajectory estimation problem with increasing measurement noise, $\sigma$. At each noise level, we conducted 100 trials with the geometry of the trajectory as in the left example of Figure 11. (left) We see that the local solver (randomly initialized) finds the global minimum with decreasing frequency (green) as the measurement noise is increased, while the SDP solver (blue) successfully produces rank-1 solutions (we consider log SVR of at least 5 to be rank 1) to much higher noise levels. For completeness, we also show how frequently the local solver converges to any minimum (red). (right) Boxplots of the log SVR of the SDP solution show that the global solution remains reasonably rank 1 over a wide range of measurement noise values.</figcaption></figure>

even at very low noise levels it is quite easy to have the local solver become trapped. With $K = 21$ poses in the trajectory, the local solver took on average 0.1928s while the SDP solver took on average 13.42s.

There are also a few noteworthy differences in the continuous-time experiment as compared to the discrete-time case. First, the log SVR values are quite a bit lower in Figure 12 as compared to Figure 9. This seems to be mainly due to the fact that we are now using a sparser set of measurements. Our continuous-time experiments had pose measurements only at the start, middle, and end, whereas the discrete-time case had pose measurements at every timestep. It is known that a sparser measurement graph can impact SDP tightness (Holmes and Barfoot 2023). Second, the low-noise test cases experienced some numerical issues with getting the SDP solver to reliably converge. It seems this is related to matrix conditioning resulting from the fact that we are marginalizing out the $\varpi_k$ variables before solving the SDP. We found it was necessary to adjust the scaling of the Q matrix in (21) to get reliable solutions. Despite our best efforts, we see that there was one out of 100 test cases at the $\sigma = 0.01$ noise level where the SDP failed to solve. On the log SVR plot this shows as a red plus sign at 0 and results in the success rate of the SDP solver being 0.99 instead of 1. Still, we know that the solver failed and therefore not to trust the answer. Overall, we still have log SVR values almost always above 5 up to about $\sigma = 1$, which makes the solution practical; it is easy for the local solver to become stuck in local minima in this noise range.

# 6 Conclusion and Future Work

We have presented several new convex relaxations for pose and rotation estimation problems based on the Cayley map. Our results indicate that for small problem sizes, we can successfully achieve global optimality with realistic amounts of noise and even with measurement sparsity in the case of continuous-time trajectory estimation. In each of the experiments, we indicated that covariance of the error associated with each pose measurement cost term is $\sigma^2\mathbf{I}$. In other words, $\sigma$ is the standard deviation of the measurement noise. In the case of rotational degrees of freedom, $\sigma = 0.5$ is already shown in Figure 1 to represent quite a lot of rotational uncertainty, indeed more than typically occurs in practice. Since the standard deviation is in radians, this implies that uncertainty spreads out over a large part of a full circle with $\sigma = 0.5$. The fact that our convex relaxations empirically remain tight (ensuring global optimality) beyond the $\sigma = 0.5$ level (often beyond $\sigma = 1$) means our technique works over most practical situations. For translational degrees of freedom, $\sigma$ will have units of distance. The trajectories in the examples have poses that are spaced one distance unit apart so when the noise on the measurement of one of these poses is $\sigma = 0.5$ distance units, that is again quite a lot of noise in comparison to the spacing of the poses. The implication is again that our convex relaxations remain tight (ensuring global optimality) for most practical situations.

While our results are promising, we are still relying on off-the-shelf solvers once our problem has been converted
to an SDP, which means that we will not be able to scale up to extremely large state sizes. To scale up, there are a few possibilities that we could explore. First, perhaps we might be satisfied with merely certifying our local solver solutions. Other works have focussed on this. The challenge is that in most of the problems of this paper, we require redundant constraints to tighten our SDPs. This means that we do not meet the technical condition of Linearly Independent Constraint Qualification (LICQ) (Boyd and Vandenberghe 2004). It turns out that this makes it more challenging to calculate an optimality certificate. Yang et al. (2020) is a practical example where a certificate has been constructed for this type of situation, but there are still scaling issues. Another possibility is to solve our problems globally using the approach of Burer and Monteiro (2005) (studied more recently by Boumal et al. (2016)). This was exploited with very impressive results by Rosen et al. (2019); Dellaert et al. (2020); however, these problems enjoyed LICQ. To our knowledge, this approach has not been applied to problems in robotics with redundant constraints. Or, perhaps generic SDP solvers can be made to better exploit problem-specific structure (e.g., chordal sparsity). We plan to explore these and other ways of scaling global optimality for a larger range of state estimation problems.

## Funding

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

## References

Alismail H, Baker LD and Browning B (2014) Continuous trajectory estimation for 3D SLAM from actuated lidar. In: *Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)*. pp. 6096–6101.

Anderson S and Barfoot TD (2015) Full steam ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on se(3). In: *Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. Hamburg, Germany, pp. 157–164. DOI: 10.1109/IROS.2015.7353368.

Anjos MF and Wolkowicz H (2002) Strengthened semidefinite relaxations via a second lifting for the Max-Cut problem. *Discrete Applied Mathematics* : 28.

Anstreicher K and Wolkowicz H (2000) On lagrangian relaxation of quadratic matrix constraints. *SIAM Journal on Matrix Analysis and Applications* 22(1): 41–55.

ApS M (2019) *The MOSEK optimization toolbox for MATLAB manual*. Version 9.3. URL http://docs.mosek.com/9.3/toolbox/index.html.

Bandeira AS, Boumal N and Singer A (2017) Tightness of the maximum likelihood semidefinite relaxation for angular synchronization. *Mathematical Programming* 163(1-2): 145–167.

Barfoot TD (2024) *State Estimation for Robotics*. 2nd edition. Cambridge University Press. ISBN 9781009299893. DOI: 10.1017/9781009299909.

Barfoot TD, Forbes JR and D'Eleuterio GMT (2022) Vectorial parameterizations of pose. *Robotica* 40(7): 2409–2427. DOI: 10.1017/S0263574721001715.

Barfoot TD and Furgale PT (2014) Associating uncertainty with three-dimensional poses for use in estimation problems. *IEEE Transactions on Robotics* 30(3): 679–693. DOI:10.1109/TRO.2014.2298059.

Barfoot TD, Tong CH and Sarkka S (2014) Batch continuous-time trajectory estimation as exactly sparse gaussian process regression. In: *Proceedings of Robotics: Science and Systems (RSS)*. Berkeley, USA.

Bauchau OA (2011) *Flexible multibody dynamics*. Springer Science & Business Media.

Bauchau OA and Choi JY (2003) The vector parameterization of motion. *Nonlinear Dynamics* 33(2): 165–188.

Bauchau OA and Trainelli L (2003) The vectorial parameterization of rotation. *Nonlinear dynamics* 32(1): 71–92.

Bishop CM (2006) *Pattern Recognition and Machine Learning*. Springer.

Borri M, Trainelli L and Bottasso CL (2000) On representations and parameterizations of motion. *Multibody System Dynamics* 4(2): 129–193.

Boumal N, Voroninski V and Bandeira A (2016) The non-convex burer-monteiro approach works on smooth semidefinite programs. *Advances in Neural Information Processing Systems* 29.

Boyd S and Vandenberghe L (2004) *Convex optimization*. Cambridge university press.

Briales J and Gonzalez-Jimenez J (2017a) Cartan-sync: Fast and global se (d)-synchronization. *IEEE Robotics and Automation Letters* 2(4): 2127–2134.

Briales J and Gonzalez-Jimenez J (2017b) Convex global 3d registration with lagrangian duality. In: *Proceedings of the IEEE conference on computer vision and pattern recognition*. pp. 4960–4969.

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

Burer S and Monteiro RD (2005) Local minima and convergence in low-rank semidefinite programming. *Mathematical programming* 103(3): 427–444.

Carlone L and Calafiore GC (2018) Convex relaxations for pose graph optimization with outliers. *IEEE Robotics and
Automation Letters 3(2): 1160-1167.

Carlone L, Calafiore GC, Tommolillo C and Dellaert F (2016) Planar Pose Graph Optimization: Duality, Optimal Solutions, and Verification. *IEEE Trans. Robot.* 32(3): 545–565.

Chaudhury KN, Khoo Y and Singer A (2015) Global registration of multiple point clouds using semidefinite programming. *SIAM Journal on Optimization* 25(1): 468–501.

Cifuentes D, Agarwal S, Parrilo PA and Thomas RR (2022) On the local stability of semidefinite relaxations. *Mathematical Programming* 193: 629–663.

Dellaert F, Rosen DM, Wu J, Mahony R and Carlone L (2020) Shonan rotation averaging: Global optimality by surfing so (p)ⁿ so (p)ⁿ. In: *Computer Vision-ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, 2020, Proceedings, Part VI 16*. Springer, pp. 292–308.

Duembgen F, Holmes C, Agro B and Barfoot TD (2023) Toward globally optimal state estimation using automatically tightened semidefinite relaxations. Submitted to *IEEE Transactions on Robotics (T-RO)* on September 8, 2023. Manuscript # 23-0944. (arXiv:2308.05783 [cs.RO]).

Dümbgen F, Holmes C and Barfoot TD (2022) Safe and smooth: Certified continuous-time range-only localization. *IEEE Robotics and Automation Letters* 8(2): 1117–1124.

Eriksson A, Olsson C, Kahl F and Chin TJ (2018) Rotation averaging and strong duality. In: *Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition*. pp. 127–135.

Eriksson A, Olsson C, Kahl F and Chin TJ (2019) Rotation averaging with the chordal distance: Global minimizers and strong duality. *IEEE Transactions on Pattern Analysis and Machine Intelligence* 43(1): 256–268.

Forbes JR and de Ruiter AH (2015) Linear-matrix-inequality-based solution to wahba's problem. *Journal of guidance, control, and dynamics* 38(1): 147–151.

Furgale PT, Barfoot TD and Sibley G (2012) Continuous-time batch estimation using temporal basis functions. In: *Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)*. St. Paul, USA, pp. 2088–2095. DOI:10.1109/ICRA.2012.6225005.

Garcia-Salguero M, Briales J and Gonzalez-Jimenez J (2022) A Tighter Relaxation for the Relative Pose Problem Between Cameras. *J Math Imaging Vis*.

Giamou M, Ma Z, Peretroukhin V and Kelly J (2019) Certifiably globally optimal extrinsic calibration from sensor egomotion. *IEEE Robotics and Automation Letters* 4(2): 367–374.

Henrion D, Korda M and Lasserre JB (2020) *The Moment-SOS Hierarchy: Lectures in Probability, Statistics, Computational Geometry, Control and Nonlinear PDEs, Series on Optimization and Its Applications*, volume 04. WORLD SCIENTIFIC (EUROPE).

Holmes C and Barfoot TD (2023) An efficient global optimality certificate for landmark-based slam. *IEEE Robotics and Automation Letters* 8(3): 1539–1546.

Horowitz MB, Matni N and Burdick JW (2014) Convex relaxations of se(2) and se(3) for visual pose estimation. In: *2014 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, pp. 1148–1154.

Iglesias JP, Olsson C and Kahl F (2020) Global optimality for point set registration using semidefinite programming. In: *Proceedings of the IEEE/CVF conference on computer vision and pattern recognition*. pp. 8287–8295.

Junkins JL, Majji M, Macomber B, Davis J, Doebbler J and Nosterk R (2011) Small body proximity sensing with a novel hd 3d ladar system. *Advances in the Astronautical Sciences* 141: 341–354.

Kahl F and Henrion D (2005) Globally optimal estimates for geometric reconstruction problems. In: *Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 1*, volume 2. pp. 978–985 Vol. 2.

Kezurer I, Kovalsky SZ, Basri R and Lipman Y (2015) Tight Relaxation of Quadratic Matching. *Computer Graphics Forum* 34(5): 115–128.

Kobilarov M (2014) Discrete optimal control on lie groups and applications to robotic vehicles. In: *Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)*. pp. 5523–5529.

Kobilarov MB and Marsden JE (2011) Discrete Geometric Optimal Control on Lie Groups. *IEEE Transactions on Robotics* 27(4): 641–655.

Krislock N and Wolkowicz H (2010) Explicit Sensor Network Localization using Semidefinite Representations and Facial Reductions. *SIAM J. Optim*. 20(5): 2679–2708.

Lasserre JB (2001) Global optimization with polynomials and the problem of moments. *SIAM Journal on optimization* 11(3): 796–817.

Lasserre JB (2010) *Moments, Positive Polynomials and Their Applications*. Number v. 1 in Imperial College Press Optimization Series. London: Signapore; Hackensack, NJ: Imperial College Press; Distributed by World Scientific Publishing Co.

Lilge S, Barfoot TD and Burgner-Kahrs J (2022) Continuum robot state estimation using gaussian process regression on se(3). *International Journal of Robotics Research (IJRR)* 41(13-14): 1099–1120. DOI:10.1177/02783649221128843.

Liu M, Huang S, Dissanayake G and Wang H (2012) A convex optimization based approach for pose SLAM problems. In: *2012 IEEE/RSJ International Conference on Intelligent Robots and Systems*. pp. 1898–1903.

Long AW, Wolfe KC, Mashner MJ, Chirikjian GS et al. (2013) The banana distribution is gaussian: A localization study with exponential coordinates. *Robotics: Science and Systems VIII* 265: 1.
Luo Zq, Ma Wk, So A, Ye Y and Zhang S (2010) Semidefinite Relaxation of Quadratic Optimization Problems. *IEEE Signal Process.* 27(3): 20–34.

Majji M, Davis J, Doebbler J, Junkins J, Macomber B, Vavrina M and Vian J (2011) Terrain mapping and landing operations using vision based navigation systems. In: AIAA Guidance, Navigation, and Control Conference. p. 6581.

Mortari D, Markley FL and Singla P (2007) Optimal linear attitude estimator. *Journal of Guidance, Control, and Dynamics* 30(6): 1619–1627.

Müller A (2021) Review of the exponential and cayley map on se (3) as relevant for lie group integration of the generalized poisson equation and flexible multibody systems. *Proceedings of The Royal Society A* 477(2253): 20210303.

Nakano G (2015) Globally optimal dls method for pnp problem with cayley parameterization. In: *Proceedings of the British Machine Vision Conference*, volume 78. pp. 1–78.

Nesterov Y, Wolkowicz H and Ye Y (2000) Semidefinite Programming Relaxations of Nonconvex Quadratic Optimization. In: Hillier FS, Wolkowicz H, Saigal R and Vandenberghe L (eds.) *Handbook of Semidefinite Programming*, volume 27. Boston, MA: Springer US, pp. 361–419.

Nocedal J and Wright SJ (1999) *Numerical optimization*. Springer.

Papalia A, Morales J, Doherty KJ, Rosen DM and Leonard JJ (2022) Score: A second-order conic initialization for range-aided slam. *arXiv preprint arXiv:2210.03177*.

Parrilo PA (2003) Semidefinite programming relaxations for semialgebraic problems. *Mathematical programming* 96: 293–320.

Qian D, Charland-Arcand G and Forbes JR (2020) Twolate: Total registration of point-clouds using a weighted optimal linear attitude and translation estimator. In: *2020 IEEE Conference on Control Technology and Applications (CCTA)*. IEEE, pp. 43–48.

Rosen DM, Carlone L, Bandeira AS and Leonard JJ (2019) Se-sync: A certifiably correct algorithm for synchronization over the special euclidean group. *The International Journal of Robotics Research* 38(2-3): 95–125.

Selig JM (2007) Cayley maps for SE(3). In: *12th International Federation for the Promotion of Mechanism and Machine Science World Congress*. London South Bank University, p. 6.

Shor NZ (1987) Quadratic optimization problems. *Soviet Journal of Computer and Systems Sciences* 25: 1–11.

Solo V and Wang Z (2019) Numerical Methods for Stochastic Differential Equations in Stiefel Manifolds via the Cayley Transform. In: *IEEE 58th Conference on Decision and Control (CDC)*. pp. 3303–3038.

Tian Y, Khosoussi K, Rosen DM and How JP (2021) Distributed certifiably correct pose-graph optimization. *IEEE Transactions on Robotics* 37(6): 2137–2156.

Wang P, Chou Y, An A and Xu G (2022a) Solving the pnl problem using the hidden variable method: an accurate and efficient

solution. *The Visual Computer* : 1–12.

Wang Y, Wen X, Yin L, Xu C, Cao Y and Gao F (2022b) Certifiably Optimal Mutual Localization With Anonymous Bearing Measurements. *IEEE Robotics and Automation Letters* 7(4): 9374–9381.

Wang Z and Solo V (2020) Lie Group State Estimation via Optimal Transport. In: *Proceedings of the IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP)*. pp. 5625–5629.

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

Wong XI and Majji M (2016) A structured light system for relative navigation applications. *IEEE Sensors Journal* 16(17): 6662–6679.

Wong XI, Singla P, Lee T and Majji M (2018) Optimal linear attitude estimator for alignment of point clouds. In: *2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition Workshops (CVPRW)*. IEEE, pp. 1577–15778.

Yang H and Carlone L (2022) Certifiably optimal outlier-robust geometric perception: Semidefinite relaxations and scalable global optimization. *IEEE transactions on pattern analysis and machine intelligence* 45(3): 2816–2834.

Yang H, Shi J and Carlone L (2020) Teaser: Fast and certifiable point cloud registration. *IEEE Transactions on Robotics* 37(2): 314–333.

Zhang L and Koch R (2014) Structure and motion from line correspondences: Representation, projection, initialization and sparse bundle adjustment. *Journal of Visual Communication and Image Representation* 25(5): 904–915.

## A Comparing Exponential and Cayley Distributions

We use this section to analyze how similar our proposed Gaussian-like distribution via the Cayley map is to one via the exponential map.

Consider two distributions for rotations, $C_1$ and $C_2$, defined according to

$$C_1 = \exp (\phi_1^\wedge) \bar{C}, \quad \phi_1 \sim \mathcal{N}(0, \Sigma_1), \quad (58a)$$

$$C_2 = \operatorname{cay} (\phi_2^\wedge) \bar{C}, \quad \phi_2 \sim \mathcal{N}(0, \Sigma_2). \quad (58b)$$

To ensure that $C_1 = C_2$ (in every instance), Bauchau and Trainelli (2003) show that we require

$$\phi_1 = \phi_1 \mathbf{a} = \varphi \mathbf{a}, \quad (59a)$$

$$\phi_2 = \phi_2 \mathbf{a} = 2 \tan \left( \frac{\varphi}{2} \right) \mathbf{a}, \quad (59b)$$

in terms of a common axis of rotation, $\mathbf{a}$, and angle of rotation, $\varphi$. Notationally, we define $\phi_1 = ||\phi_1|| = \varphi$ and $\phi_2 = ||\phi_2|| = 2 \tan(\frac{\varphi}{2})$. Rearranging, we therefore
require

$$ \phi_2 = \frac{\tan\left(\frac{\phi_1}{2}\right)}{\frac{\phi_1}{2}} \phi_1 \qquad (60) $$

to relate $\phi_1$ and $\phi_2$ directly.

We can use (60) to relate the distribution for $\phi_2$ to that of $\phi_1$, which will in turn make the distribution for $C_2$ approximately match that of $C_1$. Specifically, we will try to match the moments of the left and right sides of (60). Moment matching is known to minimize a version of Kullback-Liebler divergence between two distributions (Bishop 2006).

Expanding the tan function in (60) using a Taylor series we can write

$$ \phi_2 = \left( 1 + \frac{1}{12} \phi_1^T \phi_1 + \frac{1}{120} (\phi_1^T \phi_1)^2 + \dots \right) \phi_1. \quad (61) $$

We have only odd powers of $\phi_1$ appearing on the right side of this expression so we immediately see that we want

$$ E[\phi_2] = 0. \qquad (62) $$

This does not involve any approximation. We only need to use that $\phi_1$ follows a Gaussian distribution, which has all its odd moments zero.

For the covariance matrix, $\Sigma_2 = E[\phi_2 \phi_2^T]$, we can insert (61) and truncate after a number of terms to match the required covariance approximately. For example, keeping terms out to quartic in $\phi_1$ we have

$$ \Sigma_2 \approx E[\phi_1 \phi_1^T] + \frac{1}{6} E[\phi_1 \phi_1^T \phi_1 \phi_1^T] \\ = \Sigma_1 + \frac{1}{6} (\text{tr}(\Sigma_1)\mathbf{I} + 2\Sigma_1) \Sigma_1, \quad (63) $$

where we have employed Isserlis' theorem (Barfoot 2024, §2.2.17) to compute the expectation on the right. Thus defining

$$ \phi_2 \sim \mathcal{N} \left( 0, \underbrace{\Sigma_1 + \frac{1}{6} (\text{tr}(\Sigma_1)\mathbf{I} + 2\Sigma_1) \Sigma_1}_{\Sigma_2} \right) \quad (64) $$

will result in $C_1$ and $C_2$ approximately having the same distribution. We will refer to the approximation $\Sigma_2 \approx \Sigma_1$ as 'first order' (in $\Sigma_1$) and the better version in (64) as 'second order' (in $\Sigma_1$). More terms in the approximation could be computed depending on the desired level of accuracy.

To visualize our approach, we can provide a one-dimensional example. Consider $\Sigma_1 = \text{diag}(\sigma_1^2, 0, 0)$, so the axis of rotation, $\mathbf{a} = [1 \ 0 \ 0]^T$, is held fixed. Then the distributions for $\phi_1$ and $\phi_2$ will be

$$ p_1(\phi_1) = \mathcal{N}(0, \sigma_1^2), \quad p_2(\phi_2) = \mathcal{N}(0, \sigma_2^2), \quad (65) $$

<figure><img src="image_15.png" /><figcaption>Figure 13. Comparison of the resulting standard deviation of rotational uncertainty between the exp map (horizontal axis) and cay map (vertical) axis. Red (first order) shows the naive approach of letting $\Sigma_2 = \Sigma_1$ and green (second order) shows the improvement offered by (64).</figcaption></figure>

where either $\sigma_2^2 = \sigma_1^2$ (first order) or $\sigma_2^2 = \sigma_1^2 + \frac{1}{2}\sigma_1^4$ (second order using (64)). If we map $\phi_1$ and $\phi_2$ through exp and cay, respectively, we can produce densities on the actual rotational angle, $\varphi$:

$$ \exp : \quad p_1(\varphi) = \frac{1}{\sqrt{2\pi\sigma_1^2}} \exp\left(-\frac{1}{2}\frac{\varphi^2}{\sigma_1^2}\right), \qquad (66a) $$

$$ \text{cay} : \quad p_2(\varphi) = \frac{1}{\sqrt{2\pi\sigma_2^2}} \exp\left(-\frac{1}{2}\frac{(2\tan(\frac{\varphi}{2}))^2}{\sigma_2^2}\right) \sec^2\left(\frac{\varphi}{2}\right). \quad (66b) $$

We arrive at these by inserting the substitutions from (59) into the distributions in (65). The exp distribution is straightforward since $\phi_1 = \varphi$, which results in a Gaussian. The cay distribution is slightly less obvious so we show the steps. We first note that

$$ \phi_2 = 2 \tan \left( \frac{\varphi}{2} \right) \Rightarrow d\phi_2 = \sec^2 \left( \frac{\varphi}{2} \right) d\varphi. \quad (67) $$

Then, to continue to satisfy the axiom of total probability we require

$$ \begin{aligned} 1 &= \int \underbrace{\frac{1}{\sqrt{2\pi\sigma_2^2}} \exp\left(-\frac{1}{2}\frac{\phi_2^2}{\sigma_2^2}\right)}_{p_2(\phi_2)} d\phi_2 \\ &= \int \underbrace{\frac{1}{\sqrt{2\pi\sigma_2^2}} \exp\left(-\frac{1}{2}\frac{(2\tan(\frac{\phi}{2}))^2}{\sigma_2^2}\right)}_{p_2(\varphi)} \sec^2\left(\frac{\varphi}{2}\right) d\varphi. \end{aligned} \quad (68) $$

We see that $p_2(\varphi)$ is no longer Gaussian in this case. However, the idea is that the variances of $p_1(\varphi)$ and $p_2(\varphi)$ should be close when we compute $\sigma_2$ using (64). Figure 1 provides examples when $\sigma_1 = 0.2$ [rad] and $\sigma_1 = 0.5$ [rad]; despite the latter being a fairly large rotational uncertainty, we see the distributions match quite well; their variances are almost identical.
Figure 13 shows the standard deviation, $\tilde{\sigma}$, of $\varphi$ using the Cayley map distribution (numerically computed from $p_2(\varphi)$) as $\sigma = \sigma_1$ is increased. We see the standard deviations, $\sigma$ and $\tilde{\sigma}$, match quite well up to a large amount of rotational uncertainty. These results suggest that replacing instances of the exponential map with the Cayley map may have little effect; depending on the amount of rotational uncertainty, we may choose to ‘correct’ the covariance using (64) or we could even include more terms to improve the approximation.

## B Local Solvers

As the local solvers are not the main subject of the paper, we use this appendix to explain how these were implemented for each of the estimation problems. We begin by outlining the required Lie-group Jacobians.

### B.1 Jacobians for Rotations and Poses

In order to devise local optimization algorithms that work with Lie groups, we require the (inverse) Jacobians for $SO(3)$ and $SE(3)$. For example, for rotations parameterized through the exponential map, $C(\phi) = \exp(\phi^\wedge)$, the inverse left Jacobian expression is well known to be (Barfoot 2024)

$$ \mathbf{J}_e(\phi)^{-1} = \frac{\phi}{2} \cot \frac{\phi}{2} \mathbf{I} - \frac{\phi}{2} \mathbf{a}^\wedge + \left(1 - \frac{\phi}{2} \cot \frac{\phi}{2}\right) \mathbf{a}\mathbf{a}^T. \quad (69) $$

The rotational kinematics can thus be written in one of two ways:

$$ \dot{\mathbf{C}}(\phi) = \omega^\wedge \mathbf{C}(\phi) \Leftrightarrow \dot{\boldsymbol{\phi}} = \mathbf{J}_e(\phi)^{-1} \omega, \quad (70) $$

where $\omega$ is the angular velocity. Naturally, the expression on the right is subject to singularities. Considering an infinitesimal increment of time, the same Jacobian thus allows us to approximate the compounding of two exponentiated vectors $\phi_1, \phi_2 \in \mathbb{R}^3$ as

$$ \exp(\phi_1^\wedge) \exp(\phi_2^\wedge) \approx \exp \left( \left( \mathbf{J}_e(\phi_2)^{-1} \phi_1 + \phi_2 \right)^\wedge \right), \quad (71) $$

where $\phi_1$ is assumed to be ‘small’. A similar situation exists for $SE(3)$ poses parametrized by the exponential map, $\mathbf{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^\wedge)$. The kinematics can be written in one of two ways:

$$ \dot{\mathbf{T}}(\boldsymbol{\xi}) = \boldsymbol{\omega}^\wedge \mathbf{T}(\boldsymbol{\xi}) \Leftrightarrow \dot{\boldsymbol{\xi}} = \mathcal{J}_e(\boldsymbol{\xi})^{-1} \boldsymbol{\omega}, \quad (72) $$

where $\boldsymbol{\omega}$ is the generalized velocity or ‘twist’ and $\mathcal{J}_e(\boldsymbol{\xi})^{-1}$ is the Jacobian (expression can be found in Barfoot (2024)). Again, this allows us to approximate the compounding of two exponentiated vectors $\boldsymbol{\xi}_1, \boldsymbol{\xi}_2 \in \mathbb{R}^6$ as

$$ \exp(\boldsymbol{\xi}_1^\wedge) \exp(\boldsymbol{\xi}_2^\wedge) \approx \exp \left( \left( \mathcal{J}_e(\boldsymbol{\xi}_2)^{-1} \boldsymbol{\xi}_1 + \boldsymbol{\xi}_2 \right)^\wedge \right), \quad (73) $$

where $\boldsymbol{\xi}_1$ is assumed to be ‘small’.

To work with the Cayley map (instead of the exponential map), we will require similar Jacobian expressions, which are summarized neatly by Müller (2021). In our notation, for rotations parameterized by the Cayley map, $C(\phi) = \text{cay}(\phi^\wedge)$, the inverse left Jacobian expression is

$$ \mathbf{J}_c(\phi)^{-1} = \mathbf{I} - \frac{1}{2}\boldsymbol{\phi}^\wedge + \frac{1}{4}\boldsymbol{\phi}\boldsymbol{\phi}^T, \quad (74) $$

which is appealing in that it does not involve any trigonometric functions compared to (69). In passing, the left Jacobian itself is the tidy expression,

$$ \mathbf{J}_c(\phi) = \frac{1}{1 + \frac{1}{4}\boldsymbol{\phi}^T \boldsymbol{\phi}} \left( \mathbf{I} + \frac{1}{2}\boldsymbol{\phi}^\wedge \right). \quad (75) $$

The rotational kinematics can be written as

$$ \dot{\mathbf{C}}(\phi) = \omega^\wedge \mathbf{C}(\phi) \Leftrightarrow \dot{\boldsymbol{\phi}} = \mathbf{J}_c(\phi)^{-1} \omega, \quad (76) $$

and the compounding approximation for vectors $\phi_1, \phi_2 \in \mathbb{R}^3$ is

$$ \text{cay}(\phi_1^\wedge) \text{cay}(\phi_2^\wedge) \approx \text{cay} \left( \left( \mathbf{J}_c(\phi_2)^{-1} \phi_1 + \phi_2 \right)^\wedge \right), \quad (77) $$

where $\phi_1$ is assumed to be ‘small’. Finally, for poses parameterized by the Cayley map, $\mathbf{T}(\boldsymbol{\xi}) = \text{cay}(\boldsymbol{\xi}^\wedge)$, the kinematics are

$$ \dot{\mathbf{T}}(\boldsymbol{\xi}) = \boldsymbol{\omega}^\wedge \mathbf{T}(\boldsymbol{\xi}) \Leftrightarrow \dot{\boldsymbol{\xi}} = \mathcal{J}_c(\boldsymbol{\xi})^{-1} \boldsymbol{\omega}, \quad (78) $$

where $\boldsymbol{\omega}$ is the generalized velocity or ‘twist’ and $\mathcal{J}_c(\boldsymbol{\xi})^{-1}$ is the inverse Jacobian:

$$ \mathcal{J}_c(\boldsymbol{\xi})^{-1} = \mathbf{I} - \frac{1}{2}\boldsymbol{\xi}^\wedge + \frac{1}{4}\boldsymbol{\Lambda}(\boldsymbol{\xi}\boldsymbol{\xi}^T), \quad (79a) $$

$$ \boldsymbol{\Lambda}(\boldsymbol{\xi}\boldsymbol{\xi}^T) = \begin{bmatrix} 0 & \boldsymbol{\phi}^\wedge \boldsymbol{\rho}^\wedge \\ 0 & \boldsymbol{\phi}\boldsymbol{\phi}^T \end{bmatrix}. \quad (79b) $$

Again, we have a nice expression devoid of any trigonometric functions. The compounding approximation for two vectors $\boldsymbol{\xi}_1, \boldsymbol{\xi}_2 \in \mathbb{R}^6$ is then

$$ \text{cay}(\boldsymbol{\xi}_1^\wedge) \text{cay}(\boldsymbol{\xi}_2^\wedge) \approx \text{cay} \left( \left( \mathcal{J}_c(\boldsymbol{\xi}_2)^{-1} \boldsymbol{\xi}_1 + \boldsymbol{\xi}_2 \right)^\wedge \right), \quad (80) $$

where $\boldsymbol{\xi}_1$ is assumed to be ‘small’.

### B.2 Rotation Averaging

To devise a local solver for (22), we begin with an initial guess, $\mathbf{C}_{\text{op}}$, and perturb it according to

$$ \mathbf{C} = \text{cay}(\boldsymbol{\psi}^\wedge) \mathbf{C}_{\text{op}}, \quad (81) $$

where $\boldsymbol{\psi} \in \mathbb{R}^3$ is a small unknown perturbation. This ensures that $\mathbf{C} \in SO(3)$ during the optimization process.
The error in the cost function is then

$$ \phi_m = \mathrm{cay}^{-1} (\mathbf{C}\tilde{\mathbf{C}}_m^T)^\mathrm{\vee} = \mathrm{cay}^{-1} (\mathrm{cay}(\psi^\mathrm{\wedge})\mathbf{C}_{\mathrm{op}}\tilde{\mathbf{C}}_m^T)^\mathrm{\vee} \\ \approx \mathbf{J}_c(\phi_{\mathrm{op},m})^{-1}\psi + \phi_{\mathrm{op},m}, \quad (82) $$

where we have used (77) to arrive at the approximation and defined $\phi_{\mathrm{op},m} = \mathrm{cay}^{-1} (\mathbf{C}_{\mathrm{op}}\tilde{\mathbf{C}}_m^T)^\mathrm{\vee}$. A quadratic approximation to the cost function in (22) (in terms of the perturbation) is thus

$$ \sum_{m=1}^{M} (\mathbf{J}_c(\phi_{\mathrm{op},m})^{-1}\psi + \phi_{\mathrm{op},m})^T \times \mathbf{W}_m (\mathbf{J}_c(\phi_{\mathrm{op},m})^{-1}\psi + \phi_{\mathrm{op},m}). \quad (83) $$

To minimize this with respect to $\psi$, we take the derivative and set to zero for an optimum, resulting in the linear system of equations

$$ \left( \sum_{m=1}^{M} \mathbf{J}_c(\phi_{\mathrm{op},m})^{-T} \mathbf{W}_m \mathbf{J}_c(\phi_{\mathrm{op},m})^{-1} \right) \psi^* = \sum_{m=1}^{M} \mathbf{J}_c(\phi_{\mathrm{op},m})^{-T} \mathbf{W}_m \phi_{\mathrm{op},m}, \quad (84) $$

which we solve for the optimal perturbation, $\psi^*$. We then apply this optimal perturbation to the initial guess according to (81), and iterate the process to convergence. This is effectively Gauss-Newton adapted to work for SO(3) and will converge locally in practice.

## B.3 Pose Averaging

A very similar local solver can be devised for (28). We begin with an initial guess, $\mathbf{T}_{\mathrm{op}}$, and perturb it according to

$$ \mathbf{T} = \mathrm{cay}(\epsilon^\mathrm{\wedge})\mathbf{T}_{\mathrm{op}}, \quad (85) $$

where $\epsilon \in \mathbb{R}^6$ is a small unknown perturbation. This ensures that $\mathbf{T} \in SE(3)$ during the optimization process. A quadratic approximation to the cost function in (28) (in terms of the perturbation) is thus

$$ \sum_{m=1}^{M} (\mathcal{J}_c(\xi_{\mathrm{op},m})^{-1}\epsilon + \xi_{\mathrm{op},m})^T \times \mathbf{W}_m (\mathcal{J}_c(\xi_{\mathrm{op},m})^{-1}\epsilon + \xi_{\mathrm{op},m}), \quad (86) $$

where $\xi_{\mathrm{op},m} = \mathrm{cay}^{-1} (\mathbf{T}_{\mathrm{op}}\tilde{\mathbf{T}}_m^{-1})^\mathrm{\vee}$. To minimize this with respect to $\epsilon$, we take the derivative and set to zero for an

optimum, resulting in the linear system of equations

$$ \left( \sum_{m=1}^{M} \mathcal{J}_c(\xi_{\mathrm{op},m})^{-T} \mathbf{W}_m \mathcal{J}_c(\xi_{\mathrm{op},m})^{-1} \right) \epsilon^* = \sum_{m=1}^{M} \mathcal{J}_c(\xi_{\mathrm{op},m})^{-T} \mathbf{W}_m \xi_{\mathrm{op},m}, \quad (87) $$

which we solve for the optimal perturbation, $\epsilon^*$. We then apply this optimal perturbation to the initial guess according to (85), and iterate the process to convergence. This is effectively Gauss-Newton adapted to work for SE(3) and will converge locally in practice.

## B.4 Discrete-Time Trajectory Estimation

To devise a local solver for (37), we begin with initial guesses, $\mathbf{T}_{\mathrm{op},k}$, and perturb them according to

$$ \mathbf{T}_k = \mathrm{cay}(\epsilon_k^\mathrm{\wedge})\mathbf{T}_{\mathrm{op},k}, \quad (88) $$

where $\epsilon_k \in \mathbb{R}^6$ are small unknown perturbations. This ensures that $\mathbf{T}_k \in SE(3)$ during the optimization process. A quadratic approximation to the cost function in (37) (in terms of the perturbations) is thus

$$ \sum_{k=1}^{K} (\mathcal{J}_c(\xi_{\mathrm{op},k})^{-1}\epsilon_k + \xi_{\mathrm{op},k})^T \mathbf{W}_k (\mathcal{J}_c(\xi_{\mathrm{op},k})^{-1}\epsilon_k + \xi_{\mathrm{op},k}) + \sum_{k=1}^{K-1} (\mathcal{J}_c(\xi_{\mathrm{op},k+1,k})^{-1}(\epsilon_{k+1} - \mathcal{T}_{\mathrm{op},k+1,k}\epsilon_k) + \xi_{\mathrm{op},k+1,k})^T \times \mathbf{W}_{k+1,k} (\mathcal{J}_c(\xi_{\mathrm{op},k+1,k})^{-1}(\epsilon_{k+1} - \mathcal{T}_{\mathrm{op},k+1,k}\epsilon_k) + \xi_{\mathrm{op},k+1,k}), \quad (89) $$

where $\xi_{\mathrm{op},k} = \mathrm{cay}^{-1} (\mathbf{T}_{\mathrm{op},k}\tilde{\mathbf{T}}_k^{-1})^\mathrm{\vee}$, $\xi_{\mathrm{op},k+1,k} = \mathrm{cay}^{-1} (\mathbf{T}_{\mathrm{op},k+1}\tilde{\mathbf{T}}_{k+1,k}^{-1})^\mathrm{\vee}$, and $\mathcal{T}_{\mathrm{op},k+1,k} = \mathrm{Ad}(\mathbf{T}_{\mathrm{op},k+1}\tilde{\mathbf{T}}_{\mathrm{op},k}^{-1})$. We can tidy this up by defining

$$ \mathbf{H} = \begin{bmatrix} -\mathcal{J}_c(\xi_{\mathrm{op},1})^{-1} & 0 & \cdots & 0 \\ \mathcal{J}_c(\xi_{\mathrm{op},2,1})^{-1}\mathcal{T}_{\mathrm{op},2,1} & -\mathcal{J}_c(\xi_{\mathrm{op},2,1})^{-1} & \cdots & 0 \\ 0 & -\mathcal{J}_c(\xi_{\mathrm{op},2,2})^{-1} & \cdots & 0 \\ 0 & \mathcal{J}_c(\xi_{\mathrm{op},3,2})^{-1}\mathcal{T}_{\mathrm{op},3,2} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & -\mathcal{J}_c(\xi_{\mathrm{op},K})^{-1} \end{bmatrix}, \quad (90a) $$

$$ \xi_{\mathrm{op}} = \begin{bmatrix} \xi_{\mathrm{op},1} \\ \xi_{\mathrm{op},2,1} \\ \xi_{\mathrm{op},2} \\ \xi_{\mathrm{op},3,2} \\ \vdots \\ \xi_{\mathrm{op},K} \end{bmatrix}, \quad \epsilon = \begin{bmatrix} \epsilon_1 \\ \epsilon_2 \\ \vdots \\ \epsilon_K \end{bmatrix}, \quad (90b) $$

$$ \mathbf{W} = \mathrm{diag}(\mathbf{W}_1, \mathbf{W}_{2,1}, \mathbf{W}_{2,2}, \dots, \mathbf{W}_{K,K-1}, \mathbf{W}_K), \quad (90c) $$
whereupon the quadratic approximation to the cost function
can be written as

$$
(\xi_{\text{op}} - \mathbf{H}\epsilon)^{\mathrm{T}} \mathbf{W} (\xi_{\text{op}} - \mathbf{H}\epsilon). \quad (91)
$$

Minimizing this with respect to $\epsilon$ results in a linear system
of equations:

$$
\mathbf{H}^T \mathbf{W} \mathbf{H} \boldsymbol{\epsilon}^* = \mathbf{H}^T \mathbf{W} \boldsymbol{\xi}_{\text{op}}. \quad (92)
$$

We solve for the optimal perturbations, $\epsilon^*$, apply them to
the initial guess according to (88), and iterate the process to
convergence. This is again Gauss-Newton adapted to work
for SE(3) and will converge locally in practice. We found
a line search was needed for this problem to ensure smooth
convergence (Nocedal and Wright 1999).

B.5 Continuous-Time Trajectory Estimation

To devise a local solver for (46), we begin with initial
guesses, {$\mathbf{T}_{\text{op},k}, \boldsymbol{\varpi}_{\text{op},k}$}, and perturb them according to

$$
\mathbf{T}_k = \operatorname{cay}(\epsilon_k^\wedge) \mathbf{T}_{\operatorname{op},k}, \quad \boldsymbol{\varpi} = \boldsymbol{\varpi}_{\operatorname{op},k} + \eta_k, \quad (93)
$$

where $\epsilon_k, \eta_k \in \mathbb{R}^6$ are small unknown perturbations. This
ensures that $\mathbf{T}_k \in SE(3)$ during the optimization process.
A quadratic approximation to the cost function in (46) (in
terms of the perturbations) is thus

$$
\sum_{k=1}^{K} (\xi_{\text{op},k} - \mathbf{G}_k \mathbf{x}_k)^T \mathbf{W}_k (\xi_{\text{op},k} - \mathbf{G}_k \mathbf{x}_k) \\
+ (\tilde{\boldsymbol{\omega}}_1 - \boldsymbol{\omega}_{\text{op},1} - \mathbf{E}_1 \mathbf{x}_1)^T \mathbf{Q}_1^{-1} (\tilde{\boldsymbol{\omega}}_1 - \boldsymbol{\omega}_{\text{op},1} - \mathbf{E}_1 \mathbf{x}_1) \\
+ \sum_{k=1}^{K-1} (\mathbf{e}_{\text{op},k+1,k} + \mathbf{F}_k \mathbf{x}_k - \mathbf{E}_{k+1} \mathbf{x}_{k+1})^T \\
\times \mathbf{Q}_{k+1,k}^{-1} (\mathbf{e}_{\text{op},k+1,k} + \mathbf{F}_k \mathbf{x}_k - \mathbf{E}_{k+1} \mathbf{x}_{k+1}), \quad (94)
$$

where

$$
\mathbf{x}_k = \begin{bmatrix} \epsilon_k \\ \eta_k \end{bmatrix}, \qquad (95a)
$$

$$
\mathbf{e}_{\text{op},k+1,k} = \begin{bmatrix} (t_{k+1} - t_k)\boldsymbol{\varpi}_{\text{op},k} - \boldsymbol{\xi}_{\text{op},k+1,k} \\ \boldsymbol{\varpi}_{\text{op},k} - \boldsymbol{\varpi}_{\text{op},k+1} \end{bmatrix}, \quad (95\text{b})
$$

$$
\mathbf{G} = [-\mathcal{J}_c(\boldsymbol{\xi}_{\text{op},k})^{-1} \mathbf{0}], \quad \mathbf{E}_1 = [\mathbf{0} \quad \mathbf{I}], \quad (95c)
$$

$$
\mathbf{F}_k = \begin{bmatrix} \mathcal{J}_c(\boldsymbol{\xi}_{\text{op},k+1,k})^{-1} \mathcal{T}_{\text{op},k+1,k} & (t_{k+1} - t_k)\mathbf{I} \\ \mathbf{0} & \mathbf{I} \end{bmatrix}, \quad (95\text{d})
$$

$$
\mathbf{E}_{k+1} = \begin{bmatrix} \mathcal{J}_c(\boldsymbol{\xi}_{\text{op},k+1,k})^{-1} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix} \quad (95e)
$$

$$
\begin{align*}
& \text{with } \boldsymbol{\xi}_{\text{op},k} = \text{cay}^{-1} \left( \mathbf{T}_{\text{op},k} \tilde{\mathbf{T}}_k^{-1} \right)^{\vee}, \quad \boldsymbol{\xi}_{\text{op},k+1,k} = \\
& \text{cay}^{-1} \left( \mathbf{T}_{\text{op},k+1} \mathbf{T}_{\text{op},k}^{-1} \right)^{\vee}, \quad \text{and} \quad \mathcal{T}_{\text{op},k+1,k} =
\end{align*}
$$

Ad $(\mathbf{T}_{\text{op},k+1} \mathbf{T}_{\text{op},k}^{-1})$. We can tidy this up by defining
the following$^{‡‡}$

$$
\mathbf{F}^{-1} =
\begin{bmatrix}
\mathbf{E}_1 & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\
-\mathbf{F}_1 & \mathbf{E}_2 & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & -\mathbf{F}_2 & \mathbf{E}_3 & \cdots & \mathbf{0} & \mathbf{0} \\
\vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & -\mathbf{F}_{K-1} & \mathbf{E}_K
\end{bmatrix},
\tag{96a}
$$

$$
\mathbf{Q} = \operatorname{diag}(\mathbf{Q}_1, \mathbf{Q}_{2,1}, \dots, \mathbf{Q}_{K,K-1}), \quad (96b)
$$

$$
\mathbf{G} = \operatorname{diag}(\mathbf{G}_1, \mathbf{G}_2, \dots, \mathbf{G}_K), \quad (96c)
$$

$$
\mathbf{R} = \operatorname{diag}(\mathbf{W}_1^{-1}, \mathbf{W}_2^{-1}, \dots, \mathbf{W}_K^{-1}), \quad (96\text{d})
$$

$$
\mathbf{e}_{\text{op},v} = \begin{bmatrix} \tilde{\boldsymbol{\omega}}_1 - \boldsymbol{\omega}_{\text{op},1} \\ \mathbf{e}_{\text{op},2,1} \\ \vdots \\ \mathbf{e}_{\text{op},K,K-1} \end{bmatrix}, \quad \mathbf{e}_{\text{op},y} = \begin{bmatrix} \boldsymbol{\xi}_{\text{op},1} \\ \boldsymbol{\xi}_{\text{op},2} \\ \vdots \\ \boldsymbol{\xi}_{\text{op},K} \end{bmatrix}, \quad (96\text{e})
$$

$$
\mathbf{x} = \begin{bmatrix} \mathbf{x}_1 \\ \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_K \end{bmatrix}, \qquad (96f)
$$

whereupon the cost function in (46) can be written as

$$
(\mathbf{e}_{\text{op},y} - \mathbf{G}\mathbf{x})^T \mathbf{R}^{-1} (\mathbf{e}_{\text{op},y} - \mathbf{G}\mathbf{x}) \\
+ (\mathbf{e}_{\text{op},v} - \mathbf{F}^{-1}\mathbf{x})^T \mathbf{Q}^{-1} (\mathbf{e}_{\text{op},v} - \mathbf{F}^{-1}\mathbf{x}). \quad (97)
$$

Minimizing this with respect to $\mathbf{x}$ results in a linear system
of equations:

$$
(G^T \mathbf{R}^{-1} \mathbf{G} + \mathbf{F}^{-T} \mathbf{Q}^{-1} \mathbf{F}^{-1}) \mathbf{x}^* \\
= \mathbf{G}^T \mathbf{R}^{-1} \mathbf{e}_{\text{op},y} + \mathbf{F}^{-T} \mathbf{Q}^{-1} \mathbf{e}_{\text{op},v}. \quad (98)
$$

We solve for the optimal perturbations, $\mathbf{x}^*$, unstack into
$\epsilon_k^*$ and $\eta_k^*$, then apply them to the initial guess according
to (93), and iterate the process to convergence. This is
again Gauss-Newton adapted to work for SE(3) and will
converge locally in practice. We found a line search was
needed for this problem to ensure smooth convergence
(Nocedal and Wright 1999). Note, if we do not have pose
measurements at some of the estimation times, we can
simply delete the appropriate rows/columns of $\mathbf{G}$, $\mathbf{R}$, and
$\mathbf{e}_{\text{op},y}$.

<footnote>‡‡The notation was chosen to match the continuous-time estimation
chapter of Barfoot (2024).</footnote>