MPG - A Framework for Reasoning on 6 DOF Pose Uncertainty  Wendelin Feiten   Muriel Lang  Abstract — Reasoning about the pose, i.e. position and ori- entation   of   objects   is   one   of   the   cornerstones   of   robotic manipulation under uncertainty. In a number of joint research projects our group is developing a robotic perception system that perceives and models an unprepared kitchen scenario with many objects. Since no single sensor or measurement provides sufficient information, a technique is needed to fuse a number of uncertain estimates of the pose, i.e. estimates with a widely stretched probability density function ( pdf   ). The most frequently used approaches to describe the   pdf s   are sample based description and multivariate normal (Gaussian) distributions. Sample based descriptions in 6D can describe basically any type of   pdf s , but they require a large number of samples and there are no analytic formulae to fuse several  pdf s . For Gaussian distributions these formulae exist, but the Gaussian distributions are unimodal and don’t model widely spread distributions well. In this paper we present a framework for probabilistic modeling of 6D poses that combines the expressive power of the sample based description with the conciseness and algorithmic power of the Gaussian models. As parameterization of the 6D poses we select the dual quaternions, i.e. any pose is represented by two quaternions. The orientation part of a pose is described by a unit quaternion. The translation part is described by a purely imaginary quaternion. A basic probability density function over the poses is constructed by selecting a tangent point on the 3D sphere representing unit quaternions and taking the Cartesian set product of the tangent space with the 3D space of translations. In this 6D Euclidean space a 6D Gaussian distribution is defined. Projecting this Gaussian back to the unit sphere and renormalizing induces a distribution over 6D poses, called a Projected Gaussian. A   convex   combination   of   Projected   Gaussians   is   called   a Mixture of Projected Gaussians (MPG). The set of MPG can approximate the probability density functions that arise in our application, is closed under the operations mentioned above and allows for an efficient implementation.  I. I NTRODUCTION  A framework for reasoning on the 6D pose should allow for treating a 6D pose and a rigid motion in the same way. This is important for the propagation of information, e.g. a pose information taken by camera. The pose of this camera is in itself uncertain w.r.t. the common reference frame for several measurements. The pose representation should use as few parameters as possible. This reduces the required memory space, and if more than the minimal number of parameters is used, it makes the renormalization of the parameters easier. The parameters of a composition of rigid motions should follow from   the   parameters   of   the   single   motions   in   a   simple way. Also, the parameters should have no singularities or  Wendelin Feiten is with Corporate Technology, Intelligent Systems & Control, Siemens AG, D-80200 Munich, Germany, Muriel Lang is with Ludwig-Maximilians-University, Munich, Germany  discontinuities. The representation of   pdf s   on the parameter space should interface   with   the   standard   representations,   i.e.   on   the one hand it should be possible to find a correspondence between a representation from the new framework and a sample based description of a   pdf   , and on the other hand a unimodal parametric description (like a Gaussian) should be included in the new framework. Further on, we demand that it is easy to calculate an estimate based on two   pdf s  from the framework that describe the same pose. Last, but not least, it should be straightforward to calculate the   pdf  of a composition of rigid motions from the   pdf s   of the individual rigid motions. Since each position and orientation w.r.t a given coordinate system is the result of a translation and a rotation.   Position  and   translation   can be and will be used synonymously in this paper, as well as   orientation   and   rotation . Also,   pose  and   rigid motion   are used synonymously. In   Section   II   we   will   recapitulate   various   approaches to the parametrization of rigid motions and corresponding probability density functions. None of them fulfills all re- quirements listed above, but they provide ingredients to our synthesis. In Section III we will present our approach to probability density functions over rigid motions. The relation between sample based descriptions and the MPG framework is described in Section IV, and the convergence properties are investigated in Section V. We describe the implementation and experimental results in Section VI. In Section VII we will recollect the presented system and indicate directions of future work. II. R ELATED WORK  The Mixture of Projected Gaussians (MPG) was   first presented by Feiten et al. [1] and then expanded by Muriel Lang in her Diploma Thesis [2]. See there for additional references on previous work concerning the parameterization of the rotation in 3D and the rigid motion. The representation of rigid motions and especially of orientation in three dimensions is a central issue in various disciplines of arts, science and engineering. Rotation matrix, Euler angles, Rodrigues vector and unit quaternions are the   most   popular   representations   of   a   rotation   in   three dimensions.   Rotation   matrices   have   many   parameters, Euler angles are not invariant under transforms and have singularities and Rodrigues vectors do not allow for an easy composition algorithm. Stuelpnagel [3] points out that unit quaternions are a suitable representation of rotations on the hypersphere   S 3   with few parameters, but does not provide  arXiv:1707.01941v1 [cs.RO] 5 Jul 2017
probability distributions. Choe [4] represents the probability distribution   of   rotations   via   a   projected   Gaussian   on   a tangent space. He only deals with concentrated distributions and does not take translations into account. Goddard and Abidi [5], [6] use dual quaternions for motion tracking. In their observations the correlation between rotation and translation   is   captured   also.   The   probability   distribution over the parameters of the state model is a unimodal normal distribution.   If   the   initial   estimate   is   sufficiently   certain and if the information that shall be fused to the estimate is sufficiently well focused this is an appropriate model. As can be seen in [7] from Kavan et al. dual quaternions provide a closed form for the composition of rigid motions, similar to the transform matrix in homogeneous coordinates. Antone [8] suggests to use the Bingham distribution in order to represent weak information even though he does not give a practical algorithm for fusion of information or   propagation   of   uncertain   information.   By   now   it   is known that propagated uncertain information only can be approximated by Bingham distributions. Further Love [9] states that the renormalization of the Bingham distribution is computationally expensive. Glover [10] also works with a   mixture of   Bingham   distributions   and   recommends   to create a precomputed lookup table of approximations of the   normalizing   constant   using   standard   floating   point arithmetic. Mardia et al. [11] use a mixture of bivariate von Mises distributions. They fit the mixture model to a data set using the expectation maximization algorithm because this allows for modeling widely spread distributions. Translations are not treated by them. To propagate the covariance matrix of   a   random   variable   through   a   nonlinear   function,   the Jacobian matrix is used in general. Kraft et al. [12] use therefore an unscented Kalman Filter [13]. This technique would have to be extended to the mixture distributions. From the analysis of the previous work, we synthesize our approach as follows: We use unit quaternions to represent rotations in 3D, and dual quaternions to obtain a concise algebraic description of rigid motions and their composition. The base element of a probability distribution over the rigid motions is a Gaussian in the 6D tangent space, characterized by the tangent point to the unit quaternions and the mean and the covariance of the distribution. Such a base element is called a Projected Gaussian. We use Mixtures of Projected Gaussians to reach the necessary expressive power of the framework. III. P OSE UNCERTAINTY BY   M IXTURES OF   P ROJECTED  G AUSSIAN DISTRIBUTIONS  We assume that the quaternion as such is sufficiently well known to the reader. In order to clarify our notation, at first some basics are restated.  A. Rigid Motion and Dual Quaternions  Let   H   be the quaternions, i.e  H   =   { q | q   =   a   +   ib   +   jc   +   kd   &   a, b, c, d   ∈   R }   (1) where   a   is the real part of the quaternion, and the vector  v   = ( b, c, d ) >   is the imaginary part. The quaternions can be identified with   R 4   via the coefficients,  q   =   a   +   i   ·   b   +   j   ·   c   +   k   ·   d   ∼   [ a, b, c, d ] . The norm and the conjugate are denoted by   ‖ q ‖   and   q . Further   we   denote   the   unit   quaternions   by   H 1   and   the imaginary quaternions by   H i . Analogously to the way that unit complex numbers   z   = cos( φ ) +   i   sin( φ ) =   e iφ   represent rotations in 2D via the formula   p rot   =   zp   for any point   p   ∈   C , unit quaternions represent rotations in 3D. A point   ( u, v, w ) >   in 3D is represented as the purely imag- inary quaternion   p   =   i   ·   u   +   j   ·   v   +   k   ·   w ; a rotation around the unit 3D axis   v   by the rotation angle   θ   is represented by the quaternion   q   = cos( θ/ 2) + sin( θ/ 2) ( i   ·   v 1   +   j   ·   v 2   +   k   ·   v 3 ) . The rotated point is obtained as   p rot   =   q   ∗   p   ∗   q   . Clearly,  q   and   − q   represent the same rotation, so the set   U   of unit quaternions is a double coverage of the special orthogonal group SO (3)   of rotations in 3D. The composition of rotations corresponds to the multiplication of the corresponding unit quaternions. The rigid motions in 3D can in a similar way be repre- sented by dual quaternions. Let’s again clarify the notation. The ring of the dual quaternions with the dual unit    , which has the properties      ·   1 = 1   ·      =      and    2   = 0 , is defined as:  H D   =   { dq   |   dq   =   q 1   +      ·   q 2   &   q 1 , q 2   ∈   H }   (2) As for the quaternions, the addition and the scalar mul- tiplication are component wise. The multiplication follows from the properties of the dual unit. The quaternion conjugate (there is also a dual conjugate and a total conjugate) is given by   q 1   +      ·   q 2   :=   q 1   +      ·   q 2 . The rigid motion consists of a rotation part, represented by a rotation unit quaternion   q r   , and a translation part, represented by a purely imaginary translation quaternion   q t . A point   p   = ( x, y, z ) >   is embedded in   H D   by  p d   := [1 ,   0 ,   0 ,   0] +      ·   [0 , x, y, z ] .   (3) With these definitions, the dual quaternion representing the rigid motion is defined by  dq   :=   q r   +      1 2   ·   q t   ∗   q r   (4) A point transformed by a rigid motion is represented by  dq   ∗ ∗   p d   ∗ ∗   dq   (5) As before, a composition of rigid motions is represented by the product of the corresponding dual quaternions. Note that   H 1   ×   H i   is a double coverage of   SE (3) .  B. Base Element  A mixture distribution generally consists of a convex combination of base elements. In our case, base elements are   pdf s   that are induced from a Gaussian distribution on  R 6 . This space can be interpreted as a linearization of  SE (3)   w.r.t. the dual quaternion representation at a rotation represented by a unit quaternion   q 0   = [ a, b, c, d ] . We take
the tangent space in   R 4   to the unit sphere   S 3   at the point  ( a, b, c, d ) > . We provide it with the basis that we derive from the canonical basis   { e 1 , e 2 , e 3 , e 4 }   in   R 4   by applying the quaternion formulation for rotations in   R 4   with unit quaternions   q l   and   q r   :  rot( p )   =   q l   ∗   p   ∗   q r   (6)  b i   :=   q 0   ∗   e i   ∗   q id   for   i   = 1   . . .   4   (7) Note that   b 1   ∼   q 0 . The vectors   { b 2 , b 3 , b 4 }   are an orthonor- mal basis of the tangent plane. The coefficients of elements of the tangent plane, together with the coefficients of the translation part, constitute the   6 D   tangent space   T S q 0   of the rigid motion. This tangent space is mapped to the rigid motions by a central projection for the rotation part and an embedding for the translation part. Let   ( u, v, w, x, y, z )   be a point in this tangent space. Then  q r   :=   1  √ 1 +   u 2   +   v 2   +   w 2   ·   ( b 1   +   u   ·   b 2   +   v   ·   b 3   +   w   ·   b 4 )  (8) corresponds to a unit quaternion representing rotation. The imaginary quaternion  q t   := [0 , x, y, z ]   (9) represents the translation. Then the dual quaternion accord- ing to (4) represents the corresponding rigid motion. Since the projections depend on   q 0   we denote this mapping by  Π q 0   :   T S q 0   −→   S 3   ×   R 3   ∼   SE (3)   (10) We extend   Π q 0   to be two-valued by letting both   q r   and   − q r  be in the image. Thus all points on   S 3   are reached except for those orthogonal to   q 0 . With this mapping we can define the base element.  Definition:   Let   q 0   be an arbitrary unit quaternion. Further, let   T S q 0   be the tangent space defined above, and let   N   ( μ,   Σ)  be a Gaussian distribution on   T S q 0   , calling the corresponding probability density function   p T   . Then the function   p SE (3)   on  S 3   ×   R 3   ∼   SE (3)   given by  p SE (3) ( m )   :=   1  C   ·   p T S  ( Π − 1  q 0   ( m ) )   (11)  C   :=  ∫  S 3 × R 3  p T S  ( Π − 1  q 0   ( m ) )   d m   (12) is a   pdf   on   SE (3) . For rigid motions   m 0   with a rotation quaternion orthogonal to   q 0   we let   p SE (3) ( m 0 ) := 0   - this is a smooth completion. We call this type of distribution a Projected Gaussian or   P G   and denote it by  p SE (3)   ∼ N   ( q 0 , μ,   Σ)   (13) See Figure 1 for examples of projected Gaussians. Note that the peaks are lower due to renormalization. The subset of   pdf s   for which   μ u   =   μ v   =   μ w   = 0   in the corresponding Gaussian on the tangent space is referred to as   P G 0 . Before extending our framework to Mixtures of Projected Gaussians, we would like to explain how to fuse and how to compose two Projected Gaussians.  (a)   (b) Fig. 1.   (a) A base element on the unit circle, obtained by projecting a Gaussian on a tangent line (b) An illustration of a Gaussian distribution projected to a 2D sphere  C. Fusion and Composition of Projected Gaussians  Two Gaussian distributions   p 1   ∼ N   ( μ 1 ,   Σ 1 )   and   p 2   ∼ N   ( μ 2 ,   Σ 2 )   on   R n   pertaining to the same phenomenon are fused by  p 3   ∼ N   ( μ 3 ,   Σ 3 )  μ 3   = (Σ 1   + Σ 2 ) − 1   ·   (Σ 1   ·   μ 2   + Σ 2   ·   μ 1 ) Σ 3   =   ( Σ − 1 1   + Σ − 1 2  ) − 1  (14) We can generalize this to   P Gs p 1   ∼   N   ( q 1 , μ 1 ,   Σ 1 )  and   p 2   ∼ N   ( q 2 , μ 2 ,   Σ 2 )   only if the tangents spaces are reasonably close - the angle between the normals to the tangent spaces should be less than 15 ◦ , or, equivalently by switching to the antipodal tangent point, larger than 165 ◦ . Then we define   q 3   by renormalizing   q 1   + q 2   to length 1 and restate the original distributions approximately (we show   p 1 ,  p 2   works the same way). With the Jacobian   J   at the mean value   μ 1   of the mapping  f   = Π − 1  q 3   ◦   Π q 1   :   T S q 1   −→   T S q 3   (15) the parameters are estimated as  μ 3 , 1   =   f   ( μ 1 )   and   Σ 3 , 1   =   J   ·   Σ 1   ·   J >   (16) Renormalization of the base elements involves an integral that   is   hard   to   approximate   with   quadrature   techniques. Therefore we use Monte Carlo integration. Since the inte- grands are bounded by exponential functions that are easy to sample from, the integration is reasonably efficient. The estimates resulting from both original distributions are then fused on the common tangent space   T S q 3   using (14). Generally, the rotation part of   μ 3   is not zero. Since it is advantageous to refrain to base elements of type   P G 0 ,   p 3   is restated as above with the new tangent point   q 4   = Π q 3   ( μ 3 ) . The resulting probability density function on   S 3   ×   R 3   needs to be normalized according to Definition 1. The fused   pdf  is denoted as   p 3   =   φ   ( p 1 , p 2 ) . In robotics we frequently need to estimate the   pdf   of a composition of two subsequent rigid motions given the  pdf s   of the individual rigid motions (e.g. from an uncertain position in an uncertain camera frame to a position in world coordinates).
Without loss of generality we refrain to   P G 0   to define the composition, so   p 1   ∼ N   ( q 1 ,   0 ,   Σ 1 )   and   p 2   ∼ N   ( q 2 ,   0 ,   Σ 2 ) . From the composition in terms of dual quaternions   dq 3   =  dq 2   ∗ ∗   dq 1   the natural choice for the tangent point   q 3   of the composition is   q 3   =   q 2   ∗   q 1 . This induces a mapping   g g   :   T S q 2   ×   T S q 1   −→   T S q 3  g   ( y 2 , y 1 ) : = Π − 1  q 3   (Π q 2   ( y 2 )   ∗ ∗   Π q 1   ( y 1 ))   (17) Note that   g   (0 ,   0) = 0 , thus   μ 3   = 0 . With  J γ   =   ∂g ∂ ( y 2 ,y 1   )  ∣ ∣ ∣ (0 , 0)  and   Σ γ   =  (   Σ 2   0 0   Σ 1  )  the resulting covariance matrix of the composition is   Σ 3   =   J γ   ·   Σ C   ·   J >  γ   . The composition   p 3   ∼ N   ( q 3 ,   0 ,   Σ 3 )   is denoted by  p 3   =   γ   ( p 2 , p 1 ) .  D. Mixture of Projected Gaussians   M P G  As stated above, a precondition for the fusion of   P G   base elements is that their tangent points are sufficiently close to each other and that they are sufficiently well concentrated. For this reason, widely spread probability density functions should not be modeled in a single base element. Instead, we use a mixture of   P G   base elements. Thus let   p i   ∈   P G  be base elements, then a Mixture of Projected Gaussians is defined as  p m   = 1  n  n ∑  i =1  π i   ·   p i   with   0   ≤   π i   ≤   1   and  n ∑  i =1  π i   = 1  (18) Fusion and composition of   P Gs   carry over to   M P GS   in a similar way as this work for Mixtures of Gaussians [14]. Let   p m, 1   =   1  n  ∑ n i =1   π 1 ,i p 1 ,i   and   p m, 2   =   1  l  ∑ l j =1   π 2 ,j   p 2 ,j  be   M P GS . The fused mixture   p m, 3   = Φ ( p m, 1 , p m, 2 )   is obtained by fusing and weighting the base elements of the original mixtures:  p m, 3   = 1  C   ·  n,l ∑  i,j =1  λ i,j   ·   π 1 ,i   ·   π 2 ,j   ·   φ   ( p 1 ,i , p 2 ,j   )   (19) with a normalizing constant  C   =  n,l ∑  i,j =1  λ i,j   ·   π 1 ,i   ·   π 2 ,j   (20) The weights   π 1 ,i   and   π 2 ,j   are those of the prior mixture. The plausibility is composed of two factors,   λ i,j   =   α i,j   ·   δ i,j   . The factor   α   says whether the mixture elements can share a tangent space and thus probably pertain to the same cases in the mixture (for detail see [2]). The factor   δ   is the Mahalanobis distance of the mean values and covariances, transported to the common tangent space.  δ i,j   = e − 1 / 2 · ( μ 3 , 1 ,i − μ 3 , 2 ,j   ) · (Σ 3 , 1 ,i +Σ 3 , 2 ,j   ) − 1 · ( μ 3 , 1 ,i − μ 3 , 2 ,j   ) >  It expresses that even if the mixture elements could share a tangent space, they could still not be compatible. The composition   p m, 3   = Γ ( p m, 1 , p m, 2 )   carries over in a similar manner. In this case, there is no question of whether two base elements could apply at the same time, since the two probability distributions are assumed to be independent, so the factor   λ i,j   is omitted.  p m, 3   = 1  C   ·  n,l ∑  i,j =1  π 1 ,i   ·   π 2 ,j   ·   γ   ( p 1 ,i , p 2 ,j   )   (21) with  C   =     n,l ∑  i,j =1  π 1 ,i   ·   π 2 , j      .   (22) Note that in both cases the individually fused or combined resulting base elements are assumed to be renormalized. IV.   M P G   AND SAMPLE BASED DESCRIPTION OF   pdf s  The   M P Gs   try to fill the middle ground between sample based descriptions and unimodal parametric descriptions of  pdf s . In our perception framework, we use sample based descriptions a lot, so we need to restate   pdf s   available in one description also in the other one. Sampling from   M P G   is easy, we first sample from the discrete   pdf   induced by the weights, and then draw a sample for the Gaussian on the tangent space using the Box-Muller algorithm. For fitting a   M P G   to a sample set, we use a slight variant of the Expectation Maximization Algorithm (see [15]): 1) Set the initial value for the means   μ i , covariance matrices   Σ i   and weighting coefficients   λ i   and evaluate the log likelihood with these values. 2) E step: Evaluate the responsibilities   γ ( x n,i )   using the current parameter values  γ ( z j,i ) :=   λ i N   ( x j   | T S i , μ i ,   Σ i )  ∑  k   λ k N   ( x j   | T S k , μ k ,   Σ k )  3) M step: Reestimate the parameters using the current responsi- bilities  μ new i   =   1  N i N ∑  j =1  γ ( z j,i )   ·   x j  Σ new i   =   1  N i N ∑  j =1  γ ( z j,i )( x j   −   μ new i   )( x j   −   μ new i   ) >  λ new i   =   N i  N  where   N i   =   ∑ N j =1   γ ( z j,i )  4) Evaluate the log likelihood:  ln P( X | T S, μ,   Σ , λ ) =  N ∑  j =1  ln  (   n ∑  i =1  λ i N   ( x j   | T S i , μ i ,   Σ i )  )  and check for convergence of either the parameters or the log likelihood. If the convergence criterion is not satisfied return to the E step. Note that we want to keep our base elements in   P G 0 , so re- estimating   μ i   in the M-Step also means resetting the tangent points   q i .
V. C ONVERGENCE   P ROPERTIES OF THE   M P G  The good news of the   M P G   framework is that that we get good approximations with few base elements, and that it allows for fusing and composing   pdf s   without having to go back to the original sensor readings. The bad news is that the number of base elements tends to increase quadratically with these operations. However, we can drop base elements with small weights and we can merge base elements with similar statistics without loosing much information, while decreasing the number of base elements. In order to use the framework for the task of grasping, we defined a grasp criterion: Given a   pdf p   of an object pose and a box   B   ⊂   SE (3)   that captures the tolerances associated with the grasping task (e.g. width of the gripper, possible grasp on the object), we try to find a rigid transform  m   that maximizes the probability of successfully grasping:  m   := arg   max  m ∈ SE (3)  ∫  m ( B )  p ( x ) dμ m ( B )  If   p M   ( x ) =   ∑ n i =1   λ i   ·   p ( μ i ,   Σ i , x ) , the integral is not very much affected by dropping a base element with a small weight, let’s assume the last one. Renormalizing the remaining weights  λ ′  i   :=   λ i  1   −   λ n  we get  ∣ ∣ ∣ ∣ ∣ ∣ ∫  B n ∑  i =1  λ i p i   d μ B   −  ∫  B n − 1 ∑  i =1  λ ′  i p i   d μ B  ∣ ∣ ∣ ∣ ∣ ∣   <   2 λ n  for any box   B   (for a proof see [2]). The information loss due to combining two similar base elements is investigated in terms of a symmetric version of the Kullback-Leibler divergence, inspired by an investigation of Runnalls for ordinary mixtures of Gaussians (see [16]). Let’s assume the base elements   p 1   ∼ N   ( T S q 1   , μ 1 ,   Σ 1 )   and  p 2   ∼ N   ( T S q 2   , μ 2 ,   Σ 2 )   from a   M P G M   have already been restated to the same tangent space, i.e.   q 1   =   q 2 . Then the symmetric Kullback-Leibler divergence between them is  D   =(Σ − 1 1   + Σ − 1 2   )( μ 1   −   μ 2 )( μ 1   −   μ 2 ) >  d sKL ( p 1 , p 2 ) = 1 2 tr   ( Σ − 1 2   Σ 1   + Σ − 1 1   Σ 2   +   D )   −   6   (23) If we now replace both   p 1   and   p 2   with the combined base element  p ′   =   1  λ 1   +   λ 2  ·   ( λ 1 p 1   +   λ 2   ·   p 2 )  to obtain the modified   pdf M   ′   with less base elements, then we have:  d sKL ( M, M   ′ )   ≤   1  λ 1   +   λ 2  · ( λ 1 d sKL ( p 1 , p ′ ) +   λ 2 d sKL ( p ′ , p 2 ))  (24) For details and proofs see [2]. VI. I MPLEMENTATION AND   E XPERIMENTAL   R ESULTS  The   M P G   framework is fully implemented in Python. The probability density functions are visualized by drawing random samples and for each pose painting a flag on the screen. The foot of the flag represents position, the pole represents the z-axis of the rotated coordinate system, and the tip of the pennant marks the x-axis. As an application example we demonstrate how to estimate the pose of an object based on SIFT features, e.g. the salt box of figure 2. Let’s assume that the robot detects the features ’B’ of the word   Bad   (green) and ’l’ of   Salz   (blue). The mountain top will be used as a third feature (purple).  (a)   (b) Fig. 2.   (a) salt box (b) Stereo SIFT features on the salt box. The features have an orientation around the sight line, but the visibility range is assumed to be   15 ◦ , the translation invariance is low. Each feature is represented by a   M P G   with seven base elements.  (a)   (b) Fig. 3.   (a) From the first two SIFT features, two distributions for the object pose are derived by the composition of the rigid motions camera to feature and feature to object coordinate system (b) The two distributions are fused (red).  VII. C ONCLUSION AND OUTLOOK  In this paper we present the framework of Mixtures of Projected Gaussians that allows for modelling a large variety of possible probability distribution functions of 6D poses. In contrast to a sample based description, much fewer parame- ters are needed to describe the distribution. The framework interfaces   well   with   the   sample   based   descriptions,   and provides the inference mechanisms of fusing and composing uncertain pose information. The operations of fusion, propagation or multiplication of MPG distributions generally result in a large number of mixture elements. However, many of them have practically
(a)   (b) Fig. 4.   (a) From originally 49 base elements, ten with low weights are skipped (b) The base elements are merged until only ten are left.  (a)   (b) Fig. 5.   (a) The object pose estimate from the next feature is added ... (b) ... and fused with the previous estimate (turquoise).  zero weight, while others are approximately identical. This is used to drop some base elements, and to merge others, thus reducing their number without losing much information. The algorithms for probabilistic inference (fusion, propa- gation, multiplication) are fully implemented in   Python . The covariance matrices are currently estimated using the Jacobian of the non-linear transforms. These estimates could be improved by using the unscented estimation technique (see Julier and Uhlmann [13]). In this paper we focus on the perception of static objects. The MPG framework can be extended to the dynamic case as well, following concepts by Goddard [6] and by Brox et al. [17]. VIII. A CKNOWLEDGEMENT  This   work   was   made   possible   by   funding   from   the ARTEMIS Joint Undertaking as part of the project R3-COP and from the German Federal Ministry of Education and Research (BMBF) under grant no. 01IS10004E. R EFERENCES [1]   W.   Feiten,   P.   Atwal,   R.   Eidenberger,   and   T.   Grundmann,   “6D Pose Uncertainty in Robotic Perception,” in   Advances in Robotics Research ,   T.   Kr ̈ oger   and   F.   M.   Wahl,   Eds.   Berlin,   Heidelberg: Springer Berlin Heidelberg, 2009, pp. 89–98. [Online]. Available: http://link.springer.com/10.1007/978-3-642-01213-6 { } 9 [2]   M. Lang, “Approximation of Probability Density Functions on the Euclidean Group Parametrized by Dual Quaternions,” arXiv preprint: arXiv:1707.00532, Ludwig-Maximilians-Universitat Munchen, 2011. [Online]. Available: https://arxiv.org/abs/1707.00532 [3]   J. Stuelpnagel, “On the Parametrization of the Three-Dimensional Rotation Group,”   SIAM Review , vol. 6, no. 4, pp. 422–430, oct 1964. [Online]. Available: http://epubs.siam.org/doi/10.1137/1006093  (a)   (b) Fig. 6.   (a) After merging again, the pose estimate converges. (b) Compare to the original estimates. [4]   S.   B.   Choe,   “Statistical   Analysis   of   Orientation   Trajectories   via Quaternions with Applications to Human Motion,” no. December, p. 117, 2006. [5]   J. S. Goddard Jr., “Pose and Motion Estimation from Vision Using Dual Quaternion-based Extended Kalman Filtering,” Ph.D. disserta- tion, 1997. [6]   J.   S.   Goddard   and   M.   A.   Abidi,   “Pose   and   Motion   Estimation Using Dual quaternion-based Extended Kalman Filtering.” in   Image (Rochester, N.Y.) , R. N. Ellson and J. H. Nurre, Eds., vol. 3313, no. January, mar 1998, pp. 189–200. [7]   L. Kavan, S. Collins, C. O’Sullivan, and J. Zara, “Dual quaternions for rigid transformation blending,”   Technical report TCDCS200646 Trinity College Dublin , no. TCD-CS-2006-46, pp. 39–48, 2006. [8]   M. E. Antone and S. Teller, “Robust Camera Pose Recovery Using Stochastic Geometry,” Ph.D. dissertation, 2001. [9]   J.   J.   Love,   Bingham   Statistics .   Dordrecht:   Springer Netherlands,   2007,   pp.   45–47.   [Online].   Avail- able:   http://dx.doi.org/10.1007/978-1-4020-4423-6 { } 19http://www. springerlink.com/index/10.1007/978-1-4020-4423-6 { } 19 [10]   J. Glover, G. Bradski, R. R. Rusu, and G. Bradski, “Monte Carlo Pose Estimation with Quaternion Kernels and the Bingham Distribution,” in  Robotics: Science and Systems VII .   Robotics: Science and Systems Foundation, jun 2011, p. 97. [11]   K.   V.   Mardia,   C.   C.   Taylor,   and   G.   K.   Subramaniam,   “Protein Bioinformatics and Mixtures of Bivariate von Mises Distributions for Angular Data,”   Biometrics , vol. 63, no. 2, pp. 505–512, jun 2007. [Online]. Available: http://doi.wiley.com/10.1111/j.1541-0420. 2006.00682.x [12]   E. Kraft, “A quaternion-based unscented Kalman filter for orientation tracking,” in   Proceedings of the 6th International Conference on Information Fusion, FUSION 2003 , 2003. [13]   S. J. Julier and J. K. Uhlmann, “A New Extension of the Kalman Filter to Nonlinear Systems,” in   Proc. of AeroSense: The 11th Int. Symp. on Aerospace/Defense Sensing, Simulations and Controls , vol. The 11th I, 1997, pp. 182–193. [14]   R. Eidenberger, T. Grundmann, W. Feiten, and R. Zoellner, “Fast parametric   viewpoint   estimation   for   active   object   detection,”   in  2008   IEEE   International   Conference   on   Multisensor   Fusion   and Integration for Intelligent Systems .   IEEE, aug 2008, pp. 309–314. [Online]. Available: http://ieeexplore.ieee.org/document/4648083/ [15]   C. M. Bishop, “Pattern Recognition and Machine Learning,”   Journal of Electronic Imaging , vol. 16, no. 4, p. 049901, jan 2007. [16]   A. R. Runnalls, “Kullback-Leibler approach to Gaussian mixture reduction,”   IEEE Transactions on Aerospace and Electronic Systems , 2007. [17]   T.   Brox,   B.   Rosenhahn,   U.   G.   Kersting,   and   D.   Cremers, “Nonparametric   Density   Estimation   for   Human   Pose   Tracking,” in   Pattern Recognition, Proceedings , 2006, pp. 546–555. [Online]. Available: http://link.springer.com/10.1007/11861898 { } 55