Dual Quaternion Sample Reduction for SE(2) Estimation

Kailai Li, Florian Pfaff, and Uwe D. Hanebeck Intelligent Sensor-Actuator-Systems Laboratory (ISAS) Institute for Anthropomatics and Robotics Karlsruhe Institute of Technology (KIT), Germany [email protected], ﬂ[email protected], [email protected]

Abstract—We present a novel sample reduction scheme for random variables belonging to the SE(2) group by means of Dirac mixture approximation. For this, dual quaternions are employed to represent uncertain planar transformations. The Cramer–von´ Mises distance is modified as a smooth metric to measure the statistical distance between Dirac mixtures on the manifold of planar dual quaternions. Samples of reduced size are then obtained by minimizing the probability divergence via Riemannian optimization while interpreting the correlation between rotation and translation. We further deploy the proposed scheme for non- parametric modeling of estimates for nonlinear SE(2) estimation. Simulations show superior tracking performance of the sample reduction-based filter compared with Monte Carlo-based as well as parametric model-based planar dual quaternion filters. Figure 1: Examples of the proposed sample reduction tech- I.INTRODUCTION nique for planar rigid motions. Here, 2000 random samples Estimation of planar motions is ubiquitous and play a (yellow) drawn from different underlying distributions are fundamental role in many application scenarios, such as optimally approximated by 20 samples (blue) on the manifold odometry and scene reconstruction, object tracking as well of planar dual quaternions. as remote sensing, etc [1]–[5]. Planar motions, incorporating both rotations and translations on a plane, are mathematically described by elements belonging to the two-dimensional spe- which can be easily violated under large noise levels (e.g., cial Euclidean group SE(2). Conventionally, elements of the due to low-cost sensors or strong nonlinearities). SE(2) group are parameterized by the 3 × 3 homogeneous Dual quaternions representing rigid transformations natu- matrices. However, with nine elements used to represent the rally form a nonlinear manifold and its nonlinearity mainly three DoF, they contain a large degree of redundancy, resulting results from the rotation component. Thus, in order to stochas- in numerical instabilities and memory inefficiencies. Dual tically model uncertain dual quaternions directly on the mani- quaternions, defined as x = xr + xs, combine quaternion fold without linearization, novel probabilistic density functions and dual number theory. The real part xr is a quaternion (PDFs) have been proposed based on directional statistics. representing the rotation and xs denotes the dual part encoding In [9], the Bingham distribution was employed to model the the translation. is the dual unit and satisfies 2 = 0. real part of dual quaternions while the translation part was In contrast to matrix representations, dual quaternions can assumed to be Gaussian-distributed. A corresponding filtering parameterize planar motions in the form of four-dimensional scheme was established using the unscented transform [10] vectors with only one degree of redundancy. Conciser ways and successfully deployed for stereo visual odometry [9]. are thereby enabled for modeling the uncertainty and recursive However, the approach lacks probabilistic interpretation of estimation of planar transformations. correlations between the real and dual part, resulting in over- Therefore, we employ planar dual quaternions to parameter- optimistic estimates. Another probabilistic framework, the so- ize SE(2) states in this paper. Due to the underlying nonlinear called projected Gaussian distribution (PGD), was introduced group structure, stochastic modeling of uncertain SE(2) states in [11] based on hyperspherical geometry. Further, a mixture is usually done in a locally linearized space using Gaussian model was established for addressing multimodality. But its in- distributions [6]. Nonlinear estimation approaches can then be ference scheme requires approximations under the assumption proposed by using popular Bayesian filtering schemes such of small rotation uncertainties. In [12], a new distribution was as the extended Kalman filter (EKF) [7] or the unscented proposed based on the Bingham distribution to directly model Kalman filter (UKF) [8]. However, such a probabilistic model uncertain planar dual quaternions while considering correlated is established under the assumption of local perturbations, real and dual part. Corresponding Bayesian filters [13], [14] were established based thereon by exploiting the unscented mixtures on the manifold of planar dual quaternions. It is es- transform as well as the progressive filtering scheme [15] tablished by synthesizing the von Mises and Gaussian kernels and were further applied to simultaneous localization and that measure the probability mass around the real and dual mapping [16]. part, respectively. Components of the approximating Dirac Though stochastic modeling approaches relying on specific mixture are then located to on-manifold supports minimizing PDFs provide effective solutions for dual quaternion estima- its statistical divergence to the target Dirac mixture by means tion, several shortcomings exist. In practice, the information of Riemannian optimization. The proposed approximation geometry of the uncertainty is rather arbitrary on the manifold approach is deployed to a sample-based planar dual quaternion of dual quaternions, meaning parametric forms can be largely filter for nonlinear SE(2) estimation. As the reduced samples violated. Also, there is no closed-form solution for computing are deterministic and more representative, the novel approach the normalization constant of such distributions (e.g., for PGD shows improved sample efficiency than a plain particle filter. or Bingham-based distributions). Thus, corresponding filters Also, in contrast to parametric modeling approaches, the pro- often rely on samples, generated by either deterministic or posed scheme enables (approximated) modeling of arbitrary Monte Carlo-based sampling schemes, for recursive estima- distributions and considers the correlation between the real tion. The posterior estimate is essentially a re-approximation and dual part. of the updated samples via model fitting. Though the sampling The remainder of this paper is structured as follows. Prelim- schemes contribute to handling nonlinear dynamics [8], the re- inaries about the dual quaternion representation of SE(2) states fitting step still imposes the parametric form of the underlying will be given in Sec. II. The novel sample reduction scheme distribution. on the manifold of planar dual quaternions will be proposed in Sec. III. Based thereon, we will give the proposed sample Sample-based modeling approaches provide an intuitive reduction-based SE(2) estimator in Sec. IV, which is followed way to represent arbitrary probability distributions in a non- by the evaluation in Sec. V. The work will be concluded in parametric manner. Components of a Dirac mixture can be Sec. VI. located on the samples for discrete probabilistic modeling. Corresponding Bayesian filtering schemes, e.g., the particle II.PRELIMINARIES filter [17], have been established based thereon for nonlinear For the sake of conciseness, we formulate dual quater- estimation. However, naively exploiting empirical samples nions parameterizing planar motions into the vector form to represent stochastic dynamics typically suffers from in- x = [ x>, x> ]> ∈ 4, with efficiency issues w.r.t. representativeness and runtime. Also, r s R > 1 random samples cannot guarantee reproducible results. There- xr = [ cos(θ/2), sin(θ/2) ] ∈ S , fore, some works introduced deterministic schemes for non- 1 (1) x = t ⊗ x ∈ 2 , parametric modeling of estimates in the context of planar mo- s 2 r R tion estimation. In [18], a grid-based approach was proposed being the real and dual part, respectively. It denotes a rotation on the circular domain for modeling random angular vari- around z–axis through angle θ followed by a translation of ables and further extended via Rao-Blackwellization for SE(2) t ∈ 2 on the xy-plane. The real part is formulated as a estimation [19]. However, the performance of such schemes R quaternion for denoting the rotation and the dual part encodes depends highly on the grid resolution, since grid points are the translation by composing the real part via the Hamilton generated equidistantly on the circle and not adaptive to the product ⊗. Therefore, planar dual quaternions naturally form shape of the underlying distribution [20]. A high resolution of a manifold consisting of the circular domain and the two- the grid is usually required for the desired tracking accuracy, dimensional Euclidean space, i.e., x ∈ 1 × 2 ⊂ 4. resulting in both runtime and memory inefficiencies. S R R Aggregating two arbitrary planar dual quaternions can be Thus, using fewer but more representative samples is ap- formulated into a regular matrix–vector multiplication. For pealing for discrete stochastic modeling. In [21], a sample > > instance, ∀ x = [ x0, x1, x2, x3 ] , y = [ y0, y1, y2, y3 ] ∈ reduction scheme was established for reducing the number of 1 2 x y S × R , the aggregation x y = Qx y = Qy x, with components of a Dirac mixture by minimizing a multivariate " # " # generalization of the Cramer–von´ Mises distance. For that, x0 −x1 0 0 y0 −y1 0 0 Qx = x1 x0 0 0 , Qy = y1 y0 0 0 . the so-called localized cumulative distribution (LCD) of Dirac x x2 x3 x0 −x1 y y2 −y3 y0 y1 x −x x x y y −y y mixtures was proposed for smoothly characterizing discrete 3 2 1 0 3 2 1 0 probabilistic models. While this technique has been success- Any vector s ∈ R2 can be transformed by the planar dual fully deployed for Euclidean spaces and was further extended quaternion x ∈ S1 × R2 according to to the spherical domain [22], there exists no counterpart for 0 > > states belonging to the group of planar rigid body motions. s = ( x [ 1, 0, s ] x )3:4 , (2) > In this work, we develop an LCD-based sample reduction with x = [ x0, −x1, −x2, −x3 ] being the conjugate of x. scheme for random SE(2) states represented by dual quater- Here, s is expressed in the form of a planar dual quaternion nions (see examples in Fig. 1). A geometry-aware metric and the last two elements of the vector after the operation are is proposed for measuring the statistical distance of Dirac taken out to recover the transformed location. A more detailed introduction about the arithmetic and manifold structure of The reason of employing such a cumulative distribution for dual quaternions can be found in [14], [23]. defining the statistical distance lies in the fact that two Dirac mixtures do not share common supports, thus cannot be III.DUAL QUATERNION-BASED compared directly. The standard cumulative distribution is gen- SE(2)SAMPLE REDUCTION erally asymmetric and only defined for scalar PDFs. Therefore, A. Problem Formulation the localized cumulative distribution was established in [24] Given a set of random planar dual quaternion samples X˜ = for PDFs in higher-dimensional spaces by incorporating the m 1 2 {x˜i}i=1 ⊂ S × R , a Dirac mixture is employed to represent kernel function. its underlying distribution as Synthesizing a smooth kernel function for dual quaternion m 1 2 X sample reduction is non-trivial. Since the manifold S × R fX˜ (x) = ω˜i δ(x − x˜i) , (3) exhibits a nonlinear and partially periodic geometric structure, i=1 the kernel function should be designed in a geometry-aware Pm with ω˜i being the weights, which satisfy i=1 ω˜i = 1. We aim manner. For that, we define an isotropic and separable kernel to approximate the target Dirac mixture above using another in the form of κ(x; ν, τ) = κr(xr; νr, τ) κs(xs; νs, τ), with Dirac mixture of reduced number of components given by n > X κr(xr; νr, τ) = exp(τ νr xr) , fX(x) = ωi δ(x − xi) . (4) > (7) i=1 κs(xs; νs, τ) = exp −τ(xs − νs) (xs − νs) , Pn X n 1 2 Here, i=1 ωi = 1 and = {xi}i=1 ⊂ S × R denotes the > > > 1 2 set of approximating planar dual quaternion samples. measuring the uncertainty at x = [ xr , xs ] ∈ S × R . In [21], an optimal sample reduction technique was pro- The kernel consists of a von Mises-like component centered at 1 posed to approximate multivariate Dirac mixtures in Euclidean νr ∈ S evaluating the real part and a Gaussian-like component 2 spaces. However, this approach cannot be trivially extended to at νs ∈ R the dual part. The two kernel components share planar dual quaternions due to the underlying nonlinear man- the dispersion parameter τ. Thus, uncertainties can be quan- ifold structure. In the remainder of this section, a geometry- tified adaptively to the manifold structure. Further, the shared aware kernel will be proposed for quantifying the on-manifold dispersion parameter τ allows considering the dependency probability mass in the sense of the LCD [24]. Based thereon, between the real and dual part. We will further show in the the Cramer–von´ Mises distance will be modified as the metric next subsection that such a synthesis also guarantees a unique measuring the statistical distance between Dirac mixtures on and closed-form solution for computing the Cramer–von´ Mises the manifold of planar dual quaternions. distance in the M-LCD sense. B. On-Manifold Localized Cumulative Distribution (M-LCD) We propose the so-called on-manifold localized cumulative distribution (M-LCD) to quantify the probability mass of a C. Statistical Distance of Dirac Mixtures on the Manifold of discrete density on a smooth manifold. More specifically, the Planar Dual Quaternions M-LCD of distribution g : M → R+ is defined as Z We further propose the M-LCD-based Cramer–von´ Mises G(ν, τ) = g(x) κ(x; ν, τ) dx , distance to measure the statistical divergence between Dirac M mixtures at the approximating set X and the target set X˜ . It with κ(x; ν, τ) being the kernel function located at ν ∈ M is defined as with a dispersion of τ ∈ R+. A smooth kernel is preferable as it leads to simpler optimization problems of maintaining the Z Z X 2 probability mass for approximation [21]. Consequently, the M- D( ) = W(τ) FX(ν, τ) − FX˜ (ν, τ) dν dτ . 1 2 LCD of the approximating Dirac mixture in (4) can be derived R+ S ×R as Z n The measure is computed by integrating the M-LCD deviation X 1 2 FX(ν, τ) = ωi δ(x − xi) κ(x; ν, τ) dx over all possible locations ν ∈ S ×R and dispersions τ of the 1 2 S ×R i=1 kernel defined in (7). The weighting function W(τ) controls n (5) X the contribution of the M-LCD deviation w.r.t. the dispersion = ωi κ(xi; ν, τ) , τ. Minimizing such a distance measure essentially denotes i=1 optimally maintaining the probability mass of the target Dirac with the kernel function being evaluated at each Dirac support mixture on the manifold. The approximating samples can 1 2 xi ∈ S × R . Similarly, the target M-LCD can be derived in thereby be obtained by solving the following optimization the following form problem under the constraint of the manifold structure, namely m X FX˜ (ν, τ) = ω˜i κ(x˜i; ν, τ) . (6) ∗ n 1 2 X = arg minXD(X) , X = {xi} ⊂ × . (8) i=1 i=1 S R Computation of D(X) boils down to the aggregation of three thereby decompose the whole integral into A(x, y, τ) = integrals, i.e., D(X) = D1 − 2D2 + D3, with Ar(xr, yr, τ) As(xs, ys, τ) and obtain Z Z Z 2 D1 = W(τ) FX(ν, τ) dν dτ , Ar(xr, yr, τ) = κr(xr; νr, τ) κr(yr; νr, τ) dνr 1 2 1 R+ S ×R S Z Z q > D2 = W(τ) FX(ν, τ) FX˜ (ν, τ) dν dτ , = 2πI0 τ 2 + 2xr yr , 1 2 R+ S ×R Z Z Z 2 As(xs, ys, τ) = κs(ys; νs, τ) κs(xs; νs, τ) dνs D3 = W(τ) F ˜ (ν, τ) dν dτ . 2 X R 1× 2 R+ S R π τ > = exp − (xs − ys) (xs − ys) . Next, we incorporate the M-LCD equations of the approximat- 2τ 2 ing and target samples in (5) and (6) into the formulae above. Here, I0 denotes the modified Bessel function of the first kind We then obtain of order zero. A detailed derivation can be found in Appendix Z A. For conciseness, we further substitute the metric for the D1 = W(τ) real part on the circular domain and the dual part in the R+ > > n n Euclidean space as ζxy = xr yr and ξxy = (xs−ys) (xs−ys), Z X X · ωiωjκ(xi; ν, τ) κ(xj; ν, τ) dν dτ , respectively. Then, we have 1 2 S ×R i=1 j=1 π2 p τ Z A(x, y, τ) = I0 τ 2 + 2 ζxy exp − ξxy . D2 = W(τ) τ 2 R+ n m Further, the integral over the dispersion τ is given by Z X X · ωiω˜jκ(xi; ν, τ) κ(x˜j; ν, τ) dν dτ , Z 1 2 S ×R i=1 j=1 B(x, y) = W(τ) A(x, y; τ) dτ . (10) Z R+ D3 = W(τ) R+ As discussed in [21], the weighting function W(τ) controls m m the impact of the integrated kernel values on the distance Z X X · ω˜iω˜jκ(x˜i; ν, τ) κ(x˜j; ν, τ) dν dτ . measure w.r.t. the dispersion. In theory, there are many pos- 1 2 S ×R i=1 j=1 sible functions for W(τ). In practice, however, it is ap- The double integration can be first applied in a sample-wise pealing to introduce a carefully crafted weighting function manner, yielding such that a closed-form solution of B(x, y) can be obtained. The optimization-based sample reduction procedure can then n n X X Z benefit therefrom regarding convergence efficiency. Thus, we D1 = ωiωj W(τ) propose the following weighting function i=1 j=1 R+ Z W(τ) = τ exp(−5τ/2) . (11) · κ(xi; ν, τ) κ(xj; ν, τ) dν dτ , 1 2 S ×R n m The integral in (10) can then be derived as X X Z D = ω ω˜ W(τ) 2 i j 2π2 i=1 j=1 R+ B(x, y) = . (9) 2 1/2 (12) Z (17 − 8 ζxy + 10 ξxy + ξxy) · κ(xi; ν, τ) κ(x˜j; ν, τ) dν dτ , 1 2 S ×R We further substitute the denominator with J (x, y) such that m m 2 X X Z B(x, y) = 2π /J (x, y). A detailed derivation of the integral D3 = ω˜iω˜j W(τ) is given in Appendix B. The each individual component of the i=1 j=1 R+ Z distance measure in (9) can be computed as · κ(x˜i; ν, τ) κ(x˜j; ν, τ) dν dτ . n n 1 2 S ×R 2 X X −1 D1 = 2π ωiωjJ (xi, xj) , The individual distance components above share the same i=1 j=1 integration over the kernel parameters. The general form of n m 2 X X −1 the inner-layer integral can be formulated as D2 = 2π ωiω˜jJ (xi, x˜j) , (13) Z i=1 j=1 m m A(x, y; τ) = κ(x; ν, τ) κ(y; ν, τ) dν , 2 X X −1 1 2 D = 2π ω˜ ω˜ J (x˜ , x˜ ) . S ×R 3 i j j j i=1 j=1 with the product of kernel values being integrated over all kernel locations on the manifold S1 × R2. The kernel As stated previously, the overall distance measure is then function in (7) is separable for the real and dual part, we obtained with D(X) = D1 − 2D2 + D3. (a) n = 5, m = 2000 (b) n = 9, m = 2000 (c) n = 25, m = 2000 (d) n = 35, m = 2000

(e) n = 5, m = 4000 (f) n = 9, m = 4000 (g) n = 25, m = 4000 (h) n = 35, m = 4000 Figure 2: Reduction of equally weighted planar dual quaternion samples using the proposed approach. To visualize the poses, the planar dual quaternion samples are shown as arrows. Random poses (yellow, quantity denoted by m) are approximated by fewer deterministic samples (blue, quantity denoted by n). The proposed approach works reliably for different underlying distributions with various reduction scales.

(a) n = 6, m = 2000 (b) n = 6, m = 2000 (c) n = 6, m = 2000 (d) n = 6, m = 2000 Figure 3: Reduction of the same target planar dual quaternion samples with varying nonuniform weights. (a) shows the reduction result of equally weighted target samples. In (b)-(c), we shift the sample weights (indicated by the size of the purple disk) clockwise. The proposed reduction scheme gives effective approximation results in all cases.

D. Optimal Reduction via Riemannian Optimization is smooth and twice continuously differentiable. Thus, we As pointed out in Sec. II, the manifold of planar dual quater- apply the Riemannian trust-region method given in [26]. Each 1 iterative step is ﬁrst computed on the tangent plane and then nions is the product of unit circle S and two-dimensional 1 2 2 retracted back to × to preserve the manifold structure. Euclidean space R . During the reduction, samples are thereby S R 1 2 For that, the gradient of the objective function in the ambient conﬁned to the Riemannian manifold S ×R . Instead of treat- ing the approximation task formulated in (8) as a constrained space is essential, which is provided in Appendix C. optimization problem and applying classic approaches, e.g., By minimizing the proposed distance measure, the prob- the Lagrange multiplier, we utilize the Riemannian optimiza- ability mass of the random samples is optimally preserved tion scheme [25]. It is geometry-aware and shows improved while considering the geometric structure of the planar dual convergence behavior provided that the manifold structure quaternion manifold. We show a few representative examples is well investigated. The derived distance measure in (13) in Fig. 2 and Fig. 3 to illustrate the proposed pose reduction Algorithm 1: Sample Reduction-Based Filter (SRF) X n Input: k−1 = {xi,k−1}i=1 , measurement zk X n Output: k = {xi,k}i=1 1 Xk|k−1 ← ∅ ; 2 for i ← 1 to n do np 3 {xj,k|k−1}j=1 ← propagate (xi,k−1); X X np 4 k|k−1 ← { k|k−1, {xj,k|k−1}j=1} ; 5 W ← ∅ ; 6 for l ← 1 to n · np do 7 W ← { W, fL(zk | xl,k|k−1) } ;

8 Xk ← sampleReduction (Xk|k−1, W, n); X n 9 return k = {xi,k}i=1 technique. For target pose samples of various randomness as well as weights, the approach works reliably for different Figure 4: Exemplary trajectory segment of the evaluation run. reduction scales. It can be also observed in the figures that the approach can preserve the correlation between the real 2 estimation task. The system model is xk = xk wk, with and dual part of the target samples. 1 2 xk, wk ∈ S ×R denoting the system state and system noise, IV. SAMPLE REDUCTION-BASED SE(2)ESTIMATION respectively. wk is synthesized by composing a von Mises The proposed approximation approach is then integrated mixture-distributed rotation angle and a Gaussian-distributed w2 := w w into a sample-based Bayesian filtering scheme for planar translation according to (1). k k k is defined as motion estimation. The system model is formulated as the squared system noise and has a multimodal distribution, as evident from the yellow arrows in Fig. 2 (a). Further, the > > xk = a(xk−1, wk−1) , (14) measurement model is given as zk = ( xk [ 1, 0, z0 ] x ) + v according to (2). It measures the position of a with x , x ∈ 1 × 2 being the system state and w k 3:4 k k−1 k S R k−1 point transformed by the planar dual quaternion state x from the system noise of domain W. a : 1 × 2 × W → 1 × 2 k S R S R its initial location z . The additive measurement noise v is is the transition function. The measure model is given as 0 k assumed to follow a zero-mean Gaussian distribution.

zk = h(xk, vk) , (15) We compare the sample reduction-based filter (SRF) proposed in Sec. IV with a plain particle filter (PF) and the SE(2)- Z V with zk ∈ and vk ∈ representing the measurement and Bingham filter (SE2BF) with a progressive update step [14], 1 2 V measurement noise, respectively. Further, h : S × R × → [27]. As the SE2BF relies on the SE(2)-Bingham distribu- Z denotes the observation function. Planar dual quaternion tion [12], we exploit 105 random samples for fitting the para- samples of uniform weight are exploited as a non-parametric metric model offline. Moreover, 100 random samples are used representation of the posterior distribution. by the PF to model the estimate, whereas the proposed SRF As shown in Alg. 1, the set of n equally weighted sam- uses 5 deterministic samples via the optimal approximation. X ples k−1 representing the posterior at time step k − 1 is The evaluation is performed for 100 Monte Carlo runs with fed through the system dynamics in (14). Each planar dual 10 time steps each. To quantify the tracking accuracy, we quaternion sample xk−1 is propagated by np random samples compute the orientation and position error w.r.t. the quaternion from the system noise distribution (Alg. 1, line 1–4). A sample arc length and the Euclidean distance, respectively. X set k|k−1 of size n · np is thereby obtained to represent As shown in Fig. 5, the sample reduction-based filter gives the prior distribution. Then, each prior sample is reweighted more accurate tracking results than the particle filter. Consid- according to the likelihood fL(zk | xl,k|k−1) derived from (15) ering that the SRF deploys much fewer samples than the PF, given the measurement zk (Alg. 1, line 5–7). The prior sample this shows that the proposed approximation method is effective X set k|k−1 with nonuniform weights can then be approximated and improves the sample efficiency considerably compared using the proposed sample reduction approach (Alg. 1, line 8). with the Monte Carlo scheme. The SE2BF fails for all runs n The obtained samples {xi,k}i=1 are again equally weighted because the parametric modeling of the uncertainty is largely and deterministic, representing the posterior distribution of the violated by the multimodal noise and strong nonlinearity. We current time step. further show an exemplary run of the tracking result in Fig. 4. V. EVALUATION At each time step, we sample the system noise to propagate the state for illustrating the uncertain dynamics (depicted by The proposed sample reduction-based planar dual quater- yellow arrows). Though much more samples are used by the nion filter is then evaluated in the following nonlinear SE(2) PF, its estimates are still prone to be at wrong modes as the 0.9

0.3 0.8

0.25 0.7

0.6 0.2 0.5 0.15 0.4

0.1 0.3 position error (RMSE) rotation error (RMSE)

0.05 0.2

0.1 SRF PF SE2BF SRF PF SE2BF (a) orientation accuracy (b) position accuracy Figure 5: Results of the simulated evaluation. The proposed sample reduction-based filter gives more accurate tracking results than the particle filter. The SE(2)-Bingham filter totally fails due to its parametric modeling of estimates.

random samples are less representative than the deterministic with I0 being the zero-order modified Bessel function of ones given by the optimal approximation. the first kind. Therefore, the integral of the kernel product over S1 only depends on the geodesic arc length between VI.CONCLUSION the two points in consideration and we obtain A(x, y, τ) = p > In this paper, a novel sample reduction scheme is proposed 2πI0(τ 2 + 2xr yr). for random planar dual quaternion samples by means of Dirac The integral of the kernel over the two-dimensional Eu- mixture approximation. A geometry-aware distance measure clidean space for the dual part can be derived by applying the is proposed on the manifold of planar dual quaternions, based Gaussian identity given in [28, Appendix D], yielding on which the new Dirac supports are obtained to preserve the Z probability mass via Riemannian optimization. Furthermore, As(xs, ys, τ) = κs(xs; νs, τ) κs(ys; νs, τ) dνs 2 we propose a sample reduction-based planar dual quaternion R 2 Z filter for SE(2) estimation. It performs better than the plain π 1 1 = 2 fN (xs ; νs , I2×2) fN (ys ; νs , I2×2) dνs τ 2 2τ 2τ particle filter as well as the state-of-the-art dual quaternion R filter using the parametric model. π2 1 Z = 2 fN (ys ; xs , I2×2) fN (νs) dνs There still remains much potential to exploit from the τ τ 2 proposed methods. The current reduction approach can be π τ R = exp − (x − y )>(x − y ) . further extended to the manifold of unit dual quaternions 2τ 2 s s s s that represent spatial poses belonging to the SE(3) group [9]. B. Integral over the Kernel Dispersion Applying the sample-based approximation scheme to system identification and control-related tasks is also of great value. Given two planar dual quaternions x, y ∈ S1 × R2 and Moreover, extensive applications of the proposed filter can be the weighting function in (11), the integral of the weighted done for real-world scenarios such as visual odometry and A(x, y, τ) over the kernel dispersion τ is given by scene registration [9], [16]. Z 2 τ p B(x, y) = π exp − (5 + ξxy) I0(τ 2 + 2 ζxy) dτ . + 2 APPENDIX R A. Integral over the Kernel Location As stated in [29, Sec. 6.611], the following closed-form The integral of the von Mises kernel product over S1 is integral holds for zero-order Bessel function of the first kind Z Z p 2 2 Ar(xr, yr, τ) = κr(xr; νr, τ) κr(yr; νr, τ) dνr exp(−ατ) I0(βτ) = 1/ α − β , 1 R+ ZS > = exp τ νr (xr + yr) dνr . for ∀ α, β ∈ R+, α > β . When setting α = 0.5 (5 + ξxy) 1 1/2 S and β = (2 + 2ζxy) , it can be verified that β ∈ [ 0, 2 ] and By normalizing xr +yr = kxr +yrkx\r + yr, the integral can be α ∈ [ 2.5, ∞ ), therefore α > β. We then have obtained as the normalization constant of a scaled von Mises π2 density, i.e., B(x, y) = p 2 Z 0.25(5 + ξxy) − (2 + 2ζxy) > 2 Ar(xr, yr, τ) = exp τkxr + yrk νr x\r + yr dνr 2π 1 = . S q 17 − 8 ζ + 10 ξ + ξ2 = 2π I0 (τkxr + yrk) , xy xy xy C. Derivative of the Proposed Distance Measure [10] I. Gilitschenski, G. Kurz, S. J. Julier, and U. D. Hanebeck, “Unscented Orientation Estimation Based on the Bingham Distribution,” IEEE Derivatives of the point-wise distance measure in (12) w.r.t. Transactions on Automatic Control, vol. 61, no. 1, pp. 172–177, Jan. > > > x = [ xr , xs ] can be obtained by applying the chain rule 2016. as [11] W. Feiten and M. Lang, “MPG - A Framework for Reasoning on 6 DoF 2 Pose Uncertainty,” arXiv preprint arXiv:1707.01941, 2017. dB dB dζxy 8π [12] I. Gilitschenski, G. Kurz, S. J. Julier, and U. D. Hanebeck, “A New = = 3 yr , dxr dζxy dxr J (x, y) Probability Distribution for Simultaneous Representation of Uncertain Position and Orientation,” in Proceedings of the 17th International dB dB dξxy 2 5 + ξxy Conference on Information Fusion (Fusion 2014), Salamanca, Spain, = = −4π 3 (xs − ys) . dxs dξxy dxs J (x, y) July 2014. Thus, the derivatives of the distance measure components [13] I. Gilitschenski, G. Kurz, and U. D. Hanebeck, “A Stochastic Filter X for Planar Rigid-Body Motions,” in Proceedings of the 2015 IEEE in (13) w.r.t. the approximating sample locations xi in can International Conference on Multisensor Fusion and Integration for h > > i> dD1 dD1 dD1 Intelligent Systems (MFI 2015), San Diego, California, USA, Sept. 2015. be derived. For instance, we have dx = dx , dx , with i i,r i,s [14] K. Li, G. Kurz, L. Bernreiter, and U. D. Hanebeck, “Nonlinear Progres- n sive Filtering for SE(2) Estimation,” in Proceedings of the 21st Inter- dD1 2 X ωj national Conference on Information Fusion (Fusion 2018), Cambridge, = 16π ωi xj,r , dx J (x , x )3 United Kingdom, July 2018. i,r j=1 i j n [15] U. D. Hanebeck, K. Briechle, and A. Rauh, “Progressive Bayes: A New dD1 X 5 + ξxixj Framework for Nonlinear State Estimation,” in Proceedings of SPIE, = −8π2 ω ω (x − x ) . dx i j J (x , x )3 i,s j,s AeroSense Symposium, vol. 5099, Orlando, Florida, USA, May 2003, i,s j=1 i j pp. 256 – 267. > [16] K. Li, G. Kurz, L. Bernreiter, and U. D. Hanebeck, “Simultaneous Local- h D> D> i Further, we have dD2 = d 2 , d 2 , with ization and Mapping Using a Novel Dual Quaternion Particle Filter,” in dxi dxi,r dxi,s Proceedings of the 21st International Conference on Information Fusion m (Fusion 2018), Cambridge, United Kingdom, July 2018. dD2 2 X ω˜j = 8π ωi x˜j,r , [17] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A Tu- dx J (x , x˜ )3 torial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian i,r j=1 i j Tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. m 174–188, 2002. dD2 X 5 + ξxix˜j = −4π2ω ω˜ (x − x˜ ) . i j 3 i,s j,s [18] G. Kurz, F. Pfaff, and U. D. Hanebeck, “Discrete Recursive Bayesian dxi,s J (xi, x˜j) j=1 Filtering on Intervals and the Unit Circle,” in Proceedings of the 2016 IEEE International Conference on Multisensor Fusion and Integration dD dD dD Then, we have = 1 − 2 2 . for Intelligent Systems (MFI 2016) dxi dxi dxi , Baden-Baden, Germany, Sept. 2016. [19] ——, “Application of Discrete Recursive Bayesian Estimation on Inter- ACKNOWLEDGMENT vals and the Unit Circle to Filtering on SE(2),” IEEE Transactions on This work is supported by the German Research Foundation Industrial Informatics, vol. 14, no. 3, pp. 1197–1206, Mar. 2018. (DFG) under grant HA 3789/16-1. [20] K. Li, F. Pfaff, and U. D. Hanebeck, “Grid-Based Quaternion Filter for SO(3) Estimation,” in Proceedings of the 2020 European Control REFERENCES Conference (ECC 2020), Saint Petersburg, Russia, May 2020. [21] U. D. Hanebeck, “Optimal Reduction of Multivariate Dirac Mixture [1] S. Thrun, “Probabilistic Robotics,” Communications of the ACM, vol. 45, Densities,” at – Automatisierungstechnik, Oldenbourg Verlag, vol. 63, no. 3, pp. 52–57, 2002. no. 4, pp. 265–278, Apr. 2015. [2] M. Montemerlo, “FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association,” [22] D. Frisch, K. Li, and U. D. Hanebeck, “Optimal Reduction of Dirac Ph.D. dissertation, Carnegie Mellon University, Pittsburgh, PA, July Mixture Densities on the 2-Sphere,” in Proceedings of the 21st IFAC 2003. World Congress (IFAC 2020), Berlin, Germany, July 2020. [3] W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-Time Loop Closure [23] K. Li, F. Pfaff, and U. D. Hanebeck, “Geometry-Driven Stochastic in 2D LIDAR SLAM,” in Proceedings of the 2016 IEEE International Modeling of SE(3) States Based on Dual Quaternion Representation,” in Conference on Robotics and Automation (ICRA 2016), Stockholm, Proceedings of the 2019 IEEE International Conference on Multisensor Sweden, May 2016, pp. 1271–1278. Fusion and Integration for Intelligent Systems (MFI 2019), Taipei, [4] S. Xuan, S. Li, M. Han, X. Wan, and G. Xia, “Object Tracking in Satel- Republic of China, May 2019. lite Videos by Improved Correlation Filters With Motion Estimations,” [24] U. D. Hanebeck and V. Klumpp, “Localized Cumulative Distributions IEEE Transactions on Geoscience and Remote Sensing , vol. 58, no. 2, and a Multivariate Generalization of the Cramer-von´ Mises Distance,” in pp. 1074–1086, 2020. Proceedings of the 2008 IEEE International Conference on Multisensor [5] S. Hare, S. Golodetz, A. Saffari, V. Vineet, M.-M. Cheng, S. L. Hicks, Fusion and Integration for Intelligent Systems (MFI 2008), Seoul, IEEE and P. H. Torr, “Struck: Structured Output Tracking with Kernels,” Republic of Korea, Aug. 2008, pp. 33–39. Transactions on Pattern Analysis and Machine Intelligence, vol. 38, no. 10, pp. 2096–2109, 2015. [25] P.-A. Absil, R. Mahony, and R. Sepulchre, Optimization Algorithms on [6] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-Manifold Matrix Manifolds. Princeton University Press, 2009. Preintegration for Real-Time Visual–Inertial Odometry,” IEEE Transac- [26] N. Boumal, B. Mishra, P.-A. Absil, and R. Sepulchre, “Manopt, a Matlab tions on Robotics, vol. 33, no. 1, pp. 1–21, 2016. Toolbox for Optimization on Manifolds,” Journal of Machine Learning [7] M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated Research, vol. 15, pp. 1455–1459, 2014. Extended Kalman Filter Based Visual-Inertial Odometry Using Direct [27] K. Li, D. Frisch, S. Radtke, B. Noack, and U. D. Hanebeck, “Wavefront Photometric Feedback,” The International Journal of Robotics Research, Orientation Estimation Based on Progressive Bingham Filtering,” in vol. 36, no. 10, pp. 1053–1072, 2017. Proceedings of the IEEE ISIF Workshop on Sensor Data Fusion: Trends, [8] S. J. Julier and J. K. Uhlmann, “Unscented Filtering and Nonlinear Solutions, Applications (SDF 2018), Oct. 2018. Estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004. [9] S. Bultmann, K. Li, and U. D. Hanebeck, “Stereo Visual SLAM Based [28] R. P. Mahler, Statistical Multisource-Multitarget Information Fusion. on Unscented Dual Quaternion Filtering,” in Proceedings of the 22nd Artech House Norwood, MA, 2007, vol. 685. International Conference on Information Fusion (Fusion 2019), Ottawa, [29] I. S. Gradshteyn and I. M. Ryzhik, Table of Integrals, Series, and Canada, July 2019. Products. Academic press, 2014.