Let $ SO(3) = \{ R \mid RR^\top = I_3 \text{ and } \det(R) = 1 \} $ and $$ SE(3) = \left\{\begin{bmatrix} R & {\bf t} \\ {\bf 0}^\top & 1 \end{bmatrix}\mid R \in SO(3), {\bf t} \in \mathbb{R}^3\right\}. $$ My question is, given two matrices $H_1, H_2 \in SE(3),$ can anyone provide a formula for the geodesic distance between them?

My best guess is to add the magnitude of the rotation and the magnitude of the translation. Then, the the distance between $H_1$ and $H_2$ is given by $$ ||H_1 - H_2|| = ||{\rm Rodrigues}(R_1R_2^\top)||_2 +\frac{1}{2} \left( ||{\bf t}_1 - R_2^\top {\bf t}_2||_2+ ||{\bf t}_2 - R_1^\top {\bf t}_1||_2\right), $$ where I am using Rodrigues' formula to find the angle-axis representation for $SO(3)$ in order to easily find the magnitude of a rotation. This is rather ad-hoc, but it at least it is zero when $H_1=H_2$, strictly positive otherwise, and symmetric. Whether or not it obeys the triangle inequality I do not know for sure, but I think no.

Calin Belta and Vijay Kumar's 2002 paper seems to be relevant, but they are talking about smooth rigid motions parameterized by points in $SE(3)$, and not necessarily just looking at the absolute difference that I am interested in. Maybe its the same, but I don't know enough differential geometry to tell.


There is no bi-invariant metric on $SE(3)$. There is one bi-invariant pseudo-metric (not degenerate, but not positive definite) called the hyperbolic metric. To calculate it you have to go to the algebra $\hat\eta\in\mathfrak{se}(3)$ associated with $h\in SE(3)$.

Taking $\hat \eta_i=\begin{bmatrix} \hat \omega & v \\ 0 &0 \end{bmatrix},$ and then associating $\mathfrak{se}(3)$ with $\mathbb{R}^6$, such that $\eta=\begin{bmatrix} v \\ \omega \end{bmatrix}$, the hyperbolic metric is the quadratic form $\Psi=\begin{bmatrix}0 & I_3 \\ I_3 & 0\end{bmatrix}$. This gives $\langle \eta_1, \eta_2 \rangle_H= v_1^T\omega_2+v_2^T\omega_1.$

A good reference for this is Appendix A, Section 3.2, of A Mathematical Introduction to Robotic Manipulation by Murray, Li and Sastry.