Motor Estimation using Heterogeneous Sets of Objects in Conformal Geometric Algebra

In this paper we present a novel method for nonlinear rigid body motion estimation from noisy data using heterogeneous sets of objects of the conformal model in geometric algebra. The rigid body motions are represented by motors. We employ state-of-the-art nonlinear optimization tools and compute gradients and Jacobian matrices using forward-mode automatic differentiation based on dual numbers. The use of automatic differentiation enables us to employ a wide range of cost functions in the estimation process. This includes cost functions for motor estimation using points, lines and planes. Moreover, we explain how these cost functions make it possible to use other geometric objects in the conformal model in the motor estimation process, e.g., spheres, circles and tangent vectors. Experimental results show that we are able to successfully estimate rigid body motions from synthetic datasets of heterogeneous sets of conformal objects including a combination of points, lines and planes.


Introduction
Rigid body motion or pose estimation in geometric algebra has been an active research topic over the last decades, and it has been investigated using the conformal model since its introduction in 2001 by Li, Hestenes and Rockwood [15].Lasenby et al. [14] presented attitude estimation using rotors in the 3D Euclidean model.Daniilidis [5] presented a solution to the handeye calibration problem in robotics using dual quaternions.This solution was later extended to the motor algebra in [2], and to the conformal model in [18].Rosenhahn and Sommer [19] presented 2D-3D pose estimation using stratification of spaces.Further, Gebken, Perwass and Sommer [9] presented methods for 2D-3D and 3D-3D rigid body motion estimation from point data

Geometric Algebra and the Conformal Model
Geometric algebra is an approach to geometry based on the work of W. Clifford who combined Grassmann's exterior algebra with Hamilton's quaternions, and created what he termed geometric algebra.Hestenes developed geometric algebra further in his books [11,12] and later introduced the conformal model in [15].
The elements of a geometric algebra are called multivectors.The geometric algebra over the 3-dimensional Euclidean space R 3 is denoted R 3 .The notation R r 3 refers to the r-grade elements of R 3 ; e.g., R 2 3 refers to the elements of R 3 of grade 2 -the bivectors.The notation R + 3 refers to the elements of R 3 of even grade.The conformal model of geometric algebra is denoted R 4,1 with the null basis {e 1 , e 2 , e 3 , n o , n ∞ }.The basis vector n ∞ = e − − e + represents the point at infinity and the basis vector n o = (e − + e + )/2 represents an arbitrary origin.These basis vectors have the properties n 2 ∞ = n 2 o = 0 and The notation e ij is shorthand for the outer product e i ∧ e j of the vectors e i , e j ∈ R 1 3 .The highest grade element of R 3 , the Euclidean pseudoscalar, is denoted I 3 .The conformal pseudoscalar is denoted I.The conformal dual of a multivector X is denoted X * = X • I −1 .The element of grade r of a multivector X is extracted using the grade projection operator X r . Vectors 4,1 are constructed through the outer product of two conformal points and the point at infinity: 4,1 are constructed through the outer product of three conformal points and the point at infinity Rigid body motions can be represented in the conformal model using motors where T = 1 − 1 2 tn ∞ is a translator with translation vector t ∈ R 1 3 , and R ∈ R + 3 is a rotor.A rotor R can be written in the form where θ ∈ R is the rotation angle and B ∈ R 2 3 is a unit bivector that encodes the rotation plane.It is noted that rotors are isomorphic to the unit quaternions.
Define M = span{1, e 12 , e 13 , e 23 , e 1 n ∞ , e 2 n ∞ , e 3 n ∞ , I 3 n ∞ }.The motor manifold M is defined as the set (2. 3) The motors can be written as the exponential of a unit bivector where Λ * ∈ span{e 12 , e 13 , e 23 , e 1 n ∞ , e 2 n ∞ , e 3 n ∞ }.More specifically, Λ * = θB + tn ∞ where B ∈ R 2 3 and t ∈ R 1 3 .It can be shown that Λ * is a dual line representing the screw axis of the rigid body motion, see [6].Following [27], the exponential formulation in (2.4) can be written in terms of the constituent elements of different grades as where t ⊥ = (t ∧ B)/B is the rejection of t by B and t = (t • B)/B is the projection of t in B. As described in [7], the rotor R of a motor M = T R can be extracted by L. Tingelstad and O. Egeland Adv.Appl.Clifford Algebras The translation vector t ∈ R 3 can then be found using The bivector B is found as the normalized grade 2 part of the rotor R: (2.10) A geometric object X ∈ R 4,1 is displaced by a motor M ∈ M using the sandwich product where X is the displaced object.

Motor Estimation from Observations of Conformal Objects
In this section a new approach to motor estimation is proposed based on non-linear least squares optimization of the form min where F : M → R is the cost function to be minimized and M is a conformal motor on the motor manifold M. The cost function F can be written where each observation i corresponds to a vector f i (M ) = (r i1 , . . ., r ip ) of p residuals r ij ∈ R. It is seen that the cost function F has m = Np residuals where N is the number of observations.A residual is a scalar measure for the discrepancy between the model and the observed data [16].
It must be ensured that the solution stays on the motor manifold during the optimization.In this paper, this is achieved using the exponential map in (2.5) to (2.7).The use of the exponential map in optimization on manifolds has previously been used in pose estimation on SE(3) [21], and in numerical integration on SO(3) and SE(3) [10].The motor M k+1 ∈ M in iteration k + 1 is calculated as where a k+1 ∈ R Here, a k+1 is the step calculated using a nonlinear least-squares method [16], e.g., BFGS, nonlinear conjugate gradients or the Levenberg-Marquardt method.The method used in this work is the Levenberg-Marquardt method.This means that Λ * (a k+1 ) is in the tangent space of M at M k .Thus, M k+1 is on the motor manifold M and satisfies

Motor Estimation from Points
Consider a rigid body that is displaced by a motor be a set of points on the rigid body in the initial configuration, and let {q i = Mp i M } be the set of same points in the displaced configuration.The sets {p i } and {q i } are called point clouds.Motor estimation is to find the motor M given {p i } and {q i }.
One possible formulation of this optimization problem employs the inner product between two conformal points min (3.5) In this formulation, the measure that is optimized is the squared distance between each two points, resulting in a 1-dimensional residual vector for each observation.This however, is not a good formulation for the cost function as is shown in the experimental results in [24] where the optimization process converge slowly close to the solution.
An alternative solution is to project the points down to the 3dimensional Euclidean model after the transformation by the motor M , and then to use the residual errors along each of the coordinate axes, resulting in a 3-dimensional residual vector for each observation min In [24], we found that this gave a significantly faster rate of convergence in experiments.

Motor Estimation from 3D Line Correspondences
The next step is to consider motor estimation from two sets of 3D line correspondences.Lines have been widely used in tracking, visual servoing and pose estimation [1], however most of this work is based on optimizing 2D-3D line correspondences with error metrics based on image features and not 3D-3D line correspondences.Bartoli and Sturm [1] present two reasons for this.First, there is no global minimal parameterization for lines representing their 4 degrees of freedom with 4 global parameters, and second, there are no universally agreed error metric for comparing lines.We present three formulations for motor estimation from 3D line correspondences.Formally, given a set of direct or dual line correspondences the aim is estimate the motor M that transforms the set {Λ 1i } onto the set {Λ 2i }, that is, The first formulation is based on the "Lin 3D" method in [1], and is based on direct comparison of line coordinates.The optimization problem can then be formulated as L. Tingelstad resulting in a 6-dimensional residual vector for each observation.
The other formulation we present is based directly on the geometry of two lines [26].This formulation is based on the angle and the common normal between two lines.Given two normalized dual lines Λ * 1 and Λ * 2 , the common normal Euclidean vector w ∈ R 1  3 can be found by computing the motor (3.10) A measure for the angle between the two lines is found from (3.11) The common normal vector w can then be found as the rejection of the translation vector t ∈ R 1 3 of the translator T in the rotation plane bivector B ∈ R 2 3 of the rotor R.
Given the motor M and the set of normalized dual line correspondences The optimization problem can then be formulated as min resulting in a 4-dimensional residual vector for each observation.A 2-dimensional residual vector for each observation can be formed by using the Euclidean norm of the common normal vector w, w ∈ R, and the measure for the angle between the lines.

Motor Estimation from Plane Correspondences
This section presents a new method for motor estimation from correspondences of 3D planes.The proposed method holds for both direct and dual planes.Valkenburg and Dorst [26] employ the inner product between two planes as the similarity measure used in the optimization.For two dual planes However, this measure does not include any information regarding the distance from the origin along the respective normal vectors of each plane.The estimated motor based on this measure will only contain the rotor and not the translator.
In this work we present a formulation that is able to estimate not only the rotor components of the motor, but also the components related to the translator.Similarly to creating a motor through the geometric product of two lines, we form a general rotor G = T R T , which is the rotor R translated by the translator T = 1 − 1 2 wn ∞ , through the geometric product of two planes, Note that a translator is formed if the unit vectors of the two planes are parallel.The distance measure can then be found by decomposing the general rotor and finding the translation vector w ∈ R 1 3 and taking the Euclidean norm (3.16) The optimization problem can then be formulated with the cost function min resulting in a 2-dimensional residual vector for each observation.

Motor Estimation using Heterogeneous Conformal Objects
In the preceding sections, only homogeneous sets of conformal objects have been used in the motor estimation.Based on the ideas of Valkenburg and Dorst [26], this is now extended to motor estimation using heterogeneous sets of conformal objects that have all been transformed by the same motor, as shown in Fig. 1 for a point, a line and a sphere.
where N 1 are the number of point correspondences, N 2 are the number of line correspondences, N 3 are the number of plane correspondences and w 1i , w 2j , w 3 k ∈ R are weights to adjust the contributions from the different cost functions to the overall solution based on, e.g., the reliability of the data [26].This formulation can be used to estimate motors from more complex geometric objects as point pairs, circles and tangent vectors.For circles, this can be performed by using the center point and the carrier plane where the center points c ∈ R 1 4,1 is extracted using c = C/(−n ∞ • C) and the carrier dual plane is found by π = (C ∧ n ∞ ) * .This can similarly be done for tangent vectors by using the origins of the transformed tangent vectors and the carrier lines.Point pairs can be used by either splitting them into the constituent points or by creating the carrier line.

Convexity
The convexity of motor estimation using non-linear optimization has not been thoroughly investigated in the context of geometric algebra.However, motor estimation is closely related to estimation and control in SE (3), where the analysis can be separated into convergence of translation, and convergence in rotation as given in SO (3).It is known that if deviations in SO(3) are described by the angle θ and plane of rotation B, or with associated metrics like 1 − η or 1 − η 2 where η = cos(φ/2), the convergence to zero deviation can be achieved for all initial conditions except for initial conditions where the solution can get stuck at |θ| = π, see [8,28].
The cost functions that we use in the present paper are based on the angle-plane description (2.2) for the deviation in rotation.This is even the case for the cost function in (3.5), which is seen by applying the angle-plane form of the rotor.The step in the Levenberg-Marquart for iteration k is calculated in the tangent space at M k based on the deviation in translation and the deviation in rotation, which is given in terms of the geodesic metric and the plane of rotation in the same tangent space.The update is done with the exponential map and results in a new motor M k+1 .In this optimization process the incremental convergence properties at each step will be similar to stability properties in the control problem on SE (3).This indicates that the optimization problem will be convex except in the case that the solution gets stuck at |θ| = π, which will not be encountered in practice.This agrees with the extensive numerical testing that was performed on the method proposed in this paper, with initial angular deviations up to θ0 = π.

Experimental Results
This section presents experimental results on motor estimation using sets of heterogeneous conformal objects.The experiments were implemented using our framework GAME [23] for multivector estimation, which was presented in [24].The GAME framework is written in C++ and Python, and is based on the Ceres [20] non-linear optimization framework of Google, and the Versor [4] geometric algebra library of Pablo Colapinto.
We employ the Levenberg-Marquardt (LM) [16] method, where the Jacobian matrices are computed using forward mode automatic differentiation based on dual numbers [3,13].Automatic differentiation computes derivatives with machine precision and works by exploiting the fact that all computer implementations of mathematical functions are composed of a set simple differentiable unary or binary operations.Derivatives of more complex functions are computed by applying the chain rule at each operation and bookkeeping the results [22].The Jacobian matrices used in this work are of size N × 6, where N is the number of residuals and 6 is the dimension of the tangent space of the motor manifold M at the motor M .
In the following, the ground truth set of homogeneous conformal objects {X i }, X ∈ R 4,1 , i ∈ {1, . . ., N} are transformed by the motor M to form the set {X i } where X i = MX i M .The motor M which is to be estimated is generated by a rotation of π/3 around the e 2 axis followed by a translation of 1 unit along the e 1 , e 2 and e 3 axes resulting in a motor M = T R with coefficient vector x M ≈ (0.87, 0, −0.5, 0, −0.68, −0.43, −0.18, −0.25). ( No noise was added in the experiments and the initial motor M 0 = 1 was used in all experiments, if not explicitly stated otherwise.The performance of the different cost functions were evaluated by comparing the coefficients of the estimated motor with the ground truth motor.Two motors are considered to be equal if their coefficient vectors are equal within some tolerance.Given the coefficient vectors a, b ∈ R 8 , coefficient i of b is considered to be equal to coefficient i of a if the following equation holds where α, β ∈ R. In the following experiments we consider two coefficient vectors to be equal if (4.2) holds for α = 1 × 10 −8 and β = 1 × 10 −5 .( The termination criterion of the LM solver is that the relative function tolerance F satisfies F ≤ 1 × 10 −6 , or that the relative parameter tolerance x satisfies x ≤ 1 × 10 −8 .

Motor Estimation from 3D Point Correspondences
In [24], we presented motor estimation from sets of noisy point correspondences.Using the LM method we were able to estimate the motor that transformed one set of conformal points to the other set using 1,00,000 observations with a computation time of 0.5040 s.
With no noise added, our method was able to estimate the correct motor from 10 points in 4 iterations within tolerances α = 1×10 −9 and β = 1×10 −5 .

Motor Estimation from 3D Line Correspondences
Motor estimation using 10 3D line correspondences with three different cost functions was performed.The results are shown in Fig. 2. The cost function in (3.8) with 6 residuals converged in 4 iterations and terminated due to the parameter tolerance x , while the cost function in (3.13) with 4 residuals per observation converged in 7 iterations, with termination due to F .A version of (3.13) using the norm of the common normal between the line correspondences and thus 2 residuals per observation performed slightly worse and converged in 15 iterations, with termination due to F .The optimization problems based on (3.13) converged quickly during the first iteration while it converged slowly close to the solution, that is, when the distance and angle

Motor Estimation from 3D Plane Correspondences
Motor estimation using 10 plane correspondences was performed and the results are shown in Fig. 3. Optimization with the cost function in (3.17) with 2 residuals per observation converged in 7 iterations and satisfied the condition in (4.2) with α = 1 × 10 −3 and β = 1 × 10 −5 .The LM solver terminated due to the function tolerance F .

Motor Estimation from Sets of Heterogeneous Conformal Objects
Motor estimation with the cost function in (3.18) using points, lines and planes gave the results shown in Fig. 4. Without noise, when all objects were transformed by the same motor, we were able to estimate the motor with α = 1 × 10 −5 in 4 iterations with an initial angular error of θ0 = π/3.In addition, our method was able to estimate the motor with the same precision with an initial angular error of θ0 = π, demonstrating the robustness of our proposed method.Using the same initial motors and with noise, the optimization converged in 13 and 19 iterations.The noise was applied to the sets {X i } by applying a motor with a small translation and rotation.Different noise motors were applied to different sets, that is, the point set were With noise, the initial convergence is comparable, but due to noise the optimization terminated after 13 and 19 iterations.
Comparison with the method of Valkenburg and Dorst [26] shows that our method converged with the same accuracy in the noise free case and with improved accuracy when there was noise transformed by another noise motor than the set of lines and planes.In these experiments, the weights w 1i = w 2j = w 3 k = 1, in (3.18), were used.Our method performs equally well as the method by Valkenburg and Dorst [26] when no noise is present.When noise is present our method performs slightly better.

Conclusion
In this paper we have presented a novel method for non-linear rigid body motion estimation using point, line and plane correspondences where the rigid body motions were represented using motors in the conformal model of geometric algebra.The presented method makes is possible to estimate motors using both homogeneous and mixed or heterogeneous sets of objects, that is, using observations of points, lines and planes all transformed by the same motor.In addition, these formulations can be used to estimate conformal motors from correspondences of more complex objects, e.g., spheres, circles and tangent vectors.As opposed to the method of Valkenburg and Dorst [26] our method is able to estimate the translational part of the motor using only lines and planes.This is due to the use of the common normal for line correspondences and the translation in the rotation plane for plane Vol.27 (2017) Motor Estimation using Heterogeneous 2047 correspondences.Moreover, our method had slightly better performance than the method of Valkenburg and Dorst in the presence of noise.Our method was very robust in the numerical experiments, and converged consistently with initial angular deviations up to π.The Levenberg-Marquardt nonlinear least-squares optimization method was used and Jacobian matrices were computed using automatic differentiation based on dual numbers.The presented method ensures that the solution in each iteration is on the motor manifold by computing the update step in the tangent space and computing the resulting motor using the exponential map.
Open Access.This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/ by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.

Figure 1 .
Figure 1.A line, a point and a sphere before and after the transformation by a motor

Figure 2 .Figure 3 .
Figure 2. Convergence of the cost function for motor estimation using 3D line correspondences with two different cost functions.The cost function in (3.8) with 6 residuals per observation converged in 4 iterations and terminated due to the parameter tolerance x , while the cost function in (3.13) with 4 residuals per observation converged in 7 iterations, with termination due to the function tolerance F

π 3 3 Figure 4 .
Figure 4. Convergence of the cost function for motor estimation using sets of heterogeneous conformal objectspoints, lines and planes.Without noise, the motor was estimated with α = 1 × 10 −5 in 4 iterations with an initial angular error of θ0 = π/3, and in 9 iteration with θ0 = π.With noise, the initial convergence is comparable, but due to noise the optimization terminated after 13 and 19 iterations.Comparison with the method of Valkenburg and Dorst[26] shows that our method converged with the same accuracy in the noise free case and with improved accuracy when there was noise