Metrics proposed for measuring the distance between two rigid-body poses: review, comparison, and combination

Abstract

The concept of distance between two rigid-body poses is important in path planning, positioning precision, mechanism synthesis, and in many other applications. In the definition of such a distance, two approaches mainly prevail, which lead to a number of formulas devised to match the needs of motion tasks. Despite the different approaches and formulas, some important theoretical results, which drive toward distance-metrics definitions useful for design and application purposes, have been stated. This paper summarizes the two different approaches together with a critical review of the literature on the distance metrics they generated, and, then, it illustrates a technique, previously proposed by the author, for combining different metrics to obtain novel distance-metric definitions that are tailored to specific applications.

1. Introduction

The concept of distance between two rigid-body poses (positions and orientations) is central for many applications. For instance, path planning, positioning precision, manipulator calibration, and mechanism synthesis are some of them. A real-valued mapping, ρ(x ₁, x ₂), of two six tuples (i.e., ρ: $\mathrm{\mathbb{R}}^{6}\times \mathrm{\mathbb{R}}^{6}\rightarrow \mathrm{\mathbb{R}}$ ), x ₁ and x ₂ that identify two rigid-body poses, can be adopted as “distance metric,” if it is positive definite (i.e., ρ(x ₁, x ₂) > 0 if x ₁ ≠ x ₂ and ρ(x ₁, x ₂) = 0 if x ₁ = x ₂), symmetric (i.e., ρ(x ₁, x ₂) = ρ(x ₂, x ₁)) and satisfies the triangle inequality (i.e., ρ(x ₁, x ₂) ≤ ρ(x ₁, x ₃) + ρ(x ₃, x ₂) for any x ₃ ∈ $\mathrm{\mathbb{R}}^{6}$ ). The introduction of a distance metric into the set of rigid-body poses makes it a particular metric space. Hereafter, the terms “metric” or “distance metric” will refer to a metric of this particular metric space.

A distance metric is said to be bi-invariant if it depends neither on the choice of the reference system fixed to the rigid body (body frame) nor on the choice of the reference system fixed to the observer (inertial frame) (Fig 1a). Moreover, a distance metric is said to be left-invariant (right-invariant), if it does not depend on the choice of the inertial frame (the body frame).

Figure 1. Notations: (a) the inertial frame, fixed to the observer, and the body frame, fixed to the rigid body (points O and B are the origins of the inertial frame and of the body frame, respectively; R and b are the rotation matrix and the position vector (B–O), respectively, that identify the pose of the body frame with respect to the inertial frame], and (b) displacement of the body frame from the first pose to the second pose measured in the inertial frame.

Defining a distance metric in rigid-body’s configuration space (c-space) is a difficult task many researchers have dealt with. In the literature, it has been mainly addressed through two approaches:

1. a. By introducing a distance metric directly in the c-space [1–8];

2. b. By approximating a displacement in the c-space with a spherical or hyper-spherical displacement, and, then, using a distance metric of the spherical, SO(3), or hyper-spherical, SO(N), space [9–16].

Such investigations stated that:

1. i. No bi-invariant Riemannian metric can be defined in the special Euclidean group, SE(3) (see [4] for references);

2. ii. The size of the rigid body must be considered for defining meaningful distance metrics [2, 3];

3. iii. Bi-invariance is not necessary to define meaningful distance metrics [6].

This paper presents a critical review of the most known distance metrics proposed in the literature and a technique, proposed by the author [17], for generating distance metrics tailored for a specific application by suitably combining other metrics. The aim of this work is providing a clear view of the literature on the subject that contains sufficient details to use immediately some results or a suitable combination of them and/or to guide the reader in selecting further readings on the subject.

The paper is organized as follows. Section 2 analyzes the direct introduction of distance metrics in SE(3) (approach (a)). Section 3 discusses the indirect introduction of distance metrics in SE(3) (approach (b)). Then, Section 4 presents the author proposal, and Section 5 draws the conclusions.

2. Distance metrics directly introduced in SE(3)

The natural way to define a distance metric [4] in a differentiable manifold, M, consists in implementing the following three steps:

1. I. Definition of a Riemannian metric. It is worth reminding that a Riemannian metric is a smooth assignment of an inner product, < ·, · >, to the tangent space at each point of the manifold;

2. II. Determination of the geodesic induced by the Riemannian metric, defined in the previous step. It is worth reminding that the geodesic is the curve, X(t) ⊂ M with a ≤ t ≤ b, that minimizes the integral L = $\int _{{a}}^{{b}}\sqrt{\lt \dot{\mathbf{X}},\dot{\mathbf{X}}\gt }\mathrm{dt}$ , by definition, L is the “length” of the curve X(t) measured by using the inner product < ·, · >;

3. III. Calculation of the explicit expression of the length of the geodesic that joins two generic points, say X ₁ and X ₂, of M.

In this procedure, the distance metric is the explicit expression of the geodesic length, determined in the last step.

The special Euclidean group, SE(3), is the set that collects all the rigid-body displacements. Its generic element, X, can be represented by an homogeneous transformation, that is, $\mathbf{X}\triangleq \left[\begin{array}{c@{\quad}c} \mathbf{R} & \mathbf{b}\\[4pt] \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right]$ where (see Fig. 1a) R is the rotation matrix that identifies the orientation of the body frame with respect to the inertial frame, b is the position vector of the body-frame origin (i.e., point B in Fig. 1a) measured in the inertial frame, and 0 is the null vector.

With this representation of X, the law of composition that gives SE(3) the structure of a group is the product of matrices. Moreover, since a rigid-body path, X(t), can be easily defined together with its derivative with respect to the parameter t, hereafter denoted $\dot{\mathbf{X}}({t})$ , SE(3) has the structure of a differentiable manifold (i.e., it is a Lie group). In SE(3), the tangent space at X is the set of matrices $\dot{\mathbf{X}}\triangleq \left[\begin{array}{c@{\quad}c} \dot{\mathbf{R}} & \dot{\mathbf{b}}\\[4pt] \mathbf{0}^{{T}} & 0 \end{array}\right]=\left[\begin{array}{c@{\quad}c} \tilde{\mathbf{\omega }}\mathbf{R}\, & \dot{\mathbf{b}}\\[4pt] \mathbf{0}^{{T}} & 0 \end{array}\right]$ , where $\overset{\cdot }{(\cdot )}$ denotes the derivative of $(\cdot )$ with respect to t, and $\tilde{\mathbf{\omega }} (=\dot{\mathbf{R}}\mathbf{R}^{{T}})$ is the skew-symmetric matrix associated to the vector ω that identifies an possible elementary change of rigid-body orientation at X. It is worth stressing that vector ω, which is a geometric quantity related to a rigid-body path, X(t), would become a possible rigid-body angular velocity if the parameter t would be interpreted as the time. The so-defined tangent space together with the sum of matrices and the scalar multiplication of a matrix by a real number becomes a vector space. Also, it is isomorphic both to the tangent space at the identity element of SE(3) (called Lie algebra and denoted se(3)) and to the set of rigid-body “geometric twists,” defined as {ω ^T , $\dot{\mathbf{b}}^{{T}}$ } ^T , when referred to the inertial frame (body-fixed velocity), or {ω ^T , $(\dot{\mathbf{b}}-\tilde{\mathbf{\omega }}\mathbf{b})^{{T}}$ } ^T , when referred to the body frame (space velocity). Thus, the derivative at X can be uniquely determined either by an element of se(3) or by the corresponding geometric twist.

Hereafter, the geometric twist will be used to identify $\dot{\mathbf{X}}$ and to write the explicit expressions involving $\dot{\mathbf{X}}$ (i.e., if it is not differently specified, it will be assumed $\dot{\mathbf{X}}$ ≡ {ω ^T , $\dot{\mathbf{b}}^{{T}}$ } ^T in the inertial frame and $\dot{\mathbf{X}}$ ≡ {ω ^T , $(\dot{\mathbf{b}}-\tilde{\mathbf{\omega }}\mathbf{b})^{{T}}$ } ^T in the body frame). With this choice, a bi-invariant Riemannian metric could be assigned through the following relationship:

(1) \begin{equation} \lt \dot{\mathbf{X}}_{1},\dot{\mathbf{X}}_{2}\gt \,\triangleq \,\dot{\mathbf{X}}_{1}^{{T}}\mathbf{Q}\,\dot{\mathbf{X}}_{2} \end{equation}

if and only if a symmetric, positive-definite, 6 × 6 matrix, Q, existed which makes $\,\dot{\mathbf{X}}_{1}^{{T}}\mathbf{Q}\,\dot{\mathbf{X}}_{2}$ independent of the reference frame (i.e., inertial or base frame), used to represent $\dot{\mathbf{X}}$ , and of X (i.e., of R and b, which vary when either the inertial frame or the body frame is changed). Such a Q matrix does not exist (see [4] for demonstration and references), whereas symmetric, positive-definite Q matrices that are independent of X (i.e., of the inertial-frame and of the body-frame choices) exist, and, by using them, left-invariant and right-invariant distance metrics can be defined according to the representation of $\dot{\mathbf{X}}$ used in relationship (1).

Among the Q matrices that generate left-invariant or right-invariant metrics through definition (1), the following one has been suggested in refs. [4, 18] for left-invariant metrics:

(2) \begin{equation} \mathbf{Q}=\left[\begin{array}{c@{\quad}c} {c}_{1}\mathbf{I}_{3} & \mathbf{0}_{3}\\[4pt] \mathbf{0}_{3} & {c}_{2}\mathbf{I}_{3} \end{array}\right] \end{equation}

where c ₁ and c ₂ are two positive scalar constants, whose role will be discussed later, whereas 0 ₃ and I ₃ are the null and the identity 3 × 3 matrices, respectively. The left-invariant distance metric generated by using (2) and the inner product (1) is (see ref. [4] for details):

(3) \begin{equation}{\rm \rho}_P (\textbf{X}_1,\textbf{X}_2) = \sqrt{{c}_{1}{\delta} _{{S}}^{2}\left(\mathbf{R}_{1},\mathbf{R}_{2}\right)+{c}_{2}{\delta} _{{T}}^{2}\left(\mathbf{b}_{1},\mathbf{b}_{2}\right)} \end{equation}

where R _i and b _i (Fig. 1b), for i = 1, 2, are the rotation matrix and the position vector, respectively, which locate the i-th rigid-body pose and the corresponding SE(3) element, X _i (i = 1, 2), whereas (the image of cos⁻¹(·) is restricted to the range [0, π] radians, and $| \cdot |$ denotes the Euclidean norm of $\mathrm{\mathbb{R}}^{3}$ ):

(4a) \begin{equation} {\delta_S}(\textbf{R}_1,\textbf{R}_2)=\cos ^{-1}\left(\frac{\mathrm{tr}\left(\mathbf{R}_{1}^{{T}}\mathbf{R}_{2}\right)-1}{2}\right), \end{equation}

(4b) \begin{equation}\delta_T(\textbf{b}_1,\textbf{b}_2) = \left|\textbf{b}_2-\textbf{b}_1\right|\end{equation}

The above-defined δ_S , which is the rotation Euler angle about the Euler axis, and δ_T , which is the Euclidean distance between two points, are the bi-invariant distance metrics of SO(3) and $\mathrm{\mathbb{R}}^{3}$ , respectively. Here, it is worth stressing that the restriction cos⁻¹(·) ∈ [0, π] makes Eq. (4a) select the minimum rotation angle between the two angle values that bring the orientation of the first pose to coincide with the one of the second pose.

The parameters c ₁ and c ₂ together with relationship (3) define a two-parameter family of left-invariant scale-dependent distance metrics. The geometric meaning of a limitation on ρ_P (X ₁, X ₂) (i.e., an inequality of type ρ_P (X ₁, X ₂) < ε) is difficult to be found, while it is clear that c ₁ and c ₂ assign different weights to the rotational and translational parts of the geodesic path that brings the rigid body from X ₁ to X ₂. Analogous considerations can be done on the right-invariant distance metrics generated by using (2) and the inner product (1). Here, it is worth stressing that, since δ_S and δ_T have different measurement units, the introduction of suitable values for c ₁ and c ₂, at least for homogenizing the two terms appearing in Eq. (3), is always necessary, but this introduction makes Eq. (3) intrinsically arbitrary. In ref. [4], Park applied the proposed metrics to planar-mechanism design problems and highlighted the critical role of the values assigned to c ₁ and c ₂ in those specific problems. In particular, he showed that c ₁ and c ₂ play the role of scaling factors that change the contribution to the value of ρ_P (X ₁, X ₂) of the frame rotation and of the frame translation.

Rico-Martinez and Duffy [3] highlighted how critical is the scaling-factor choice for finite bodies through the comparison of two distances referred to a square lamina with a unit-length side (see Fig. 2). The first one, say d ₁, is between a reference pose (the blue one in Fig. 2, located by the points A ₁ and C ₁) and the pose (the red one in Fig. 2, located by the points A ₂ and C ₂) obtained from the reference one through a small counterclockwise rotation of 5° around the axis (z axis in Fig. 2) perpendicular to the lamina plane. The second one, say d ₂, is between the same reference pose and the pose (the orange one in Fig. 2, located by the points A ₃ and C ₃) obtained from the reference one through a translation (b ₂ – b ₁) = (5, 3, 2) (see Fig. 1). Here, the vector components of (b ₂ – b ₁) together with d ₁ and d ₂ are measured by taking the square-lamina side as unit length and the radian as unit angle, which makes all the lengths dimensionless since they are ratios of lengths the same as the angles in radians and overcomes the non-homogeneity issue involved in many definitions of rigid-body metrics. Such a comparison relies on the fact that, due to the lamina sizes, in all the manipulator-design/manipulation-task problems a distance metric that makes sense must give d ₂ >> d ₁. Indeed, if a rigid-body is shrunken until to become a point at the three different poses shown in Fig. 2, a metric correctly defined for those problems must give results that concur with the Euclidean distance between two points. This consideration, over demonstrating that the rigid-body sizes are relevant in metric definitions, made Rico-Martinez and Duffy test a benchmark that many authors adopted to measure the quality of their metrics. In the case of ρ_P (X ₁, X ₂) (see Eq. (3)) [4], d ₁ = 0.0873 $\sqrt{{c}_{1}}$ and d ₂ = 6.164 $\sqrt{{c}_{2}}$ ; thus, the condition d ₂ >> d ₁ yields (c ₂/c ₁) >> 0.0002, which concurs with the results presented in ref. [4].

Figure 2. Rico-Martinez and Duffy [3] test for rigid-body metrics: (a) top view and (b) 3D view.

Over proposing the above-described test, in ref. [3], Rico-Martinez and Duffy showed that SE(3) is an ordered set under the lexicographic order, which is based on the ordered couple (θ, d) where θ is the rotation angle and d is the distance along the screw axis of the displacement, ¹ and that in the general case the characteristic length is a spurious concept. Moreover, in ref. [19], Rico-Martinez showed that, in the comparison between two displacements, if the rotation angle of one is greater, then it is always possible to find a sufficiently large rigid body such that the “length” of its displacement is greater than the other displacement with a smaller rotation angle and provided many argumentations on this effect, which is due to the lexicographic order of SE(3).

Successively, Zefran et al. [5] analyzed the possible Riemannian metrics (i.e., defined through Eq. (1) with a positive-definite Q matrix) and affine connections on SE(3) that could be of interest in kinematic analysis and path planning. Their analysis brought them to demonstrate that

(z.1) There is no Riemannian metric whose geodesics are screw motion (i.e., a displacement in which the rigid body simultaneously translates along and rotates around the finite screw axis, identified by its initial and final poses, with constant translation and rotation velocities);

(z.2) There is a two-parameter family of bi-invariant semi-Riemannian (i.e., with Q matrix of Eq. (1) that is not positive definite) metrics whose geodesics are screw motions; the Q matrix of such metrics is a linear combination of the Killing form and of the Klein form [20], that is, it can be written as follows: $\mathbf{Q}=\left[\begin{array}{c@{\quad}c} {\alpha} \mathbf{I}_{3} & {\beta} \mathbf{I}_{3}\\[4pt] {\beta} \mathbf{I}_{3} & \mathbf{0}_{3} \end{array}\right]$ where α and β are two constants;

(z.3) The left-invariant metrics defined by Eq. (2) are the unique ones compatible with the kinematic connection (see section 4.1 of ref. [5] for the definition), which is the unique symmetric affine connection on SE(3) that produces physically meaningful accelerations (i.e., as defined in rigid-body kinematics).

Moreover, the same authors in refs. [21, 22] investigated the use of Riemannian metrics, defined by means of Eq. (2), to formulate the problem of generating a smooth rigid-body path between two assigned poses as a variational problem on SE(3) that looks for the minimal path (i.e., the geodesic) between the two assigned poses (which, here, are the “boundary conditions” of the variational problem). Their investigations proved through planar and spatial examples that the found minimal path depends on the used metric (i.e., on the coefficients c ₁ and c ₂ appearing in Eq. (2)), on the choice of the body frame and, of course, on the boundary conditions.

The above-reported technique has the advantage of providing definitions, which allow the measure of rigid-body paths’ lengths. Such an appealing feature is not required in many technical applications that involve only discrete poses (e.g., precision points of synthesis problems, generation of a continuous path by interpolation of a finite number of assigned poses, etc.). In these cases, the direct introduction of a distance metric into SE(3) can be done without considering the tangent space. This was done, for instance, by Kazerounian and Rastegar in ref. [2] where they proposed the following family of distance metrics, named “object norms”:

(5) \begin{equation}\rho_\mathrm{KR} (\textbf{X}_1, \textbf{X}_2) = \frac{1}{{V}}\int _{{V}}\mathrm{w}\left(\mathbf{P}\right)\left| \mathbf{P}\left(\mathbf{X}_{2}\right)-\mathbf{P}\left(\mathbf{X}_{1}\right)\right| ^{2}\mathrm{dV} \end{equation}

where V is the volume of the rigid body that assumes the two poses identified by X ₁ and X ₂; P is the position vector, measured in the inertial frame, that locates a generic point P of the rigid body; and w(P) is a weighting function whose values depend on P.

Distance metrics assigned through relationship (5) measure weighted average square errors on the positioning of a given rigid body. They are bi-invariant, even though they depend on the rigid-body shape and are cumbersome to be repeatedly evaluated, for instance in optimization or control procedures, since their evaluation requires the computation of volume integrals. Relationship (5) allows the distance metric to be calibrated so that it matches suitable design criteria by changing the weighting function. As observed in ref. [4], the “object norms” (i.e., definition (5)) are not Riemannian distance metrics on SE(3) since they depend on the rigid-body shape and are not able to measure the length of a curve, X(t), in SE(3). That is why they can be bi-invariant, which is an attractive feature. Unfortunately, when there is no standard shape of the rigid body, as it happens in mechanism design, they are not usable. Also, since they compute an average displacement of the rigid-body points, a limitation on ρ _KR(X ₁, X ₂) (i.e., an inequality of type ρ_KR(X ₁, X ₂) < ε) limits the maximum displacement of the points of a finite-size rigid body in a way that is difficult to evaluate, which is a problem when positioning precision is evaluated as in manipulator calibration or in mechanism synthesis (e.g., motion generation) even when the shape of the rigid body is completely defined. Anyway, definition (5), for the way it is built, always passes the Rico-Martinez and Duffy test [3] (Fig. 2), which brings the conclusion that a well-defined metric, in addition to purely geometric conditions, must also accomplish practical-use requirements.

The computation burden of definition (5) can be eliminated if only a few points fixed to the rigid body are considered so that the integral is transformed into a summation. On this line, Mazzotti et al. [23] propose the use of the vertices of platonic solids embedded in the rigid body by defining the distance metric as the root mean square (RMS) of the distances between the two point-positions these vertices assume in the two rigid-body poses. Such a metric is fast to compute, but the distances it computes depend on the size of the chosen platonic solid and on where the solid is fixed to the rigid body. Successively, Fontana et al. [24] try to solve these issues in the context of point cloud registration by choosing the points on the external surface of the rigid body and by defining the distance metric as the average of the distances between homologous points divided by the distances of the same points from the rigid-body centroid.

Other techniques are usable to introduce a distance metric directly into SE(3). Among these other techniques, it is worth mentioning the direct introduction into SE(3) of distance metrics by the definition of a mapping (kinematic mapping) [25], from the rigid-body c-space into a suitable image projective space, and the successive introduction of a distance metric into the image space of the kinematic mapping. Even though this technique, proposed by Ravani and Roth in ref. [1] for planar motions and successively extended in refs. [26, 27] by using Study’s parameters through a reconfiguration of Study’s soma space into a three-dimensional dual projective space, gives the bases for the indirect introduction of distance metrics, it is a direct introduction. Actually, in principle, the image space is just a different representation of the c-space (i.e., the kinematic mapping practically is a change of coordinates in the c-space) and the idea of approximating an actual displacement with a fictitious one is not present here. In ref. [1], the pose of a rigid body constrained to perform planar motion is given through four parameters, say q_i for i = 1,…, 4, constrained to satisfy the following scalar equation:

(6) \begin{equation} {q}_{3}^{2}+{q}_{4}^{2}=1 \end{equation}

which are interpretable as a special case of Study’s parameters in the plane under constraint (6).

The quadruplet $\textbf{q} \triangleq \{{q}_{1},{q}_{2},{q}_{3},{q}_{4}\}^{{T}}$ , named planar quaternion, identifies a point that lies on an hypersurface, σ, of $\mathrm{\mathbb{R}}^{4}$ . Such a hypersurface is the geometric representation of the projective image space into which the c-space is mapped. With reference to Fig. 3, if b = {b_x , b_y } ^T is the position vector locating the origin B of the body frame in the inertial frame, and $\phi$ is the rotation angle, positive if counterclockwise, the inertial frame must rotate to have the same orientation of the body frame, the triplet {b_x , b_y , $\phi$ } will collect all the c-space coordinates. Accordingly, the kinematic mapping from the c-space into σ is given by the following relationships:

(7a) \begin{equation} {q}_1=\frac{1}{2}\left[{b}_{{x}}\sin \left(\frac{{\phi} }{2}\right)-{b}_{{y}}\cos \left(\frac{{\phi} }{2}\right)\right] \end{equation}

(7b) \begin{equation} q_2 = \frac{1}{2}\left[{b}_{{x}}\sin \left(\frac{{\phi} }{2}\right)+{b}_{{y}}\cos \left(\frac{{\phi} }{2}\right)\right] \end{equation}

(7c) \begin{equation} q_3 = \sin \left(\frac{{\phi} }{2}\right) \end{equation}

(7d) \begin{equation} q_4 = \cos \left(\frac{{\phi} }{2}\right) \end{equation}

Then, in the image space, the following distance metric is defined [1]:

(8) \begin{equation} \rho_{RR}(\mathbf{q}_{1}, \mathbf{q}_{2}) = \sqrt{\left(\mathbf{q}_{2}-\mathbf{q}_{1}\right)^{{T}}\left(\mathbf{q}_{2}-\mathbf{q}_{1}\right)} \end{equation}

where q ₁ and q ₂ are the two planar quaternions corresponding to the two poses whose distance has to be computed. Definition (8) is the Euclidean norm of $\mathrm{\mathbb{R}}^{4}$ , it is scale-dependent, and it is not bi-invariant [3]. In ref. [1], Ravani and Roth show how to use definition (8) in the dimensional synthesis of four-link planar mechanisms, thus proving the effectiveness of definition (8) in mechanism synthesis. Nevertheless, the design constraint given through a limitation on ρ _RR(q ₁, q ₂) (i.e., an inequality of type ρ _RR(q ₁, q ₂) < ε) is difficult to identify. Rico-Martinez and Duffy test [3] can be used for planar displacements by considering only the x and y components of the translation displacement, that is, b_x = 5 and b_y = 3 for the pose A₃C₃ (Fig. 2), which corresponds to consider only the projections of the square lamina onto the xy-coordinate plane (Fig. 2a). In addition, points A₁, A₂, and A₃ (see Fig. 2a) are chosen as positions of body-frame’s origin. So doing, the values d ₁ = 1 and d₂ = 2.3452 are obtained through definition (8). This result satisfies the condition d₂ >> d ₁ and concurs with the conclusion reported in ref. [1] for the four-bar dimensional synthesis.

Figure 3. Inertial frame and body frame in planar motion.

On the same line, in ref. [26, 27], this approach has been extended to spatial motions. In ref. [26], Ravani and Roth showed that Euclidean geometry with spatial displacements as elements corresponds to elliptic geometry of points in a projective dual three-dimensional space and that Study’s eight parameters are usable to define the mapping of spatial kinematics into points of this projective dual three-dimensional space. Then, in this projective space (Study’s soma), they defined the distance between two points by using their normalized coordinates, which are normalized dual quaternions, thus getting an analytic expression of the distance that appears as the dot product of two four-dimensional unit vectors. Eventually, they proved the effectiveness of their proposal by applying it to the motion analysis of four-bar spatial mechanisms. Nevertheless, Rico-Martinez and Duffy [3] proposed and applied their above-described test (Fig. 2) specifically for this spatial extension computing d ₁ = 5 and d₂ = 0 (i.e., the condition d₂ >> d₁ is not satisfied for the spatial extension) thus proving that metrics coming from this approach are not suitable for solving spatial synthesis problems, which always involve finite rigid bodies. Later, in ref. [27], Eberharter and Ravani used a stereographic projection of Study’s quadric to define a local affine space where the Euclidean definition of a metric can be used for rigid body displacements and techniques from design of curves and surfaces can be directly utilized for motion design. The so-defined local metrics depend on the chosen type of stereographic projection, and an optimization technique aiming at choosing a stereographic projection with minimal local distortions is necessary. They tested their approach with the Rico-Martinez and Duffy test [3] to see whether the limitation of proposal [26] was over; so doing, they found d ₁ = 0.0436 and d ₂ = 3.0822, that is, the condition d ₂ >> d ₁ is satisfied in the new approach and only the scale-dependence problem, which is common to other metrics, is still present in it, but it can be eliminated through a suitable scaling factor. In addition, they advised that their approach is not able to compute a rigid body pose exactly half way between two other poses since their local metric is not defined in the Euclidean space where the two others are defined. Eventually, they proved the effectiveness of their proposal in motion design through a numerical example.

In the context of path planning, Kuffner [28] presented a comparison on the use of different orientation parameters. In [28], the distance metric is defined as the sum of two terms multiplied by arbitrary weights: one term is the Euclidean norm of the translation parameters and the other is a norm built only with the orientation parameters whose expression changes according to the used parameters but remains simple to compute. The result of this comparison is that utilizing unit quaternions to represent the rotation component is both efficient and effective for path planning, and it is recommended over other alternatives such as Euler angles.

3. Distance metrics indirectly introduced in SE(3)

In $\mathrm{\mathbb{R}}^{3}$ , a spherical motion is a mapping σ: $\mathrm{\mathbb{R}}^{3}\rightarrow \mathrm{\mathbb{R}}^{3}$ that does not change the distances between points (i.e., corresponds to a rigid motion) and keeps one point fixed. Such a motion is representable through the motion of an oriented great-circle arc on a sphere centered at the fixed point and, accordingly, it is sometimes named two-spherical motion since a sphere in $\mathrm{\mathbb{R}}^{3}$ is a two-dimensional manifold. By analogy, in $\mathrm{\mathbb{R}}^{{n}}$ , an (n–1)-spherical motion is a mapping ψ: $\mathrm{\mathbb{R}}^{{n}}\rightarrow \mathrm{\mathbb{R}}^{{n}}$ that does not change the distances between points and keeps one point fixed, that is, it is representable through the motion of an oriented great-circle arc on a hypersphere centered at the fixed point. In this case, the name comes from the fact that an hypersphere in $\mathrm{\mathbb{R}}^{{n}}$ is an (n–1)-dimensional manifold.

McCarthy [9] showed how planar and spatial rigid-body motions can be approximated by spherical and three-spherical motions, respectively. He exploited the fact that the motion of an oriented great-circle arc on a sphere with radius r completely identifies a rigid-body spherical motion centered at the sphere center, and that, if r is sufficiently large, the spherical motion approximately describes a planar motion in a plane tangent to the sphere.

By formalizing this concept, he stated a first-order correspondence in r ⁻¹ between the 3 × 3 homogeneous transformation matrix, which identifies the pose of a rigid body constrained to perform planar motion, and a rotation matrix of SO(3). Then, he generalized this approximation technique till to state a first-order correspondence in r ⁻¹ between the 4 × 4 homogeneous transformation matrix, which identifies the finite motion that brings the body frame from the coincidence with the inertial frame to its actual pose, and a 4 × 4 orthogonal matrix with determinant equal to 1, which is an element of SO(4) and identifies a six-parameter non-physical motion, named three-spherical motion. These correspondences can be seen as kinematic mappings, from the c-space into SO(3) or SO(4), which depend on the parameter r ⁻¹. Once they are stated, the distance metrics of the image space (i.e., SO(3) or SO(4)) can be adopted as distance metrics of the c-space [11], as same as it was done in ref. [1]. In the remaining part of this section, this procedure will be briefly illustrated.

The 16 entries of a 4 × 4 orthogonal matrix satisfy 10 scalar conditions: 4 conditions impose that the columns are unit vectors, and 6 more conditions impose that the 4 columns are mutually orthogonal. Therefore, they can be parameterized as a function of only six parameters. The four column vectors of such a matrix constitute a basis of $\mathrm{\mathbb{R}}^{4}$ , and they can be considered four-dimensional unit vectors that identify the directions of four coordinate axes, say x, y, z, and w, that, when combined two by two, locate six coordinate hyperplanes: xy, yz, zx, xw, yw, and zw. An elementary rotation in $\mathrm{\mathbb{R}}^{4}$ can be defined as a four-dimensional motion of a four-dimensional vector, with constant magnitude, that changes only two components of that vector measured in the reference basis. The four-dimensional vector collecting the new components after the rotation can be obtained by multiplying a suitable 4 × 4 orthogonal matrix by the four-dimensional vector collecting the old component (i.e., the ones before the rotation). Such suitable matrices will be called elementary rotation matrices. Since six couples of coordinate axes can be counted in $\mathrm{\mathbb{R}}^{4}$ , as many elementary rotations (one for each coordinate hyper-plane) can be defined in $\mathrm{\mathbb{R}}^{4}$ , and the associated elementary rotation matrices are (c_(·) and s_(·) denote cos(·) and sin(·), respectively)

(9a) \begin{equation} \textbf{H}_{xy}(\phi) = \left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} \mathrm{c}_{\phi } & -\mathrm{s}_{\phi } & 0 & 0\\[4pt] \mathrm{s}_{\phi } & \mathrm{c}_{\phi } & 0 & 0\\[4pt] 0 & 0 & 1 & 0\\[4pt] 0 & 0 & 0 & 1 \end{array}\right];\, \textbf{H}_{zx}(\psi) = \left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} \mathrm{c}_{\psi } & 0 & \mathrm{s}_{\psi } & 0\\[4pt] 0 & 1 & 0 & 0\\[4pt] -\mathrm{s}_{\psi } & 0 & \mathrm{c}_{\psi } & 0\\[4pt] 0 & 0 & 0 & 1 \end{array}\right] ;\end{equation}

(9b) \begin{equation} \textbf{H}_{yz}(\theta) = \left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0\\[4pt] 0 & \mathrm{c}_{\theta } & -\mathrm{s}_{\theta } & 0\\[4pt] 0 & \mathrm{s}_{\theta } & \mathrm{c}_{\theta } & 0\\[4pt] 0 & 0 & 0 & 1 \end{array}\right];\, \textbf{H}_{xw}(\alpha) =\left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} \mathrm{c}_{\alpha } & 0 & 0 & \mathrm{s}_{\alpha }\\[4pt] 0 & 1 & 0 & 0\\[4pt] 0 & 0 & 1 & 0\\[4pt] -\mathrm{s}_{\alpha } & 0 & 0 & \mathrm{c}_{\alpha } \end{array}\right] ;\end{equation}

(9c) \begin{equation} \textbf{H}_{yw}(\beta)=\left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0\\[4pt] 0 & \mathrm{c}_{\beta } & 0 & \mathrm{s}_{\beta }\\[4pt] 0 & 0 & 1 & 0\\[4pt] 0 & -\mathrm{s}_{\beta } & 0 & \mathrm{c}_{\beta } \end{array}\right];\,\textbf{H}_ {zw}(\gamma) = \left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0\\[4pt] 0 & 1 & 0 & 0\\[4pt] 0 & 0 & \mathrm{c}_{{\gamma} } & \mathrm{s}_{{\gamma} }\\[4pt] 0 & 0 & -\mathrm{s}_{{\gamma} } & \mathrm{c}_{{\gamma} } \end{array}\right] ;\end{equation}

Moreover, the generic element, H, of SO(4) can always be written as follows:

(10) \begin{equation} \textbf{H}=\textbf{J}(\alpha, \beta, \gamma)\, \textbf{K}(\theta,\psi,\phi) \end{equation}

with

(11a) \begin{equation}\textbf{J}(\alpha,\beta,\gamma) =\textbf{H}_{zw}(\gamma)\,\textbf{H}_{yw}(\beta)\,\textbf{H}_{xw}(\alpha)=\left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} \mathrm{c}_{{\alpha} } & 0 & 0 & \mathrm{s}_{{\alpha} }\\[4pt] -\mathrm{s}_{{\alpha} }\mathrm{s}_{{\beta} } & \mathrm{c}_{{\beta} } & 0 & \mathrm{c}_{{\alpha} }\mathrm{s}_{{\beta} }\\[4pt] -\mathrm{s}_{{\alpha} }\mathrm{c}_{{\beta} }\mathrm{s}_{{\gamma} } & -\mathrm{s}_{{\beta} }\mathrm{s}_{{\gamma} } & \mathrm{c}_{{\gamma} } & \mathrm{c}_{{\alpha} }\mathrm{c}_{{\beta} }\mathrm{s}_{{\gamma} }\\[4pt] -\mathrm{s}_{{\alpha} }\mathrm{c}_{{\beta} }\mathrm{c}_{{\gamma} } & -\mathrm{s}_{{\beta} }\mathrm{c}_{{\gamma} } & -\mathrm{s}_{{\gamma} } & \mathrm{c}_{{\alpha} }\mathrm{c}_{{\beta} }\mathrm{c}_{{\gamma} } \end{array}\right] \end{equation}

(11b) \begin{equation} \textbf{K}(\theta,\psi,\phi) =\textbf{H}_{xy}(\phi)\,\textbf{H}_{zx}(\psi)\,\textbf{H}_{yz}(\theta)=\left[\begin{array}{c@{\quad}c} \mathbf{R}\left({\theta},{\psi},{\phi} \right) & \mathbf{0}\\[4pt] \mathbf{0}^{{T}} & 1 \end{array}\right] \end{equation}

where R(θ, ψ, $\phi$ ) is an element of SO(3) (i.e., a 3 × 3 rotation matrix) parameterized through the angles θ, ψ, and $\phi$ that are the roll, pitch, and yaw angles of the RPY parameterization [29]. It is worth noting that the subset {H∈ SO(4)|α= β= γ= 0} of SO(4) is a different representation of SO(3) made through 4 × 4 matrices (i.e., in this representation, SO(3) is a subgroup of SO(4)).

The generic element, $\mathbf{X}\triangleq \left[\begin{array}{c@{\quad}c} \mathbf{R} & \mathbf{b}\\[4pt] \mathbf{0}^{{T}} & 1 \end{array}\right]$ , of SE(3) can be mapped into an element H(b, R) of SO(4) through the following analytic relationships (b = {b_x , b_y , b_z } ^T ):

(12) \begin{equation} {\alpha} =\frac{{b}_{{x}}}{{r}} ;\, {\beta} =\frac{{b}_{{y}}}{{r}} ;\, {\gamma} =\frac{{b}_{{z}}}{{r}} ;\end{equation}

where r is a parameter denoting the radius of an hypersphere of $\mathrm{\mathbb{R}}^{4}$ . The coordinates of a generic point, P, lying on this hypersphere can be given through the following four-dimensional position vector:

(13) \begin{equation} \hat{\mathbf{P}}={r}\left\{\begin{array}{c} \mathrm{s}_{{\xi} }\mathrm{c}_{{\eta} }\mathrm{c}_{{\zeta} }\\[4pt] \mathrm{s}_{{\eta} }\mathrm{c}_{{\zeta} }\\[4pt] \mathrm{s}_{{\zeta} }\\[4pt] \mathrm{c}_{{\xi} }\mathrm{c}_{{\eta} }\mathrm{c}_{{\zeta} } \end{array}\right\}\, \end{equation}

where ξ, η, and ζ are three angular parameters that locate the point on the four-dimensional hypersphere. It is worth noting that $\hat{\mathbf{P}}^{{T}}\hat{\mathbf{P}}$ is always equal to r ² (i.e., the equation of the hypersphere, $\hat{\mathbf{P}}^{{T}}\hat{\mathbf{P}}$ = r ², is satisfied).

A position vector, p = {p_x , p_y , p_z } ^T , which locates a point P of $\mathrm{\mathbb{R}}^{3}$ , can be mapped into a four-dimensional position vector, $\hat{\mathbf{P}}$ , which locates a point of the hypersphere, through the following analytic relationships:

(14) \begin{equation} {\xi} =\frac{{p}_{{x}}}{{r}} ;\, {\eta} =\frac{{p}_{{y}}}{{r}} ;\, {\zeta} =\frac{{p}_{{z}}}{{r}} ;\end{equation}

If the radius, r, of the hypersphere is sufficiently greater than the components of b and of p, so that the angles α, β, γ, ξ, η, and ζ, given by the linear mappings (12) and (14), are small enough to approximate their sine with the argument and their cosine with 1, the following relationship holds:

(15) \begin{equation} \mathbf{H}=\left[\begin{array}{c@{\quad}c} \mathbf{R} & \frac{1}{{r}}\mathbf{b}\\[4pt] \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] ;\, \hat{\mathbf{P}}=\left\{\begin{array}{c} \mathbf{p}\\[4pt] {r} \end{array}\right\} \end{equation}.

The analysis of Eq. (15) shows that, by increasing r, the hypersphere points can be confused with the points of the hyper-plane w = r, which gives a particular homogeneous representation of the points of $\mathrm{\mathbb{R}}^{3}$ . Moreover, it reveals that the product H $\hat{\mathbf{P}}$ is exactly equal to {(R p + b) ^T , r} ^T , which gives the homogeneous coordinates of the point P, located by p in $\mathrm{\mathbb{R}}^{3}$ after the displacement, X of SE(3), given by R and b, so matrix H can be considered another representation of the element X of SE(3). Therefore, the mapping of SE(3) into SO(4) given by Eq. (10), together with Eqs. (12), (13), and (14), identifies a correspondence between 4 × 4 matrices that tend to coincide with one another when r increases.

An one-to-one correspondence between the elements of SO(4) and suitable couples of unit quaternions, named biquaternions, has been presented in ref. [30]. By exploiting this correspondence, the following distance metric of SO(4), which becomes a distance metric of SE(3) through relationships (11) and (12), has been proposed in ref. [11]:

(16) \begin{equation}\rho_\mathrm{EM}(\textbf{H}_1, \textbf{H}_2) = \sqrt{\left(\hat{\mathbf{g}}_{2}-\hat{\mathbf{g}}_{1}\right)\left(\hat{\mathbf{g}}_{2}-\hat{\mathbf{g}}_{1}\right)^{*}+\left(\hat{\mathbf{h}}_{2}-\hat{\mathbf{h}}_{1}\right)\left(\hat{\mathbf{h}}_{2}-\hat{\mathbf{h}}_{1}\right)^{*}} \end{equation}

where H ₁ and H ₂ are the two elements of SO(4) whose distance has to be measured, $(\hat{\mathbf{g}}_{{i}},\hat{\mathbf{h}}_{{i}})$ for i = 1, 2 is the couple of unit quaternions corresponding to H _i , and (·)^* denotes the conjugate quaternion of (·). In ref. [11], Etzel and McCarthy applied the Rico-Martinez and Duffy test [3] (Fig. 2) to their metric (Eq. (16)) by using three different values of the hypersphere radius, r, and obtained the results reported in Table I. The analysis of Table I reveals that the greater is r, the lower is the weight of the translational component of the displacement with respect to the one of the rotational components of the displacement, which is not affected by the adopted r value, that is, r plays the role of a scaling factor. In addition, the same analysis shows that by increasing r always an r value is reached that makes condition d ₂ >> d ₁ not satisfied. So r must be sufficiently high to make the angles α, β, γ, ξ, η, and ζ (see Eqs. (12) and (14)) small enough to approximate their sine with the argument and their cosine with 1, but not too high for keeping condition d ₂ >> d ₁ (i.e., the Rico-Martinez and Duffy test) satisfied. In ref. [11], the relationship r = L/ε ^0.5, where ε is the maximum position error introduced when approximating SE(3) with SO(4) and L is the maximum length characterizing the workspace (i.e., the maximum displacement of rigid-body’s points), is deduced. In the case of Fig. 2, where L = 5, this relationship gives r = 50 for ε = 0.01. Unfortunately, Table I for r = 50 gives d ₂ > d ₁, but not d ₂ >> d ₁, which brings the conclusion that error greater than 0.01 must be accepted to have the Rico-Martinez and Duffy test fully satisfied. Despite this, Etzel and McCarthy [11] showed with one example that their metric (Eq. (16)) computes a mid-pose between two other assigned poses that has a very low dependence on the body-frame choice and that really appears as located between the two assigned poses more or less in the middle, which would make their metric suitable for the mechanism synthesis.

Table I. Results of the Rico-Martinez and Duffy test [3] for definition (16) (all the parameters are dimensionless since the length of the square-lamina side (Fig. 2) is the length unit and the angles are measured in radians).

Relationship (16), proposed in ref. [11], comes from a distance metric that is bi-invariant in SO(4). Another bi-invariant distance metric of SO(4) can be defined as follows [16]:

(17) \begin{equation} \rho_L(\textbf{H}_1, \textbf{H}_2) = \left\| \mathbf{I}_{4}-\mathbf{H}_{2}\mathbf{H}_{1}^{{T}}\right\| _{{F}} \end{equation}

where I ₄ is the 4 × 4 identity matrix, and $\| \mathbf{A}\| _{{F}}$ denotes the Frobenius norm of matrix A. In ref. [16], definition (17) is used after having computed through the polar decomposition (PD) of the 4 × 4 homogeneous matrix X∈SE(3), to be approximated, an orthogonal matrix H∈SO(4) that is demonstrated [31] to be the nearest one to X. Purwar and Ge [32] demonstrated that both ρ_EM(H ₁,H ₂) (definition (16)) and ρ_L(H ₁,H ₂) (definition (17)) can be computed with an unified method that exploits a particular biquaternion-based approach [30]. In this context, defining a bi-invariant distance metric of SO(4) induces an approximate bi-invariant distance metric of SE(3) [12, 13] and, also, studies on the inertial-frame choice that is more suitable to use for this purpose have been presented in refs. [33, 34].

With reference to any type of metric defined on SE(3), Chirikjian, in ref. [35], introduced the concept of partial bi-invariant metric. He stressed the fact that many metrics, which are left- or right-invariant when the whole SE(3) group is considered, can be bi-invariant when only specific displacement subsets of SE(3) are considered and called this property “partial bi-invariance.” Indeed, for instance, all the metrics obtained through a linear combination of a bi-invariant metric of SO(3) (e.g., definition (4a)) and of a bi-invariant metric of the spatial-translation subgroup (e.g., definition (4b)) are bi-invariant when only pure rotations or only pure translations are considered. In ref. [35], he explicitly cited definition (3) [4] and the below-reported definition (18) [17] to illustrate this case. Then, he went in depth on this aspect and demonstrated that, for any left-invariant metric, there is a five-dimensional subspace of SE(3) where it is bi-invariant, and that, in the general case, there is an eleven-dimensional space of pose pairs whose distances are bi-invariant. Later, Chirikjian et al., in ref. [36], showed that, when changes in pose are viewed from an inertial frame, the space of pose changes, named PCG(3), can be endowed with a direct product group structure, which is different from the semi-direct product structure of SE(3) and that, in PCG(3), a bi-invariant metric can be defined. Nevertheless, the same authors stressed that the actions in PCG(3) are very different from the ones in SE(3).

Eventually, Ge et al. [37] proved that dual quaternions are an efficient tool for solving the problem of computing an average (or mean) displacement from a set of given spatial displacements when a suitably defined distance metric based on relative displacements is adopted.

4. Combination of different SE(3) distance metrics

This author, in a previous paper [17], has proposed a technique for generating new distance metrics of SE(3) by combining already defined distance metrics that can be either directly or indirectly introduced in SE(3). Such a technique brings one to determine distance metrics that contain arbitrary coefficients into their final expressions. The presence of arbitrary coefficients has the advantage that they are computable to make the distance metric fit particular design criteria, which is relevant for engineering applications even though the introduction of such coefficients could be questionable from the theoretical assessment’s point of view. In this section, this technique is illustrated.

A distance metric in an n-dimensional c-space (n ≤ 6) is a real-valued mapping d: $\mathrm{\mathbb{R}}^{{n}}\times \mathrm{\mathbb{R}}^{{n}}\rightarrow \mathrm{\mathbb{R}}$ of type d(y ₁, y ₂), where y ₁, y ₂ ∈ $\mathrm{\mathbb{R}}^{{n}}$ identify two poses in the c-space, that satisfies the following properties:

d(y ₁, y ₂) = d(y ₂,y ₁); (symmetry)

d(y ₁, y ₂) > 0 if y ₁ ≠ y ₂ and d(y ₁, y ₂) = 0 if y ₁ = y ₂; (positive definiteness)

d(y ₁, y ₂) ≤ d(y ₁, y ₃) + d(y ₃, y ₂) for any y ₃ ∈ $\mathrm{\mathbb{R}}^{{n}}$ ; (triangle inequality)

The following statement holds:

Statement 1. If d_i (y ₁, y ₂) for i = 1,…, m are m different distance metrics defined on the same n-dimensional c-space, then any linear combination, δ(y ₁, y ₂), with non-negative coefficients of the d _i(y ₁, y ₂) is a distance metric of the same c-space, provided that at least one coefficient be different from zero (in formulas: δ(y ₁, y ₂) ∈ { $\sum _{{i}=1, {m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{2})$ |(a_i ≥ 0) & (∃k : a_k ≠ 0)}).

Proof.

1. – Symmetry: since d_i (y ₁, y ₂) = d_i (y ₂, y ₁), then $\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{2})$ = $\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{2},\mathbf{y}_{1})$ ; that is, δ(y ₁, y ₂) = δ(y ₂, y ₁).

2. – Positive definiteness: since each d_i (y ₁, y ₂) is positive definite and, for the coefficient a_i , the following properties hold (a_i ≥ 0) & (∃k : a_k ≠ 0), then $\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{2})$ > 0 if y ₁ ≠ y ₂ and $\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{2})$ = 0 if y ₁ = y ₂; that is, δ(y ₁, y ₂) is positive definite.

3. – Triangle inequality: since each d_i (y ₁, y ₂) satisfies the triangle inequality and, for the coefficient a_i, the following properties hold (a_i ≥ 0) & (∃k : a_k ≠ 0), then

$\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{2})$ ≤ $\sum _{{i}=1,{m}}{a}_{{i}}[{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{3})+{d}_{{i}}(\mathbf{y}_{3},\mathbf{y}_{2})]$ = $\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{1},\mathbf{y}_{3})$ + $\sum _{{i}=1,{m}}{a}_{{i}}{d}_{{i}}(\mathbf{y}_{3},\mathbf{y}_{2})$ for any y ₃ ∈ $\mathrm{\mathbb{R}}^{{n}}$ ; that is, δ(y ₁, y ₂) satisfies the triangle inequality.

Considering the displacement of a body frame from the coincidence with the inertial frame to an assigned rigid-body pose, and, then, giving the geometric parameters of that displacement is a common way to identify the assigned pose in the c-space. The special Euclidean group, SE(3), collects all the possible displacements of a rigid body. SE(3) admits 10 classes of subgroups [38] with dimension greater than zero and lower than six (lower-mobility subgroups).

The elements of SE(3) are 4 × 4 transformation matrices that depend on six geometric parameters. These parameters can be collected in a six-tuple, say x. Hereafter, for simplifying the notation, the six-tuple x is confused with the element of SE(3) computed through the entries of x. The same notation simplification is used for the subgroups of SE(3). A generic element of SE(3) can be always obtained through the composition of elements that belong to particular lower-mobility subgroups (e.g., one translation composed with one spherical motion). This property allows a rigid-body pose to be parameterized through a six-tuple, x, whose entries can be collected into p n_k -tuples, ^k y with k = 1, …, p and $\sum _{{k}=1,{p}}{n}_{{k}}=6$ , where each ^k y identifies an element of a given lower-mobility subgroup, S_k, with dimension n_k (in formulas: x∈ {(¹ y ^T , …, ^p y ^T ) ^T | ^k y∈ S _k < SE(3)}).

With these notations, the following statement holds:

Statement 2. If δ_k ( ^k y ₁, ^k y ₂) is a distance metric of S_k , then any linear combination, ρ(x ₁, x ₂), where x ₁ = (¹ y ₁ ^T , …, ^p y ₁ ^T ) ^T and x ₂ = (¹ y ₂ ^T , …, ^p y ₂ ^T ) ^T , of the δ_k ( ^k y ₁, ^k y ₂) with positive coefficients is a distance metric of SE(3) (in formulas: ρ(x ₁, x ₂) ∈ { $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})$ | (x ₁ = (¹ y ₁ ^T , …, ^p y ₁ ^T ) ^T ) & (x ₂ = (¹ y ₂ ^T , …, ^p y ₂ ^T ) ^T ) & (∀b_k > 0)}).

Proof.

1. – Symmetry: since δ_k ( ^k y ₁, ^k y ₂) = δ_k ( ^k y ₂, ^k y ₁), then $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})$ = $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{2}^{}},{}^{k}{\mathbf{y}}{_{1}^{}})$ ; that is, ρ(x ₁, x ₂) = ρ(x ₂, x ₁).

2. – Positive definiteness: since each δ_k ( ^k y ₁, ^k y ₂) is positive definite and the coefficient b_k are all positive, then $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})$ > 0 if x ₁ = (¹ y ₁ ^T , …, ^p y ₁ ^T ) ^T ≠ x ₂ = (¹ y ₂ ^T , …, ^p y ₂ ^T ) ^T and $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})$ = 0 if x ₁ = x ₂; that is, ρ(x ₁, x ₂) is positive definite.

3. – Triangle inequality: since each δ_k ( ^k y ₁, ^k y ₂) satisfies the triangle inequality and the coefficient b_k are all positive, then

$\sum _{{k}=1,\mathrm{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})$ ≤ $\sum _{{k}=1,{p}}{b}_{{k}}[{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{3}^{}})+{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{3}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})]$ = $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{1}^{}},{}^{k}{\mathbf{y}}{_{3}^{}})$ + $\sum _{{k}=1,{p}}{b}_{{k}}{\delta} _{{k}}({}^{k}{\mathbf{y}}{_{3}^{}},{}^{k}{\mathbf{y}}{_{2}^{}})$ for any x ₃ = (¹ y ₃ ^T , …, ^p y ₃ ^T ) ^T ∈ $\mathrm{\mathbb{R}}^{6}$ ; that is, ρ(x ₁, x ₂) satisfies the triangle inequality.

Statement 2 makes it possible to generate a large family of distance metrics of SE(3) by decomposing a generic displacement into displacements of lower-mobility subgroups and by defining one distance metric in each subgroup. Once a family of distance metrics is determined, how to select meaningful distance metrics among the members of this family still is an open problem. In the next subsection, this problem will be addressed by looking for distance metrics that have an immediate geometric meaning.

4.1. Applicative example

The decomposition of a generic rigid-body displacement into displacements that belong to lower-mobility subgroups can be implemented in many ways. Nevertheless, only some subgroups have distance metrics that are easy to use and with a straightforward geometric interpretation. The subgroup of the spatial translations, T(3), and the subgroup of the spherical motions, S(3), are among these subgroups. Since any displacement can be obtained by composing one spatial translation with one spherical motion, T(3) and S(3) are usable to decompose spatial displacements.

When a rigid body is constrained to translate, its pose (≡position) is uniquely identified by the coordinates of the origin, B (Fig. 1), of the body frame measured in the inertial frame. The distance metric δ_T (b ₁, b ₂) defined by relationship (4b) is commonly adopted in T(3). δ_T (b ₁, b ₂) is bi-invariant in T(3). Moreover, a limitation on δ_T (b ₁, b ₂) (e.g., δ_T (b ₁, b ₂) < c) has a clear geometric meaning. In fact, it means that the position B₂ of B must be located inside a sphere with center at the position B₁ of B and radius given by the imposed condition (Fig. 4a).

Figure 4. Geometric interpretation of (a) condition δ_T (b ₁, b ₂) < c and of (b) condition δ_S (R ₁, R ₂) $\lt \phi$ .

When a rigid body is constrained to perform spherical motions with the same center, hereafter assumed coincident with the origin of the body frame, its pose (≡orientation) is uniquely identified by the rotation matrix R, whose columns are the three unit vectors of the body-frame axes projected onto the inertial frame (Fig. 1). The set that collects all the rotation matrices is named SO(3), and the above considerations state an isomorphism between S(3) and SO(3). The distance metric δ_S (R ₁,R ₂), defined by relationship (4a), can be adopted in SO(3). δ_S (R ₁, R ₂) is bi-invariant in SO(3). It measures the convex rotation angle, in radians, of the finite rotation that brings the first rigid-body orientation to coincide with the second one. On a unit sphere centered at the center of the spherical motion, such an angle measures the length of the great-circle arc between two points that lie on the equatorial circle perpendicular to the rotation axis and coincide with each other after the above-mentioned finite rotation. A limitation on δ_S (R ₁, R ₂) (e.g., δ_S (R ₁, R ₂) < $\phi$ ) has a clear geometric meaning. In fact, it means that each body-frame axis at the second orientation is confined to lie inside a circular cone with vertex at the center of the spherical motion, cone axis coincident with the homologous body-frame axis at the first orientation, and vertex angle given by the imposed condition (Fig. 4b).

The distance metrics (4a) and (4b) and Statement 2 can be used to generate the following family of distance metrics of SE(3):

(18) \begin{equation}\rho_u (\textbf{X}_1, \textbf{X}_2) = \delta_{T}(\textbf{b}_1, \textbf{b}_2)+u\,\delta_{S} (\textbf{R}_1, \textbf{R}_2) \end{equation}

where u is an arbitrary positive constant that is measured with the same measurement unit as δ_T .

The analysis of definition (18) reveals that a limitation on ρ_u (X ₁, X ₂) expressed in the following form:

(19) \begin{equation}\rho_u(\textbf{X}_1, \textbf{X}_2) \lt h \end{equation}

implies the following limitations on δ_T and δ_S , and the associated geometric meanings:

(20a) \begin{equation}\delta_T (\textbf{b}_1, \textbf{b}_2) \lt h \end{equation}

(20b) \begin{equation} \delta_{S} (\textbf{R}_1, \textbf{R}_2) \lt \frac{{h}}{{u}} \end{equation}

Since the value of ρ_u (X ₁, X ₂) depends on the choice of the origin, B, of the body frame, the distance metrics defined by Eq. (18) are, in general, left-invariant, but, as explained in ref. [35], they are bi-invariant if either only pure rotations or only pure translations are considered (i.e., they are partial bi-invariant).

In ref. [17], the author also shows how easy it is to deduce the value of u by imposing a specific applicative condition. Indeed, in ref. [17], the imposition that a limitation on ρ_u (X ₁, X ₂) must limit the maximum displacement of all the rigid-body’s points immediately brought the conclusion that u must be equal to the maximum distance, r _max, of these points from the body-frame origin, B, which also implies that choosing B coincident with the centroid of the rigid body should be in general the best choice. Such a result confirms that the sizes of the rigid body must be considered when meaningful metrics are sought after. Later, Merzić et al. [39] used definition (18) in an algorithm for visual localization and found it valid and efficient to use, whereas Brégier et al. [40] found reasonable the proposed u choice, and Chen et al. [41] used it to build their own metric definition. Then, Kendall et al. [42] and, recently, Ocegueda-Hernández et al. [43] used definition (18) by choosing two different metrics for SO(3) and found it efficient to compute the pose error when estimating the position and orientation of a three-dimensional object from its projection onto a two-dimensional image. It is worth noting that the above-proposed direct deduction of the u value immediately solves the problem of choosing a suitable scaling factor together with the limitations highlighted by Rico-Martinez and Duffy [3] for finite rigid bodies. In particular, for the square lamina (Fig. 2) of the Rico-Martinez and Duffy test [3], choosing the centroid of the square lamina as point B yields u = r _max = $1/\sqrt{2}$ whose introduction into formula (18) gives d ₂ = 6.164 and d₁ = 0.1234, that is, the condition d ₂ >> d ₁ is satisfied. Table II summarizes the above-reported results of the Rico-Martinez and Duffy test [3] for different metric definitions.

Table II. Results of the Rico-Martinez and Duffy test [3] when applied to different metric definitions (all the parameters are dimensionless since the length of the square-lamina side (Fig. 2) is the length unit and the angles are measured in radians).

Eventually, it is worth stressing that even though definition (18) is just a simple applicative example of this combination technique, which can generate many other distance metrics, it shows that this combination technique is more prone to provide definitions with a clear geometric meaning (i.e., Eqs. (19) and (20)) than others. Indeed, for instance, definition (3) that comes out from a specific choice of matrix Q and is somehow similar to definition (18) has a less direct interpretation when used into inequalities like Eq. (19) and the two coefficients that appear in it are more difficult to compute through the imposition of a particular further requirement the metric has to satisfy.

5. Conclusion

In the literature, metrics for measuring the distance between two rigid-body poses are defined mainly through two approaches. The first one aims at directly introducing a metric in rigid-body’s configuration space (i.e., in SE(3)), whereas the second one indirectly introduces it by approximating a displacement of SE(3) with a spherical or hyperspherical displacement, and, then, by using a distance metric of the spherical, SO(3), or hyperspherical, SO(4), space.

These two approaches have generated a number of formulas, each one with its pros and cons. Nevertheless, in the literature, some general theoretical results have been stated: (i) no bi-invariant Riemannian metric can be defined in SE(3), (ii) meaningful distance metrics must take into account the sizes of the rigid-body, (iii) bi-invariance is not necessary for getting meaningful distance metrics, and (iv) the rules for combining different distance metrics to generate other distance metrics have been determined.

Since different distance metrics can be combined to generate other distance metrics, all the possible distance metrics can be seen as a unique family inside which the one that is the most suitable for a specific application can be searched. Accordingly, finding distance metrics tailored on specific applications could be a research field for future works on distance metrics.