Defining the Pose of Any 3D Rigid Object and an Associated Distance

Abstract

The pose of a rigid object is usually regarded as a rigid transformation, described by a translation and a rotation. However, equating the pose space with the space of rigid transformations is in general abusive, as it does not account for objects with proper symmetries—which are common among man-made objects. In this article, we define pose as a distinguishable static state of an object, and equate a pose to a set of rigid transformations. Based solely on geometric considerations, we propose a frame-invariant metric on the space of possible poses, valid for any physical rigid object, and requiring no arbitrary tuning. This distance can be evaluated efficiently using a representation of poses within a Euclidean space of at most 12 dimensions depending on the object’s symmetries. This makes it possible to efficiently perform neighborhood queries such as radius searches or k-nearest neighbor searches within a large set of poses using off-the-shelf methods. Pose averaging considering this metric can similarly be performed easily, using a projection function from the Euclidean space onto the pose space. The practical value of those theoretical developments is illustrated with an application of pose estimation of instances of a 3D rigid object given an input depth map, via a Mean Shift procedure.

Appendices

Appendix A: Distance Simplification for a Revolution Object Without Rotoreflection Invariance

Using the same definition of $\varvec{\varLambda }$ as in Sect. 5.3, the rotation part of the proposed distance for a revolution object without rotoreflection invariance can be rewritten in the following way:

$$\begin{aligned} \begin{aligned}&{{\mathrm{d_{rot}^2}}}(\mathscr {P}_1, \mathscr {P}_2) \\&\quad = \min _{\phi _1, \phi _2} \frac{1}{S} \int _{\mathscr {S}} \mu (\mathbf {x}) \Vert \mathbf {R}_2 \mathbf {R}_z^{\phi _2} \mathbf {x} - \mathbf {R}_1 \mathbf {R}_z^{\phi _1} \mathbf {x}\Vert ^2 ds \\&\quad =\min _{\phi _1, \phi _2} \Vert \mathbf {R}_2 \mathbf {R}_z^{\phi _2} \varvec{\varLambda }- \mathbf {R}_1 \mathbf {R}_z^{\phi _1} \varvec{\varLambda }\Vert _F^2. \end{aligned} \end{aligned}$$

(100)

Frobenius norm being invariant under rotations, this expression can be rewritten with the relative rotation $\mathbf {R} \triangleq \mathbf {R}_1^{-1} \mathbf {R}_2$:

$$\begin{aligned} {{\mathrm{d_{rot}^2}}}(\mathscr {P}_1, \mathscr {P}_2) = \min _{\phi _1, \phi _2} \Vert \mathbf {R}_z^{-\phi _1} \mathbf {R} \mathbf {R}_z^{\phi _2} \varvec{\varLambda }- \varvec{\varLambda }\Vert _F^2. \end{aligned}$$

(101)

We parametrize $\mathbf {R}$ using Euler angles $(\tilde{\psi }, \theta , \tilde{\phi }) \in \mathbb {R}^3$ such as $\mathbf {R} = \mathbf {R}_z^{\tilde{\psi }} \mathbf {R}_x^\theta \mathbf {R}_z^{\tilde{\phi }}$, considering the following elementary rotations:

$$\begin{aligned} \mathbf {R}_z^{\alpha } \triangleq \left( {\begin{matrix} \cos (\alpha ) &{}\quad -\sin (\alpha ) &{}\quad 0 \\ \sin (\alpha ) &{}\quad \cos (\alpha ) &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 1 \end{matrix}} \right) , \mathbf {R}_x^{\alpha } \triangleq \left( {\begin{matrix} 1 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad \cos (\alpha ) &{}\quad -\sin (\alpha ) \\ 0 &{}\quad \sin (\alpha ) &{}\quad \cos (\alpha ) \end{matrix}} \right) . \end{aligned}$$

(102)

Injecting this parametrization into the previous expression and performing the changes of variables $\psi \leftarrow \tilde{\psi } -\phi _1$ and $\phi \leftarrow \tilde{\phi } + \phi _2$ leads us to the following expression:

$$\begin{aligned} \begin{aligned} {{\mathrm{d_{rot}^2}}}(\mathscr {P}_1, \mathscr {P}_2)&= \min _{\phi _1, \phi _2} \Vert \mathbf {R}_z^{-\phi _1} \mathbf {R}_z^{\tilde{\psi }} \mathbf {R}_x^\theta \mathbf {R}_z^{\tilde{\phi }} \mathbf {R}_z^{\phi _2} \varvec{\varLambda }- \varvec{\varLambda }\Vert _F^2 \\&= \min _{\psi , \phi } \Vert \mathbf {R}_z^\psi \mathbf {R}_x^\theta \mathbf {R}_z^\phi \varvec{\varLambda }- \varvec{\varLambda }\Vert _F^2. \end{aligned} \end{aligned}$$

(103)

Because of the specific shape of $\varvec{\varLambda }$ (Eq. 30), the term to minimize can be decomposed into two parts:

$$\begin{aligned}&\Vert \mathbf {R}_z^\psi \mathbf {R}_x^\theta \mathbf {R}_z^\phi \varvec{\varLambda }- \varvec{\varLambda }\Vert _F^2 = \lambda _z^2 \underbrace{\Vert \mathbf {R}_z^\psi \mathbf {R}_x^\theta \mathbf {R}_z^\phi \mathbf {e}_z - \mathbf {e}_z \Vert ^2}_{a_{\psi , \phi }} \nonumber \\&\quad +\, \lambda _r^2 \underbrace{(\Vert \mathbf {R}_z^\psi \mathbf {R}_x^\theta \mathbf {R}_z^\phi \mathbf {e}_x -\mathbf {e}_x \Vert ^2 \!+ \Vert \mathbf {R}_z^\psi \mathbf {R}_x^\theta \mathbf {R}_z^\phi \mathbf {e}_y - \mathbf {e}_y \Vert ^2)}_{b_{\psi , \phi }}.\nonumber \\ \end{aligned}$$

(104)

Developing this expression thanks to the definition of the elementary rotations (102), we evaluate those terms into:

$$\begin{aligned} \left\{ \begin{aligned} a_{\psi , \phi }&= 2(1-\cos (\theta )) \\ b_{\psi , \phi }&= 4 - 2 \cos (\psi + \phi )(1 + \cos (\theta ))). \\ \end{aligned} \right. \end{aligned}$$

(105)

The first term is independent of $\psi$ and $\phi$. The second one can be minimized easily relatively to those two parameters, and admits a minimum that appears to be equal to the first term:

$$\begin{aligned} \min _{\psi , \phi } b_{\psi , \phi } = 2(1-\cos (\theta )). \end{aligned}$$

(106)

This result enables us to estimate the distance between the two poses in a closed form. However, having to refer to a relative rotation between the two poses and perform an Euler decomposition is cumbersome and would not enable to propose a representation of a pose efficient for neighborhood queries. We prefer instead to use the following property

$$\begin{aligned} \begin{aligned} 2(1-\cos (\theta ))&= \Vert \mathbf {R} \mathbf {e}_z - \mathbf {e}_z \Vert ^2 \\&= \Vert \mathbf {R}_2 \mathbf {e}_z - \mathbf {R}_1 \mathbf {e}_z \Vert ^2 \end{aligned} \end{aligned}$$

(107)

in order to express the rotation part of the square distance as a function of the distance between the revolution axes of the object at the two poses:

$$\begin{aligned} {{\mathrm{d_{rot}^2}}}(\mathscr {P}_1, \mathscr {P}_2) = (\lambda _r^2 + \lambda _z^2) \Vert \mathbf {R}_2 \mathbf {e}_z - \mathbf {R}_1 \mathbf {e}_z \Vert ^2. \end{aligned}$$

(108)

Appendix B: Minimum Distance Between Representatives of the Same Pose

In this appendix, we show how to compute the minimum distance T between representatives of the same pose (see the Definition 3) for the objects of our application example.

The bunny and the candlestick admit one representative per pose, hence $T=+\infty$ for those by convention.

The case of the rocket requires some calculus. For the sake of simplicity we consider an object frame whose z axis corresponds to the symmetry axis of the rocket. In this frame, the proper symmetry group of the rocket can be expressed as

$$\begin{aligned} G= \left\{ \mathbf {I}, \mathbf {R}_z^{2 \pi /3}, \mathbf {R}_z^{-2 \pi /3} \right\} \end{aligned}$$

(109)

and the square root of the covariance matrix as

$$\begin{aligned} \varvec{\varLambda }= {{\mathrm{diag}}}(\lambda _r, \lambda _r, \lambda _z). \end{aligned}$$

(110)

We choose to consider the reference pose $\mathscr {P}_0$ and one of its representatives $\mathbf {p}$ (underbraced below) for the computation of T as it makes the computation simpler. Representatives $\mathscr {R}(\mathscr {P}_0)$ of this pose are

$$\begin{aligned} \left\{ \underbrace{\left( \begin{array}{c} {{\mathrm{vec}}}(\varvec{\varLambda }) \\ \mathbf {0}_3 \end{array} \right) }_{\mathbf {p}}, \left( \begin{array}{c} {{\mathrm{vec}}}(\mathbf {R}_z^{2 \pi /3} \varvec{\varLambda }) \\ \mathbf {0}_3 \end{array} \right) , \left( \begin{array}{c} {{\mathrm{vec}}}(\mathbf {R}_z^{-2 \pi /3} \varvec{\varLambda }) \\ \mathbf {0}_3 \end{array} \right) \right\} . \end{aligned}$$

(111)

Thanks to those choices, we can evaluate T into:

$$\begin{aligned} \begin{aligned} T&= \min _{\mathbf {q} \in \mathscr {R}(\mathscr {P}_0), \mathbf {q}\ne \mathbf {p}} \Vert \mathbf {q} - \mathbf {p} \Vert \\&= \min \Vert \mathbf {R}_z^{\pm 2 \pi /3} \varvec{\varLambda }- \varvec{\varLambda }\Vert _F\\&= \sqrt{6} \lambda _r. \end{aligned} \end{aligned}$$

(112)

The threshold $\frac{T}{4}$ of Proposition 9 therefore corresponds for the rocket to the value $\frac{\sqrt{3}}{2} \lambda _r$.

Appendix C: Numerical Recipes for a Triangular Mesh

Center of mass, area and covariance matrix of the surface of a triangular mesh $\mathscr {S} = \bigcup _i \mathscr {T}(\mathbf {a}_i, \mathbf {b}_i, \mathbf {c}_i)$—where $\mathscr {T}(\mathbf {a}, \mathbf {b}, \mathbf {c})$ is a triangle defined by three vertices $\mathbf {a}, \mathbf {b}, \mathbf {c} \in \mathbb {R}^3$—can be computed easily through the contributions of its triangles.

Let $\mathscr {T}(\mathbf {a}, \mathbf {b}, \mathbf {c})$ be a given triangle. Its area can be computed thanks to a cross product:

$$\begin{aligned} S_{\mathbf {a}, \mathbf {b}, \mathbf {c}} = \frac{\Vert (\mathbf {b} - \mathbf {a}) \times (\mathbf {c} - \mathbf {a})\Vert }{2}, \end{aligned}$$

(113)

its center of mass through:

$$\begin{aligned} \mathbf {o}_{\mathbf {a}, \mathbf {b}, \mathbf {c}} = \frac{\mathbf {a} + \mathbf {b} + \mathbf {c}}{3}, \end{aligned}$$

(114)

and its uncentered covariance matrix via:

$$\begin{aligned} \varvec{\sigma }_{\mathbf {a}, \mathbf {b}, \mathbf {c}} = \frac{S_{\mathbf {a}, \mathbf {b}, \mathbf {c}}}{12} \left( 9 \mathbf {o}_{\mathbf {a}, \mathbf {b}, \mathbf {c}} \mathbf {o}_{\mathbf {a}, \mathbf {b}, \mathbf {c}}^\top + \mathbf {a} \mathbf {a}^\top + \mathbf {b} \mathbf {b}^\top + \mathbf {c} \mathbf {c}^\top \right) . \end{aligned}$$

(115)

From those results, we deduce the expression of the surface area of the mesh:

$$\begin{aligned} S= \sum _i S_{\mathbf {a}_i, \mathbf {b}_i, \mathbf {c}_i}, \end{aligned}$$

(116)

its center of mass:

$$\begin{aligned} \mathbf {o}= \sum _i S_{\mathbf {a}_i, \mathbf {b}_i, \mathbf {c}_i} \mathbf {o}_{\mathbf {a}_i, \mathbf {b}_i, \mathbf {c}_i}, \end{aligned}$$

(117)

and its normalized covariance matrix, if the center of mass of the mesh is chosen as origin of the object frame:

$$\begin{aligned} \varvec{\varLambda }^2 = \frac{1}{S} \sum _i \varvec{\sigma }_{\mathbf {a}_i, \mathbf {b}_i, \mathbf {c}_i}. \end{aligned}$$

(118)