A Novel Distribution for Representation of 6D Pose Uncertainty

Abstract

The 6D Pose estimation is a crux in many applications, such as visual perception, autonomous navigation, and spacecraft motion. For robotic grasping, the cluttered and self-occlusion scenarios bring new challenges to the this field. Currently, society uses CNNs to solve this problem. The CNN models will suffer high uncertainty caused by the environmental factors and the object itself. These models usually maintain a Gaussian distribution, which is not suitable for the underlying manifold structure of the pose. Many works decouple rotation from the translation and quantify rotational uncertainty. Only a few works pay attention to the uncertainty of the 6D pose. This work proposes a distribution that can capture the uncertainty of the 6D pose parameterized by the dual quaternions, meanwhile, the proposed distribution takes the periodic nature of the underlying structure into account. The presented results include the normalization constant computation and parameter estimation techniques of the distribution. This work shows the benefits of the proposed distribution, which provides a more realistic explanation for the uncertainty in the 6D pose and eliminates the drawback inherited from the planar rigid motion.

1. Introduction

The Pose Estimation of rigid objects has a wide range of applications in today’s life, such as robot grasping [1], aerospace applications, autonomous driving [2], and so on.

Throughout history, pose estimation has played a vital role, from Ceres’s position predicting to navigation on the sea ([3], Introduction 1.1). One day, Captain cook sailing in the sea has lost his bearings. The ship moves on a wavy and wind sea. With the reasonable assumption, He estimates the optimal localization of the ship from the noisy measurements (Figure 1, top).

New challenges come to this field. The success of service robotics relies on well-perceiving an unfamiliar kitchen scenarios, such as bottles, dishes, and containers. Robot grasps under cluttered or limited lighting environments are becoming common in a rapidly growing warehouse automation industry [4]. Consequently, robots need to know the full 6D pose of objects to manipulate objects, which is difficult due to the uncertainty caused by environmental factors and sensing technologies. The intuition is to deploy expensive high-precision sensors to avoid such uncertainty. However, all sensors have limited precision, and measurements derived from actual sensors have associated uncertainty ([3], Introduction 1.2).

The CNN [5] broke through in the ImageNet [6] challenge, motivating more and more works to embrace deep learning. However, most prior works in this area failed to take the weak information [7] and the 6D pose uncertainty into account, and they only provide a single best guess for each object pose [8,9,10]. The accurate metric of the model cannot handle unprepared situations, such as dim lighting. The uncertainty will rise with the increase in accuracy over time.

The pose estimation of symmetric objects is currently the most challenging task [11]. Existing multiple correct poses for the same visual appearance [12]. This issue also results in lousy training performance since a network receives inconsistent loss signals. Xiang et al. [9] design an MSE loss function based on quaternions and regress the 3D rotation using the neural network. It reports better performance for symmetric objects such as bowls. However, they do not take the underlying structure of the manifolds into consideration.

Deng et al. [13] detect the object’s position by a bounding box using a neural network. They assume the Gaussian position and approximate the observation likelihood to obtain the conditioning orientation density by a Rao–Blackwellized particle filter. The result is considerably robust in rotational uncertainty. However, this approach is computationally expensive, and it is not clear which kind of distribution holds on the orientations. Furthermore, uncertain rotation axes need to be considered when underlying uncertainty is not axis aligned [14].

Nonetheless, the neural network correctly assumes the underlying structure of the data is robust. Prokudin [15] exhibits a good example dealing with periodic circular data. The same loss will emerge when the predicted angle is equal to $1°$ or $359°$ with the ground truth angle of $0°$. He presents a loss based on the directional setting and assumes the angle obeys the von Mises distribution. As a result, the model works suitably for the data with the periodic nature.

The Gaussian assumption is commonly embedded, leading the neural network to fail to learn the underlying manifold structure of the 6D pose. Holding the robust assumption of uncertainty is the foremost for 6D pose estimation.

Inevitably, coupling or decoupling is the first consideration when parametrizing the 6D pose. It is also related to further modeling uncertainty. Daniilidis [16] employs dual quaternion to parameterize the 6D pose for hand–eye calibration and establishes a linear homogeneous system, simultaneously solving rotation and translation. Goddard and Addi [17] capture the correlation between rotation and translation for rigid motion tracking by dual quaternions.

Horn [18] proposes the method that decouples the orientation from translation. Inspired by Horn, Srivatsan [19] makes a linear assumption on the state of the $SE\left(3\right)$ elements by adopting dual quaternions. Li [20] explains the correlation between rotation and translation terms based on hyperspherical parallel transport and gives a Bingham distribution on the orientation decoupled from the translation. Manhardt [12] illustrates the rotation uncertainty caused by occlusion in the 6D pose estimation by employing Bingham distribution. However, it is still unknown how the Bingham distribution extends to $SE\left(3\right)$ space without variations.

The work’s most Bingham distribution of interest is motivated by Glover. He presents the BPA [21] algorithm to recover the full poses from patches of local features of the 3D point cloud. This work extends from the $SE\left(2\right)$ [22].

Main Contribution

In this work, we aim to model the uncertain 6D pose by extending the probability density function on the $SE\left(2\right)$ to the $SE\left(3\right)$. This is achieved through parameterizing the 6D pose by dual quaternions. The work simplifies a future generalization to the 6D pose estimation of objects (Figure 1, bottom). The proposed distribution appears as a marginal distribution for the rotational part of the proposed model. We further discuss the relationship between orientation and position. To the best of the author’s knowledge, there is no work exactly claimed for this.

This paper is organized as follows. We will compare several existing 6D parameterization techniques and select the most promising one in Section 2. In Section 3, we propose a new probability distribution on the 6D pose. The normalization constant and parameter estimation techniques will be discussed. Later, in Section 4, we use the proposed framework to represent the transformation of the 6D pose, and the algorithm that recovers the transformation from the samples of our proposed distribution will be presented. Lastly, the discussion and conclusions of this work are in Section 5.

2. Preliminaries

There are several existing methods to parameterize $SE\left(3\right)$ elements, including orthonormal rotation matrices, Euler angles, Rodrigues vectors, quaternions, etc. For orthonormal rotation matrices, the compounding of two elements is much more complex ([23], p. 45). Although Euler angles are invariant under transforms and easy to comprehend, there exist singularities [24]. Rodrigues vectors are complex to implement as a composition algorithm [25]. For parameterization of the 3D rotation, the quaternion is the best from the analog computation view [26]. Still, it is also limited to representing rotation in a full 6D pose, and the translation must be dealt with separately.

The dual quaternions this work introduced takes both rotation and translation into consideration. It provides a closed-form solution for the composition of 6D poses, which is the analogy to the transform matrix in homogeneous coordinates [27]. Kenwright [28] finds that the transforms by the dual quaternion multiplication 10 percent faster compared to matrix multiplication on average. Nonetheless, Kavan et al. [29] present a practical example, and they utilize dual quaternions to solve the shortage of the linear blend skinning.

Conventions. In this work, lower-case letters represent scalars, and matrices are represented by capital letters, vectors, and quaternions in bold. Quaternions are distinguished from dual quaternions by a caret, e.g., $\tilde{\mathit{q}}$ denotes a quaternion and $\widehat{\mathit{q}}$ denotes a dual quaternion. Two quaternions $\tilde{\mathit{p}}$ and $\tilde{\mathit{q}}$ multiplication is denoted as $\tilde{\mathit{p}}\odot \tilde{\mathit{q}}$, while the multiplication of the two dual quaternions is denoted as $\widehat{\mathit{p}}\otimes \widehat{\mathit{q}}$. The dot product and cross product of vectors $\mathit{v}$ and $\mathit{w}$ are denoted as $\langle \mathit{v},\mathit{w}\rangle$ and $\mathit{v}\times \mathit{w}$, respectively. Finally, we use $\mathbb{H}$ represent the skew-field of quaternions. With this in mind, consider the quaternions $\tilde{\mathit{q}}\in \mathbb{H}$. The pose is synonymous with the 6D pose or transformation; the rotation is synonymous with orientation; the translation is synonymous with the position.

2.1. Quaternions

Similar to complex numbers, the sum of a real number and three complex numbers represent quaternions. The quaternion $\tilde{\mathit{q}}\in {\mathbb{R}}^4$ can be treated as a 4-tuple $(q_0,q_1,q_2,q_3)$. It can also be identified with the typical basis elements i, j, k via the coefficients:

$$\mathbb{H}=\{\tilde{\mathit{q}}|\tilde{\mathit{q}}=q_0+q_{1}i+q_{2}j+q_{3}k,q_0,q_1,q_2,q_3\in \mathbb{R}\}.$$

(1)

The multiplication of the two quaternions $\tilde{\mathit{p}}=(p_0,\mathit{p})$, $\tilde{\mathit{q}}=(q_0,\mathit{q})$ is given by:

$$\tilde{\mathit{p}}\odot \tilde{\mathit{q}}=p_0{q}_0-\langle \mathit{p},\mathit{q}\rangle +q_0\mathit{p}+p_0\mathit{q}+\mathit{p}\times \mathit{q},$$

(2)

where $p_0,q_0$ names the scalar part and $\mathit{p},\mathit{q}\in {\mathbb{R}}^3$ is the vector part; note that the product is not commutative.

Furthermore, linear operators [30] ${\mathit{Q}}_p^{+}\mathrm{and}{\mathit{Q}}_q^{-}$$\to {\mathbb{R}}^{4\times 4}$ and associated with (2) can be also defined by matrix-vector form as:

$$\tilde{\mathit{p}}\odot \tilde{\mathit{q}}={\mathit{Q}}_p^{+}·\tilde{\mathit{q}}={\mathit{Q}}_q^{-}·\tilde{\mathit{p}},$$

(3)

with

$$\begin{array}{c}\hfill {\mathit{Q}}_p^{+}=\left[\begin{array}{cc}{p}_0& -{\mathit{p}}^T\\ \mathit{p}& {\mathit{p}}^{\times }+p_0{\mathit{I}}_3\end{array}\right],{\mathit{Q}}_q^{-}=\left[\begin{array}{cc}{q}_0& -{\mathit{q}}^T\\ \mathit{q}& -{\mathit{q}}^{\times }+q_0{\mathit{I}}_3\end{array}\right].\end{array}$$

where ${\left[\right]}^{\times }$ is the skew-symmetric matrix generated from the corresponding vector.

The canonical norm on $\mathbb{H}$ of quaternions is defined by $\left|\right|\tilde{\mathit{q}}\left|\right|=\sqrt{\tilde{\mathit{q}}\odot {\tilde{\mathit{q}}}^{*}}=\sqrt{{\tilde{\mathit{q}}}^{*}\odot \tilde{\mathit{q}}}=\sqrt{{q_0}^2+{q_1}^2+{q_2}^2+{q_3}^2}.$

The conjugate of the quaternion is obtained by changing the sign of each element in the imaginary part: ${\tilde{\mathit{q}}}^{*}=(q_0,-\mathit{q})=(q_0,-q_1,-q_2,-q_3)$.

Last but not the least, the quaternion addition is simply the 4-tuple addition of quaternion representations: $\tilde{\mathit{p}}+\tilde{\mathit{q}}=(p_0,\mathit{p})+(q_0,\mathit{q})=(p_0+q_0,\mathit{p}+\mathit{q})$.

2.2. Representation of 3D Rotation

Unit quaternions $\tilde{\mathit{q}}$ are quaternions with $\left|\right|\tilde{\mathit{q}}\left|\right|=1$ and commonly represent the rotations in 3D Euclid space. The inverse of a unit quaternion is obtained by its conjugate form ${\tilde{\mathit{q}}}^{-1}={\tilde{\mathit{q}}}^{*}$.

The quaternion can represent the rotation around a 3D axis with a unit-length vector $\mathit{v}$ and the rotation angle $\theta \in [-\pi ,\pi ]$:

$$\tilde{\mathit{q}}=cos\left(\frac{\theta }{2}\right)+\mathit{v}sin\left(\frac{\theta }{2}\right).$$

(4)

A point $\mathit{p}=(p_1,p_2,p_3)\in {\mathbb{R}}^3$ is denoted as the purely imaginary quaternion without real part $\tilde{\mathit{p}}=p_{1}i+p_{2}j+p_{3}k$. From (4), the rotated point ${\tilde{\mathit{p}}}_{rot}$ is obtained as [25]:

$$\begin{array}{c}\hfill {\tilde{\mathit{p}}}_{rot}=\tilde{\mathit{q}}\odot \tilde{\mathit{p}}\odot {\tilde{\mathit{q}}}^{*}.\end{array}$$

In addition, $\tilde{\mathit{q}}$ and $-\tilde{\mathit{q}}$ represent the same rotation due to the antipodal property on the hypersphere ${\mathit{S}}^3$, so the set $\mathit{U}$ of unit quaternion is a double coverage of the $SO\left(3\right)$ of the 3D rotations.

2.3. Dual Quaternions

The dual theory is helpful when understanding the concept of dual quaternions. A dual number combines the non-dual part $a_1$, and the dual part $b_1$ is represented as $a_1+ϵb_1,$ where $a_1,b_1\in \mathbb{R}$. The $ϵ$ is the dual unit; note that $ϵ\ne 0$, ${ϵ}^2=0$. The multiplication of two dual numbers is given as $(a_1+ϵb_1)(a_2+ϵb_2)=a_1{a}_2+ϵ(a_1{b}_2+a_2{b}_1).$

Dual quaternions ($\widehat{\mathit{x}}\in {\mathbb{H}}_D$) are quaternions equivalent to dual numbers, i.e., replacing real numbers $a_1,b_1$ with quaternions $\tilde{\mathit{p}}$ and $\tilde{\mathit{q}}$. Thus, a dual quaternion is given as follows:

$${\mathbb{H}}_D=\left\{\widehat{\mathit{x}}\right|\widehat{\mathit{x}}=\tilde{\mathit{p}}+ϵ\tilde{\mathit{q}},\tilde{\mathit{p}},\tilde{\mathit{q}}\in \mathbb{H}\}.$$

(5)

The multiply operation of two dual quaternions is similar to dual numbers. As two dual quaternions ${\widehat{\mathit{x}}}_1={\tilde{\mathit{p}}}_1+ϵ{\tilde{\mathit{q}}}_1$ and ${\widehat{\mathit{x}}}_2={\tilde{\mathit{p}}}_2+ϵ{\tilde{\mathit{q}}}_2$ (${\widehat{\mathit{x}}}_1,{\widehat{\mathit{x}}}_2\in {\mathbb{H}}_D$), the product of the two is:

$${\widehat{\mathit{x}}}_1\otimes {\widehat{\mathit{x}}}_2={\tilde{\mathit{p}}}_1\odot {\tilde{\mathit{p}}}_2+ϵ\left({\tilde{\mathit{p}}}_1\odot {\tilde{\mathit{q}}}_2+{\tilde{\mathit{p}}}_2\odot {\tilde{\mathit{q}}}_1\right).$$

(6)

Note that the product is non-commutative.

The dual quaternions apply the same strategy utilized by quaternions, linear operators: ${(·)}^{+}$ and ${(·)}^{-}\in {\mathbb{R}}^{8\times 8}$ can be defined for dual quaternions by:

$${\widehat{\mathit{x}}}_1\otimes {\widehat{\mathit{x}}}_2={\mathit{Q}}_{x_1}^{+}·{\widehat{x}}_2={\mathit{Q}}_{x_2}^{-}·{\widehat{x}}_1,$$

with

$${\mathit{Q}}_{x_1}^{+}=\left[\begin{array}{cc}{\mathit{Q}}_{p_1}^{+}& \mathit{O}\\ {\mathit{Q}}_{q_1}^{+}& {\mathit{Q}}_{p_1}^{+}\end{array}\right],{\mathit{Q}}_{x_2}^{-}=\left[\begin{array}{cc}{\mathit{Q}}_{p_2}^{-}& \mathit{O}\\ {\mathit{Q}}_{q_2}^{-}& {\mathit{Q}}_{p_2}^{-}\end{array}\right].$$

Dual quaternions have three conjugates [30]: (1) ${\widehat{\mathit{x}}}^{1*}=\tilde{\mathit{p}}-ϵ\tilde{\mathit{q}}$, (2) ${\widehat{\mathit{x}}}^{2*}={\tilde{\mathit{p}}}^{*}+ϵ{\tilde{\mathit{q}}}^{*}$, and (3) ${\widehat{\mathit{x}}}^{3*}={\tilde{\mathit{p}}}^{*}-ϵ{\tilde{\mathit{q}}}^{*}$. A dual quaternion $\widehat{\mathit{x}}$ is a unit if $\widehat{\mathit{x}}\otimes {\widehat{\mathit{x}}}^{2*}=1$.

The norm of a dual quaternion in (5) is denoted as $\left|\right|\widehat{\mathit{x}}\left|\right|=\sqrt{\widehat{\mathit{x}}\otimes {\widehat{\mathit{x}}}^{2*}}=\sqrt{{\widehat{\mathit{x}}}^{2*}\otimes \widehat{\mathit{x}}}$, which expands to:

$$\begin{array}{c}\hfill \left|\right|\widehat{\mathit{x}}\left|\right|=\sqrt{\widehat{\mathit{x}}\otimes {\widehat{\mathit{x}}}^{2*}}=\left|\right|\mathit{p}\left|\right|+ϵ\frac{\langle \mathit{p},\mathit{q}\rangle }{\left|\right|\mathit{p}\left|\right|}.\end{array}$$

The unit dual quaternions have the norm that equals one. A dual quaternion satisfies the unit dual quaternion if and only if the non-dual part $\left|\right|\mathit{p}\left|\right|$ equals one and the dot product of two parts $\langle \mathit{p},\mathit{q}\rangle$ equals zero. This property helps to understand the relationship between the two parts of the dual quaternion. Especially, a unit quaternion is a unit dual quaternion when the dual part is zero.

2.4. Representation of 6D Pose

The 6D pose composes a 3D rotation part represented by a unit quaternion ${\tilde{\mathit{q}}}_r=[q_0,q_1,q_2,q_3]$ in the form of (4) and a 3D translation part $\mathit{t}\in {\mathbb{R}}^3$. A unit dual quaternion representing the 6D pose is defined by the following equation:

$${\widehat{\mathit{x}}}_d={\tilde{\mathit{q}}}_r+ϵ{\tilde{\mathit{q}}}_d,$$

(7)

where ${\tilde{\mathit{q}}}_d$ for translation $\mathit{t}$ is defined by:

$${\tilde{\mathit{q}}}_d=\frac{\tilde{\mathit{t}}\odot {\tilde{\mathit{q}}}_r}{2},$$

(8)

where $\tilde{\mathit{t}}$ is represented by a purely imaginary translation quaternion with zero real parts.

We introduce the Hamilton operators ${\mathit{H}}^{+},{\mathit{H}}^{-}$ to replace the linear operators defined above. The term Hamilton operator, which is borrowed from Akyar [31], is not commonly used, at least in the robotics literature, but it seems appropriate here:

$${\mathit{H}}_r^{-}=\left[\begin{array}{cc}{q}_0& -{\mathit{q}}^T\\ \mathit{q}& -{\mathit{q}}^{\times }+q_0{\mathit{I}}_3\end{array}\right]=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_q& q_0& q_3& -q_2\\ q_2& -q_3& q_0& q_1\\ q_3& q_2& -q_1& q_0\end{array}\right]$$

so that

$${\tilde{\mathit{q}}}_d=\frac{1}{2}{\mathit{H}}_t^{+}{\tilde{\mathit{q}}}_r=\frac{1}{2}{\mathit{H}}_r^{-}\tilde{\mathit{t}}.$$

(9)

where ${\mathit{H}}_t^{+}$ is a ${\mathbb{R}}^{4\times 4}$ skew-symmetric matrix generated from translation vector $\mathit{t}$.

A point $p={(x,y,z)}^T\in {\mathbb{R}}^3$, we can be embedded in skew-field ${\mathbb{H}}_D$ by using a dual quaternion ${\widehat{\mathit{x}}}_d$ in (7). The transformation can be mathematically described as:

$${\widehat{\mathit{x}}}_d\otimes {\widehat{\mathit{p}}}_d\otimes {\widehat{\mathit{x}}}_d^{3*}$$

(10)

Consistent with the unit quaternion in rotations, the dual quaternions ${\widehat{\mathit{x}}}_d$ and $-{\widehat{\mathit{x}}}_d$ represent the same pose.

3. Methods

3.1. Base Element

Consider the fact that the unit dual quaternions ${\widehat{\mathit{d}}}_{\mathit{q}}$ and $-{\widehat{\mathit{d}}}_{\mathit{q}}$ in the form of (5) represent the same pose. Furthermore, the underlying structure of the periodic nature of the 6D pose is no longer linear. Hence, a distribution that can characterize the non-linear structure antipodal symmetric property is required. Consequently, we can tackle multiple, conflicting hypotheses that naturally arise in ambiguous situations.

A Bingham distribution [32] is exactly a distribution on a unit hypersphere ${\mathit{S}}^{d-1}$. The property of hte most interest is the antipodal symmetric, where $dF(-\mathit{x})=dF\left(\mathit{x}\right)$ (i.e., opposite points on ${\mathit{S}}^{d-1}$ have equal probability). It commonly represents uncertainty on 3D rotations, denoted as $SO\left(3\right)$ mathematically, by the unit quaternion form [33], but not on the 6D pose.

Definition 1.

Let a random vector on the hypersphere${\mathit{S}}^{d-1}=\{\mathit{x}\in {\mathbb{R}}^d:\left|\right|\mathit{x}\left|\right|=1\}\subset {\mathbb{R}}^d$be the unit hypersphere in${\mathbb{R}}^d$. The probability density function(p.d.f)

$$\begin{array}{c}\hfill f:{\mathit{S}}^{d-1}\to \mathbb{R}\end{array}$$

of a Bingham distribution is given by:

$$\begin{array}{c}\hfill f\left(\mathit{x}\right)=\frac{1}{N}·\exp \left({\mathit{x}}^{T}V\mathsf{\Lambda }{\mathit{V}}^T\mathit{x}\right)\end{array}$$

where$\mathit{V}\in {\mathbb{R}}^{d\times d}$is an orthogonal matrix ($\mathit{V}{\mathit{V}}^T={\mathit{V}}^T\mathit{V}={\mathit{I}}_{d\times d}$), describing the orientation, $\mathsf{\Lambda }=\mathrm{diag}({\lambda }_1,\dots ,{\lambda }_{d-1},0)\in {\mathbb{R}}^{d\times d}$ with ${\lambda }_1\le \cdots \le {\lambda }_{d-1}\le 0$ as the concentration matrix, and N as a corresponding normalization constant.

For the representation of small uncertainty, Bingham is almost near the Gaussian distribution, and for the large uncertainty, the Gaussian is worse (or even quite poor) than the Bingham distribution [34].

For those who are familiar with multivariate Gaussian, the parameter matrices $\mathit{V}$ and $\mathsf{\Lambda }$ can be derived via the eigendecomposition of a symmetric matrix $\mathit{C}$ in subsequent Section 3.2 (which is denoted as the inverse covariance matrix ${\mathrm{\Sigma }}^{-\mathit{1}}$ in multivariate Gaussian distribution).

3.2. A New Distribution Model

For a quite intuitive interpretation, we decompose a unit dual quaternion $\widehat{\mathit{x}}$ into $({\mathit{x}}_r,{\mathit{x}}_d)$ in vector form. Thus, a joint probability density over the non-dual part, and the dual part can be denoted as the following Lemma 1.

Lemma 1.

A random vector  $\mathit{x}=({\mathit{x}}_r,{\mathit{x}}_d)\in {\mathbf{S}}^3\times {\mathbb{R}}^4$ is distributed to the proposed distribution, and the p.d.f is:

$$\begin{array}{c}\hfill f({\mathit{x}}_r,{\mathit{x}}_d)={N\left(\mathit{C}\right)}^{-1}·\exp \left({\left(\begin{array}{c}{\mathit{x}}_r\\ {\mathit{x}}_d\end{array}\right)}^T\mathit{C}\left(\begin{array}{c}{\mathit{x}}_r\\ {\mathit{x}}_d\end{array}\right)\right),\end{array}$$

where ${\mathit{x}}_r\in {\mathbf{S}}^3$ and ${\mathit{x}}_d\in {\mathbb{R}}^4$, symmetric and positive definite $\mathit{C}\in {\mathbb{R}}^{8\times 8}$, and a normalization constant $N\left(\mathit{C}\right)$.

It is always possible to break a joint density into the product of two factors. We can work out the details for the joint case by employing the Schur complement. First, let $\mathit{C}$ in Lemma 1 be denoted as:

$$\begin{array}{c}\hfill \mathit{C}=\left(\begin{array}{cc}{\mathit{C}}_{11}& {\mathit{C}}_{12}\\ {\mathit{C}}_{21}& {\mathit{C}}_{22}\end{array}\right)\end{array}$$

with ${\mathit{C}}_{ij}\in {\mathbb{R}}^{4\times 4}$; note that ${\mathit{C}}_{21}={\mathit{C}}_{12}^T$, according to the settings [22], ${\mathit{C}}_{11}$ needs to be symmetric, ${\mathit{C}}_{12}$ may be arbitrary, and ${\mathit{C}}_{22}$ has to be symmetric negative definite to ensure the antipodal symmetry, which we will further discuss in Section 3.3.

Lemma 2.

The proposed probability density function can be rewritten as:

$$\begin{array}{cc}\hfill f({\mathit{x}}_r,{\mathit{x}}_d)=N{\left(\mathit{C}\right)}^{-1}& ·\exp ({\mathit{x}}_r^T{\mathit{A}}_1{\mathit{x}}_r\hfill \\ \hfill & +{({\mathit{x}}_d-{\mathit{A}}_2{\mathit{x}}_r)}^T{\mathit{C}}_{22}({\mathit{x}}_d-{\mathit{A}}_2{\mathit{x}}_r))\hfill \end{array}$$

where${\mathit{A}}_1={\mathit{C}}_{11}-{\mathit{C}}_{12}{\mathit{C}}_{22}^{-1}{\mathit{C}}_{21}$, ${\mathit{A}}_2=-{\mathit{C}}_{22}^{-1}{\mathit{C}}_{21}$.

A proof is given in Appendix A.

From Lemma 2, the rotation part of the dual quaternion evidently appears as a Bingham distribution, which can be derived by marginalizing out the corresponding conditional distribution of the dual part.

Since the dual part, ${\mathit{x}}_d$, combines the rotation and translation by a Hamilton product, a canonical way to describe dependencies between the position and the orientation of a dual quaternion is still unknown [22].

From (9), the dual part given the non-dual part $f\left({\mathit{x}}_d\right|{\mathit{x}}_r)$ can be treated as a multivariate Gaussian distribution $\mathcal{N}({\mathit{A}}_2{\mathit{x}}_r,-\frac{{\mathit{C}}_{22}^{-1}}{2})$. Thus, the joint density function from Lemma 2 can be rewritten as:

$$\begin{array}{cc}\hfill f({\mathit{x}}_r,{\mathit{x}}_d)& =f\left({\mathit{x}}_r\right)f\left({\mathit{x}}_d\right|{\mathit{x}}_r)\hfill \\ \hfill & ={N_{\mathcal{B}}\left({\mathit{A}}_1\right)}^{-1}·\exp \left({\mathit{x}}_r^T{\mathit{A}}_1{\mathit{x}}_r\right)·\hfill \\ \hfill & {N_{\mathcal{G}}}^{-1}·\exp \left({({\mathit{x}}_d-{\mathit{A}}_2{\mathit{x}}_r)}^T{\mathit{C}}_{22}({\mathit{x}}_d-{\mathit{A}}_2{\mathit{x}}_r)\right).\hfill \end{array}$$

where ${\mathit{A}}_1={\mathit{C}}_{11}-{\mathit{C}}_{12}{\mathit{C}}_{22}^{-1}{\mathit{C}}_{21}$, ${\mathit{A}}_2=-{\mathit{C}}_{22}^{-1}{\mathit{C}}_{21}$.

3.3. Normliazation Constant

The Bingham distribution is flexible for 3D rotation represented by the unit quaternion on the hypersphere ${\mathbf{S}}^3$. Although it represents uncertainty, it is still not well propagated in the computer vision and robotics communities because the computation of the normalization constant is complex. The normalization constant, F, in the Bingham distribution is not closed form, which means the normalization constant, F, only has the numerical solution. However, several techniques can overcome this difficulty, such as caching techniques [35] and saddlepoint approximations. It is still an area of active research [14].

Fortunately, the computational burden is alleviated by the method adapted from [22]. The normalization constant is written as follows according to the discussion:

$$\begin{array}{cc}\hfill N\left(\mathit{C}\right)=& N_{\mathcal{B}}\left({\mathit{A}}_1\right)·N_{\mathcal{G}}=\frac{2\pi \sqrt{det\left(-\frac{1}{2}{\mathit{C}}_{22}^{-1}\right)}}{F\left({\mathit{A}}_1\right)},\hfill \end{array}$$

where ${\mathit{A}}_1={\mathit{C}}_{11}-{\mathit{C}}_{12}{\mathit{C}}_{22}^{-1}{\mathit{C}}_{21}$.

Furthermore, this work calculates $N_{\mathcal{B}}\left({\mathit{A}}_1\right)$ by the hypergeometric function [34] of a matrix argument:

$$F\left(\mathsf{\Lambda }\right):=\left|{\mathbf{S}}^d\right|·{}_1{F}_1\left(\frac{1}{2};\frac{d+1}{2};\mathsf{\Lambda }\right),$$

For the case $d=3$, this reduces to:

$$F\left(\mathsf{\Lambda }\right)=2{\pi }^2·{}_1{F}_1\left(\frac{1}{2};\frac{4}{2};z_1,z_2,z_3\right).$$

A statistics library [36] is used for the computation of the normalization constant, F, in this work.

3.4. Parameter Estimation

We hope to obtain the parameters of the proposed distribution given a $d\times N$ sample matrix $\mathit{X}=[{\mathit{x}}_1,\dots ,{\mathit{x}}_N]$, where each column vector is assumed to be generated i.i.d from our proposed distribution. This procedure can be divided into two stages. First, the first four entries of samples will recover the parameter of the Bingham distribution. Second, the eight entries recover the parameter in the Gaussian distribution.

From Lemma 2, we observed that the base element with $d=4$ in Section 3.1 appears a marginal distribution of our model. Thus, we can apply Definition 1:

$$f({\mathit{x}}_r;\mathsf{\Lambda },\mathit{V})=\frac{1}{F\left(\mathsf{\Lambda }\right)}exp\left({{\mathit{x}}_r}^T\mathit{V}\mathsf{\Lambda }{\mathit{V}}^T{\mathit{x}}_r\right)$$

(11)

where ${\mathit{x}}_r\in {\mathbb{R}}^4$ is the first four entries from the proposed distribution.

The parameter $\mathit{V}$ can be obtained as the matrix of eigenvectors of the covariance ${\mathit{A}}_1$ with corresponding eigenvalues $\mathsf{\Lambda }$ in the order ${\lambda }_1\le {\lambda }_2\le {\lambda }_3$, equivalent to the eigendecomposition of ${\mathit{A}}_1=\mathit{V}·diag\left({\lambda }_1,{\lambda }_2,{\lambda }_3\right)·{\mathit{V}}^T$.

The probability density function of a 4D Bingham distribution projected on ${\mathbf{S}}^3$ by a unit vector is shown in Figure 2, which appears in the marginal distribution of the first four entries $x_r$ in our model.

As we have already discussed in Section 3.2, the dual part ${\mathit{x}}_d$ given the non-dual ${\mathit{x}}_r$ is assumed to be a Gaussian distribution $\mathcal{N}({\mathit{A}}_2{\mathit{x}}_r,-\frac{{\mathit{C}}_{22}^{-1}}{2})$. The parameters ${\mathit{A}}_2$ and $-\frac{{\mathit{C}}_{22}^{-1}}{2}$ can be obtained by maximum likelihood estimation of a multivariate linear regression ([37], Theorem 8.2.1).

The mode of proposed distribution is related to the order of the column vector in matrix $\mathit{V}$. It turns to be the last column, according to diagonal entries when we enforce ${\lambda }_1\le \cdots \le {\lambda }_{d-1}$ in $\mathsf{\Lambda }$. It is also possible to swap columns of $\mathit{V}$ without changing the distribution [38]. There exist two correct modes in the proposed distribution because of the antipodal symmetry. For one of the modes $m=(m_r,m_d)$, where $m_r$ is the normalized eigenvector of ${\mathit{A}}_1$ and $m_d={\mathit{A}}_2{m}_r$, the $-m$ is the another correct one.

4. Results

As expected, unit dual quaternions can be used to represent 3D rotation while the dual part is zero. Thus, (4) can be extended to a dual form as:

$${\widehat{\mathit{d}}}_{\mathit{r}}=[cos\left(\frac{\theta }{2}\right)+sin\left(\frac{\theta }{2}\right)(v_{1}i+v_{2}j+v_{3}k)]+ϵ·0$$

(12)

where $\mathit{v}=(v_1,v_2,v_3),\left|\right|\mathit{v}\left|\right|=1$.

This work utilizes dual quaternions to generate the form of the 3D translation. A vector $\mathit{t}={(t_1,t_2,t_3)}^T\in {\mathbb{R}}^3$ embedded in the skew-field ${\mathbb{H}}_D$ is represented by the following:

$${\widehat{\mathit{d}}}_{\mathit{t}}=1+\frac{ϵ}{2}(0+t_{1}i+t_{2}j+t_{3}k)$$

(13)

To combine rotations and translations, the product of two unit dual quaternions is used according to (6). A rotation with a subsequent translation is given as follows:

$$\begin{array}{cc}\hfill {\widehat{\mathit{d}}}_{\mathit{t}}\otimes {\widehat{\mathit{d}}}_{\mathit{r}}& =\left[1+\frac{ϵ}{2}(t_{1}i+t_{2}j+t_{3}k)\right]·\left[cos\left(\frac{\theta }{2}\right)+\mathit{v}sin\left(\frac{\theta }{2}\right)\right]\hfill \\ \hfill & =cos\left(\frac{\theta }{2}\right)+sin\left(\frac{\theta }{2}\right)(v_{1}i+v_{2}j+v_{3}k)+\frac{ϵ}{2}\{\hfill \\ \hfill & -sin\left(\frac{\theta }{2}\right)(v_1{t}_1+v_2{t}_2+v_3{t}_3)\hfill \\ \hfill & +[cos\left(\frac{\theta }{2}\right)t_1+sin\left(\frac{\theta }{2}\right)(v_3{t}_2-v_2{t}_3)]i\hfill \\ \hfill & +[cos\left(\frac{\theta }{2}\right)t_2+sin\left(\frac{\theta }{2}\right)(v_1{t}_3-v_3{t}_1)]j\hfill \\ \hfill & +[cos\left(\frac{\theta }{2}\right)t_3+sin\left(\frac{\theta }{2}\right)(v_2{t}_1-v_1{t}_2)]k\}\hfill \\ \hfill & =:d_0+d_{1}i+d_{2}j+d_{3}k+ϵ(d_4+d_{5}i+d_{6}j+d_{7}k)\hfill \end{array}$$

(14)

A dual quaterinon in the form of (14) is still a unit dual quaternion, and there is no restriction on translation that must be unit-length. According to the defined norm property of the unit dual quaternion, that is, $p_0{q}_0+p_1{q}_1+p_2{q}_2+p_3{q}_3=0$, this property provides ingredients to our algorithm for translation.

According to (5), the first four entries are interpreted as the non-dual part, which describes the rotation, and its last four entries are interpreted as a dual part of ${\mathit{d}}_{\widehat{\mathit{x}}}$. That is:

$${\mathit{d}}_{\widehat{\mathit{x}}}={(d_0,d_1,d_2,d_3,d_4,d_5,d_6,d_7)}^T.$$

(15)

Represented by the vector form, the unit dual quaternion can be written as ${\mathit{d}}_{\widehat{\mathit{x}}}={\tilde{\mathit{q}}}_r+ϵ{\tilde{\mathit{q}}}_d$, decomposing into the non-dual part ${\tilde{\mathit{q}}}_r=(d_0,d_1,d_2,d_3)$ and the dual part ${\tilde{\mathit{q}}}_d=(d_4,d_5,d_6,d_7)$.

Furthermore, the Hamilton operator ${(·)}^{+}\in {\mathbb{R}}^{4\times 4}$ is associated with (8), and the translation part can be derived from (14) as follows:

$${\tilde{\mathit{q}}}_t=2·{\tilde{\mathit{q}}}_r^{*+}·{\tilde{\mathit{q}}}_d,$$

(16)

where ${\tilde{\mathit{q}}}_r^{*}=(q_0,-\mathit{q})=(d_0,-d_1,-d_2,-d_3)$ is the conjugate of ${\tilde{\mathit{q}}}_r$.

Algorithm  1 shows a robust method to generate the rotation angle, axis, and translation in 3D space by sampling from the proposed distribution. Furthermore, this work applies Algorithm 1 to several sets of the samples generated from the proposed distribution, and the results are shown in Figure 3.

Algorithm 1: Recover the corresponding translation and rotation from sampled vector on the proposed distribution.
Input: A sampled random vector ${\mathit{w}}_{\widehat{\mathit{x}}}={(w_0,\dots ,w_7)}^T\in {\mathbb{R}}^8$
/*  $\phantom{(}$Compute the rotation angle$\phantom{(}$	*/
$\varphi \leftarrow 2·atan2(\sqrt{{w_1}^2+{w_2}^2+{w_3}^2},w_0)$
/*  $\phantom{(}$Compute the rotational arbitrary vector$\phantom{(}$	*/
$v_1\leftarrow w_1/sin(\varphi /2)$
$v_2\leftarrow w_2/sin(\varphi /2)$
$v_3\leftarrow w_3/sin(\varphi /2)$
/* $\phantom{(}$Compute the translation$\phantom{(}$	*/
$t_1\leftarrow 2·(w_0{w}_5-w_1{w}_4+w_2{w}_7-w_3{w}_6)$
$t_2\leftarrow 2·(w_0{w}_6-w_1{w}_7-w_2{w}_4+w_3{w}_5)$
$t_3\leftarrow 2·(w_0{w}_7+w_1{w}_6-w_2{w}_5-w_3{w}_4)$
Output: Rotation angle $\varphi$, rotational arbitrary vector $\mathit{v}=(v_1,v_2,v_3)$ and translation vector $\mathit{t}=(t_1,t_2,t_3)$

The 6D pose is obtained by the random vector sampled from the proposed distribution. Each sample from our distribution appears in the antipodal symmetry in rotations. Compared with the planar rigid motion case [22], our distribution breaks the antipodal symmetry in the translation that $\mathit{t}$ and $-\mathit{t}$ are denoted as positions that always have the same probability values.

5. Conclusions

This work proposes a distribution over the 6D pose in order to capture the uncertainty which is typically ignored by the pose estimation task. Our model takes the periodic nature of the underline structure into consideration, such as antipodal symmetry, which is suitable for high-noise regimes. The proposed distribution eliminates the drawbacks from initializing the $SE\left(2\right)$, in which the position is ambiguous due to antipodal symmetry, and our distribution setting is closer to reality.

However, we assume that the Gaussian on the correlation part leads to a better, but still imperfect, answer. Many beautiful properties in the parameter matrix of the proposed distribution have not been explored. Still, a unified distribution exists to couple the rotation and translation.

In the future, we will verify the proposed distribution in practical applications, such point cloud registration, as well as the most important one, the 6D pose estimation task. Last but not least, the combination of the neural network with the proposed probability to regress the object pose is worth endeavoring.