Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra

TL;DR

This paper develops a framework for accurately modeling and propagating the uncertainty of multiple correlated poses in the Lie Algebra, improving over previous methods that assumed pose independence.

Contribution

It introduces a novel approach to characterize joint pose uncertainty in the Lie Algebra, accounting for correlations induced by pose graph factors.

Findings

More accurate uncertainty estimates in simulated datasets

Effective modeling of correlated pose uncertainties

Open-source C++ library implementation

Abstract

An accurate characterization of pose uncertainty is essential for safe autonomous navigation. Early pose uncertainty characterization methods proposed by Smith, Self, and Cheeseman (SCC), used coordinate-based first-order methods to propagate uncertainty through non-linear functions such as pose composition (head-to-tail), pose inversion, and relative pose extraction (tail-to-tail). Characterizing uncertainty in the Lie Algebra of the special Euclidean group results in better uncertainty estimates. However, existing approaches assume that individual poses are independent. Since factors in a pose graph induce correlation, this independence assumption is usually not reflected in reality. In addition, prior work has focused primarily on the pose composition operation. This paper develops a framework for modeling the uncertainty of jointly distributed poses and describes how to perform the equivalent of the SSC pose operations while characterizing uncertainty in the Lie Algebra. Evaluation on simulated and open-source datasets shows that the proposed methods result in more accurate uncertainty estimates. An accompanying C++ library implementation is also released. This is a pre-print of a paper submitted to IEEE TRO in 2019.

Tables

Table 1. TABLE I: Summary of covariance error statistics for proposed Lie algebra relative pose estimation when correlation is taken into account and ignored for a total of 44425 pose pairs extracted from a solution to the Manhattan3500 dataset [ 30 ] , with pose offsets ranging from 5 to 500 nodes.

Method	Covariance Error Mean	Covariance Error Std. Dev.
Proposed Lie algebra Rel. Pose	0.00675104 $\pm 2.2e^{-4}$	0.0461455
Proposed (Ignoring Correlation)	2.05667 $\pm 1.0e^{-2}$	2.12371

Table 2. TABLE II: Summary of normalized covariance error statistics for proposed and SSC [ 5 ] relative pose estimation for a total of 44425 pose pairs extracted from a solution to the Manhattan3500 dataset [ 30 ] , with pose offsets ranging from 5 to 500 nodes.

Method	Normalized Cov. Error Mean	Normalized Cov. Error Std. Dev.
Proposed Rel. Pose	0.0493121 $\pm 1.7e^{-4}$	0.0353072
SSC Tail-to-Tail	0.28778 $\pm 2.0e^{-3}$	0.411625

Equations

$$\mathrm{SE}(3):=\left\{\mathbf{T}=\left[\begin{array}[]{cc}\mathbf{R}{}&\mathbf{t}\\ \mathbf{0}^{\top}&1\end{array}\right]\in\mathbb{R}^{4\times 4}\middle|\mathbf{R}\in\mathrm{SO}(3),\mathbf{t}\in\mathbb{R}^{3}\right\},$$

$$\mathrm{SO}(3):=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}|\mathbf{R}{\mathbf{R}}^{\top }={\mathbf{I}}_3,\det \mathbf{R}=1\right\},$$

$$\mathfrak{so}(3):=\{\mathbf{\omega }\in {\mathbb{R}}^{3\times 3}|{\mathbf{\omega }}^{\top }=-\mathbf{\omega }\}$$

$$\boldsymbol{\phi}^{\wedge}:=\left[\begin{array}[]{c}\phi_{1}\\ \phi_{2}\\ \phi_{3}\end{array}\right]^{\wedge}=\left[\begin{array}[]{ccc}0&-\phi_{3}&\phi_{2}\\ \phi_{3}&0&-\phi_{1}\\ -\phi_{2}&\phi_{1}&0\end{array}\right]\in\mathfrak{so}(3),$$

$$\mathfrak{se}(3):=\left\{\left[\begin{array}[]{cc}\boldsymbol{\omega}&\boldsymbol{\rho}\\ \mathbf{0}^{\top}&0\end{array}\right]~{}\middle|~{}\boldsymbol{\omega}\in\mathfrak{so}(3),\boldsymbol{\rho}\in\mathbb{R}^{3}\right\}.$$

$$\boldsymbol{\xi}^{\wedge}:=\left[\begin{array}[]{c}\boldsymbol{\rho}\\ \boldsymbol{\phi}\end{array}\right]^{\wedge}=\left[\begin{array}[]{cc}\boldsymbol{\phi}^{\wedge}&\boldsymbol{\rho}\\ \mathbf{0}^{\top}&0\end{array}\right]\in\mathfrak{se}(3),$$

$$\exp ({\mathbf{\xi }}^{\wedge })=\sum _{k=0}^{\infty }\frac{({\mathbf{\xi }}^{\wedge }{)}^k}{k!}={\mathbf{I}}_4+{\mathbf{\xi }}^{\wedge }+\frac{({\mathbf{\xi }}^{\wedge }{)}^2}{2}+\cdots .$$

$$\log (\mathbf{T})=\sum _{k=1}^{\infty }(-1{)}^{k+1}\frac{(\mathbf{T}-{\mathbf{I}}_4{)}^k}{k}.$$

$${\mathrm{Ad}}_{\mathbf{T}}(\mathbf{\xi }):={\mathrm{Ad}}_{\mathbf{T}}\mathbf{\xi }=\log (\mathbf{T}\exp (\mathbf{\xi }){\mathbf{T}}^{-1}).$$

$$\mathbf{T}\exp (\mathbf{\xi })=\exp ({\mathrm{Ad}}_{\mathbf{T}}\mathbf{\xi })\mathbf{T}$$

$${\mathbf{\xi }}_{ac}$$

$$={\mathbf{\xi }}_{ab}+{\mathbf{\xi }}_{bc}+\frac{1}{2}[{\mathbf{\xi }}_{ab},{\mathbf{\xi }}_{bc}]+$$

$$+\frac{1}{12}([{\mathbf{\xi }}_{ab},[{\mathbf{\xi }}_{ab},{\mathbf{\xi }}_{bc}]]+[{\mathbf{\xi }}_{bc},[{\mathbf{\xi }}_{bc},{\mathbf{\xi }}_{ab}]])+\cdots ,$$

$$\boldsymbol{\xi}_{i}^{\curlywedge}:=\left[\begin{array}[]{c}\boldsymbol{\rho}_{i}\\ \boldsymbol{\phi}_{i}\end{array}\right]^{\curlywedge}=\left[\begin{array}[]{cc}\boldsymbol{\phi}_{i}^{\wedge}&\boldsymbol{\rho}_{i}^{\wedge}\\ \mathbf{0}&\boldsymbol{\phi}_{i}^{\wedge}\end{array}\right],$$

$${\mathbf{\xi }}_{ac}$$

$$-\frac{1}{24}{\mathbf{\xi }}_{bc}^{⋏}{\mathbf{\xi }}_{ab}^{⋏}{\mathbf{\xi }}_{ab}^{⋏}{\mathbf{\xi }}_{bc}+\dots .$$

$${\mathbf{T}}_{\ell }:=\exp ({\mathbf{\xi }}_{\ell }^{\wedge })\mathrm{bar}{\mathbf{T}}_{\ell }$$

$${\mathbf{x}}_{ij}=[x_{ij},y_{ij},z_{ij},{\varphi }_{ij},{\theta }_{ij},{\psi }_{ij}{]}^{\top },$$

$$\mathbf{x}$$

$${\mathbf{x}}_{ik}\triangleq {\mathbf{x}}_{ij}\oplus {\mathbf{x}}_{jk}=f_{\oplus }({\mathbf{x}}_{ij},{\mathbf{x}}_{jk}).$$

$${\hat{\mathbf{x}}}_{ik}={\hat{\mathbf{x}}}_{ij}\oplus {\hat{\mathbf{x}}}_{jk}$$

$${\mathbf{\Sigma }}_{{\mathbf{x}}_{ik}}\approx J_{\oplus }({\hat{\mathbf{x}}}_{ij},{\hat{\mathbf{x}}}_{jk})\hat{\mathbf{\Sigma }}{J}_{\oplus }({\hat{\mathbf{x}}}_{ij},{\hat{\mathbf{x}}}_{jk}{)}^{\top }$$

$$\hat{\mathbf{\Sigma }}$$

$${\mathbf{x}}_{ji}\triangleq \ominus {\mathbf{x}}_{ij}=f_{\ominus }({\mathbf{x}}_{ij}).$$

$${\hat{\mathbf{x}}}_{ji}=\ominus {\hat{\mathbf{x}}}_{ij}$$

$${\mathbf{\Sigma }}_{{\mathbf{x}}_{ji}}\approx J_{\ominus }({\hat{\mathbf{x}}}_{ij}){\mathbf{\Sigma }}_{{\mathbf{x}}_{ij}}{J}_{\ominus }({\hat{\mathbf{x}}}_{ij}{)}^{\top }$$

$${\mathbf{x}}_{jk}\triangleq (\ominus {\mathbf{x}}_{ij})\oplus {\mathbf{x}}_{ik}=f_{\ominus \oplus }({\mathbf{x}}_{ij},{\mathbf{x}}_{ik}).$$

$${\hat{\mathbf{x}}}_{jk}=(\ominus {\hat{\mathbf{x}}}_{ij})\oplus {\hat{\mathbf{x}}}_{ik}$$

$${\mathbf{\Sigma }}_{{\mathbf{x}}_{jk}}\approx J_{\ominus \oplus }({\hat{\mathbf{x}}}_{ij},{\hat{\mathbf{x}}}_{ik})\hat{\mathbf{\Sigma }}{J}_{\ominus \oplus }({\hat{\mathbf{x}}}_{ij},{\hat{\mathbf{x}}}_{ik}{)}^{\top }$$

$$\hat{\mathbf{\Sigma }}$$

$$\boldsymbol{\xi}_{1:n}=\left[\begin{array}[]{c}\boldsymbol{\xi}_{1}\\ \vdots\\ \boldsymbol{\xi}_{n}\end{array}\right],~{}~{}~{}\mathbf{\Sigma}_{1:n}=\left[\begin{array}[]{ccc}\mathbf{\Sigma}_{1}&\dots,&\mathbf{\Sigma}_{1,n}\\ \vdots&\ddots&\vdots\\ \mathbf{\Sigma}_{1,n}^{\top}&\dots&\mathbf{\Sigma}_{n}\end{array}\right],$$

$$\mathrm{bar}{\mathbf{T}}_{1:n}=\{\mathrm{bar}{\mathbf{T}}_1,\dots ,\mathrm{bar}{\mathbf{T}}_n\}$$

Full text

I Introduction

An accurate characterization of robot pose (location and orientation) uncertainty is essential to robust long-term autonomy because planning and safety decisions are often predicated on their value [3]. For example, an over-confident position estimate could potentially result in a self-driving car crossing out of its lane or an underwater vehicle colliding with a submerged structure. On the other hand, under-confidence can lead to slow or sluggish behavior.

One of the first papers to characterize pose uncertainty of coordinate frame relationships represents the relative pose of objects using a multivariate Gaussian parameter vector and associated covariance matrix [4]. This paper was later extended by Smith, Self, and Cheesman [5] by representing multiple uncertain spatial relationships as a stochastic map which could be used to evaluate the uncertainty of any given pose with respect to any other. They also proposed several operations (such as the relative pose operation shown in Fig. 1) that enable the extraction of additional information not directly estimated, along with first order coordinate-based methods for propagating uncertainty through these operations. For brevity, the operations proposed in [5] are often referred to by the initials of the paper’s authors (SSC).

Although it is well-understood that the rigid body transformation (or the motion group of $\mathbb{R}^{3}$) is described by the three dimensional (3D) Special Euclidean group [6, 7], $\mathrm{SE}(3)$, the uncertainty of these transformations is often modeled in local coordinates leading to inconsistencies in the estimation problem [8] or the loss of monotonicity in uncertainty propagation [9]. Wang and Chirikjian [10] and Long et al. [11] were able to overcome these problems by representing each pose using exponential coordinates located in the Lie algebra of the $\mathrm{SE}(d)$.

Barfoot and Furgale [1] were then able to show that propagation computations could be simplified by modeling the uncertainty directly in the Lie algebra and then using the exponential map to induce a distribution in the group space. Since the Lie algebra is a vector space, a small perturbation term can be modeled as zero-mean Gaussian noise in $\mathbb{R}^{6}$ and then used to perturb a mean (or nominal) pose in the group space. [1] then builds on this by deriving first and second order uncertainty propagation for the pose composition operation when the associated poses are independent. Our approach for modeling the uncertainty of a set of poses is similar, however we drop the independence requirement since the poses estimated by SLAM are rarely independent and additionally describe the additional operations of pose inversion and relative pose extraction (See Fig. 1).

The main contributions of this paper are as follows:

we present a framework that describes how to represent jointly correlated poses while using the Lie algebra to characterize uncertainty; 2. 2.

we derive the equivalent of the SSC operations under the proposed framework; 3. 3.

we describe how to convert from alternative uncertainty characterization parameterizations to the proposed framework (including Lie algebra covariance extraction from a MLE solution); and, 4. 4.

we release an accompanying C++ library implementation along with examples presented here.

The remainder of this paper is organized as follows: Section II provides a brief introduction to the Special Euclidean group and some necessary concepts from Lie group Theory. Section III provides a summary of the SSC uncertainty representation framework and its associated operations. Section IV describes how to use the Lie algebra to characterize uncertainty for jointly distributed poses. Sections V, §VI, and §VII describe the derivation of the pose composition, pose inversion, and relative pose operations, respectively, while characterizing uncertainty on the Lie algebra. Section VIII describes how to convert from a coordinate based representation of uncertainty to the Lie algebra based representation and how to extract an estimate of pose uncertainty from a MLE solution. Section IX describes an experimental evaluation of the proposed methods. Section X describes the implementation of the released library. Finally, Section XI concludes the paper.

II The Special Euclidean Group and

Lie Group Theory

Estimation of the relative pose (position and orientation) between objects or coordinate frames in space is a common problem in robotic navigation, perception, and manipulation. Formally, we represent 3D relative pose transformations as elements of the Special Euclidean group. This section provides a brief introduction to the Special Euclidean group and relevant aspects of Lie group theory that are important during the subsequent derivation and discussion.

II-A The Special Euclidean group

The Special Euclidean group, or $\mathrm{SE}(d)$, represents the space of homogeneous transformation matrices or the space of matrices that apply a rigid body rotation and translation to points in $\mathbb{R}^{d}$ (represented in homogeneous form). Formally, in three dimensions, $\mathrm{SE}(3)$ is defined as follows:

[TABLE]

where $\mathrm{SO}(3)$ is the Special Orthogonal group is the space of valid rotation matrices:

[TABLE]

and $\mathbf{I}_{d}$ is the identity matrix of dimension $d$. A variety of methods have been developed for parameterizing these objects such as Euler angles or quaternions [6].

Both $\mathrm{SE}(3)$ and $\mathrm{SO}(3)$ are matrix Lie groups meaning they are smooth manifolds that also satisfy the formal definition of a mathematical group [12, 13] with the standard matrix multiplication operation. Intuitively, this means that while the general group is non-linearm, each of these groups can be locally approximated using a Euclidean vector space. Additionally, for any given point on the manifold, consider the set of all paths on the manifold that pass through that point. The set of all velocities (both in terms of direction and speed) of those paths at the given point form a vector space called the tangent space. The tangent space centered at the identity is called the Lie algebra. This relationship is depicted in Fig. 2.

The Lie algebra of $\mathrm{SO}(3)$ is denoted $\mathfrak{so}(3)$ and is the space of skew-symmetric $3\times 3$ matrices [14, 12]:

[TABLE]

This space is isomorphic to $\mathbb{R}^{3}$ since skew-symmetric matrices have zeros on the diagonal entries and the lower entries are completely identified by three upper entries (hence $\mathrm{dim}~{}\mathfrak{so}(3)=3$). Therefore, it is very convenient to work with $\mathbb{R}^{3}$ instead. Note, we can use the $\wedge$ operator to take an element of $\mathbb{R}^{3}$ and transform it to an element of $\mathfrak{so}(3)$:

[TABLE]

where $\boldsymbol{\phi}\in\mathbb{R}^{3}$. The $\vee$ operator denotes the inverse of $\wedge$.

Similarly, the Lie algebra of $\mathrm{SE}(3)$, or $\mathfrak{se}(3)$, is defined as follows:

[TABLE]

This space of matrices is isomorphic to $\mathbb{R}^{6}$ and we overload the $\wedge$ operator to convert between the Euclidean vector and matrix forms:

[TABLE]

where $\boldsymbol{\xi}\in\mathbb{R}^{6}$ and $\boldsymbol{\rho},\boldsymbol{\phi}\in\mathbb{R}^{3}$.

Understanding the relationship between the group and algebra spaces can enable one to leverage the fact that the algebra is a vector space. The next few subsections cover some important concepts from Lie group Theory that we need in the rest of the paper. In doing so, we use $\mathcal{G}$ to represent a given Lie group and $\mathfrak{g}$ to represent its associated Lie algebra.

II-B The Exponential Map

The Lie algebra, $\mathfrak{g}$, represents the tangent space of the manifold at the identity. However, given a specific tangent vector, we may want to convert it to its associated transformation in the group space $\mathcal{G}$. The exponential map, $\operatorname{exp}:\mathfrak{g}\rightarrow\mathcal{G}$, (which can be defined in closed form for $\mathrm{SE}(3)$), enables us to perform this conversion,

[TABLE]

The logarithm map, $\operatorname{log}:\mathcal{G}\rightarrow\mathfrak{g}$, on the other hand, enables us to go the other direction from an action/transformation in the group space to the velocity that would have induced it,

[TABLE]

This relationship is visualized in Fig. 2.

II-C The Adjoint Action

Assuming $\mathbf{T}\in\mathcal{G}$ and $\boldsymbol{\xi}\in\mathfrak{g}$, the adjoint action of $\mathbf{T}$ on $\boldsymbol{\xi}$, or $\operatorname{Ad}_{\mathbf{T}}(\boldsymbol{\xi})$ is defined as follows:

[TABLE]

The adjoint action describes the affect that transforming to the group space applying a transformation on the left and its inverse on the right has on an element of the Lie algebra. This gives rise to the following property, which we use later on:

[TABLE]

II-D The Baker-Campbell-Hausdorff (BCH) Formula

Finally, we also want to characterize the effect that multiplication in the group space has on the Lie algebra. More specifically, suppose we want to compute the Lie algebra element, $\boldsymbol{\xi}_{ac}\in\mathfrak{g}$, that is generated by taking the logarithm of the product of the exponential of two Lie algebra elements $\boldsymbol{\xi}_{ab},\boldsymbol{\xi}_{bc}\in\mathfrak{g}$. The Baker-Campbell-Hausdorff (BCH) formula describes this relationship purely in the Lie algebra space without requiring the application of the exponential or the logarithm:

[TABLE]

where $[\cdot,\cdot]$ is the Lie bracket of $\mathfrak{g}$ [15, (7.18)]. For brevity, in the special case where we consider elements in $\mathrm{SE}(3)$, we adopt the notation of [1]:

[TABLE]

which allows us to write the BCH formula as:

[TABLE]

II-E Defining Random Variables over Poses

Finally, as discussed in [1], one can define random variables for $\mathrm{SE}(3)$ according to

[TABLE]

where $\mathrm{bar}{\mathbf{T}}_{\ell}\in\mathrm{SE}(3)$ is a ‘large’ noise free value and $\boldsymbol{\xi}_{\ell}\in\mathbb{R}^{6}$ is a ‘small’ noisy perturbation (using the nomenclature of [1]). Two examples of this noisy perturbation are depicted in the middle row of Fig. 1. By defining $\boldsymbol{\xi}_{\ell}$ to be a zero-mean Gaussian random variable $\boldsymbol{\xi}_{\ell}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{\ell})$ in the Lie algebra, we induce a probability distribution function over $\mathrm{SE}(3)$ that is parameterized with a mean $\mathrm{bar}{\mathbf{T}}_{\ell}\in\mathrm{SE}(3)$ and a covariance $\mathbf{\Sigma}_{\ell}$ defined in the Lie algebra [1].

We use these properties in Sections IV, V, VI, and VII to propose a framework for modeling jointly correlated poses and to derive uncertainty propagation formulas for the operations in Fig. 3. However, we first review the SSC coordinate based method.

III Review of SSC

The stochastic map, proposed by Smith et al. [5], consists of multiple uncertain spatial relationships that are treated as jointly Gaussian multivariate random variables and parameterized using a mean vector of positions and Euler angles and an associated covariance matrix. Smith et al. [5] also proposes three operations that can extract information from this map that may not have been directly estimated. These three operations are pose composition (head-to-tail in [5]), pose inverse, and relative pose estimation (tail-to-tail in [5]), as shown in Fig. 3. This section reviews the pose representation and SSC operations formulated in [5].

III-A Pose Representation and the Stochastic Map

Under SSC notation, the relative transformation between the coordinate frame $i$ and $j$, or the pose of coordinate frame $j$ with respect to frame $i$, is denoted by

[TABLE]

where $x_{ij}$, $y_{ij}$, and $z_{ij}$ are position coordinates and $\phi_{ij}$, $\theta_{ij}$, and $\psi_{ij}$ are Euler angles that encode the position and orientation of the coordinate frame $j$ with respect to the frame $i$. However, because each relative transformation is derived from noisy measurements, our estimate is uncertain and we do not know the true value of $\mathbf{x}_{ij}$. Instead, we track a nominal mean value of its parameters and an associated $6\times 6$ covariance matrix, $\hat{\mathbf{x}}_{ij}$ and $\mathbf{\Sigma}_{\mathbf{x}_{ij}}$, respectively.

The stochastic map, proposed in [5], treats all of the uncertain transformations as jointly Gaussian multivariate random variables by stacking them into a single state vector $\mathbf{x}$ and tracking the mean and covariance of that state. Assuming we have $n$ relative transformations we want to track, if we index them from $a=1,\dots,n$, then $\mathbf{x}$ and its associated mean and covariance are:

[TABLE]

where $\mathbf{x}$ is a vector of length $6n$, $\mathbf{\Sigma}_{\mathbf{x}_{a}}$ is the $6\times 6$ covariance matrix of the relative transformation $\mathbf{x}_{a}$ and the off diagonal blocks of the form $\mathbf{\Sigma}_{\mathbf{x}_{a}\mathbf{x}_{b}}$ are the respective $6\times 6$ cross covariance matrices.

III-B Pose Composition (Head-to-Tail)

Suppose we are given a noisy observation of a robot’s relative pose between time steps $i$ and $j$ $(\mathbf{x}_{ij})$ and another observation of its relative pose between time steps $j$ and $k$ $(\mathbf{x}_{jk})$, we may want to calculate the relative pose between time steps $i$ and $k$ $(\mathbf{x}_{ik})$ by composing the observations $\mathbf{x}_{ij}$ and $\mathbf{x}_{jk}$ (see Fig. 3). The SSC head-to-tail operation is a nonlinear function $f_{\oplus}:\mathbb{R}^{6}\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ that takes the parameter vectors $\mathbf{x}_{ij}$ and $\mathbf{x}_{jk}$ as input and outputs the parameter vector $\mathbf{x}_{ik}$ that results from composing the respective homogeneous transformation matrices. The $\oplus$ operator denotes this operation:

[TABLE]

The mean and covariance of the resulting pose are estimated up-to first order as

[TABLE]

and

[TABLE]

where $J_{\oplus}(\hat{\mathbf{x}}_{ij},\hat{\mathbf{x}}_{jk})$ is the Jacobian of $f_{\oplus}$ at $\hat{\mathbf{x}}_{ij}$ and $\hat{\mathbf{x}}_{jk}$, and

[TABLE]

III-C Pose Inverse

Suppose we are given a robotic vehicle that is tasked with navigating through an a priori unknown environment after which it must return to the origin location. While it is common to represent the robotic vehicle’s current location with respect to the origin using estimation theory, it may be useful to instead characterize the pose of the origin with respect to the local robot coordinate frame. Formally, given an uncertain estimate of the pose of coordinate frame $j$ with respect to frame $i$ ($\mathbf{x}_{ij}$), we want to determine the pose of frame $i$ with respect to frame $j$ ($\mathbf{x}_{ji}$). This amounts to finding the vector $\hat{\mathbf{x}}_{ji}$ that corresponds to the inverse of $\hat{\mathbf{x}}_{ij}$ (in terms of homogeneous transformation matrices) and representing the uncertainty with respect to this new frame of reference.

The SSC pose inverse operation is a nonlinear function $f_{\ominus}:\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ takes a pose and computes the inverse of the pose as a homogeneous transformation matrix:

[TABLE]

The mean and covariance of the resulting pose are estimated up-to first order as

[TABLE]

and

[TABLE]

where $J_{\ominus}(\hat{\mathbf{x}}_{ij})$ is the Jacobian of $f_{\ominus}$ at $\hat{\mathbf{x}}_{ij}$.

III-D Relative Pose (Tail-to-Tail)

Finally, the SSC tail-to-tail operation takes uncertain estimates of two coordinate frames with respect to a single origin frame and evaluates the relative pose between them. More succinctly, given $\mathbf{x}_{ij}$ and $\mathbf{x}_{ik}$, find $\mathbf{x}_{jk}$. This operation is useful when using a method such as pose graph SLAM (SLAM) for navigation since the robot pose at multiple time steps is often estimated with respect to a single fixed coordinate frame.

The SSC relative pose operation is a nonlinear function $f_{\ominus\oplus}:\mathbb{R}^{6}\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ that is defined by first applying the inverse and then the head-to-tail operation:

[TABLE]

The mean and covariance of the resulting pose up-to first order are estimated as

[TABLE]

and

[TABLE]

where $J_{\ominus\oplus}(\hat{\mathbf{x}}_{ij},\hat{\mathbf{x}}_{ik})$ is the Jacobian of $f_{\ominus\oplus}$ at $\hat{\mathbf{x}}_{ij}$ and $\hat{\mathbf{x}}_{ik}$, and

[TABLE]

IV Jointly Characterizing Uncertainty in the Lie algebra

While the SSC operations proposed in [5] have been used widely since they were introduced, over the past decade Lie algebra based methods have been shown to provide a more accurate characterization of uncertainty [1, 16]. However, while recent years have seen an increase in use of Lie algebra based methods for uncertainty propagation [17, 18, 19], existing methods assume that individual measurements are independent [1], which may not be the case when the underlying pose estimates are derived from a SLAM solution. We now describe how this assumption can be dropped and poses can be modeled as jointly correlated within the Lie algebra space.

Assume that we have $n$ uncertain poses $\{\mathbf{T}_{1},\dots,\mathbf{T}_{n}\}$, each of which is defined according to (14). If we assume that the poses are statistically independent, then we can parameterize this distribution of poses with the set of associated mean and covariance matrices $\{\mathrm{bar}{\mathbf{T}}_{1},\mathbf{\Sigma}_{1},\dots,\mathrm{bar}{\mathbf{T}}_{n},\mathbf{\Sigma}_{n}\}$. However, if the uncertainty associated with the set of poses is correlated, then this can be modeled by concatenating the set of perturbation vectors $\{\boldsymbol{\xi}_{1},\dots,\boldsymbol{\xi}_{n}\}$ into a single vector $\boldsymbol{\xi}_{1:n}\in\mathbb{R}^{6n}$ and represent the uncertainty of the distribution using a single covariance matrix:

[TABLE]

where $\boldsymbol{\xi}_{1:n}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{1:n})$. Thus, we can parameterize the distribution of $n$ poses using the set of each of their means

[TABLE]

and the covariance matrix $\mathbf{\Sigma}_{1:n}$. Note that this only extends since we define the uncertainty in the Lie algebra. The next three sections describe how to apply the composition, inverse, and relative pose operations on uncertain poses represented in this manner.

V Jointly Distributed Pose Composition

This section describes how to compose two uncertain poses who’s associated perturbation vectors are jointly Gaussian in the Lie algebra as described in the last section. This is the Lie group-based or coordinate free equivalent to the SSC head-to-tail operation described in Section III-B.

V-A Pose Composition Operation Derivation

Suppose we have two uncertain poses $\mathbf{T}_{ij}$ and $\mathbf{T}_{jk}$ with perturbations $\boldsymbol{\xi}_{ij}$ and $\boldsymbol{\xi}_{jk}$ that are jointly Gaussian in the Lie algebra with covariance matrix

[TABLE]

Our goal is to find the mean and covariance of $\mathbf{T}_{ik}$, $\{\mathrm{bar}{\mathbf{T}}_{ik},\mathbf{\Sigma}_{ik}\}$. Under the standard group multiplication operation:

[TABLE]

Following the random variable definition in (14) and using the property described in (10),

[TABLE]

where $\mathrm{Ad}_{\mathrm{bar}{\mathbf{T}}_{ij}}$ is the matrix form of the adjoint action of $\mathrm{bar}{\mathbf{T}}_{ij}$ on $\mathfrak{se}(3)$. Letting

[TABLE]

gives us

[TABLE]

We can now use the BCH (BCH) formula (13) to show that

[TABLE]

where $\boldsymbol{\xi}^{\prime}_{jk}=\mathrm{Ad}_{\mathrm{bar}{\mathbf{T}}_{ij}}\boldsymbol{\xi}_{jk}$. Computing the covariance $\mathbf{\Sigma}$ amounts to evaluating $E[\boldsymbol{\xi}_{ik}\boldsymbol{\xi}_{ik}^{\top}]$. Multiplying out up-to fourth order, we get:

[TABLE]

This derivation is identical to the one proposed in [1] until the last step. Since [1] assumes the individual poses are independent, the cross terms in (51) and (53) are zero and the evaluation of the 4th order terms in (52) is simplified. If this assumption is true and $\boldsymbol{\xi}_{ij}$ and $\boldsymbol{\xi}^{\prime}_{jk}$ are independent of one another, as is the case when $\mathbf{T}_{ij}$ and $\mathbf{T}_{jk}$ are independent measurements of consecutive robot motion, then the cross terms in (52) and the covariance $\mathbf{\Sigma}$ can be calculated up to 4th order as described in [1]. However, if the poses $\mathbf{T}_{ij}$ and $\mathbf{T}_{jk}$ are correlated, as is often the case when they are derived from the solution of a MLE (MLE) problem such as Pose Graph SLAM or in the case of wheel slip, the cross terms must be included and evaluation of the fourth order terms in (52) and (53) becomes more difficult111If increased accuracy is needed then Isserlis Theorem can be applied to evaluate the fourth order terms in (52) and (53).. If the correlation is not taken into account, then the result of the pose composition operation under-approximates the true distribution and consistency is lost as shown in Fig. 4.

A first order estimate of covariance (second order in the perturbation variables) can be obtained be evaluating

[TABLE]

resulting in

[TABLE]

Thus, first order pose composition on the Lie algebra can be performed for correlated poses using (48) for mean propagation and (55) for covariance propagation. As noted previously, this is equivalent to the first order method presented in [1] with the exception of the two additional cross terms in (55) needed to model cross correlation.

V-B Accurately Modeling Group Structure

While both the SSC operations presented in [5] and the Lie algebra based methods presented in this paper (and $\Sigma_{2nd}$ in [1, (55)]) are first order approximation methods (in terms of covariance), characterizing rotation uncertainty in the Lie algebra space results in a more accurate characterization of pose uncertainty. This is because Euler angle parameterization is a chart and does not cover the entire manifold whereas the Lie algebra inherently captures the group structure and can model any arbitrary group element. In addition, since the Lie algebra is a vector space [12], modeling the uncertainty as a Gaussian distribution is convenient and well-defined.

To demonstrate this, we performed an experiment similar to the one proposed in [1]. We generated a sequence of $N$ noisy pose transformations of the form $\mathbf{T}_{ab}=\operatorname{exp}(\boldsymbol{\xi}_{ab}^{\wedge})\mathrm{bar}{\mathbf{T}}_{ab}$, with

[TABLE]

[TABLE]

and

[TABLE]

where $\sigma_{t}$ and $\sigma_{r}$ are scaling parameters for the translation and rotation noise respectively. In addition, each consecutive perturbation variable $\boldsymbol{\xi}_{ab}$ was drawn such that it was correlated with the perturbation variable before it with correlation coefficient $\rho$.

We then composed these transformations end-to-end and repeated the process to generate 10000 sample trajectories. These Monte-Carlo simulation results were then used to evaluate the uncertainty predicted by the SSC head-to-tail operation as well as the Lie-algebra based pose composition method derived in this section, both with and without correlation being taken into account. A top down view of one such experiment with $N=10$, $\sigma_{t}=5$, $\sigma_{r}=5$, and $\rho=0.4$ is shown in Fig. 4.

As expected, the Lie algebra based method significantly outperforms SSC head-to-tail because it takes into account the structure of the $\mathrm{SE}(3)$ group. In addition, it is easily seen that dropping the cross covariance terms results in an under-approximation of the true covariance, if positive correlation is indeed present. A more thorough investigation of this experiment is described in §IX-A.

In the next two sections, we provide a derivation of the inverse and relative pose operations on the Lie algebra, which as far as we know have not been previously published.

VI The Pose Inverse Operation

The pose inverse operation corresponds to a change in reference frame. Given an uncertain pose distribution $\mathbf{T}_{ij}$ that represents the pose of coordinate frame $j$ with respect to frame $i$, we want to find the inverse $\mathbf{T}_{ji}$ that represents the pose of coordinate frame $i$ with respect to frame $j$. The distribution of an inverse pose can be derived as follows [20]:

[TABLE]

where $\boldsymbol{\xi}^{\prime}_{ij}$ is the original perturbation $\boldsymbol{\xi}_{ij}$ transformed by the negative adjoint of $\mathrm{bar}{\mathbf{T}}_{ij}^{-1}$. Due to the linearity of the adjoint operation, we can represent the inverse distribution $\mathbf{T}_{ji}$ with mean and covariance

[TABLE]

and

[TABLE]

VII Finding the Relative Transformation Between Jointly Distributed Poses

This section combines the derivations from the previous two sections to formulate a first order method for characterizing the uncertainty of the relative pose operation or the coordinate-free equivalent to the SSC tail-to-tail operation described in Section III-D. As far as we can tell, this is the first time this operation has been published while using the Lie algebra to characterize uncertainty.

VII-A Relative Pose Operation Derivation

Given uncertain estimates of two coordinate frames with respect to a common base frame, our aim is estimate the relative transformation between the two poses. For example, given possibly correlated uncertain transformations $\mathbf{T}_{ij}$ and $\mathbf{T}_{ik}$ representing the poses of coordinate frames $j$ and $k$ with respect to frame $i$, we want to find the mean $\mathrm{bar}{\mathbf{T}}_{jk}$ and covariance $\mathbf{\Sigma}_{jk}$ that parameterize the pose uncertainty of frame $k$ with respect to frame $j$.

We start by assuming that the $\mathrm{bar}{\mathbf{T}}_{ij}$ and $\mathrm{bar}{\mathbf{T}}_{ik}$ are known and that the associated perturbations $\boldsymbol{\xi}_{ij}$ and $\boldsymbol{\xi}_{ik}$ are jointly correlated in the Lie algebra with known covariance

[TABLE]

Under the standard group inverse and multiplication actions, the following must hold:

[TABLE]

Expanding (65) using the random variable definition in (14) and the derivation in (59) results in

[TABLE]

where $\boldsymbol{\xi}_{ij}^{\prime}=-\mathrm{Ad}_{\mathrm{bar}{\mathbf{T}}_{ij}^{-1}}\boldsymbol{\xi}_{ij}$ and $\boldsymbol{\xi}_{ik}^{\prime}=\mathrm{Ad}_{\mathrm{bar}{\mathbf{T}}_{ij}^{-1}}\boldsymbol{\xi}_{ik}$. Letting

[TABLE]

gives us

[TABLE]

Expanding the BCH formula in a similar manner to (50) and taking the expectation as in (51) results in the following up to first order:

[TABLE]

Evaluating the expectations in (69) results in

[TABLE]

Thus, uncertainty can be propagated through the relative pose function via (67) and (70). A summary of the uncertainty propagation methods derived in the last three sections is provided in Fig. 3.

VII-B Ignoring Correlation Leads to Inconsistency

Ignoring correlation of the associated perturbation variables leads to under/over-estimation of uncertainty, depending on if the correlation is positive or negative and if the operation being performed is pose composition or relative pose estimation. Fig. 5 and Fig. 6 show an example of this for the case of relative pose with positive correlation.

To create these plots, we generated $M=10000$ sets of two uncertain poses $\mathbf{T}^{m}_{g1}=\operatorname{exp}(\boldsymbol{\xi}_{g1}^{m\wedge})\mathrm{bar}{\mathbf{T}}_{g1}$ and $\mathbf{T}_{g2}=\operatorname{exp}(\boldsymbol{\xi}_{g2}^{m\wedge})\mathrm{bar}{\mathbf{T}}_{g2}$, with mean values

[TABLE]

and

[TABLE]

under the assumption that the perturbation variables ($\boldsymbol{\xi}^{m}_{g1}$) and ($\boldsymbol{\xi}^{m}_{g2}$) are jointly correlated in the Lie algebra with marginal covariance matrices

[TABLE]

and cross covariance

[TABLE]

where $\alpha$ is a scaling parameter. We then used the relative pose operation presented in this section to estimate $\mathbf{T}_{12}$ both with and without taking uncertainty into account.

In Fig. 6, we evaluate the estimated covariance error for increasing values of $\alpha$ with respect to the Monte-Carlo simulation under the following covaraiance error metric

[TABLE]

where

[TABLE]

with

[TABLE]

and

[TABLE]

Fig. 4, Fig. 5, and Fig. 6 show that ignoring correlation leads to inconsistent estimates of uncertainty.

VIII Converting to a Lie algebra Based Representation

Although uncertainty characterization in the Lie algebra is more accurate than coordinate based methods, in some cases an existing estimation algorithm may output pose estimates in an alternative parameterization. This section describes how to convert from an existing parameterization such as the SSC multi-variate Gaussian representation (defined in (15) and (25)) to the proposed representation, as well as how to extract the mean and covariance of a jointly correlated set of poses from a MLE solution such as iSAM [21] or g2o [22] directly.

VIII-A Converting from a Coordinate Based Representation

Assuming we are given a mean parameter vector $\hat{\mathbf{x}}$ and associated covariance $\hat{\boldsymbol{\Sigma}}$ as defined in (25), to apply our proposed propagation methods, we must first transform $\hat{\mathbf{x}}$ and $\hat{\boldsymbol{\Sigma}}$ into into our proposed representation such that the means of the associated transformations are represented in the group space via $\mathrm{bar}{\mathbf{T}}_{1:n}$ and the perturbations are represented as multivariate Guassian random variables, $\boldsymbol{\xi}_{1:n}$, with covariance $\boldsymbol{\Sigma}_{1:n}$ in the associated Lie algebra as defined in (41) and (42). Converting from a coordinates based method for uncertainty characterization to the Lie algebra representation is a non-linear operation. As such, the unscented transform [23] is a better choice for converting between representations than a first order linearization method.

The unscented transform, first proposed by Julier and Uhlmann [24] in 1997, takes an input Gaussian distribution defined in one space and a non-linear function that maps the input space to an output space and finds a Gaussian approximation for the transformed distribution. This is done by using the input mean and covariance to create a deterministic set $\mathcal{X}_{1:K}$ of $K=2m+1$ sigma points, where $m$ is the dimension of the input space. These sigma points are then passed through the non-linear function and a weighted mean and sample covariance is used to find a Gaussian approximation to the output distribution.

Assuming we have a function $f:\mathbb{R}^{d}\mapsto\mathcal{G}$ that maps from a parameter vector, $\mathbf{x}_{i}$, to an element of the Lie group, $\mathcal{G}$, we can define a function $\ell_{i}$ that takes a perturbed parameter vector $\tilde{\mathbf{x}}_{i}$, centers it on the identity, and transforms it to the Lie algebra space as follows:

[TABLE]

where $\hat{\mathbf{x}}_{i}$ is the mean parameter vector associated with $\tilde{\mathbf{x}}_{i}$.

We can then use the standard unscented transform [24, (12)] to generate a set $\mathcal{X}_{1:K}$ of $2nd+1$ sigma points from $\hat{\mathbf{x}}$ and $\hat{\boldsymbol{\Sigma}}$, where $n$ is the number of modeled Lie group elements. After which, the predicted Lie group representation for the jointly correlated poses can be obtained as follows:

[TABLE]

[TABLE]

where $\ell$ is a vectorized version of $\ell_{i}$ and $\mathcal{W}_{k}$ is the standard weight from [24, (12)]. Fig. 7 shows a result of this process.

While, in many cases the use of the Unscented Transform may be the best that can be done, the use of the coordinates based representation (even as an intermediate representation) does lead to some loss of information. This loss of information can be avoided if we can directly represent the uncertainty in the proposed framework when extracting it from the prior estimation solution.

VIII-B Extracting Pose Uncertainty from a MLE Solution

State-of-the-Art Pose Graph SLAM solvers find a solution by estimating the set of robot poses that maximizes the likelihood of the observed measurements [25, 21, 26, 27, 28]. Traditional, iterative non-linear solvers [25, 21, 26, 22] do this by building up a measurement Jacobian, $A$, with columns that correspond to elements of the parameter vector $\mathbf{x}$ and with block rows that correspond to weighted, measurement residual error functions that minimize the error between predicted measurements and what was actually measured. At each iteration, this measurement Jacobian is used to form a linear least squares problem of the following form:

[TABLE]

where $\mathbf{b}$ is the measurement vector not needed for the following derivation. The algorithm alternates between solving this linear least squares optimization problem and relinearizing $A$ around $\hat{\mathbf{x}}$ until convergence [25, 21]. Algorithms such as those described in [27] and [28] that provide a guarantee of global optimality formulate the problem slightly differently, but once a solution has been obtained, a matrix $A$ can still be formed by linearizing the cost around the current solution.

After a solution has been reached, an estimate of the uncertainty of that solution can be found by using $A$ to form the information matrix $\mathcal{I}=A^{\top}A$ and using the non-zero elements of its Cholesky factorization $\mathcal{I}=R^{\top}R$ to calculate the necessary elements of the marginal covariance $\hat{\boldsymbol{\Sigma}}$ as detailed in [29]. The trick to extracting this covariance with respect to $\boldsymbol{\xi}_{1:n}$ as opposed to $\hat{\mathbf{x}}$ is to make sure that the Jacobian $A$ used to form $\mathcal{I}$ is evaluated with respect to $\boldsymbol{\xi}_{1:n}$ as opposed to $\hat{\mathbf{x}}$. This can be done by numerically evaluating the Jacobian and by perturbing $\boldsymbol{\xi}_{1:n}$ around $\mathbf{0}$ and propagating that perturbation to the linearization point by means of the exponential function as opposed to perturbing the parameters of $\hat{\mathbf{x}}$ directly. Doing this enables the direct extraction of $\boldsymbol{\Sigma}_{1:n}$ and results in increased accuracy because $\boldsymbol{\xi}_{1:n}$ lies in a vector space, while $\hat{\mathbf{x}}$ does not.

IX Evaluation

This section evaluates the proposed uncertainty characterization method by performing two experiments. The first, preforms a parameter sweep over the experiment introduced in §V-B. The second, extracts covariance information from the result of a Pose Graph SLAM algorithm and compares the predicted relative pose covariance with the sample covariance obtained from Monte Carlo.

IX-A Compounding Correlated Odometry

The exact accuracy of our proposed uncertainty characterization method is dependent on a variety of parameters including the number of poses compounded end-to-end and the rotation and translation noise. We explore this dependence by performing a parameter sweep across each of these parameters based on the experiment introduced in §V-B.

We began by varying the number of poses in the trajectory ($N$) and held both noise scale parameters fixed at $\sigma_{t}=\sigma_{r}=3$. The results are shown in Fig. 8. As the number of steps increases, the accuracy of the final estimate drops off, this is because each of the methods are only characterizing uncertainty up-to first order and the lost higher order information builds up as the number of poses increases.

For the noise parameter experiments, we varied the noise scale parameters $\sigma_{r}$ and $\sigma_{t}$ in turn, while keeping the number of poses fixed at $N=10$ and the non-varying noise parameter fixed at $3$. The results are shown in Fig. 9. Rotation noise has a much more significant effect, again this is because of the lost higher-order information (either in terms of the higher order terms of the BCH formula or through linearization for SSC). Both Lie group methods consistently outperform SSC except when rotation noise is very low and translation noise is very high. This is because when rotation error is very low, the non-linearity of the transformation becomes almost negligible and because the increased translation error causes the covariance ellipsoid to increase to the point where it includes the final robot position. In addition the joint composition method in the Lie algebra is consistently more accurate than when correlation is ignored. For all three parameter sweeps, the induced correlation was held fixed at $\rho=0.4$.

IX-B Extracting Relative Pose From a SLAM Solution

To evaluate relative pose extraction, we used iSAM [21] to find a solution to the Manhattan 3500 dataset [30]. We then extracted joint mean and covariance as described in §VIII-B for pairs of poses at offsets varying from 5 to 50 in increments of 5 as well as for offsets of 100, 200, and 500 nodes. A visualization of the extracted pose pairs and the correlation between them for offsets of 50 are shown in Fig. 10 (a-c). Equivalent visualizations for offsets of 10, 100, 200, and 500 are shown in Fig. 11, Fig. 12, Fig. 13, and Fig. 14.

To investigate the importance of taking into account correlation when estimating relative pose, we performed a Monte Carlo simulation to estimate the true relative pose covariance as described in (76) - (78), where $\boldsymbol{\xi}_{g1}^{m}$ and $\boldsymbol{\xi}_{g2}^{m}$ are the perturbation variables sampled from the extracted joint covariance and $\mathrm{bar}{\mathbf{T}}_{g1}$ and $\mathrm{bar}{\mathbf{T}}_{g2}$ are the mean values extracted from the iSAM solution. We then used the metric defined in (75) to evaluate the covariance error of our method proposed in (70) when taking into account correlation (using all four terms in (70)) or ignoring correlation (using only the first two terms in (70)). Summary statistics are shown in Table I and visualizations of the error for offsets of 50, 10, 100, 200, and 500 are shown in (d-e) of Fig. 10, Fig. 11, Fig. 12, Fig. 13, and Fig. 14.

The results in Table I show that ignoring correlation can lead to a covariance error more than 3 orders of magnitude higher than if correlation is taken into account.

To compare our proposed method to SSC [5], we performed a similar experiment except that the Monte Carlo ”groundtruth” covariance for SSC was derived by taking the sample relative poses from the previous experiment and converting them to the parameter vector format described in (15) and taking the sample covariance. To fairly compare our proposed method and SSC, the covariance matrices were normalized by the Frobenious norm of the Monte Carlo covariance matrix before evaluating the covariance error as defined in (75). Summary statistics of the experiment are shown in Table II and visualizations of the error for offsets of 50, 10, 100, 200, and 500 are shown in (f-g) of Fig. 10, Fig. 11, Fig. 12, Fig. 13, and Fig. 14.

It should be noted that this is not a perfect comparison because the Monte Carlo covariance to which SSC is being compared is a multivariate Gaussian fit to a set of parameter vectors that cannot be accurately modeled as a Gaussian. However, the results do show that even when modeling the true error in the format assumed by the SSC representation, our proposed Lie algebra based method results in an order of magnitude lower covariance error than SSC (see Table II).

X Library Implementation

We have released an open source C++ library implementation of our method. It can be downloaded at: https://bitbucket.org/jmangelson/lie. We have tried to design it to be simple, intuitive, and easily extendable.

X-A Creating Known and Uncertain $\mathrm{SE}(3)$ Objects

Creating both known and uncertain $\mathrm{SE}(3)$ objects is syntactically easy. After importing the library a variety of constructors can be used to create known $\mathrm{SE}(3)$ transformations:

#include <lie/se3.hpp>

// Via rotation and translation Eigen::MatrixXd R(3,3); // 3x3 matrix Eigen::VectorXd t(3); // 3x1 vector Lie::SE3 T_12(R, t);

// Via translation and Euler angle params Lie::SE3 T_23(x, y, z, theta, phi, psi);

Independent and jointly distributed sets of unknown poses can also be created by passing in a mean value or a tuple of mean values and a covariance matrix.

Lie::SE3 T_ab_mu(R_ab, t_ab); Eigen::MatrixXd Sigma_ab(6,6); auto T_ab_uncertain = Lie::make_uncertain_state( T_ab_mu, Sigma_ab);

Lie::SE3 T_ac_mu(R_ac, t_ac); Eigen::MatrixXd Sigma_Joint(12,12); auto refs = Lie::make_uncertain_state( std::make_tuple(T_ab_mu, T_ac_mu), Sigma_Joint); auto T_ab_uncertain = std::get<0>(refs); auto T_ac_uncertain = std::get<1>(refs);

The Lie::make_uncertain_state function returns either a single $\mathrm{SE}(3)$ reference object or a tuple of such objects that can then be used to perform operations.

X-B Performing Operations

The pose composition, inverse, and relative pose operations can be applied interchangeably regardless of whether or not individual $\mathrm{SE}(3)$ objects are known or uncertain via the Lie::compose, inverse, and Lie::between functions respectively. The compiler automatically determines whether or not the individual objects are known/unknown or independent/jointly distributed and apply the appropriate formulation.

// Composing known poses auto T_13 = Lie::compose(T_12, T_23);

// Invert both known and uncertain poses auto T_21 = T_12.inverse(); auto T_ba_uncertain = T_ab_uncertain.inverse();

// Calculating relative pose auto T_bc_uncertain = Lie::between( T_ab_uncertain, T_ac_uncertain); auto T_bc_uncertain2 = Lie::between( T_ab_known, T_ac_uncertain);

Each function returns a new (from then on assumed independent) $\mathrm{SE}(3)$ reference object that can be used for additional operations as necessary.

X-C Additional Lie groups

In addition, because the uncertainty propagation method we present simply uses the properties of Lie groups, it can easily be applied to other Lie group types. We have also implemented the Lie::SO3, Lie::SE2, and Lie::SO2 classes for the $\mathrm{SO}(3)$, $\mathrm{SE}(2)$, and $\mathrm{SO}(2)$ Lie groups and plan to extend it to other Lie group types as needed.

XI Conclusion

Recent interest has shown that pose uncertainty characterization through the use of the Lie algebra leads to more accurate uncertainty propagation. However, recent work assumes that individual poses are independent from one another and have primarily focused on pose composition, ignoring the equally important inverse and relative pose operations.

This paper describes how to represent multiple jointly correlated poses while using the Lie algebra to characterize uncertainty. We also derive the equivalent of the Smith, Self, and Cheeseman [5] pose composition, pose inverse, and relative pose operations when using the proposed framework. Finally, we have released an open source C++ library implementation of our method.

The proposed methods can be used to increase the accuracy of data association consistency checks when extracting pose uncertainty information from a pose graph SLAM solution [31]. It can also be used to compose odometry measurements that are potentially correlated with one another such as can be the case in the presence of wheel slip. By accurately modeling pose uncertainty, the proposed method increases the robustness and reliability of autonomous navigation.