How do I approximate a covariance in $\langle \mathbb{R}^3, SO(3) \rangle$ as a covariance in $SE(3)$?

Question

I am working with two different code bases. One represents the state of the system as $\langle \mathbb{R}^3, SO(3) \rangle$ while the other (correctly) represents the state of the system as $SE(3)$. I need to use information from the first source in the second source. This requires me to convert the state and its covariance from the $\langle \mathbb{R}^3, SO(3) \rangle$ to an $SE(3)$ representation. Converting the state itself seems simple; it's the covariance that is more difficult.

Pretty much everything I know about Lie groups comes from "A micro Lie theory for state estimation in robotics". Combining the information from Example 7 of this paper with my knowledge of covariance matrices, here are my thoughts.

For a simple vector space, consider a random vector $\boldsymbol{x}$ with mean $\bar{\boldsymbol{x}}$. The covariance of $\mathbf{x}$ is given by \begin{align} \mathbf{P}_{\mathbf{x}\mathbf{x}} &= \mathbb{E} \left[ \left( \mathbf{x} - \bar{\mathbf{x}} \right) \left( \mathbf{x} - \bar{\mathbf{x}} \right)^\text{T} \right] \label{eq:Pxxdefinition} . \end{align}

By analogy with the above equation, the $SE(3)$ covariance is \begin{align} \mathbf{P} &= \mathbb{E} \left[ \left( \mathbf{T} \ominus \bar{\mathbf{T}} \right) \left( \mathbf{T} \ominus \bar{\mathbf{T}} \right)^\text{T} \right] \label{eq:PSe3def} . \end{align}

And the $\langle \mathbb{R}^3, SO(3) \rangle$ covariance is \begin{align} \mathbf{Q} &= \mathbb{E} \left[ \left( \mathbf{T} \boxminus \bar{\mathbf{T}} \right) \left( \mathbf{T} \boxminus \bar{\mathbf{T}} \right)^\text{T} \right] \label{eq:PSO3xR3def} . \end{align}

with the $\ominus$ and $\boxminus$ subtraction operators defined in Example 7 of the above paper (except that my $\boxminus$ here should be a \diamondminus).

Now I've attempted to expand each of these expressions to find a valid approximation. Starting with $SE(3)$, we can expand $\ominus$ operator such that \begin{align} \mathbf{T}_2 \ominus \mathbf{T}_1 &= \begin{bmatrix} \mathbf{V} ( \text{Log} (\mathbf{R}_1^\text{T} \mathbf{R}_2 ) )^{-1} \mathbf{R}^\text{T}_1 \left( \mathbf{p}_2 - \mathbf{p}_1 \right) \\ \text{Log} (\mathbf{R}^\text{T}_1 \mathbf{R}_2) \end{bmatrix} = \begin{bmatrix} \Delta \boldsymbol{\rho} \\ \Delta \boldsymbol{\theta} \end{bmatrix} \label{eq:SE3minusoperator}. \end{align}

The $\langle \mathbb{R}^3, SO(3) \rangle$ minus operator can similarly be expanded as \begin{align} \mathbf{T}_2 \boxminus \mathbf{T}_1 &= \begin{bmatrix} \left( \mathbf{p}_2 - \mathbf{p}_1 \right) \\ \text{Log} ( \mathbf{R}^\text{T}_1 \mathbf{R}_2 ) \end{bmatrix} = \begin{bmatrix} \Delta \mathbf{p} \\ \Delta \boldsymbol{\theta} \end{bmatrix} = \begin{bmatrix} \mathbf{R}_1 \mathbf{V} ( \Delta \boldsymbol{\theta} ) \Delta \boldsymbol{\rho} \\ \Delta \boldsymbol{\theta} \end{bmatrix} \label{eq:SO3xR3minusoperator}. \end{align}

So while $$ \mathbf{P} = \mathbb{E} \begin{bmatrix} \Delta \boldsymbol{\rho} \Delta \boldsymbol{\rho}^\text{T} & \Delta \boldsymbol{\rho} \Delta \boldsymbol{\theta}^\text{T} \\ \Delta \boldsymbol{\theta} \Delta \boldsymbol{\rho}^\text{T} & \Delta \boldsymbol{\theta} \Delta \boldsymbol{\theta}^\text{T} \end{bmatrix}, $$

$$ \begin{align} \mathbf{Q} &= \mathbb{E} \begin{bmatrix} \Delta \mathbf{p} \Delta \mathbf{p}^\text{T} & \Delta \mathbf{p} \Delta \boldsymbol{\theta}^\text{T} \\ \Delta \boldsymbol{\theta} \Delta \mathbf{p}^\text{T} & \Delta \boldsymbol{\theta} \Delta \boldsymbol{\theta}^\text{T} \end{bmatrix} \\ &= \mathbb{E} \begin{bmatrix} \mathbf{R}_1 \mathbf{V} ( \Delta \boldsymbol{\theta} ) \Delta \boldsymbol{\rho} \left( \mathbf{R}_1 \mathbf{V} ( \Delta \boldsymbol{\theta} ) \Delta \boldsymbol{\rho} \right)^\text{T} & \mathbf{R}_1 \mathbf{V} ( \Delta \boldsymbol{\theta} ) \Delta \boldsymbol{\rho} \Delta \boldsymbol{\theta}^\text{T} \\ \Delta \boldsymbol{\theta} \left( \mathbf{R}_1 \mathbf{V} ( \Delta \boldsymbol{\theta} ) \Delta \boldsymbol{\rho} \right)^\text{T} & \Delta \boldsymbol{\theta} \Delta \boldsymbol{\theta}^\text{T} \end{bmatrix} \\ \end{align} $$

I'm not sure how to simplify this to get something more useful. Ideally I'd like to find some function such that $$ \mathbf{P} \approx convert(\mathbf{Q}) $$

Edit: The above math suggests that an exact conversion can be given by $$ \mathbf{P} = \mathbf{H} \mathbf{Q} \mathbf{H}^\text{T} $$ where $$ \mathbf{H} = \begin{bmatrix} \mathbf{V}^{-1} \mathbf{R}_1^\text{T} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} \end{bmatrix} $$

however, I am leery of this answer because I don't think there should exist an exact conversion. I know that an $SE(3)$ covariance matrix can represent uncertainty in a banana-shaped distribution while $\langle \mathbb{R}^3, SO(3) \rangle$ cannot represent such distributions. See "The Banana Distribution is Gaussian: A Localization Study with Exponential Coordinates" for more details.

Answer 1

The fundamental problem in your analysis is that the $\Delta \rho$ in the $SE(3)$ case and the $\Delta \rho$ in the $(SO(3)\times\mathbb{R}^3)$ case are not the same thing.

I think it's easiest to consider how perturbing the different components of each "error" space would change the associated group.

• $SE(3)$: If I perturb the translation part of the lie algebra what happens? Remember that the translation part is "curved" by the rotation in the screw motion. This is why we have the $V$ term in the exponential. We are taking that linear velocity-like term and curling it by recursively multiplying in the rotation (think about the definition of $\exp(A)$). Therefore, if I "bump" the $\mathbf{t}_x$-component of the lie algebra, then the change in the resulting translation is going to be dependent on the rotation. And the first-order approximation will have a dependency on $V$ to predict the final translation change. It can be helpful for me to think of the algebra of $SE(3)$ as the 6-DOF body-frame velocities that would yield some transform after 1 second of integration.
• $\left(SO(3)\times\mathbb{R}^3\right)$: There is no curving motion. If I bump the x-component of the "algebra" then it will exactly correspond to a change in the translation, no dependence on the rotation. This would be like pinning the linear velocities in an inertial frame and moving only along that direction while also rotating. If I want to convert from $\left(\mathfrak{so}(3)\times\mathbb{R}^3\right)$ to $\mathfrak{se}(3)$ (note the algebra) it makes sense to me that there is a conversion happening in the translation part. They describe different things. "How different" you ask? Well, in how much curving motion is being captured by the object. This is encapsulated in $V$.

So, then how do I convert a covariance represented in $\mathfrak{se}(3)$ to $\left(\mathfrak{so}(3)\times\mathbb{R}^3\right)$?

It's going to depend on a couple of factors:

1. The "sided-ness" of the covariances. (left or right side)
2. If there are also any coordinate frame transformations from one parameterization to the other.

Without knowing those items, I can't say for sure what it is, but there is definitely some (likely non-linear) function $$ y = f\left(x\right)\\ x\in \mathfrak{se}(3),\;y\in\left(\mathfrak{so}(3)\times\mathbb{R}^3\right) $$ that converts from one parameterization to the other. Given this and the respective covariance matrices $$ \Sigma_x=E\left[xx^\top\right], $$ and $$ \Sigma_y=E\left[yy^\top\right]. $$ we can derive $\Sigma_y$ with a Taylor Series approximation. $$ \begin{aligned} \Sigma_y&=E\left[yy^\top\right] \\ &=E\left[f(x)f(x)^\top\right] \\ &\approx E\left[\frac{\partial f}{\partial x}x\left(\frac{\partial f}{\partial x}x\right)^\top\right] & (\mathrm{taylor\ series})\\ &= \frac{\partial f}{\partial x} E\left[xx^\top\right]\frac{\partial f}{\partial x}^\top\\ &= \frac{\partial f}{\partial x} \Sigma_x \frac{\partial f}{\partial x}^\top \end{aligned} $$

You may find these tables helpful when working out the jacobian $\frac{\partial f}{\partial x}$

So what do you lose? The $AQA^\top$ is going to end up converting the "banana" into an ellipse. There is no way around this, since covariance matrices can't represent anything other than ellipsoids. The only reason that a covariance over $\mathfrak{se}(3)$ can be projected into a banana is because we are really capturing uncertainty that then gets "curled". It's actually an ellipsoid on the manifold. When we map it into euclidean coordinates then we see that it looks like a banana.

Where will this break down? We need to think about how this is actually a linearization. It's going to have problems wherever the magnitude of the rotation uncertainty is large relative to the magnitude of translation uncertainty. Basically trying to fit an ellipsoid to a banana gets harder the more like an arc the banana is. Luckily, you're dealing with uncertainties which should always be small.