Rewriting exponential function for a se(3) with application to computer perception algorithm

45 Views Asked by At

Problem

$SE(3)$ is the group of isometries in $R^3$ and $se(3)$ its lie algebra.

Let $M \in SE(3), \Delta m \in se(3), n \in se(3)$.

Consider the function $f(\Delta m) = M \exp(n + \Delta m) $. Is there a function $g_1, g_2$ so that $f(\Delta m) = g_1(M, n) \exp(g_2(\Delta m))$? If not, is there a good approximation?

Right now I'm using $f(\Delta m) \approx M \exp(n) \exp(\Delta m)$ and I don't know how bad this is.

Background

I am writing a computer algorithm to do Bayesian inference for elements of $SE(3)$.

Fix some basis of $se(3)$ so that we can equate it with $R^6$ and sensibly define $\exp : R^6 \rightarrow SE(3)$.

Let $M \in SE(3)$ and let $\Delta m$ be a random $R^6$ element distributed as a mean zero gaussian with covariance $\Sigma$.

The prior $P = M \exp( \Delta m)$ can be considered as a sort of gaussian distribution in $SE(3)$ that's "centered" around $M$ and whose parameter $\Sigma$ expresses the spread of uncertainty about where the pose is actually located.

Take it as a given that I have determined the posterior is $P^+ = M\exp(n + \Delta m^+)$ where $n \in R^6$ and $\Delta m^+$ is a zero mean random $R^6$ element with covariance $\Sigma^+$. This is done by, say, operating a Kalman filter in $se(3)$.

To set up the algorithm to continue on to the next iteration, I need to now represent $P^+$ in the form $A \exp(\Delta b)$ where $\Delta b$ is a zero mean gaussian.

Is $P^+$ even representable in such a form? If not, is there an approximation that might be acceptable?

1

There are 1 best solutions below

0
On BEST ANSWER

Our goal is to find an expression of $\Delta y$ that will justify the following approximation expression. $M \exp(n+ \Delta m) \approx M \exp(n) \exp(\Delta y) $

We can use the BCH formula.
$\exp(-n)\exp(n+ \Delta m) \approx \exp(\Delta y) $

To first order $\Delta y \approx-n + (n + \Delta m) + 1/2[-n, (n + \Delta m)] + 1/12[-n,[-n,(n + \Delta m)]]$

The expression is linear in $\Delta m$ and therefore viewing $\Delta m$ and $\Delta y$ as vectors in $R^6$ we will have some matrix $\Delta y = K \Delta x$.

If $\Delta x$ is distributed as a guassian $N(0, \Sigma)$ then $\Delta y$ is distributed $N(0, K \Sigma K^t)$.