Metrics for rigid body motions in SE(3)

Question

As SE(3) does not have any bi-invariant metric, what are the available pseudo metrics for comparing two homogeneous matrices for rigid body motions ? In other words, how to compare two dual quaternions ?

Answer 1

You can use

$$d(T_1, T_2) = \sqrt{a\left\Vert \log(R_1^{T}R_2) \right\Vert^{2} + b\Vert p_2 - p_1 \Vert^{2}},$$

where $T = \begin{bmatrix}R & p\\0 & 1\end{bmatrix}$, $a$ and $b$ are some (arbitrary) scaling factors (scalars), and $\Vert \cdot \Vert$ denotes the Euclidean norm. This metric is left-invariant. (See also papers by Prof. Frank Park, for example. He has a number of works in this area.)

Note that here the logarithm map of a matrix $R \in SO(3), \text{Tr}(R) \neq 1$ is $$\log(R) = \frac{\phi}{2\sin\phi}(R - R^T),$$

where $1 + 2\cos\phi = \text{Tr}(R)$. From the above expression, $\log(R)$ is actually a skew symmetric matrix so we can write $$\log(R) = [r]_{\times},$$ where $r \in \mathbf{R}^3$, and $[\cdot]_\times$ denotes a cross-product matrix. In this sense, the norm of $\log(R_1^TR_2)$ in the first equation should be thought of as the norm of the corresponding vector $r \in \mathbf{R}^3$.

Also, the norm $\left\Vert \log(R_1^{T}R_2) \right\Vert$ is actually the length of the minimal geodesic connecting $R_1$ and $R_2$ (see this paper).