Camera Pose Filtering with Local Regression Geodesics on the Riemannian Manifold of Dual Quaternions

TL;DR

This paper introduces a novel Riemannian manifold-based local regression filter for smooth, accurate 3D camera pose estimation using dual quaternions, outperforming existing methods in robustness and precision.

Contribution

It proposes a new manifold-aware filtering method on dual quaternions for joint orientation and translation smoothing, with an outlier-robust IRLS algorithm.

Findings

Improved pose smoothing accuracy on synthetic and real data.

Enhanced robustness to outliers in pose estimation.

Demonstrated theoretical and practical advantages over existing methods.

Abstract

Time-varying, smooth trajectory estimation is of great interest to the vision community for accurate and well behaving 3D systems. In this paper, we propose a novel principal component local regression filter acting directly on the Riemannian manifold of unit dual quaternions $\mathbb{D} \mathbb{H}_1$. We use a numerically stable Lie algebra of the dual quaternions together with $\exp$ and $\log$ operators to locally linearize the 6D pose space. Unlike state of the art path smoothing methods which either operate on $SO\left(3\right)$ of rotation matrices or the hypersphere $\mathbb{H}_1$ of quaternions, we treat the orientation and translation jointly on the dual quaternion quadric in the 7-dimensional real projective space $\mathbb{R}\mathbb{P}^7$. We provide an outlier-robust IRLS algorithm for generic pose filtering exploiting this manifold structure. Besides our theoretical analysis, our experiments on synthetic and real data show the practical advantages of the manifold aware filtering on pose tracking and smoothing.

Equations

$$\text{q}=q_1\text{1}+q_2\text{i}+q_3\text{j}+q_4\text{k}={\left(q_1,q_2,q_3,q_4\right)}^{\text{T}},$$

$$\bar{\text{q}}:=q_1-q_2\text{i}-q_3\text{j}-q_4\text{k}.$$

$$1\stackrel{\text{!}}{=}\Vert \text{q}\Vert :=\text{q}\cdot \bar{\text{q}}$$

$$\text{r}=\left[\cos \left(\theta /2\right),\sin \left(\theta /2\right)\text{v}\right].$$

$$\text{p}=x\text{i}+y\text{j}+z\text{k}.$$

$$\text{p}\mapsto \text{r}\cdot \text{p}\cdot \bar{\text{r}}.$$

$$\text{Q}=\text{r}+\epsilon \text{s}={\left(q_1,q_2,q_3,q_4,q_5,q_6,q_7,q_8\right)}^{\text{T}},$$

$${\epsilon }^2=0,\epsilon \text{i}=\text{i}\epsilon ,\epsilon \text{j}=\text{j}\epsilon ,\epsilon \text{k}=\text{k}\epsilon .$$

$$\bar{\text{Q}}:=\bar{\text{r}}+\epsilon \bar{\text{s}},$$

$$1\stackrel{\text{!}}{=}\Vert \text{Q}\Vert :=\text{Q}\cdot \bar{\text{Q}}=\text{r}\bar{\text{r}}+\epsilon \left(\text{r}\bar{\text{s}}+\text{s}\bar{\text{r}}\right),$$

$$\text{r}\bar{\text{r}}=1\text{and}\text{r}\bar{\text{s}}+\text{s}\bar{\text{r}}=0.$$

$$\mathbb{D}{\mathbb{H}}_1\ni \text{Q}=\text{r}+\epsilon \frac{1}{2}\text{t}\text{r}.$$

$$\text{P}\mapsto$$

$${\exp }_{\text{x}}:T_{\text{x}}\mathbb{G}\to \mathbb{G},$$

$${\log }_{\text{x}}:\mathbb{G}\to T_{\text{x}}\mathbb{G}.$$

$${\exp }_{\text{x}}\left(\text{s}\right)=\text{x}\exp \left({\text{x}}^{-1}\text{s}\right),$$

$${\log }_{\text{x}}\left(\text{q}\right)=\text{x}\log \left({\text{x}}^{-1}\text{q}\right).$$

$$\mathbb{H}\ni \text{q}=\left[0,\varphi \text{v}\right]$$

$$\exp :T_{\text{1}}{\mathbb{H}}_1$$

$$\mapsto 1+\sum _{k=1}^{\infty }\frac{{\text{q}}^k}{k!}:=\sum _{k=0}^{\infty }\frac{{\text{q}}^k}{k!}$$

$$=\cos \left(\varphi \right)+\frac{\sin \left(\varphi \right)}{\varphi }\text{q}$$

$$=\left[\cos \left(\varphi \right),\sin \left(\varphi \right)\text{v}\right]=:\text{r}.$$

$$\log :{\mathbb{H}}_1$$

$$\mapsto \left[0,\varphi \text{v}\right].$$

$$\mathbb{DH}\ni \text{Q}$$

$$\exp :T_{\text{1}}\mathbb{D}{\mathbb{H}}_1$$

$$\mapsto \sum _{k=0}^{\infty }\frac{{\text{Q}}^k}{k!}$$

$$=\frac{1}{2}\left(2\cos \left(\omega \right)+\omega \sin \left(\omega \right)\right)$$

$$-\frac{1}{2\omega }\left(\omega \cos \left(\omega \right)-3\sin \left(\omega \right)\right)\text{Q}$$

$$+\frac{1}{2\omega }\left(\sin \left(\omega \right)\right){\text{Q}}^2$$

$$-\frac{1}{2{\omega }^3}\left(\omega \cos \left(\omega \right)-\sin \left(\omega \right)\right){\text{Q}}^3.$$

$$\text{Q}=\left[\varphi ,\text{v}\right]+\epsilon \left[{\varphi }_{\epsilon },{\text{v}}_{\epsilon }\right]=:\left[\Phi ,\text{V}\right]$$

$$\Phi$$

$$=\text{v}+{\text{v}}_{\epsilon }\epsilon .$$

$$\sin \left(\Phi \right)$$

$$\cos \left(\Phi \right)$$

Full text

1 Introduction

Many 3D computer vision tasks require a robust and reliable understanding of position and orientation in space and a outlier-free localization of the camera with respect to its surroundings is a fundamental requirement in machine vision tasks such as registration, reconstruction or tele-robotics. Independent on the given data modality, the natural underlying structure of the input is a temporally ordered set which can be analyzed sequentially. In an example scenario such as SLAM, egocentric vision or marker tracking, a single camera provides a stream of consecutive images generating a pose path. In this paper, we address motion smoothing, where - given the per frame motion estimates - the goal is to synthesize a new camera trajectory, which is smooth and closer to the underlying movement or intended trajectory.

1.1 Pose Parametrization

Different parametrizations are prominent to accurately and efficiently describe spatial displacements. Most widely used representations include homogeneous matrices, quaternions or twist-coordinates. While many of these are sufficient to handle orientation, the translational component is usually treated separately [10, 15]. Moreover, the algorithms which use the semi-direct product of $SO\left(3\right)$ and $\mathbb{R}^{3}$ to describe elements of the group $SE\left(3\right)$ suffer from the structural embedding of the rotational part in an higher dimensional space. One solution to circumvent this problem emerges with the dual quaternion (DQ) parametrization, in which all pose components form a common space. Besides a smaller memory footprint the representation has the advantage of being cheaper in case of consecutive transformations while it is numerically more stable to calculate due to the fact that the matrix space is higher dimensional and a re-orthogonalization [3] is computationally much more expensive than a re-normalization. The DQ formulation has already been successful in applications of interpolation [5], skinning [17] or rigid body dynamics [37]. Unfortunately, the applications are still not numerous. This is partially due to the fact that the manifold is more complex than the one of the quaternions and immediate geometric intuitions are lacking. With this paper, we devise a general picture on how to operate on the quadric shaped Riemannian manifolds of dual quaternions by studying the exponential and logarithm maps and use them to construct an outlier aware, robust pose smoothing algorithm.

1.2 Manifold Filter

In practice, acquired camera poses follow complex noise models as partially interdependent factors such as sensor noise, hand-shake, wrong pose estimates and velocity appear. The behaviour is highly non-linear and tedious to model directly. Thus, the requirement of tuning the model parameters for Kalman filter methods is not suited for certain applications. We employ a non-parametric smoother without explicit distribution assumptions on the overall data which we embed in a framework on the DQ-manifold. At this point, the local regression (LR) reveals itself to be the method of choice. In the simplest form of LR, one of the points is treated as the center and a linear relationship between the independent and response variables in its local neighbourhood are sought. This point is then projected onto the line, gaining a new, smoothed position. This procedure iterates over the sequence, while iteratively denoising the points. While this procedure is fairly easy for Euclidean spaces, many of these operations do not generalize to non-Euclidean manifolds. We make use of the fact that a point on the pose path is selected and the regression considers only the immediate neighbourhood. Hence, we utilize the exponential and logarithm maps in the quaternion notation to operate on the local tangent space, alleviating the tedious non-Euclidean geometry. We contribute a PCA based revised LR formulation on the local linearized space, making effective use of the Riemannian DQ-manifold.

Our experiments show that, while a weighted manifold-PC regression is mostly satisfactory, for pose sequences with outliers, IRLS smoothing on DQ outperforms in terms of fidelity. The advantage of manifold smoothing also reveals as the noise level increases.

2 Related Literature

The Clifford Algebra of dual quaternions [8] extends the non-commutative division algebra of the quaternions $\mathbb{H}$ [14]. Besides pure [2] and applied geometry [29], quaternions have a broad history in computer vision [28].

Rotations are more efficiently concatenated if a quaternion representation is chosen and effects such as gimbal lock are avoided [21], which is why quaternions are applied to various real-time 3D processing pipelines [23]. In particular the fact that $\mathbb{H}$ can be identified with points on a 3-dimensional hypersphere $S^{3}$ makes it attractive for animation and rendering as SLERP [31] interpolation can be calculated efficiently.

In spite of their usefulness for many real-time calculations, dual approaches did not receive the same attention.

A dual quaternion $\textbf{Q}\in\mathbb{D}\mathbb{H}_{1}$ of unit length can be used to represent a rigid body displacement of an object. Kuang [20] uses this to show the advantages of dual quaternions for real-time motion animation for clothed body movements and Kavan [17] applied $\mathbb{D}\mathbb{H}_{1}$ for skinning. Dual methods are also present in blending [27] and complex hierarchical rigid body transforms [18] and more recently, movement extrapolation [5] has been an application field for $\mathbb{D}\mathbb{H}_{1}$.

We represent rigid body movements as trajectories on $\mathbb{D}\mathbb{H}_{1}$ where we filter noisy paths. Regression models for interpolation are essential for computer animation and refinement of pose data [26]. Kavan [16] approximates the geodesic distance with an L2-norm in $\mathbb{R}^{8}$ for real-time transformation blending. However, the unit dual quaternion space is non-Euclidean.

A diffusion approach in this regard has been proposed by Torsello [33] where the Riemannian metric is minimized for multiview registration. We also take the Riemannian nature of $\mathbb{D}\mathbb{H}_{1}$ into account and perform a differential correction by moving to the tangent space where we apply a local smoothing method. Similar filtering approaches have been studied extensively [10, 15] in the joint space $\mathbb{H}_{1}\times\mathbb{R}^{3}$, where the pose is split into rotation and translation parts. Ng [25] proposes a Gaussian smoothing on the non-dual quaternion manifold [25] while Srivatsan [32] explicitly assumes an underlying noise model and uses a Linear Kalman filter on dual quaternions in $SE\left(3\right)$ which is done with an Extended Kalman Filter (EKF) for pose estimation by Filipe [12]. On the application side, smoothing techniques are used for of Video Stabilization [15] and robotics [10].

3 Mathematical Formulation

A substantial part of the proposed framework uses approaches from differential geometry which are applied to the space of poses. We first introduce the pose spaces $\mathbb{H}_{1}$ and $\mathbb{D}\mathbb{H}_{1}$ of unit (dual) quaternions which we analyze thereafter locally and conclude by giving the exponential and logarithm maps in quaternion representation such that these can be implemented in the proposed algorithms afterwards. The notation follows the convention of Busam [5] where the algebraic structures are presented in more detail.

3.1 Quaternions

Quaternions extend the complex numbers with three imaginary units i, j, k.

Definition 1

A quaternion q is an element of the algebra $\mathbb{H}$ in the form

[TABLE]

with $\left(q_{1},q_{2},q_{3},q_{4}\right)^{\text{T}}\in\mathbb{R}^{4}$ and $\textbf{i}^{2}=\textbf{j}^{2}=\textbf{k}^{2}=\textbf{i}\textbf{j}\textbf{k}=-\textbf{1}$.

We also write $\textbf{q}:=\left[a,\textbf{v}\right]$ with the scalar part $a=q_{1}\in\mathbb{R}$ and the vector part $\textbf{v}=\left(q_{2},q_{3},q_{4}\right)^{\text{T}}\in\mathbb{R}^{3}$. The conjugate $\bar{\textbf{q}}$ of the quaternion q is given by

[TABLE]

Quaternions are of particular interest in computer vision due to their connection with spatial rotations [11]. A unit quaternion $\textbf{q}\in\mathbb{H}_{1}$ with

[TABLE]

gives a compact and numerically stable parametrization to represent orientation and rotation of objects in $\mathbb{R}^{3}$ which avoids gimbal lock [21].

Rotations with Quaternions

The rotation around the unit axis $\textbf{v}=\left(v_{1},v_{2},v_{3}\right)^{\text{T}}\in\mathbb{R}^{3}$ with angle $\theta$ is thereby given by

[TABLE]

Identifying antipodal points q and $-\textbf{q}$ with the same element in $SO\left(3\right)$, the unit quaternions form a double covering group of the 3D rotations and any pure quaternion

[TABLE]

of the point $\textbf{u}=\left(x,y,z\right)^{\text{T}}\in\mathbb{R}^{3}$ is rotated by the unit quaternion r via the sandwiching product map

[TABLE]

3.2 Dual Quaternions

Similar to the representation of rotations by quaternions, we can use dual quaternions of unit length to represent spatial displacements. We can define a dual quaternion as an ordered pair of quaternions with dual numbers as coefficients. A dual number $Z$ is an element of the algebra $\mathbb{D}$ that can be written [19] as $Z=r+\varepsilon s$ where $r,s\in\mathbb{R}$ and $\varepsilon^{2}=0$, where $r$ is the real-part, $s$ is the dual part, and $\varepsilon$ is called the dual operator. The dual conjugate is similar to the complex conjugate of $\mathbb{R}+i\mathbb{R}$. It is given by $\hat{Z}:=r-\varepsilon s$.

Extending this concept to quaternions, we can define dual quaternions.

Definition 2

A dual quaternion $\textbf{Q}\in\mathbb{D}\mathbb{H}$ is an ordered set of quaternions

[TABLE]

where $\textbf{r},\textbf{s}\in\mathbb{H}$, $\left(q_{1},q_{2},q_{3},q_{4},q_{5},q_{6},q_{7},q_{8}\right)^{\text{T}}\in\mathbb{R}^{8}$ and

[TABLE]

The Clifford algebra of dual quaternions contains the real numbers $\mathbb{R}$, the complex numbers $\mathbb{C}$, the dual numbers $\mathbb{D}$, and the quaternions $\mathbb{H}$ as sub-algebras. With the conjugate $\bar{\textbf{Q}}$ of the dual quaternion $\textbf{Q}=\textbf{r}+\varepsilon\textbf{s}$

[TABLE]

we can study the constraints given for unit unit dual quaternions $\textbf{Q}\in\mathbb{D}\mathbb{H}_{1}$. If we demand unit length, it holds

[TABLE]

which gives the two distinct constraints

[TABLE]

Displacements with Dual Quaternions

The unit dual quaternions are isomorphic to the group of rigid body displacement $SE\left(3\right)$ [1] and the two constraints (11) reduce the eight parameters of the dual quaternions to the six degrees of freedom of a rigid motion in space with its translation and rotation. If we write the translation as a pure quaternion t (5) and the rotation as a unit quaternion r (4), we can construct the unit dual quaternion

[TABLE]

Analogously to the quaternions, we formulate the dual pure quaternion P for the point $\textbf{p}=\left[0,\textbf{u}\right]$ as $\textbf{P}=\textbf{1}+\varepsilon\textbf{u}$ and the spatial displacement becomes the sandwiching product map on dual quaternions

[TABLE]

where the conjugates for the dual quaternion and the dual are calculated consecutively.

3.3 Riemannian Geometry

The spaces $\mathbb{H}_{1}$ and $\mathbb{D}\mathbb{H}_{1}$ can also be considered as differentiable Riemannian manifolds $\mathbb{G}$ [34]. A continuous collection of inner products on the tangent space of $\mathbb{G}$ at $\textbf{x}\in\mathbb{G}$ defines a Riemannian metric. The shortest path on the manifold defined by such a metric is called the geodesic.

With these concepts, we take a closer look to the geometry of the (dual) quaternion space and calculate a specific mapping into the tangent space which we generalize with the help of parallel transport. This will pave the way to subsequently define different local geodesic regressors in pose space.

3.3.1 Geometry of $\mathbb{H}_{1}$ and $\mathbb{D}\mathbb{H}_{1}$

With constraint (3), the unit quaternions form the three dimensional hypersphere $S^{3}\in\mathbb{R}^{4}$. Thus $\mathbb{H}_{1}$ is isomorphic to the real projective space $\mathbb{R}\mathbb{P}^{3}$. Looking at the two constraints from (11), we can analyze the structure of the unit dual quaternion space. The first equation $\left\|\textbf{r}\right\|=1$ forces the real part r of Q to be of unit length, hence $\textbf{r}\in\mathbb{H}_{1}$. This gives the 7-dimensional hypersphere $S^{7}\in\mathbb{R}^{8}$ and the identification of antipodal points forms the seven dimensional real projective space $\mathbb{R}\mathbb{P}^{7}$. The second equation reads as $\textbf{r}\bar{\textbf{s}}=-\textbf{s}\bar{\textbf{r}}$ and thus defines a quadric in $\mathbb{R}\mathbb{P}^{7}$. Thus $\mathbb{D}\mathbb{H}_{1}$ is not a hypersphere. This need to be considered for any operation on the manifold.

3.3.2 Lie Groups and Parallel Transport

From a differential geometry perspective, a Lie group can be viewed as a differentiable Riemannian manifold. The Lie algebra to the Lie group is the tangent space at the identity of the group. Thus it gives a linearization of the Lie group near the identity. The map from the tangent space $T_{\textbf{x}}\mathbb{G}$ at x to the Lie group $\mathbb{G}$ is called the exponential map

[TABLE]

which is locally defined and maps a vector in the tangent space to a point on the manifold following the geodesic on $\mathbb{G}$ through x. Its inverse is called the logarithm map

[TABLE]

The mapping $\exp_{\textbf{x}}$ at point $\textbf{x}\in\mathbb{G}$ can be computed by parallel transport [13] as illustrated in Figure 1. With the exponential map $\exp_{\textbf{1}}=:\exp$ at the identity $\textbf{1}\in\mathbb{G}$ and the logarithm map it holds

[TABLE]

As a next step, we want to use this to derive the exponential and logarithm maps at the identity for the elements of the groups $SO\left(3\right)$ and $SE\left(3\right)$ in quaternion notation. For matrices these maps are well studied objects [35], [24]. We study the exponential maps directly in (dual) quaternion space using its definition as a Maclaurin series.

3.3.3 Exponential and Logarithm map in $\mathbb{H}$

The identity in $\mathbb{H}_{1}$ is given by $\textbf{1}=\left(1,0,0,0\right)^{\text{T}}$. The tangent space $T_{\textbf{1}}\mathbb{H}_{1}$ is thus the hyperplane to the hypersphere $S^{3}\in\mathbb{R}^{4}$ and parallel to the axes $x_{2}$, $x_{3}$, $x_{4}$ passing through 1. Any quaternion in $T_{\textbf{1}}\mathbb{H}_{1}$ is of the form

[TABLE]

with $\textbf{v}\in\mathbb{R}^{3}$, $\left\|\textbf{v}\right\|=1$ and the series writes as

[TABLE]

where the second last step is done by recognizing the Taylor series for the sine and cosine function at [math]. Note, that this relationship directly aligns with the notation in (4) while $\phi=\theta/2$ and the inverse function is given by

[TABLE]

3.3.4 Exponential and Logarithm map in $\mathbb{D}\mathbb{H}$

Let

[TABLE]

be a pure dual quaternion with the two pure quaternions $\textbf{q},\textbf{q}_{\varepsilon}\in\mathbb{H}_{1}$. Simplification [30] of the Maclaurin series for the exponential map then yields

[TABLE]

Before we compute the inverse function, we make the observation that any unit dual quaternion

[TABLE]

with the dual entities

[TABLE]

can be written [9] equivalently to (4). For this, we calculate the dual trigonometric operators through a series expansion which brings

[TABLE]

We proof the following lemma by explicit calculation of the dual quaternion representation.

Lemma 1

Any unit dual quaternion $\textbf{Q}\in\mathbb{D}\mathbb{H}_{1}$ can be written as

[TABLE]

where $\textbf{V}\in\mathbb{D}\mathbb{H}$ is a pure dual quaternion of form (25).

Proof.

Analogously to the quaternion rotation, the formulation (37) can be understood as a parametrization of the rigid body motion. According to Chasles’ Theorem [7], a displacement can be modeled via a translation along a unique axis with a simultaneous rotation about the same axis. This is visualized in Figure 2. We construct the dual quaternion displacement for this motion explicitly in the form (37).

Let a rigid body transformation be given by a translation $\textbf{t}\in\mathbb{R}^{3}$ and a rotation $\textbf{R}\in\mathbb{R}^{3\times 3}$ around the axis v with $\left\|\textbf{v}\right\|=1$ with angle $\theta$. From (12) we know already the unit dual quaternion for this displacement.

The parameters for the screw motion are angle $\theta$, pitch $\theta_{\varepsilon}$, screw axis l with moment $\textbf{v}_{\varepsilon}$ (i.e. $\textbf{v}_{\varepsilon}=\textbf{p}\times\textbf{v}\ \forall\ \textbf{p}\in\textbf{l}$) and direction v. The angle $\theta$ is directly given. We first compute the pitch $\theta_{\varepsilon}$ in the direction v of the axis as the projection of the translation onto the axis. This is $\theta_{\varepsilon}=\textbf{t}^{\text{T}}\textbf{v}$. In order to recover the moment $\textbf{v}_{\varepsilon}$, we pick a point u on the axis. With this we can describe t in terms of $\theta_{\varepsilon}$, v, R and u as

[TABLE]

and with the Rodrigues formula it holds

[TABLE]

Thus substituting this into (38) gives with $\textbf{u}^{\text{T}}\textbf{v}=0$

[TABLE]

which brings for the moment vector

[TABLE]

Substituting the rotation quaternion $\textbf{r}=\left[q_{0},\textbf{q}\right]$ and using $\theta_{\varepsilon}=\textbf{t}^{\text{T}}\textbf{v}$ yields

[TABLE]

which is the pure quaternion of the dual part in (12). Thus

[TABLE]

which equals (37) if we apply the trigonometric operators (35), (36) and the dual entity represantations (33), (34). ∎

We note that this representation separates the line information of the screw axis from the pitch and angle values in an algebraical way where the dual vector V represents the axis of a screw motion with its direction vector and the dual angle $\Theta$ contains both the translation length and the angle of rotation.

Since for the exponential of a dual quaternion of the form $\textbf{Q}=\textbf{V}\frac{\Theta}{2}$ it holds [16]

[TABLE]

the inverse function of $\exp$ for quaternions of the form (37) is then given by

[TABLE]

4 Pose Filter

With the ideas from Section 3 it is now possible to linearize the pose spaces $SO\left(3\right)$ and $SE\left(3\right)$ locally with the logarithm maps (24) and (47) using parallel transport (17) such that Euclidean methods can be applied for investigations and processing of pose data. A solution of an operation in tangent space $T_{\textbf{x}}\mathbb{G}$ can be mapped back thereafter with the exponential maps (22), (27) and (16) without affecting the Riemannian geometry of the manifold.

In the following, we develop a robust smoothing method in (dual) quaternion space by using the aforementioned operations to filter the pose data with a linear approximation of the rigid body movement in the tangent space.

4.1 Robust Motion Stabilization

A time-varying, sequential motion can be interpreted on the pose space $\mathbb{D}\mathbb{H}_{1}$ as a high dimensional trajectory. Generally, due to lack of constraints on physical motion or tracking errors, this trajectory is highly non-linear, non-Gaussian and includes outliers. Moreover, it is non-uniformly sampled because of velocity changes or sudden jumps. We take this non-ideal setting into account and propose a parameter-free path smoothing algorithm which is robust, flexible, intuitive and could naturally benefit from the availability of the uncertainties in the pose estimation. Locally, we treat the trajectory as a linear one, and seek to find the linear association of the data points X to the responses Y such that $\textbf{Y}=\textbf{X}\beta+\epsilon$. A common way to discover this local relationship is using the Generalized Least Squares minimization

[TABLE]

with diagonal weight matrix W. The solution is given by weighted least squares regression

[TABLE]

Equivalently, when the data is rather unevenly distributed, one likes to prevent distinguishing the predictor and response, thereby rather looking for meaningful linear projections (e.g. maximum variance). Thus, such an hyper-line can be obtained from the first principal component of the data points X. Let $\textbf{X}=\textbf{U}\textbf{S}\textbf{V}^{T}$ denote the singular value decomposition (SVD) of X, with right singular vectors V. Then, $\textbf{V}\Delta\textbf{V}^{T}$ gives the spectral decomposition of the covariance matrix $\textbf{X}\textbf{X}^{T}$ with the non-negative eigenvalues $\Delta=\text{diag}(\lambda_{1}\ldots\lambda_{p})$. Weights are then transferred directly to the covariance matrix by

[TABLE]

The columns of V give an orthonormal set of eigenvectors and $\mathbf{X}\mathbf{v}_{j}$, the $j^{th}$ principal component. The data can then be projected onto the first principal subspace resulting in the principal covariates $\mathbf{X}\textbf{V}_{k}:=\{\mathbf{X}\mathbf{v}_{1}\ldots\mathbf{X}\mathbf{v}_{k}\}$. To smooth the trajectory, the central point c is projected onto the PC-line as illustrated on the plane in Figure 3. Note that this fit assumes a local Gaussian distribution, while the global distribution can be arbitrary.

While such PCA scheme holds for the Euclidean spaces, it does not generalize to arbitrary manifolds such as the dual quaternion space, because these spaces are not necessarily Euclidean. Buss and Fillmore [6] show that even regressing a great arc on the quaternion hypersphere has ambiguities. For the case of local regression, the central point of fitting is known which enables us to map the immediate neighbourhood onto the tangent space $T_{\textbf{x}}\mathbb{G}$, thereby circumventing the non-Euclidean nature of the dual quaternions. Thanks to the manifold structure, the tangent space locally behaves Euclidean and we can perform the fit. To smooth the curve, the central point is projected onto the regressed 8D-line and mapped back onto the manifold using the exponential map. We refer to this method as plain PCA filtering. Naturally, the 8D data points, which are closer to the center of the local model c are more relevant for the fit, as the linearity assumption decreases with the distance. Therefore we multiply each data point by a Gaussian prior function to downweight the points based on their relative position:

[TABLE]

where D is a positive semi-definite distance metric explaining the region of influence. In the following experiments this approach is named weigthed PCA (wPCA).

Oftentimes the pose space contains outliers, for which a naive fit does not work. To care for this, we introduce a re-weighted procedure, in which the residuals of the current fit are used to update the weights for the next iteration. This is commonly referred as iteratively reweighted least squares (IRLS). We use a simple distance based weight update:

[TABLE]

where $\delta$ is a small number, preventing division by zero and $\{\textbf{r}_{i}^{k}\}$ are the residuals at iteration $i$.

Algorithm 2 summarizes our final implementation. As this method is independent of the structure of the parameter space, we can either use it individually on rotations and translations or on the dual quaternion space.

5 Experiments

In the following, the pose filtering methods presented in Section 4 are evaluated and compared to other approaches in two different scenarios. A first synthetic experiment analyzes the ability of the smoothing algorithms to recover a noisy pose series with outliers while the second evaluation is performed on a real dataset of natural hand movement in a collaborative medical robotic environment where tracking accuracy is crucial.

5.1 Synthetic Tests

Our first test evaluates the robustness and accuracy of the tangent space regressors. In order to evaluate these properties we generate a synthetic dataset from a ground truth rigid body movement.

A set of five points $\textbf{v}_{i}\in\mathbb{R}^{3}$ together with five values $\theta_{i}\in\left[0,2\pi\right]$ is chosen as query points representing the rotation axis and angle of the rotations $\textbf{R}_{i}$. Five points $\textbf{t}_{i}\in\left[0,1\right]^{3}$ represent the translational component of the poses. A cubic spline interpolates both the axes and angles and with (4) and (12) we get a pose representations in $\mathbb{H}_{1}\times\mathbb{R}^{3}$ and $\mathbb{D}\mathbb{H}_{1}$. As the space $\mathbb{R}^{3}$ is already Euclidean we can perform a pose filtering for the 6-DoF pose both in $\mathbb{D}\mathbb{H}_{1}$ and $\mathbb{H}_{1}\times\mathbb{R}^{3}$. For the latter, all methods are applied twice on both spaces independently.

To evaluate the performance of the local regression, we sample the ground truth pose series densely and apply additional noise uniformly distributed in $\left[-\sigma,\sigma\right]$ with $\sigma=0.02$ to the angle and the axis of rotation as well as to the translation. On top, random outliers for $5~{}\%$ of the data points are created with an additional noise of $\sigma=0.2$.

Then we run the methods PCA, wPCA, IRLS, Dual PCA, Dual wPCA, Dual IRLS as well as a Linear Kalman Filter on the data. We use a window size of 19 and the Kalman implementation [36] of MATLAB [22] with a covariance tuple of $\left[0.5,2\right]$ for the rotation and $\left[0.2,1\right]$ for the translation process noise and measurement noise covariance. The resulting pose set is illustrated together with the results for the Dual IRLS method in Figure 4.

An error quantification for the different methods is given in Figure 5.

For the Kalman filter, tradeoff values have been chosen which are still able to recover the pose without over-smoothing. However, the method only evaluates the past points and thus information of half the window size for future poses is not included which explains the performance difference. The direct local PCA methods perform equally well in the same error range while the weighting gain in the separate treatment is slightly better with $0.010\pm 0.020$ in translation and $0.37\pm 0.55^{\circ}$ rotation. The outlier aware IRLS method performs best for the angle while the improvement for the translation is with $2.1\cdot 10^{-3}$ only minimal for the non-dual quaternions. It can be clearly seen that the treatment of outliers in the dual space $\mathbb{D}\mathbb{H}_{1}$ helps to increase the accuracy to $4.3\cdot 10^{-3}$ and $0.26^{\circ}$ in the median. This can be intuitively understood through the fact that the local neighbourhood of the linear regression line in $\mathbb{D}\mathbb{H}_{1}$ is much more restrictive than the joint neighbourhoods in $\mathbb{H}_{1}\times\mathbb{R}^{3}$ where the effect of an outlier in the separate parameter spaces is higher.

It is also worth mentioning that the only parameter of the presented methods is the window size which directly reflects the movement speed of the displacements while the parameter adjustments for a filter method such as Kalman are more elaborate.

For visual comparison of the different methods on a synthetic pose stream with noise, please be referred to the supplementary video.111http://campar.in.tum.de/Chair/PublicationDetail?pub=busam2017_mvr3d

5.2 Tracking Stream Refinement

We compare our methods on the dataset of Busam et al. [5] where a robotic arm is set to gravity compensation mode with zero stiffness and a human operator performs a natural hand movement manipulating its end effector. The robot is tracked via a marker based stereo vision system running the tracking algorithm [4] and calibrated such that the forward kinematics of the industrial robotic manipulator provide ground truth poses with a precision of $0.05$ mm.

The $30$ Hz pose stream is fed into our filter pipelines and compared to the absolute poses of the dataset. Figure 6 illustrates the results where we use the same naming and parameters as in Section 5.1.

It is noteworthy that in this scenario, the wPCA methods perform best with $11.2\pm 3.9\mu m$ (median $11.4\mu m$) and $0.7\pm 1.4^{\circ}$ (median $0.5^{\circ}$) for the non-dual one while the IRLS methods give only mediocre results between PCA and wPCA both for quaternions ($13.0\mu m$, $0.6^{\circ}$ median) and dual quaternions ($14.1\mu m$, $0.6^{\circ}$ median). The Kalman filter again gives acceptable results for which heuristical parameter fine-tuning did not show any significant improvements.

The advantage of the dual space robustification - which can yield a significant improvement (see Section 5.1) in the case of outliers - is not applicable as there are only few outliers in the already quite accurate optical tracking data. The IRLS methods suffer from this problem as the weights for equally important data points are reduced. This results in case of reliable pose data in the fact that the separate treatment of translation and rotation is preferable as the non-dual regressors perform better.

6 Conclusion

We presented a method for camera pose filtering to recover smooth trajectories in pose space. The use of differential geometry helps to describe a local regression problem in the tangent space of unit (dual) quaternions. Applying a robust line fitting on the principal component of the pose measurements allows to filter an ordered pose series in a way that both the translation path as well as its orientation can be recovered. Besides smoothing and outlier-awareness, the method benefits from being non-parametric. In particular no explicit noise model needs to be modeled. Our experiments revealed that the dual space formulation robustifies the smoothing. As the logarithm and exponential maps are defined locally, the obvious downside of a local linearization is its need for a dense sampling around the touch point of the tangent space with the manifold. For an online application of the filtering also the moving window has to be cut to half or future points need to be extrapolated.

This work could be extended towards including an uncertainty measure into the local weights. An intuitive vision measure would be for example the backprojection error in a feature point tracking algorithm or the registration RMS of a 3D localization method. Moreover, the mentioned need for dense samples could be explicitly modeled including the timestamps in a tracking application. We believe that problems such as video stabilization or visual odometry could also benefit from the robust regression.