Certifiably optimal rotation and pose estimation based on the Cayley map

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.

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

$\underset{\mathbf{x}}{\text{min}}\sum _{m=1}^M{\left({\mathbf{y}}_m-\mathbf{g}(\mathbf{x})\right)}^T\left({\mathbf{y}}_m-\mathbf{g}(\mathbf{x})\right),$

(1)

where x is the unknown state, y _m are noisy measurements, and g(⋅) 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. 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) Semi-definite 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 (Krislock and Wolkowicz, 2010; Luo et al., 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; Dellaert et al., 2020; Eriksson et al., 2018, 2021); 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; Carlone et al., 2016; Rosen et al., 2019; Tian et al., 2021), point-set registration (Chaudhury et al., 2015; Iglesias et al., 2020; Yang et al., 2021), 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 (Dümbgen et al., 2023; Papalia et al., 2023).

It is worth also mentioning the works of Forbes and De Ruiter (2015); Horowitz et al. (2014) 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}\text{tr}\left((\mathbf{C}-\overline{\mathbf{C}}){(\mathbf{C}-\overline{\mathbf{C}})}^T\right)\right),$

(2)

where C is a rotation matrix, $\overline{\mathbf{C}}$ is the mode, and σ > 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., Barfoot and Furgale (2014); Long et al. (2013)), where a rotational distribution can have a density of the form

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

(3)

where C is a member of the matrix Lie group SO(3), $\overline{\mathbf{C}}$ is also a member of SO(3) and represents the mean, and Σ is an anisotropic matrix covariance. We also have exp(⋅) as the matrix exponential, ln(⋅) the matrix logarithm, and ∨ 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, e.g., Barfoot and Furgale (2014); Long et al. (2013)). 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 the form

$p\left(\mathbf{C}\right)\propto \text{cay}(-\frac{1}{2}{\text{cay}}^{-1}{\left(\mathbf{C}{\overline{\mathbf{C}}}^T\right)}^{{\vee }^T}{\mathbf{\Sigma }}^{-1}{\text{cay}}^{-1}{\left(\mathbf{C}{\overline{\mathbf{C}}}^T\right)}^{\vee }),$

(4)

where cay(⋅) 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 be 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)=\left\{\mathbf{C}\in {\mathbb{R}}^{3\times 3}|\mathbf{C}{\mathbf{C}}^T=\mathbf{I},\text{det}(\mathbf{C})=1\right\},$

(5)

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

$\mathbf{C}(\mathit{\varphi })=\exp \left({\mathit{\varphi }}^{\wedge }\right),$

(6)

where (⋅)^∧ is the skew-symmetric operator,

${\mathit{\varphi }}^{\wedge }={\left[\begin{array}{c}\hfill {\varphi }_1\hfill \\ \hfill {\varphi }_2\hfill \\ \hfill {\varphi }_3\hfill \end{array}\right]}^{\wedge }=\left[\begin{array}{ccc}\hfill 0\hfill & \hfill -{\varphi }_3\hfill & \hfill {\varphi }_2\hfill \\ \hfill {\varphi }_3\hfill & \hfill 0\hfill & \hfill -{\varphi }_1\hfill \\ \hfill -{\varphi }_2\hfill & \hfill {\varphi }_1\hfill & \hfill 0\hfill \end{array}\right],$

(7)

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

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

$SE(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\hfill \mathbf{C}\hfill & \hfill \mathbf{r}\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 1\hfill \end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{C}\in SO(3),\mathbf{r}\in {\mathbb{R}}^3\right\}.$

(8)

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

$\mathbf{T}(\mathit{\xi })=\exp \left({\mathit{\xi }}^{\wedge }\right),$

(9)

where we have overloaded the ∧ operator as

${\mathit{\xi }}^{\wedge }={\left[\begin{array}{c}\hfill \mathit{\rho }\hfill \\ \hfill \mathit{\varphi }\hfill \end{array}\right]}^{\wedge }=\left[\begin{array}{cc}\hfill {\mathit{\varphi }}^{\wedge }\hfill & \hfill \mathit{\rho }\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 0\hfill \end{array}\right].$

(10)

Notationally, we will use ∨ to mean the inverse operation of ∧. As is common practice (Barfoot, 2024), we have broken the pose vector, ξ , into a translational component, ρ , and a rotational component, ϕ . The mapping is also surjective-only, meaning every T can be produced by many different values for ξ .

Finally, the adjoint of a pose is given by

$\mathcal{T}\left(\mathit{\xi }\right)=\text{Ad}\left(\mathbf{T}\right)=\left[\begin{array}{cc}\mathbf{C}& {\mathbf{r}}^{\wedge }\mathbf{C}\\ \mathbf{0}& \mathbf{C}\end{array}\right],$

(11)

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

$\mathcal{T}\left(\mathit{\xi }\right)=\exp \left({\mathit{\xi }}^{⋏}\right),$

(12)

where

${\mathit{\xi }}^{⋏}={\left[\begin{array}{c}\hfill \mathit{\rho }\hfill \\ \hfill \mathit{\varphi }\hfill \end{array}\right]}^{⋏}=\left[\begin{array}{cc}\hfill {\mathit{\varphi }}^{\wedge }\hfill & \hfill {\mathit{\rho }}^{\wedge }\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\mathit{\varphi }}^{\wedge }\hfill \end{array}\right].$

(13)

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

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, 2003), SE(3) (Barfoot et al., 2022), and Ad(SE(3)) (Bauchau, 2011; Bauchau and Choi, 2003).

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, $\text{cay}(\mathbf{A})={\left(\mathbf{I}-\frac{1}{2}\mathbf{A}\right)}^{-1}\left(\mathbf{I}+\frac{1}{2}\mathbf{A}\right)$ , according to Bauchau and Trainelli (2003); Borri et al. (2000):

$\begin{array}{ll}\hfill \mathbf{C}(\mathit{\varphi })& =\text{cay}({\mathit{\varphi }}^{\wedge })={\left(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}^{\wedge }\right)}^{-1}\left(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}^{\wedge }\right),\hfill \end{array}$

(14a)

$\begin{array}{ll}\hfill \mathit{\varphi }& ={\text{cay}}^{-1}{(\mathbf{C})}^{\vee }={\left(2(\mathbf{C}-\mathbf{I}){(\mathbf{C}+\mathbf{I})}^{-1}\right)}^{\vee },\hfill \end{array}$

(14b)

for some $\mathit{\varphi }=2\tan \frac{\mathrm{\phi }}{2}\mathbf{a}\in {\mathbb{R}}^3$ with φ the rotation angle and 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

$\begin{array}{ll}\hfill \mathbf{T}(\mathit{\xi })& =\text{cay}({\mathit{\xi }}^{\wedge })={\left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}^{\wedge }\right)}^{-1}\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}^{\wedge }\right),\hfill \end{array}$

(15a)

$\begin{array}{ll}\hfill \mathit{\xi }& ={\text{cay}}^{-1}{(\mathbf{T})}^{\vee }={\left(2(\mathbf{T}-\mathbf{I}){(\mathbf{T}+\mathbf{I})}^{-1}\right)}^{\vee },\hfill \end{array}$

(15b)

for some $\mathit{\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

$\begin{array}{ll}\mathcal{T}\left(\mathit{\xi }\right)& =\text{cay}\left({\mathit{\xi }}^{⋏}\right)={\left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}^{⋏}\right)}^{-1}\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}^{⋏}\right),\end{array}$

(16a)

$\begin{array}{ll}\mathit{\xi }& ={\text{cay}}^{-1}{\left(\mathcal{T}\right)}^{⋎}={\left(2(\mathcal{T}-\mathbf{I}){(\mathcal{T}+\mathbf{I})}^{-1}\right)}^{⋎}.\end{array}$

(16b)

However, Selig (2007) demonstrates that starting from the same ξ and applying (15a), (15b), (16a) and (16b) does not result in an equivalent transformation, that is, $\mathcal{T}(\mathit{\xi })\ne \text{Ad}(\mathbf{T}(\mathit{\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.

Figure 1.

Comparison of uncertainty on rotation angle, φ, 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 σ = 0.2 [rad]. (right) σ = 0.5 [rad]. The match is good in both cases with more divergence as rotational uncertainty increases.

The Cayley map has been used in the past for rotation, pose, and trajectory estimation (Alismail et al., 2014; Barfoot et al., 2022; Junkins et al., 2011; Majji et al., 2011; Mortari et al., 2007; Qian et al., 2020; Wong et al., 2018; Wong and Majji, 2016), 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 (2014); Kobilarov and Marsden (2011), 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}{ccc}\hfill \min & f(\mathbf{z})\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{z}\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& g_i(\mathbf{z})=0(\forall i).\hfill & \hfill \end{array}$

(17)

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

$\begin{array}{ccc}\min & {\mathbf{x}}^T\mathbf{Q}\mathbf{x}\hfill & \\ \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{x}\hfill & \\ \mathrm{s}.\mathrm{t}.& {\mathbf{x}}^T{\mathbf{A}}_0\mathbf{x}=1\hfill & \\ & {\mathbf{x}}^T{\mathbf{A}}_i\mathbf{x}=0(\forall i\ne 0).& \end{array}$

(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 X = xx ^T , we rewrite this problem exactly as

$\begin{array}{ccc}\hfill \min & \text{tr}\left(\mathbf{Q}\mathbf{X}\right)\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{X}\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{X}⪰0\hfill & \hfill \\ \hfill & \text{rank}(\mathbf{X})=1\hfill & \hfill \\ \hfill & \text{tr}\left({\mathbf{A}}_0\mathbf{X}\right)=1\hfill & \hfill \\ \hfill & \text{tr}\left({\mathbf{A}}_i\mathbf{X}\right)=0(\forall i\ne 0).\hfill & \hfill \end{array}$

(19)

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

$\begin{array}{ccc}\hfill \min & \text{tr}\left(\mathbf{Q}\mathbf{X}\right)\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{X}\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{X}⪰0\hfill & \hfill \\ \hfill & \text{tr}\left({\mathbf{A}}_0\mathbf{X}\right)=1\hfill & \hfill \\ \hfill & \text{tr}\left({\mathbf{A}}_i\mathbf{X}\right)=0(\forall i\ne 0).\hfill & \hfill \end{array}$

(20)

This is known as Shor’s relaxation (Shor, 1987). If the solution to this problem happens to result in rank(X) = 1, then we have an a posteriori ² guarantee that we also have a global solution to the original problem, 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 rank(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., 2021; Yang and Carlone, 2023; Giamou et al., 2019; Wang et al., 2022b) and machine vision (Briales et al., 2018; Briales and Gonzalez-Jimenez, 2017b; Garcia-Salguero et al., 2022; Kezurer et al., 2015) literature. With redundant constraints, our problem becomes

$\begin{array}{ccc}\hfill \min & \text{tr}\left(\mathbf{Q}\mathbf{X}\right)\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{X}\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{X}⪰0\hfill & \hfill \\ \hfill & \text{tr}\left({\mathbf{A}}_0\mathbf{X}\right)=1\hfill & \hfill \\ \hfill & \text{tr}\left({\mathbf{A}}_i\mathbf{X}\right)=0(\forall i\ne 0)\hfill & \hfill \\ \hfill (\mathrm{r}\mathrm{e}\mathrm{d}\mathrm{u}\mathrm{n}\mathrm{d}\mathrm{a}\mathrm{n}\mathrm{t})& \text{tr}\left({\mathbf{B}}_j\mathbf{X}\right)=0(\forall j\ne 0),\hfill & \hfill \end{array}$

(21)

where the 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 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.

3.1. Rotation averaging

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

$\begin{array}{ccc}\hfill \min & \sum _{m=1}^M\ln {\left(\mathbf{C}{\tilde{\mathbf{C}}}_m^T\right)}^{{\vee }^T}{\mathbf{W}}_m\ln {\left(\mathbf{C}{\tilde{\mathbf{C}}}_m^T\right)}^{\vee }\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{C}\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{C}\in SO(3),\hfill & \hfill \end{array}$

(22)

where ${\tilde{\mathbf{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{\mathbf{C}}}_m=\exp \left({\mathit{\varphi }}_m^{\wedge }\right)\mathbf{C},{\mathit{\varphi }}_m\sim \mathcal{N}(\mathbf{0},{\mathbf{W}}_m^{-1}).$

(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 map without too much effect on the stated problem (see Figure 1). With this substitution, our generative model for noisy rotations becomes

${\tilde{\mathbf{C}}}_m=\text{cay}\left({\mathit{\varphi }}_m^{\wedge }\right)\mathbf{C},{\mathit{\varphi }}_m\sim \mathcal{N}(\mathbf{0},{\mathbf{W}}_m^{-1}),$

(24)

and so our optimization problem can be restated as

$\begin{array}{cc}\hfill \min & \sum _{m=1}^M{\text{cay}}^{-1}{\left(\mathbf{C}{\tilde{\mathbf{C}}}_m^T\right)}^{{\vee }^T}{\mathbf{W}}_m{\text{cay}}^{-1}{\left(\mathbf{C}{\tilde{\mathbf{C}}}_m^T\right)}^{\vee }\hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{C}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{C}\in SO(3).\hfill \end{array}$

(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}{ccc}\hfill \min & \sum _{m=1}^M{\mathit{\varphi }}_m^T{\mathbf{W}}_m{\mathit{\varphi }}_m\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_1,{\mathbf{c}}_2,{\mathbf{c}}_3,{\mathit{\varphi }}_m(\forall m)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_i^T{\mathbf{c}}_j={\delta }_{ij}(\forall i,j)\hfill & \hfill \\ \hfill & \left(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }\right){\mathbf{c}}_i=\left(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }\right){\tilde{\mathbf{c}}}_{m,i}(\forall i,m),\hfill & \hfill \end{array}$

(26)

where δ _ij is the Kronecker delta and

$\mathbf{C}=\left[\begin{array}{ccc}\hfill {\mathbf{c}}_1\hfill & \hfill {\mathbf{c}}_2\hfill & \hfill {\mathbf{c}}_3\hfill \end{array}\right],{\tilde{\mathbf{C}}}_m=\left[\begin{array}{ccc}\hfill {\tilde{\mathbf{c}}}_{m,1}\hfill & \hfill {\tilde{\mathbf{c}}}_{m,2}\hfill & \hfill {\tilde{\mathbf{c}}}_{m,3}\hfill \end{array}\right].$

(27)

We have essentially introduced variables, ϕ _m , for the residual errors of each term in the cost and used these to connect C to each ${\tilde{\mathbf{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 ϕ _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 ⁵ ; our optimization then only guarantees C ∈ 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 ⁶ if a good initial guess is given to the local solver. Figure 2 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 3 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 σ 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.

Figure 2.

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).

Figure 3.

Rotation Averaging: A quantitative evaluation of the tightness of the rotation averaging problem with increasing measurement noise level, σ. 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 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.

3.2. Pose averaging

Pose averaging follows a very similar approach to the previous section. ⁴ An optimization problem based on the Cayley map can be stated as

$\begin{array}{ccc}\hfill \min & \sum _{m=1}^M{\text{cay}}^{-1}{\left(\mathbf{T}{\tilde{\mathbf{T}}}_m^{-1}\right)}^{{\vee }^T}{\mathbf{W}}_m{\text{cay}}^{-1}{\left(\mathbf{T}{\tilde{\mathbf{T}}}_m^{-1}\right)}^{\vee }\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{T}\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{T}\in SE(3),\hfill & \hfill \end{array}$

(28)

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

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

$\begin{array}{ccc}\hfill \min & \sum _{m=1}^M{\mathit{\xi }}_m^T{\mathbf{W}}_m{\mathit{\xi }}_m\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& \mathbf{T},{\mathit{\xi }}_m(\forall m)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{C}}^T\mathbf{C}=\mathbf{I}\hfill & \hfill \\ \hfill & \left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}_m^{\wedge }\right)\mathbf{T}=\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}_m^{\wedge }\right){\tilde{\mathbf{T}}}_m(\forall m),\hfill & \hfill \end{array}$

(29)

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

$\begin{array}{ll}\mathbf{T}& =\left[\begin{array}{cc}\mathbf{C}& \mathbf{r}\\ {\mathbf{0}}^T& 1\end{array}\right]=\left[\begin{array}{cccc}{\mathbf{c}}_1& {\mathbf{c}}_2& {\mathbf{c}}_3& \mathbf{r}\\ 0& 0& 0& 1\end{array}\right],\end{array}$

(30a)

$\begin{array}{ll}{\tilde{\mathbf{T}}}_m& =\left[\begin{array}{cc}{\tilde{\mathbf{C}}}_m& {\mathbf{r}}_m\\ {\mathbf{0}}^T& 1\end{array}\right]=\left[\begin{array}{cccc}{\tilde{\mathbf{c}}}_{m,1}& {\tilde{\mathbf{c}}}_{m,2}& {\tilde{\mathbf{c}}}_{m,2}& {\tilde{\mathbf{r}}}_m\\ 0& 0& 0& 1\end{array}\right],\end{array}$

(30b)

$\begin{array}{ll}\hfill {\mathit{\xi }}_m& =\left[\begin{array}{c}\hfill {\mathit{\rho }}_m\hfill \\ \hfill {\mathit{\varphi }}_m\hfill \end{array}\right],\hfill \end{array}$

(30c)

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

$\begin{array}{ccc}\hfill \min & \sum _{m=1}^M{\mathit{\xi }}_m^T{\mathbf{W}}_m{\mathit{\xi }}_m\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_i,\mathbf{r},{\mathit{\rho }}_m,{\mathit{\varphi }}_m(\forall i,m)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_i^T{\mathbf{c}}_j={\delta }_{ij}(\forall i,j)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }){\mathbf{c}}_i=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }){\tilde{\mathbf{c}}}_{m,i}(\forall i,m)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_m^{\wedge })\mathbf{r}=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }){\tilde{\mathbf{r}}}_m+{\mathit{\rho }}_m(\forall m).\hfill & \hfill \end{array}$

(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 r ^T whereupon

${\mathbf{r}}^T\mathbf{r}-\frac{1}{2}\underset{\mathbf{0}}{\underbrace{{\mathbf{r}}^T{\mathit{\varphi }}_m^{\wedge }\mathbf{r}}}={\mathbf{r}}^T{\tilde{\mathbf{r}}}_m-\frac{1}{2}{\mathbf{r}}^T{\tilde{\mathbf{r}}}_m^{\wedge }{\mathit{\varphi }}_m+{\mathbf{r}}^T{\mathit{\rho }}_{m.}$

(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, 2023). 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{\mathit{\varphi }}_m^{\wedge }=-{\left({\mathbf{c}}_i-{\tilde{\mathbf{c}}}_{m,i}\right)}^T,$

(33)

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

$\begin{array}{l}\hfill {({\mathbf{c}}_i+{\tilde{\mathbf{c}}}_{m,i})}^T(\mathbf{r}-{\tilde{\mathbf{r}}}_m)=\underset{-{({\mathbf{c}}_i-{\tilde{\mathbf{c}}}_{m,i})}^T}{\underbrace{\frac{1}{2}{({\mathbf{c}}_i+{\tilde{\mathbf{c}}}_{m,i})}^T{\mathit{\varphi }}_m^{\wedge }}}(\mathbf{r}+{\tilde{\mathbf{r}}}_m)\\ \hfill +{({\mathbf{c}}_i+{\tilde{\mathbf{c}}}_{m,i})}^T{\mathit{\rho }}_m.\end{array}$

(34)

After performing the indicated substitution, this becomes

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

(35)

which is once again a quadratic constraint.

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

$\begin{array}{ccc}\hfill \min & \sum _{m=1}^M{\mathit{\xi }}_m^T{\mathbf{W}}_m{\mathit{\xi }}_m\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_i,\mathbf{r},{\mathit{\rho }}_m,{\mathit{\varphi }}_m(\forall i,m)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_i^T{\mathbf{c}}_j={\delta }_{ij}(\forall i,j)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }){\mathbf{c}}_i=(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }){\tilde{\mathbf{c}}}_{m,i}(\forall i,m)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_m^{\wedge })\mathbf{r}=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_m^{\wedge }){\tilde{\mathbf{r}}}_m+{\mathit{\rho }}_m(\forall m)\hfill & \hfill \\ \hfill (\mathrm{r}\mathrm{e}\mathrm{d}.)& \frac{1}{2}{({\mathbf{c}}_i+{\tilde{\mathbf{c}}}_{m,i})}^T{\mathit{\rho }}_m={\mathbf{c}}_i^T\mathbf{r}-{\tilde{\mathbf{c}}}_{m,i}^T{\tilde{\mathbf{r}}}_m(\forall i,m)\hfill & \hfill \\ \hfill & {\mathbf{r}}^T\mathbf{r}={\mathbf{r}}^T{\tilde{\mathbf{r}}}_m-\frac{1}{2}{\mathbf{r}}^T{\tilde{\mathbf{r}}}_m^{\wedge }{\mathit{\varphi }}_m+{\mathbf{r}}^T{\mathit{\rho }}_m(\forall m).\hfill & \hfill \end{array}$

(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 σ 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.

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.

Figure 5.

Pose Averaging: A quantitative evaluation of the tightness of the pose averaging problem with increasing measurement noise level, σ. 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.

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 we add, the larger the noise region for which we can a priori predict that we will achieve rank-1 SDP solutions.

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.

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{\mathbf{T}}}_k$ , as well as noisy relative measurements, ${\tilde{\mathbf{T}}}_{k+1,k}$ , from one pose to the next. The optimization problem that we want to solve is

$\begin{array}{ccc}\hfill \min & \sum _{k=1}^K{\text{cay}}^{-1}{\left({\mathbf{T}}_k{\tilde{\mathbf{T}}}_k^{-1}\right)}^{{\vee }^T}{\mathbf{W}}_k{\text{cay}}^{-1}{\left({\mathbf{T}}_k{\tilde{\mathbf{T}}}_k^{-1}\right)}^{\vee }\hfill & \hfill \\ \hfill & +\sum _{k=1}^{K-1}{\text{cay}}^{-1}{\left({\mathbf{T}}_{k+1}{\mathbf{T}}_k^{-1}{\tilde{\mathbf{T}}}_{k+1,k}^{-1}\right)}^{{\vee }^T}\hfill & \hfill \\ \hfill & \times {\mathbf{W}}_{k+1,k}{\text{cay}}^{-1}{\left({\mathbf{T}}_{k+1}{\mathbf{T}}_k^{-1}{\tilde{\mathbf{T}}}_{k+1,k}^{-1}\right)}^{\vee }\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{T}}_k(\forall k)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{T}}_k\in SE(3)(\forall k),\hfill & \hfill \end{array}$

(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:

$\begin{array}{ll}\hfill {\mathit{\xi }}_k& ={\text{cay}}^{-1}{\left({\mathbf{T}}_k{\tilde{\mathbf{T}}}_k^{-1}\right)}^{\vee },\hfill \end{array}$

(38a)

$\begin{array}{ll}\hfill {\mathit{\xi }}_{k+1,k}& ={\text{cay}}^{-1}{\left({\mathbf{T}}_{k+1}{\mathbf{T}}_k^{-1}{\tilde{\mathbf{T}}}_{k+1,k}^{-1}\right)}^{\vee },\hfill \end{array}$

(38b)

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).

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

$\begin{array}{ccc}\hfill \min & \sum _{k=1}^K{\mathit{\xi }}_k^T{\mathbf{W}}_k{\mathit{\xi }}_k+\sum _{k=1}^{K-1}{\mathit{\xi }}_{k+1,k}^T{\mathbf{W}}_{k+1,k}{\mathit{\xi }}_{k+1,k}\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{T}}_k,{\mathit{\xi }}_k,{\mathit{\xi }}_{k+1,k}(\forall k)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{C}}_k^T{\mathbf{C}}_k=\mathbf{I}(\forall k)\hfill & \hfill \\ \hfill & \left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}_k^{\wedge }\right){\mathbf{T}}_k=\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}_k^{\wedge }\right){\tilde{\mathbf{T}}}_k(\forall k)\hfill & \hfill \\ \hfill & \left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}_{k+1,k}^{\wedge }\right){\mathbf{T}}_{k+1}\hfill & \hfill \\ \hfill & =\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}_{k+1,k}^{\wedge }\right){\tilde{\mathbf{T}}}_{k+1,k}{\mathbf{T}}_k(\forall k),\hfill & \hfill \end{array}$

(39)

where the det(C _k ) = 1 constraints have been dropped. Decomposing the matrices according to

$\begin{array}{l}\hfill {\mathbf{T}}_k=\left[\begin{array}{cc}\hfill {\mathbf{C}}_k\hfill & \hfill {\mathbf{r}}_k\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 1\hfill \end{array}\right]=\left[\begin{array}{cccc}\hfill {\mathbf{c}}_{k,1}\hfill & \hfill {\mathbf{c}}_{k,2}\hfill & \hfill {\mathbf{c}}_{k,3}\hfill & \hfill {\mathbf{r}}_k\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right],\end{array}$

(40a)

$\begin{array}{l}\hfill {\tilde{\mathbf{T}}}_k=\left[\begin{array}{cc}\hfill {\tilde{\mathbf{C}}}_k\hfill & \hfill {\tilde{\mathbf{r}}}_k\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 1\hfill \end{array}\right]=\left[\begin{array}{cccc}\hfill {\tilde{\mathbf{c}}}_{k,1}\hfill & \hfill {\tilde{\mathbf{c}}}_{k,2}\hfill & \hfill {\tilde{\mathbf{c}}}_{k,2}\hfill & \hfill {\tilde{\mathbf{r}}}_k\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right],\end{array}$

(40b)

$\begin{array}{l}\hfill {\tilde{\mathbf{T}}}_{k+1,k}=\left[\begin{array}{cc}\hfill {\tilde{\mathbf{C}}}_{k+1,k}\hfill & \hfill {\tilde{\mathbf{r}}}_{k+1,k}\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 1\hfill \end{array}\right]\end{array}$

(40c)

$\begin{array}{l}\hfill =\left[\begin{array}{cccc}\hfill {\tilde{\mathbf{c}}}_{k+1,k,1}\hfill & \hfill {\tilde{\mathbf{c}}}_{k+1,k,2}\hfill & \hfill {\tilde{\mathbf{c}}}_{k+1,k,2}\hfill & \hfill {\tilde{\mathbf{r}}}_{k+1,k}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right],\end{array}$

(40d)

$\begin{array}{l}\hfill {\mathit{\xi }}_k=\left[\begin{array}{c}\hfill {\mathit{\rho }}_k\hfill \\ \hfill {\mathit{\varphi }}_k\hfill \end{array}\right],{\mathit{\xi }}_{k+1,k}=\left[\begin{array}{c}\hfill {\mathit{\rho }}_{k+1,k}\hfill \\ \hfill {\mathit{\varphi }}_{k+1,k}\hfill \end{array}\right],\end{array}$

(40e)

the QCQP optimization problem can be rewritten compactly as

$\begin{array}{ccc}\hfill \min & \sum _{k=1}^K{\mathit{\xi }}_k^T{\mathbf{W}}_k{\mathit{\xi }}_k+\sum _{k=1}^{K-1}{\mathit{\xi }}_{k+1,k}^T{\mathbf{W}}_{k+1,k}{\mathit{\xi }}_{k+1,k}\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_{k,i},{\mathbf{r}}_k,{\mathit{\rho }}_k,{\mathit{\varphi }}_k,{\mathit{\rho }}_{k+1,k},{\mathit{\varphi }}_{k+1,k}(\forall i,k)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_{k,i}^T{\mathbf{c}}_{k,j}={\delta }_{ij}(\forall i,j,k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{c}}_{k,i}=(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{c}}}_{k,i}(\forall i,k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{r}}_k=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{r}}}_k+{\mathit{\rho }}_k(\forall k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{c}}_{k+1,i}\hfill & \hfill \\ \hfill & =(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i}(\forall i,k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{r}}_{k+1}\hfill & \hfill \\ \hfill & =(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge })\left({\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{r}}_k+{\tilde{\mathbf{r}}}_{k+1,k}\right)\hfill & \hfill \\ \hfill & +{\mathit{\rho }}_{k+1,k}(\forall k).\hfill & \hfill \end{array}$

(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 ξ _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 ξ _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

$\begin{array}{l}\hfill \frac{1}{2}{({\mathbf{c}}_{k+1,i}+{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i})}^T{\mathit{\varphi }}_{k+1,k}^{\wedge }\\ \hfill =-{\left({\mathbf{c}}_{k+1,i}-{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i}\right)}^T,\end{array}$

(42)

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

$\begin{array}{l}\hfill {({\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})\\ \hfill =\underset{-{({\mathbf{c}}_{k+1,i}-{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i})}^T}{\underbrace{\frac{1}{2}{({\mathbf{c}}_{k+1,i}+{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i})}^T{\mathit{\varphi }}_{k+1,k}^{\wedge }}}\\ \hfill \times ({\mathbf{r}}_{k+1}+{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{r}}_k+{\tilde{\mathbf{r}}}_{k+1,k})\\ \hfill +{({\mathbf{c}}_{k+1,i}+{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i})}^T{\mathit{\rho }}_{k+1,k}.\end{array}$

(43)

After performing the indicated substitution, this becomes

$\begin{array}{l}\hfill \frac{1}{2}{({\mathbf{c}}_{k+1,i}+{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i})}^T{\mathit{\rho }}_{k+1,k}\\ \hfill ={\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}),\end{array}$

(44)

which is once again a quadratic constraint.

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

$\begin{array}{ccc}\hfill \min & \sum _{k=1}^K{\mathit{\xi }}_k^T{\mathbf{W}}_k{\mathit{\xi }}_k+\sum _{k=1}^{K-1}{\mathit{\xi }}_{k+1,k}^T{\mathbf{W}}_{k+1,k}{\mathit{\xi }}_{k+1,k}\hfill & \hfill \\ \hfill \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_{k,i},{\mathbf{r}}_k,{\mathit{\rho }}_k,{\mathit{\varphi }}_k,{\mathit{\rho }}_{k+1,k},{\mathit{\varphi }}_{k+1,k}(\forall i,k)\hfill & \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_{k,i}^T{\mathbf{c}}_{k,j}={\delta }_{ij}(\forall i,j,k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{c}}_{k,i}=(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{c}}}_{k,i}(\forall i,k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{r}}_k=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{r}}}_k+{\mathit{\rho }}_k(\forall k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{c}}_{k+1,i}\hfill & \hfill \\ \hfill & =(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i}(\forall i,k)\hfill & \hfill \\ \hfill & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{r}}_{k+1}\hfill & \hfill \\ \hfill & =(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge })\left({\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{r}}_k+{\tilde{\mathbf{r}}}_{k+1,k}\right)\hfill & \hfill \\ \hfill & +{\mathit{\rho }}_{k+1,k}(\forall k)\hfill & \hfill \\ \hfill (\mathrm{r}\mathrm{e}\mathrm{d}.)& \frac{1}{2}{({\mathbf{c}}_{k,i}+{\tilde{\mathbf{c}}}_{k,i})}^T{\mathit{\rho }}_k={\mathbf{c}}_{k,i}^T{\mathbf{r}}_k-{\tilde{\mathbf{c}}}_{k,i}^T{\tilde{\mathbf{r}}}_k(\forall i,k)\hfill & \hfill \\ \hfill & {\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 }{\mathit{\varphi }}_k+{\mathbf{r}}_k^T{\mathit{\rho }}_k(\forall k)\hfill & \hfill \\ \hfill & \frac{1}{2}{({\mathbf{c}}_{k+1,i}+{\tilde{\mathbf{C}}}_{k+1,k}{\mathbf{c}}_{k,i})}^T{\mathit{\rho }}_{k+1,k}\hfill & \hfill \\ \hfill & ={\mathbf{c}}_{k+1,i}^T{\mathbf{r}}_{k+1}\hfill & \hfill \\ \hfill & -{\mathbf{c}}_{k,i}^T({\mathbf{r}}_k+{\tilde{\mathbf{C}}}_{k+1,k}^T{\tilde{\mathbf{r}}}_{k+1,k})(\forall i,k).\hfill & \hfill \end{array}$

(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 σ 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.

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.

Figure 9.

Discrete-time Trajectory Estimation: A quantitative evaluation of the tightness of the discrete-time trajectory estimation problem with increasing measurement noise, σ. 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.

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 (Anderson and Barfoot, 2015; Barfoot et al., 2014) 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), $\left\{{\mathbf{T}}_k,{\mathit{\varpi }}_k\right\}$ . Figure 10 depicts the situation as a factor graph. In practice, we may not actually have pose measurements at every time at which we introduce a state variable.

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).

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

$\begin{array}{ccc}\min & \sum _{k=1}^K{\text{cay}}^{-1}{\left({\mathbf{T}}_k{\tilde{\mathbf{T}}}_k^{-1}\right)}^{{\vee }^T}{\mathbf{W}}_k{\text{cay}}^{-1}{\left({\mathbf{T}}_k{\tilde{\mathbf{T}}}_k^{-1}\right)}^{\vee }\hfill & \\ & +{({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)}^T{\mathbf{Q}}_1^{-1}({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)& \\ & +\sum _{k=1}^{K-1}{\mathbf{e}}_{k+1,k}^T{\mathbf{Q}}_{k+1,k}^{-1}{\mathbf{e}}_{k+1,k}& \\ \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{T}}_k,{\mathit{\varpi }}_k(\forall k)\hfill & \\ \mathrm{s}.\mathrm{t}.& {\mathbf{T}}_k\in SE\left(3\right)(\forall k),\hfill & \end{array}$

(46)

for some weight matrices, W _k , and

$\begin{array}{l}{\mathbf{e}}_{k+1,k}=\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\text{cay}}^{-1}{\left({\mathbf{T}}_{k+1}{\mathbf{T}}_k^{-1}\right)}^{\vee }\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right],\end{array}$

(47a)

$\begin{array}{l}\hfill {\mathbf{Q}}_{k+1,k}=\left[\begin{array}{cc}\hfill \frac{1}{3}{(t_{k+1}-t_k)}^3{\mathbf{Q}}_c\hfill & \hfill \frac{1}{2}{(t_{k+1}-t_k)}^2{\mathbf{Q}}_c\hfill \\ \hfill \frac{1}{2}{(t_{k+1}-t_k)}^2{\mathbf{Q}}_c\hfill & \hfill (t_{k+1}-t_k){\mathbf{Q}}_c\hfill \end{array}\right].\end{array}$

(47b)

The t _k are known timestamps of the states, Q _c is a power-spectral density matrix affecting smoothness of the GP prior, and ${\check{\mathit{\varpi }}}_1$ together with Q₁ 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 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, Q_k+1,k, comes from integrating the WNOA prior over the same time interval (Barfoot, 2024; Barfoot et al., 2014).

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

$\begin{array}{ll}\hfill {\mathit{\xi }}_k& ={\text{cay}}^{-1}{\left({\mathbf{T}}_k{\tilde{\mathbf{T}}}_k^{-1}\right)}^{\vee },\hfill \end{array}$

(48a)

$\begin{array}{ll}\hfill {\mathit{\xi }}_{k+1,k}& ={\text{cay}}^{-1}{\left({\mathbf{T}}_{k+1}{\mathbf{T}}_k^{-1}\right)}^{\vee },\hfill \end{array}$

(48b)

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

$\begin{array}{ccc}\min & \sum _{k=1}^K{\mathit{\xi }}_k^T{\mathbf{W}}_k{\mathit{\xi }}_k+{({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)}^T{\mathbf{Q}}_1^{-1}({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)\hfill & \\ & +\sum _{k=1}^{K-1}{\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\mathit{\xi }}_{k+1,k}\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right]}^T& \\ & \times {\mathbf{Q}}_{k+1,k}^{-1}\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\mathit{\xi }}_{k+1,k}\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right]& \\ \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{T}}_k,{\mathit{\varpi }}_k,{\mathit{\xi }}_k,{\mathit{\xi }}_{k+1,k}(\forall k)\hfill & \\ \mathrm{s}.\mathrm{t}.& {\mathbf{C}}_k^T{\mathbf{C}}_k=\mathbf{I}(\forall k)\hfill & \\ & \left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}_k^{\wedge }\right){\mathbf{T}}_k=\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}_k^{\wedge }\right){\tilde{\mathbf{T}}}_k(\forall k)\hfill & \\ & \left(\mathbf{I}-\frac{1}{2}{\mathit{\xi }}_{k+1,k}^{\wedge }\right){\mathbf{T}}_{k+1}=\left(\mathbf{I}+\frac{1}{2}{\mathit{\xi }}_{k+1,k}^{\wedge }\right){\mathbf{T}}_k(\forall k),\hfill & \end{array}$

(49)

where the det(C _k ) = 1 constraints have been dropped. Decomposing the matrices according to

$\begin{array}{l}\hfill {\mathbf{T}}_k=\left[\begin{array}{cc}\hfill {\mathbf{C}}_k\hfill & \hfill {\mathbf{r}}_k\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 1\hfill \end{array}\right]=\left[\begin{array}{cccc}\hfill {\mathbf{c}}_{k,1}\hfill & \hfill {\mathbf{c}}_{k,2}\hfill & \hfill {\mathbf{c}}_{k,3}\hfill & \hfill {\mathbf{r}}_k\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right],\end{array}$

(50a)

$\begin{array}{l}\hfill {\tilde{\mathbf{T}}}_k=\left[\begin{array}{cc}\hfill {\tilde{\mathbf{C}}}_k\hfill & \hfill {\tilde{\mathbf{r}}}_k\hfill \\ \hfill {\mathbf{0}}^T\hfill & \hfill 1\hfill \end{array}\right]=\left[\begin{array}{cccc}\hfill {\tilde{\mathbf{c}}}_{k,1}\hfill & \hfill {\tilde{\mathbf{c}}}_{k,2}\hfill & \hfill {\tilde{\mathbf{c}}}_{k,2}\hfill & \hfill {\tilde{\mathbf{r}}}_k\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right],\end{array}$

(50b)

$\begin{array}{l}\hfill {\mathit{\xi }}_k=\left[\begin{array}{c}\hfill {\mathit{\rho }}_k\hfill \\ \hfill {\mathit{\varphi }}_k\hfill \end{array}\right],{\mathit{\xi }}_{k+1,k}=\left[\begin{array}{c}\hfill {\mathit{\rho }}_{k+1,k}\hfill \\ \hfill {\mathit{\varphi }}_{k+1,k}\hfill \end{array}\right],\end{array}$

(50c)

the QCQP optimization problem can be rewritten compactly as

$\begin{array}{ccc}\min & \sum _{k=1}^K{\mathit{\xi }}_k^T{\mathbf{W}}_k{\mathit{\xi }}_k+{({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)}^T{\mathbf{Q}}_1^{-1}({\stackrel{\mathbf{ˇ}}{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)\hfill & \\ & +\sum _{k=1}^{K-1}{\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\mathit{\xi }}_{k+1,k}\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right]}^T& \\ & \times {\mathbf{Q}}_{k+1,k}^{-1}\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\mathit{\xi }}_{k+1,k}\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right]& \\ \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_{k,i},{\mathbf{r}}_k,{\mathit{\rho }}_k,{\mathit{\varphi }}_k,{\mathit{\rho }}_{k+1,k},{\mathit{\varphi }}_{k+1,k},{\mathit{\varpi }}_k(\forall i,k)\hfill & \\ \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_{k,i}^T{\mathbf{c}}_{k,j}={\delta }_{ij}(\forall i,j,k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{c}}_{k,i}=(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{c}}}_{k,i}(\forall i,k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{r}}_k=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{r}}}_k+{\mathit{\rho }}_k(\forall k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{c}}_{k+1,i}\hfill & \\ & =(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{c}}_{k,i}(\forall i,k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{r}}_{k+1}\hfill & \\ & =(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{r}}_k+{\mathit{\rho }}_{k+1,k}(\forall k).\hfill & \end{array}$

(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 ξ _k and ξ _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 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 ${\mathit{\varphi }}_k^T$ so that

$\begin{array}{l}\hfill {\mathit{\varphi }}_k^T\left(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }\right){\mathbf{c}}_{k,i}={\mathit{\varphi }}_k^T\left(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }\right){\tilde{\mathbf{c}}}_{k,i}\\ \hfill \Rightarrow {\mathit{\varphi }}_k^T{\mathbf{c}}_{k,i}={\mathit{\varphi }}_k^T{\tilde{\mathbf{c}}}_{k,i},\end{array}$

(52)

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

${\mathit{\varphi }}_k^T{\mathbf{r}}_k={\mathit{\varphi }}_k^T{\tilde{\mathbf{r}}}_k+{\mathit{\varphi }}_k^T{\mathit{\rho }}_k.$

(53)

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

$\begin{array}{l}\hfill {\mathit{\varphi }}_{k+1,k}^T{\mathbf{c}}_{k+1,i}={\mathit{\varphi }}_{k+1,k}^T{\mathbf{c}}_{k,i},\end{array}$

(54a)

$\begin{array}{l}\hfill {\mathit{\varphi }}_{k+1,k}^T{\mathbf{r}}_{k+1}={\mathit{\varphi }}_{k+1,k}^T{\mathbf{r}}_k+{\mathit{\varphi }}_{k+1,k}^T{\mathit{\rho }}_{k+1,k}.\end{array}$

(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 \left\{\mathrm{123,231,312}\right\}$ . If we premultiply the second constraint of (51) by ${\mathbf{c}}_{k,m}^T$ we have

$\begin{array}{l}\hfill {\mathbf{c}}_{k,m}^T\left(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }\right){\mathbf{c}}_{k,\ell }={\mathbf{c}}_{k,m}^T\left(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }\right){\tilde{\mathbf{c}}}_{k,\ell }\\ \hfill \Rightarrow {\mathbf{c}}_{k,m}^T{\mathbf{c}}_{k,\ell }-\frac{1}{2}{\mathit{\varphi }}_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 }{\mathit{\varphi }}_k,\end{array}$

(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{\mathit{\rho }}_{k+1,k}+{\mathbf{r}}_k^T{\mathit{\rho }}_{k+1,k}+{\mathbf{r}}_k^T{\mathbf{r}}_{k,}$

(56)

which is once again a quadratic constraint.

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

$\begin{array}{ccc}\min & \sum _{k=1}^K{\mathit{\xi }}_k^T{\mathbf{W}}_k{\mathit{\xi }}_k+{({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)}^T{\mathbf{Q}}_1^{-1}({\check{\mathit{\varpi }}}_1-{\mathit{\varpi }}_1)\hfill & \\ & +\sum _{k=1}^{K-1}{\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\mathit{\xi }}_{k+1,k}\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right]}^T\hfill & \\ & \times {\mathbf{Q}}_{k+1,k}^{-1}\left[\begin{array}{c}(t_{k+1}-t_k){\mathit{\varpi }}_k-{\mathit{\xi }}_{k+1,k}\\ {\mathit{\varpi }}_k-{\mathit{\varpi }}_{k+1}\end{array}\right]\hfill & \\ \mathrm{w}.\mathrm{r}.\mathrm{t}.& {\mathbf{c}}_{k,i},{\mathbf{r}}_k,{\mathit{\rho }}_k,{\mathit{\varphi }}_k,{\mathit{\rho }}_{k+1,k},{\mathit{\varphi }}_{k+1,k},{\mathit{\varpi }}_k(\forall i,k)\hfill & \\ \mathrm{s}.\mathrm{t}.& {\mathbf{c}}_{k,i}^T{\mathbf{c}}_{k,j}={\delta }_{ij}(\forall i,j,k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{c}}_{k,i}=(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{c}}}_{k,i}(\forall i,k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\mathbf{r}}_k=(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_k^{\wedge }){\tilde{\mathbf{r}}}_k+{\mathit{\rho }}_k(\forall k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{c}}_{k+1,i}\hfill & \\ & =(\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{c}}_{k,i}(\forall i,k)\hfill & \\ & (\mathbf{I}-\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{r}}_{k+1}\hfill & \\ & =(\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}_{k+1,k}^{\wedge }){\mathbf{r}}_k+{\mathit{\rho }}_{k+1,k}(\forall k)\hfill & \\ (\mathrm{r}\mathrm{e}\mathrm{d}.)& \frac{1}{2}{({\mathbf{c}}_{k,i}+{\tilde{\mathbf{c}}}_{k,i})}^T{\mathit{\rho }}_k={\mathbf{c}}_{k,i}^T{\mathbf{r}}_k-{\tilde{\mathbf{c}}}_{k,i}^T{\tilde{\mathbf{r}}}_k(\forall i,k)\hfill & \\ & {\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 }{\mathit{\varphi }}_k+{\mathbf{r}}_k^T{\mathit{\rho }}_k(\forall k)\hfill & \\ & {\mathit{\varphi }}_k^T{\mathbf{c}}_{k,i}={\mathit{\varphi }}_k^T{\tilde{\mathbf{c}}}_{k,i}(\forall i,k)\hfill & \\ & {\mathit{\varphi }}_k^T{\mathbf{r}}_k={\mathit{\varphi }}_k^T{\tilde{\mathbf{r}}}_k+{\mathit{\varphi }}_k^T{\mathit{\rho }}_k(\forall k)\hfill & \\ & {\mathbf{c}}_{k,m}^T{\mathbf{c}}_{k,\ell }-\frac{1}{2}{\mathit{\varphi }}_k^T{\mathbf{c}}_{k,n}\hfill & \\ & ={\mathbf{c}}_{k,m}^T{\tilde{\mathbf{c}}}_{k,\ell }-\frac{1}{2}{\mathbf{c}}_{k,m}^T{\tilde{\mathbf{c}}}_{k,\ell }^{\wedge }{\mathit{\varphi }}_k\hfill & \\ & (\forall \ell mn\in \left\{\mathrm{123,231,312}\right\},k)\hfill & \\ & \frac{1}{2}{({\mathbf{c}}_{k+1,i}+{\mathbf{c}}_{k,i})}^T{\mathit{\rho }}_{k+1,k}\hfill & \\ & ={\mathbf{c}}_{k+1,i}^T{\mathbf{r}}_{k+1}-{\mathbf{c}}_{k,i}^T{\mathbf{r}}_k(\forall i,k)\hfill & \\ & {\mathbf{r}}_{k+1}^T{\mathbf{r}}_{k+1}\hfill & \\ & ={\mathbf{r}}_{k+1}^T{\mathit{\rho }}_{k+1,k}+{\mathbf{r}}_k^T{\mathit{\rho }}_{k+1,k}+{\mathbf{r}}_k^T{\mathbf{r}}_k(\forall k)\hfill & \\ & {\mathit{\varphi }}_{k+1,k}^T{\mathbf{c}}_{k+1,i}={\mathit{\varphi }}_{k+1,k}^T{\mathbf{c}}_{k,i}(\forall i,k)\hfill & \\ & {\mathit{\varphi }}_{k+1,k}^T{\mathbf{r}}_{k+1}={\mathit{\varphi }}_{k+1,k}^T{\mathbf{r}}_k+{\mathit{\varphi }}_{k+1,k}^T{\mathit{\rho }}_{k+1,k}(\forall k).\hfill & \end{array}$

(57)

We again leave it to the reader to manipulate this into the standard form of (21). We can also notice that the ϖ _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 σ 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 = 21 poses in the trajectory, the local solver took on average 0.1928s while the SDP solver took on average 13.42s.

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).

Figure 12.

Continuous-time Trajectory Estimation: A quantitative evaluation of the tightness of the continuous-time trajectory estimation problem with increasing measurement noise, σ. 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.

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 ϖ _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 σ = 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 σ = 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 σ²I. In other words, σ is the standard deviation of the measurement noise. In the case of rotational degrees of freedom, σ = 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 σ = 0.5. The fact that our convex relaxations empirically remain tight (ensuring global optimality) beyond the σ = 0.5 level (often beyond σ = 1) means our technique works over most practical situations. For translational degrees of freedom, σ 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 σ = 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. (2021) 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.