Given $\mathbf{X} = \exp(\xi^{\wedge})\hat{\mathbf{X}} \in SE(3)$ where $\xi \sim (0, \mathbf{P})$ and $\hat{\mathbf{X}}$ is the mean.

here $\exp : \mathfrak{se}(3) \to SE(3)$ and $(\cdot)^{\wedge} : \mathbb{R}^{6} \to \mathfrak{se}(3)$ thus, we have

$\xi^{\wedge} = \begin{bmatrix} [\xi^{\mathbf{R}}]_{\times} & \xi^{\mathbf{t}} \\ \mathbf{0} & 0\end{bmatrix}$ and I have that $\mathbf{X} = \begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} & 1\end{bmatrix}$

Illustration of the mapping

What I want is to find the equivalent covariance of $\mathbf{P}$ corresponding to the mapping from

$\begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} & 1\end{bmatrix} \to \begin{bmatrix} x \\ y \\ z \\ \phi \\ \chi \\ \psi\end{bmatrix}$ where $(\phi, \chi, \psi)$ is yaw-pitch-roll.

In the MRPT library https://www.mrpt.org/ documentation described in here http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf. In Chapter 2.5.2 they describe almost the same equivalence transformation, however, as I understand it is from an uncertainty representation in the Lie algebra and not from $\mathbb{R}^{6}$.

They use the approximation given $x\sim N(\bar{x}, \Sigma_{x})$:

$\bar{y} = f(\bar{x})$

$\Sigma_{y} = \frac{\partial f(x)}{\partial x}|_{x=\bar{x}}\Sigma_{x}\frac{\partial f(x)}{\partial x}|_{x=\bar{x}}^{T}$


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.