Distance measure for noisy $SE(3)$ transforms

Question

I have a transformation $T \in SE(3)$ parameterized by a mean quaternion $q$ with covariance matrix $\Sigma_q \in R^{4\times4}$ and a mean translation $t \in R^3$ with covariance matrix $\Sigma_t \in R^{3\times3}$.

If I were to transform some point $p_1 \in R^3$ by $T$, I can easily compute its Euclidean distance to some second point $p_2 \in R^3$ as $\lVert T p_1 - p_2\rVert$.

What I would like to do instead is incorporate the covariance matrices of my transformation to compute a distance measure similar in function to a Mahalanobis distance. Is there a closed form way to do this?

2D Example

To help clarify, I've plotted the problem in a two dimensional world below, where points are in $R^2$, and transforms are in $SE(2)$. Here, a point $p_1$ can be transformed by $T$ to find a point $T p_1$. However, because we know the covariances of the rotation and translation, we can see that random samples of $T p_1$ would be distributed according to the banana shaped distribution shown in red. What I would like to find then is a distance measure that would will take these covariances into account such that $p_2$ would be significantly "closer" than $p_3$ to $T p_1$.

Answer 1

A modification of the Mahalanobis distance for quaternion data is presented in section 3.5 of Ultramicroscopy 133, 16-25 (2013).

Answer 2

Even though this question is a little old, I still find it valuable to update with an appropriate answer.

Let us write the Euclidean distance of the transformed point to the target as: $$ \label{eq1} \tag{1} d(\mathbf{p}_1,\mathbf{p}_2) = {\lVert \mathbf{R}\mathbf{p}_1 + \mathbf{t} - \mathbf{p}_2 \rVert}_2^2 $$ where $\mathbf{R}$ and $\mathbf{t}$ are rotation matrix and translation vector, respectively. The Mahalonobis distance gives the distance between two points as: $$ \begin{equation} \label{eq2} \tag{2} d_M(\mathbf{x},\mathbf{y}) = (\mathbf{x}-\mathbf{y})^TC^{-1}(\mathbf{x}-\mathbf{y}) \end{equation} $$ where $C^{-1}$ is the inverse covariance matrix (or equivalently the conic matrix of the ellipse). We could then plug in $\ref{eq1}$ to $\ref{eq2}$ resulting in :

$$ \begin{equation} \tag{3} d_M(\mathbf{p}_1,\mathbf{p}_2) = (\mathbf{R}\mathbf{p}_1 + \mathbf{t} - \mathbf{p}_2)^TC^{-1}(\mathbf{R}\mathbf{p}_1 + \mathbf{t} - \mathbf{p}_2) \end{equation} $$

$\mathbf{R}\mathbf{p}_1 + \mathbf{t}$ can also be viewed as $\mathbf{T}\mathbf{p}_1$, by assembling the augmented matrix $\mathbf{T} = [\mathbf{R}|\mathbf{t}]$.