Estimating SE(3) elements using a dual based linear Kalman filter

Rangaprasad Arun Srivatsan, Gillian T. Rosen, D. Feroze Naina Mohamed and Howie Choset Robotics Institute, Carnegie Mellon University, 5000 Forbes Avenue, Pittsburgh, PA 15213. Email: (arangapr,gtr,fdheenmo)@andrew.cmu.edu, [email protected]

Abstract—Many applications in robotics such as registration, our knowledge, this is the first attempt to derive a linear update object tracking, sensor calibration, etc. use Kalman filters to model for estimating time invariant SE(3) elements using a estimate a time invariant SE(3) element by locally linearizing a Kalman filter. non-linear measurement model. Linearization-based filters tend to suffer from inaccurate estimates, and in some cases divergence, The linear measurement model comes at the cost of state in the presence of large initialization errors. In this work, we use a dependent measurement uncertainty. Measurement uncertainty dual quaternion to represent the SE(3) element and use multiple is typically state independent and can be obtained based on the measurements simultaneously to rewrite the measurement model physical characteristics of the sensor and/or the measurement in a truly linear form with state dependent measurement noise. process. However, in case of state dependence, there is an addi- Use of the linear measurement model bypasses the need for any linearization in prescribing the Kalman filter, resulting in tional burden of estimating the measurement uncertainty after accurate estimates while being less sensitive to initial estimation each state update. State dependent measurement uncertainties error. To show the broad applicability of this approach, we have been used in systems for satellite tracking [27] and robot derive linear measurement models for applications that use either navigation [28]. We use an approach similar to [27], [5] to position measurements or pose measurements. A procedure to formulate the expressions for the state dependent measurement estimate the state dependent measurement uncertainty is also discussed. The efficacy of the formulation is illustrated using uncertainties. It should be noted that the measurement uncer- simulations and hardware experiments for two applications in tainties have a linear dependence on their state vector, which robotics: rigid registration and sensor calibration. allows for derivation of exact expressions of uncertainties [14]. In this work we look at two broad classes of applications I.INTRODUCTION based on the type of measurements used to estimate SE(3) Estimation of SE(3) elements plays an important role in elements: 1) those that use position measurements such as many applications of robotics, ranging from registration [11] registration from medical imaging [21], object tracking using and manipulation [18] to sensor calibration [9] and object laser range scanners [32], etc. and 2) those that use pose (posi- tracking [31]. Probabilistic estimation techniques such as tion and orientation) measurements such as sensor calibration Kalman filters have been a popular choice for estimation of using inertial measurement units [19], hand-eye calibration SE(3) elements due to their ability to adapt to noisy sensor using stereo vision [12], etc . The linear measurement models measurements. While the process models used for estimation and state-dependent uncertainties are derived for both of these of SE(3) elements are linear, the update models are non- cases and the results are compared with non-linear filtering linear and the estimates provided by linear filters or their variants. We evaluate the formulation through simulations variants are often inaccurate and even diverge in cases where and experiments for two applications: registration and sensor the initial estimation error is high. In this work, we derive calibration. We show that our dual quaternion-based linear a linear update model for estimation of SE(3) using a dual filtering (DQF) produces more accurate and fast estimates even quaternion representation. in the presence of high initial errors. Dual provide a means to combine both rotations and translations while retaining the advantages of using quater- II.BACKGROUND nions for representing rotations [6]. While dual-quaternions have been used with iterated extended Kalman filter (IEKF) Estimation of SE(3) elements has been of interest for a to estimate SE(3) elements, the update model was non- long time in robotics literature. Horn et al [13] and Besl linear [10]. Non-linear update models can be highly sensi- et al [3] developed methods for least squares estimation of tive to initial estimation errors, and can be computationally SE(3) elements for point registration. Park et al [23] and expensive. As a result in this work, we focus on deriving a Chen et al [4] developed optimization based methods for esti- linear update model to estimate SE(3) elements. Chaukron mating SE(3) elements in sensor calibration problems. In the et al [5] come closest to our work in terms of formulating presence of noisy measurements, deterministic optimization a linear update model, but they only estimate the SO(3) methods have been observed to perform poorly [24]. However, element. In this work, we start with a non-linear update model, probabilistic estimation techniques such as Kalman filters and using multiple sensor measurements simultaneously, we are effective at handling noisy measurements and producing rearrange the update model into a linear form. To the best of accurate estimates of the state and associated uncertainty [15]. Several researchers have noted that filters used for esti- where is the quaternion multiplication operator and [v]× mating SE(3) elements have non-linear update models [11], is the skew-symmetric matrix formed from the vector v. and hence linear Kalman filters produce poor estimates of Given a quaternion q, its conjugate q∗ can be written as: ∗ e e SE(3) elements. Several variants of the Kalman filter have qe = (q0, −q1, −q2, −q3). If the scalar part of a quaternion been introduced to handle this non-linearity. The extended is 0,

Kalman filter (EKF) and unscented Kalman filter (UKF) have ∗ ∗ been used to estimate SE(3) elements for satellite orienta- qe = −qe . (3) tion [27], manipulation [18], registration [24, 21] and sensor ∗ The conjugate has the following property: vec qe qe = 0. calibration [9]. EKF based filters perform first-order linear p ∗ The norm of a quaternion is |q| = scalar(q q ) and a unit approximations of the non-linear update models and produce e e e quaternion is one with |q| = 1. Unit quaternions can be used estimates which are known to diverge in the presence of e to represent rotation about an axis (denoted by the unit vector high initial estimation errors [5]. UKF based methods do not k) by an angle θ ∈ [−π, π] as follows linearize the models but instead require evaluation at multiple specially chosen points (called sigma points), which can be θ  θ  q = cos + k sin . (4) expensive for a high-dimensional system such as SE(3). e 2 2 In addition UKF based methods require tuning of multiple parameters, which is not intuitive. B. Dual quaternions Prior work also has looked at several parameterizations of A dual quaternion xˆ is an 8-tuple SE(3) that would improve the performance of the filters. (p0, p1, p2, p3, q0, q1, q2, q3), which can be written in In [11] the state variables are confined over a known Rie- the form: xˆ = pe + qe, where pe = (p0, p1, p2, p3), mannian manifold and a UKF is used to estimate the SE(3) qe = (q0, q1, q2, q3), and  is an element having the following element. Lie algebra elements were used with an iterated property:  6= 0 and 2 = 0.  is a mathematical construct extended Kalman filter (IEKF) in [29]. Both these methods with a defined property and is not to be confused as having involve highly non-linear update models with trigonometric a small value close to 0. pe is called the real part and qe is terms in them. Quaternions are used to parametrize the rotation called the dual part of the dual quaternion. component of SE(3) and an EKF is used to estimate the state Multiplication of two dual quaternions xˆ1 = pe1 + qe1 and in [20, 2]. Quaternions are used with a UKF in [17]. Quater- xˆ2 = pe2 + qe2 is given as nion representation-based filters usually involve a quadratic update model. Dual quaternions with an IEKF has been used xˆ1 ⊗ xˆ2 = pe1 pe2 +  (pe1 qe2 + qe1 pe2) , (5) in [10]. where ⊗ is the dual quaternion multiplication operator. In this work, we use dual-quaternions to represent the Dual quaternions have three conjugates: 1) First conjugate: SE(3) element; using multiple simultaneous measurements, 1∗ 2∗ ∗ ∗ xˆ = p − q, 2) Second conjugate: xˆ = p + q , and we derive a linear update model which can be used with a e e 3∗ ∗ ∗ e e 3) Third conjugate: xˆ = p − q . A dual quaternion Kalman filter without the need for linearization. 2∗ e e is called “unit” if xˆ ⊗ xˆ = 1. An important property III.DUAL QUATERNIONS of the third conjugate that will be used in this work is, (xˆ ⊗ xˆ )3∗ = xˆ3∗ ⊗ xˆ3∗. There are many representations for SE(3) elements such 1 2 2 1 A dual quaternion used to represent a vector a ∈ R3 has as Euler angles, quaternions, axis angles, etc. for rotation the following form and Cartesian coordinates for translation. Dual quaternions compactly represent both translation and rotation, and with the aˆ = 1 +  (ae) , where ae = 0 + a. (6) methods presented in this paper, give rise to a linear update model. A detailed discussion on dual quaternions can be found A dual quaternion that is used to represent an SE(3) element in [16]. For the rest of this work, vectors will be described has the following form in bold font, quaternions will be represented by a (e) over a q q bold font and dual quaternion by a (ˆ) over a bold font. xˆ = q +  et er , (7) er 2 A. Quaternions where qer is the rotation quaternion whose form is as shown q = 0 + t A quaternion qe is a 4-tuple (q0, q1, q2, q3), where q0 is the in Eq. 4 and et is the quaternion representation of the T SE(3) t ∈ R3 scalar part and q = (q1, q2, q3) = vec (qe) is the vector part translational component of the element, . For of the quaternion. A 3-vector can be denoted by a quaternion the sake of simplicity, we rewrite Eq. 7 as with a 0 scalar part. xˆ = q + q , where (8) Multiplication of two quaternions p and q is given by er ed e e q q q = et er . (9) pe qe = p0q0 − p · q + q0p + p0q + p × q, (1) ed 2  T   T  p0 −p q0 −q It is important to note that xˆ is a unit dual quaternion since = × qe = × pe (2) p p + p0I3 q −q + q0I3 its dual-product with the second conjugate is unity. Let point a ∈ R3 be obtained by transforming point Subtracting Eq. 16 from Eq. 15, we obtain b ∈ R3 using a dual quaternion xˆ. The transformation can    be mathematically described as ( (ae1 − ae2)) ⊗ (qer − qed) = (qer + qed) ⊗  eb1 − eb2    3∗ aˆ = xˆ ⊗ bˆ ⊗ xˆ , (10) ⇒  ((ae1 − ae2) qer) =  qer eb1 − eb2   ˜ where aˆ and bˆ are obtained using Eq. 6. ⇒ (ae1 − ae2) qer − qer eb1 − eb2 = 0. (17) Note that Eq. 17 does not have q and contains only the rota- Lemma III.1. For a unit dual quaternion, xˆ = qer + qed, ed the product of third and first conjugate equals unity: tion quaternion. Using the quaternion multiplication described xˆ3∗ ⊗ xˆ1∗ = 1. in Eq. 1, Eq. 17 can be rewritten in the following form Proof: ˜ Hqer = 0, where (18) 3∗ 1∗ ∗ ∗  T  xˆ ⊗ xˆ = (qr − qd) ⊗ (qr − qd) 0 −(a1 − a2 − b1 + b2) e e e e H = × . ∗ ∗ ∗  (a1 − a2 − b1 + b2)(a1 + a2 + b1 + b2) = qer qer −  qer qed + qed qer , from Eq. 1 4×4  ∗ ∗  (19) ∗ qet qer qer qet = 1 −  qer + qer . (11) 2 2 The rotation quaternion qer lies in the null space of H. In order to estimate qr we use a Kalman filter whose state vector is Using the property that q is a unit quaternion and q∗ = −q e er et et qr. For this filter, the pseudo measurement model is from Eq. 3. Eq. 11 can be further simplified as xˆ3∗ ⊗ xˆ1∗ = 1. e h = Hqer. (20) IV. MATHEMATICAL MODELING We enforce the pseudo measurement model h = 0 ∈ R4. Most applications that estimate a time invariant SE(3) The measurement in Eq. 20 is called “pseudo-measurement” element can be broadly divided into two cases: Case I, ones because the h does not represent a true measurement. The that use position measurements and Case II, that use pose pseudo-measurement is dependent on qer, aei and ebi all of measurements for updating the state. The measurement model which have associated uncertainties. In section IV-C1, we for both these cases are non-linear and algebraically very discuss the procedure to compute the uncertainty in the different. Dual quaternions provide us the means to rewrite the pseudo-measurement. Subsequently in section V, we describe measurement models for both these cases in a linear form. The the equations of the Kalman filter that estimates qer using the rest of the section deals with the derivation of measurement linear measurement model. models for the two cases and the corresponding uncertainties. After estimating qer using a Kalman filter, we need to estimate q . Adding the Eq. 15 and Eq. 16 we have A. Measurement model for Case I et (2 + (ae1 + ae2)) ⊗ (qer − qed) = Systems that use position-measurements for model update   have the following general form (qer + qed) ⊗ 2 + (eb1 + eb2) ,

a = Rb + t, (12) ⇒2qet qer = (ae1 + ae2) qer − qer (eb1 + eb2), where a is the sensor measurement, R ∈ SO(3) is the rotation ae1 + ae2 eb1 + eb2 ∗ ⇒qet = − qer qer. (21) matrix, b ∈ R3 is the point to be transformed and t ∈ R3 is the 2 2 translation vector. In an application such as rigid registration Thus, Eq. 21 determines qet directly using the estimated value of images, a is the sensed location of points and b is the of qer without the need for a Kalman filter. This is a helpful corresponding point on the CAD model of the object. Eq. 12 byproduct of using multiple measurements simultaneously in can be rewritten using dual-quaternions from Eq. 10 as shown Eq. 15. Since the scalar part of qet is 0 and vector part is t, we can rewrite Eq. 21 in the following vector form aˆ = xˆ ⊗ bˆ ⊗ xˆ3∗, (13) a + a b + b  t = 1 2 − R 1 2 , (22) where xˆ is as defined in Eq. 8. Applying Lemma III.1, Eq. 13 2 qer 2 can be rewritten as where R is the rotation matrix formed using the quaternion qer aˆ ⊗ xˆ1∗ = xˆ ⊗ bˆ. (14) qer. Section IV-C2 describes the uncertainty associated with t. B. Measurement model for Case II Let us consider the case of a pair of measurements ai, i = 1, 2. From Eq. 14, we have Systems that use pose-measurements for model update typically have the following general form [23] aˆ ⊗ xˆ1∗ = xˆ ⊗ bˆ , i i AX − XB = 0, (23) ⇒(1 + a1) ⊗ (q − q ) = (q + q ) ⊗ (1 + eb1), (15) e er ed er ed where A, X, B ∈ SE(3). A and B are pose-measurements (1 + ae2) ⊗ (qer − qed) = (qer + qed) ⊗ (1 + eb2) (16) and X is the desired transformation to be estimated. A Kalman filter used to estimate X such as Unlike the case discussed in section IV-A, t cannot always in [9], would have a pseudo-measurement of the form, be directly obtained from the estimated qer. This is because h3×3 = AX − XB. One again we enforce the pseudo- estimation of t would require inversion of Ht, which need measurement model h3×3 = 0. A UKF with a state matrix not always be invertible as it is formed from arbitrary sensor instead of state vector can directly handle measurement measurements. As shown in section V, a linear Kalman filter models in matrix forms [11]. The pseudo-measurements can is employed with the following pseudo-measurement model to also be converted to a vector form as shown in [9] and then estimate t, estimated using a UKF. Using dual quaternions we rewrite h = H t + σ . (35) Eq. 23 in an alternate form, which would ultimately result in t t e3 a linear pseudo-measurement, thus allowing us to use a linear We enforce the pseudo-measurement model ht = 0. The Kalman filter for state estimation. uncertainty associated with ht is derived in section IV-C3. Let aˆ, xˆ, bˆ be the dual quaternions corresponding to C. Uncertainty in pseudo-measurements A, X, B respectively. Eq. 23 can be rewritten as In order to estimate the uncertainties associated with the ˆ ˆ aˆ ⊗ xˆ − xˆ ⊗ b = 0. (24) pseudo-measurements as well as the translational vector de- Using Eq. 8, Eq. 24 can be written as scribed in the previous sections, we make use of an important ˆ result from stochastic theory [14, pp. 90–91], [5, Appendix A] 0 =(aer + aed) ⊗ (qer + qed) − (qer + qed) ⊗ (ebr + ebd), described in Proposition 1.   = aer qer − qer ebr + Proposition 1. Let us consider b ∈ Rm and c ∈ Rn which are   sequences with zero mean. Let h ∈ Rn, x ∈ Rl and a linear  ad q + ar q − q ebr − q ebd . (25) e er e ed ed er matrix function G(·): Rl → Rn×m, such that h = G(x)b+c. Hence we have Assume that x, b and c are independent. Then Σh is given by 0e = aer qer − qer ebr (26) Σh = G(x)ΣbGT (x) + N(Σb Σx)N T + Σc, (36) 0e = aed qer + aer qed − qed ebr − qer ebd. (27) ~ {·} Eq. 26 has a form very similar to Eq. 17, with the only where ~ is the Kronecker product, Σ is the uncertainty associated with {·} and N ∈ Rn×lm is defined as follows difference being that the scalar parts of aer, ebr are not 0. If ar = a0 + ar and ebr = b0 + br, using Eq. 1 we rewrite e N , [G1 G2 ··· Gm]. Eq. 26 as n×m Gi ∈ R is obtained from the following identity: Hrqer = 0, where (28) a − b −(a − b )T  Gix = G(x)ei, H = 0 0 r r . (29) r a − b (a + b )× + (a − b ) I Rm r r r r 0 0 3 4×4 where ei is the unit vector in with 1 at position i and 0 The pseudo-measurement is everywhere else. 1) Uncertainty in pseudo-measurement for estimating the hr = Hrqer, (30) rotation in Case I: To find the uncertainty in the linear pseudo- measurement, we rewrite h from Eq. 20 in the following form and the pseudo-measurement model is hr = 0. The uncer- tainty associated with hr is derived in section IV-C3. h = H(p1, p2, q1, q2)qer, Similar to section IV-A, we use the estimated value of q er = G(q )v , where v = (pT , pT , qT , qT )T to estimate t. Using Eq. 9, Eq. 27 can be rewritten as er true true 1 2 1 2   = G1 −G1 G2 −G2 vtrue. (37) 0e = ar qt qr − qt qr ebr + σ1, (31) e e e e e e  T   T  −qr qr In Eq. 37, G1 = and G2 = , where σ1 = 2ad qr − 2qr ebd. Multiplying both sides of × 0 × 0 e ∗e e e −qr + q I3 −qr − q I3 Eq. 31 with q , we obtain: 0 er where qer = q + qr are obtained from Eq. 1. Eq. 37 is ∗ ∗ the pseudo-measurement for a noise-free sensor measurement 0e = aer qet − qet qer ebr qer + σe1 qer, vtrue. If v is the sensor measurement with noise δv, then = aer qet − qet σe2 + σe3, (32) ∗ ∗ δv , v − vtrue. (38) where σe2 = qer ebr qer and σe3 = σe1 qer. The structure of Eq. 32 is similar to Eq. 26, with the only differences being Solving for vtrue from Eq. 38 and substituting in Eq. 37 yields: the addition of σe3 term and the scalar part of qet is 0. If 0 h(qer) = G(v − δv) σe2 = σ2 + σ2, using Eq. 1 we rewrite Eq. 32 as = Gv + ν1, (39) 0 = Htt + σe3, where (33) where ν = −G (q ) δv is a zero mean noise. From Eq. 39,  −(a − σ )T  1 er H = r 2 . (34) the uncertainty in the pseudo measurement Σh can be obtained t (a + σ )× + a − σ0 I r 2 0 2 3 4×3 using Eq. 36. 2) Uncertainty in translation for Case I: The expression with a large initial uncertainty. If the uncertainty in rotation for t assuming perfect measurements pi and qi is given in is known in terms of some other parametrizations such as Eq. 22. In the presence of noise in the measurements, similar Euler angles, then the uncertainty is propagated to the space to the derivation of Eq. 38, we obtain from Eq. 21 of quaternions using a Jacobian mapping as shown in [22].   Since the SE(3) element to be estimated is time-invariant, p1 + p2 qe1 + qe2 ∗ t = − vec qer qer + ν2, where, the process model is static, i.e., xk|k−1 = xk−1|k−1. Upon ob- 2 2 ˆ   taining pi and qi (i = 1, 2) for Case I, or aˆ, b for Case II, we δp1 + δp2 δq1 + δq2 ∗ ν = − + vec q e e q . (40) formulate the pseudo-measurements h(xk|k−1) = Hkxk|k−1. 2 2 er 2 er The observation matrix Hk is given by Eq. 19 for Case I t and by Eq. 29 for Case II. The measurement uncertainty Σh From Eq. 40, ν2 is a zero mean noise with variance Σ ∈ k R3×3, is then calculated as shown in section IV-C1 for Case I and section IV-C3 for Case II. Σp1 + Σp2 Σt = + Στ , (41) The state is updated using standard equations of the Kalman 4 filter [15]   where τ = vec q δqe1+δqe2 q∗ , δq = 0 + δq . Στ is  er 2 er ei i xk|k = xk|k−1 − Kk Hkxk|k−1 , (44) computed using Eq. 36 as shown below x x Σk|k = (I − KkH)Σk|k−1, (45)  δq + δq  e1 e2 ∗  −1 τ =vec qer qer x T x T h 2 where Kk = Σk|k−1H HΣk|k−1H + Σk . It has already been discussed that q is a unit quaternion; =vec (qer (0 + σ)) = −G2σ er   which implies that the state vector has to be a unit vector. 1   δqe1  × 0  This requirement is not enforced by the equations of the where σ˜ = 2 G3 G3 and G3 = qr + q I3 is δqe2 Kalman filter directly. However, there are three methods Σσ, Στ obtained from Eq. 1. Eq. 36 is then used to find . to enforce unit-normalization of state vector (1) including 3) Uncertainty in pseudo-measurement models for Case the constraint as an additional pseudo-measurement [2], (2) II: For pose based measurements, there are two pseudo- reducing the dimension of the state vector by substituting measurements corresponding to estimation to qR and t. Eq. 30 p 2 2 2 e q0 = 1 − q1 − q2 − q3 [27], (3) normalizing the state vector and Eq. 35 are rewritten in the following form at the end of each update step [5]. The first two methods result

hr = Grutrue, (42) in non-linear measurement models, which defeats our purpose of developing equations for a truly linear filter. As a result we h = G w + σ , (43) t t true 3 resort to the third method of normalizing the state vector after  T T  q0 −qr −q0 qr every update and suitably scaling the uncertainty, where Gr = × × , qr q0I3 − qr qr −q0I3 − qr Σx  T T  ∗ xk|k x∗ k|k T T T 0 −t 0 t xk|k = , Σk|k = 2 . (46) utrue = (a0, a , b0, b ) , Gt = and x r r t −t× t −t× k|k xk|k T 0 T T wtrue = (a0, ar , σ2, σ2 ) . Such an approach has been shown to estimate efficiently in Eq. 42 and Eq. 43 are the pseudo-measurements for [20] and [10]. noise free sensor measurements utrue, wtrue. If u and w x Upon estimating xk|k and Σk|k, Eq. 22 and Eq. 41 are used are the sensor measurements with noise δu and δw respec- t to estimate tk|k and Σk|k, for Case I. For Case II, we initiate tively, then hr = Gru + ν3, ht = Gtw + σ3 + ν4, where another Kalman filter whose process model is static as in the ν3 = −Grδu and ν4 = −Gtδw − δσ3 are zero mean noise case of q . The measurement model is also linear as in the h h er with covariance Σ r and Σ t respectively, which can be case of qer. The equation for the measurement model is as obtained using Eq. 36. shown in Eq. 35. The observation matrix is evaluated at the V. KALMAN FILTER EQUATIONS estimated value of qer.

As shown in Eq. 17 and in Eq. 25, qer and t can be estimated VI.RESULTS in a decoupled manner. In this work, we formulate a Kalman We apply the filtering method developed in the earlier sec- filter that first estimates qr and then uses the estimated qr tions to two examples: rigid registration and sensor calibration e t e to estimate t. For Case I, t and Σ can be directly estimated representing Case I and Case II respectively. Simulation as qer from Eq. 22 and Eq. 41 upon estimating qer and Σ . However well as experimental results are provided in the following for Case II, a Kalman filter is used to estimate the mean and sections. uncertainty in t. The state vector of the Kalman filter that is used to estimate A. Rigid registration R4 qer is xk = qer, xk ∈ .. The state vector is initialized with The rigid-registration problem can be defined as finding the a suitable guess for mean and uncertainty. In the absence of a SE(3) element that aligns points in one reference frame to the T good initial guess, the state is initialized to x0 = (1, 0, 0, 0) points in another reference frame. Usually points in one frame are computed from a CAD model of the object and points in by locally perturbing the initial guess by translation sampled the other frame are estimated from images, position sensors, uniformly from [−15 15]mm along each axis and a rotation laser range scanners, etc. sampled uniformly from [−30 30]deg along each axis. Since Iterative closest point (ICP) is one of the most popular the problem has several local minima, using multiple initial methods to perform rigid-registration [3]. A of variants guesses improves the chances of finding the global minimum. to the ICP have been introduced [25, 30]. ICP and most of its variants are batch processing tools; i.e., one needs to wait CAD model Initial position Estimated position for all the measurements to be collected before estimating the 80 transformation. Also in the presence of noisy data, ICP and 60 most of its variants have been observed to perform poorly [24]. (a) 40 As a result, online estimation techniques have been developed 20 to account for noise in the measurements [24, 21, 11]. (mm) axis Z 0 In this work, we apply DQF method to register 100 points 40 40 20 randomly sampled from the surface of a “Stanford bunny”, to 0 0 -20 -40 its CAD model. We first assume that the point correspondence Y axis (mm) X axis (mm) is known and estimate the registration with DQF, whose actual registration parameters are all 0s. We sample 1000 initial registration estimates uniformly drawn from large initial errors 27 DQF EKF UKF in position, for x, y, z ∈ [−10000, 10000]mm and orientation 24 21 θx, θy, θz ∈ [−180, 180]deg. From Fig. 1, we observe that 18 DQF correctly estimates the registration for all the initial (b) 15 12 estimates. Following this, we perform two more experiments 9

with noise added to the sampled points. The noise is uniformly mm) (in error RMS 6 sampled from [−2, 2]mm along each axis, in one case and 3 0 [−3, 3]mm in the other. Fig. 1 shows that the RMS error for 0 20 40 60 80 100 all the estimates is only due to the noise in the measurements Number of points and its magnitude matches well with the noise added to the Fig. 2. (a) Initial position and DQF estimated position of 100 points are points. Thus, when the point correspondences are known, shown against the CAD model of the “Stanford bunny”. DQF accurately DQF accurately estimates the registration parameters even in registers the points to the CAD model. (b) A plot of the RMS error wrt number of points for DQF, EKF and UKF. DQF and EKF converge quickly, presence of very high errors in the initial estimate. while UKF takes a while to converge. DQF however converges to lower RMS error, with computation time an order of magnitude lower than EKF and UKF as shown in Table I.

Fig. 2(a) shows the CAD model of the Stanford bunny in green. The blue diamond markers show the initial guess for the location of the points and the red circular markers show the DQF estimated location of the points. Fig. 2(b) shows

RMS error (in mm) the RMS error versus number of points used to estimate the parameters. The RMS error decreases with the usage of more Estimate index point measurements. DQF and EKF converge to a smaller Fig. 1. RMS error upon estimating registration parameters with DQF for RMS error at about 10 points, while the UKF takes many 1000 runs with different initial estimates, when the point correspondence is more points to converge. First four rows of Table I show the known. Three experiments were carried out: 1) noise uniformly sampled from actual registration parameters and the estimated registration [-1,1], 2) noise uniformly sampled from [-2,2] and 3) no noise was added. DQF accurately estimates he registration parameters in all cases parameters. The right column of Table I shows the time taken by the filters to update for 100 point measurements. DQF Following this observation, we now perform registration converges an order of magnitude faster than EKF and UKF in a more realistic scenario where point correspondence is and also has the lowest RMS error. unknown. Point correspondence to the CAD model is found Fig. 3 shows the results for the case where the sampled using a closest point rule as in [3, 24, 21]. We repeat the points are corrupted with a noise uniformly sampled from exercise by adding noise to the points and then estimating [−2 2]mm along each axis. DQF accurately registers the points the transformation. In both the cases, we compare the dual to the CAD model as shown in the last three rows of Table I. quaternion based filtering to an EKF based estimator [24] and DQF once again performs better than EKF and UKF, and takes 1 a UKF based estimator [21]. We choose an initial guess of lower computational time . zero rotation and zero translation and an initial covariance of 1 qer t The computational time taken is calculated for a code running on Σ0 = 5I4 for rotation and Σ0 = 100I3 for translation. DQF, MATLAB R2015a software from MathWorks, running on a ThinkPad T450s EKF and UKF are implemented with 40 initial starts obtained computer with 8 GB RAM. TABLE I REGISTRATION RESULTS FOR STANFORD BUNNY

No noise x y z θx θy θz RMS Time (mm) (mm) (mm) (deg) (deg) (deg) (mm) (s) Actual 22 -23 20 15 -10 -10 – – DQF 22.54 -21.52 20.03 17.28 -9.94 -10.15 1.12 23.44 EKF 22.35 -26.39 21.11 11.43 -11.44 -14.76 3.88 155.02 UKF 21.36 -23.89 18.94 16.39 -5.95 -10.55 2.47 247.56 With noise DQF 22.34 -24.22 18.79 13.37 -9.09 -10.18 2.70 47.05 EKF 20.29 -26.09 20.69 8.76 -12.79 -8.08 3.81 324.23 UKF 20.08 -24.78 14.6 11.90 -6.08 -8.04 4.80 510.73

CAD model# Initial position Estimated position

80 60 (a) 40 20 Fig. 4. (a) The setup shows a da Vinci robot with an EM tracker rigidly

Z axis (mm) axis Z 0 attached to the tool. The reference frame for the EM tracker is shown in red. The reference frame for the robot is located at its remote center of motion 40 40 (RCM), shown in yellow. The pose of the tip of the robot, Ai is shown in blue 20 0 0 and the pose of the sensor, Bi is shown in green. X is the transformation -20 -40 between the tip of the robot and the EM tracker. (b) The robot is shown at Y axis (mm) X axis (mm) two time instances i and j. Aij is obtained from kinematics and Bij is obtained from the EM tracker measurements. The unknown to be solved for is X, which can be posed in the form: A X = XB . 24 DQF EKF UKF ij ij 30 DQF EKF UKF 22 20 18 (b) 2016 analytically from a pair of measurements: A12X = XB12 14 and A23X = XB23 [23, 4]. But sensors are seldom noise- 12 10 free, and hence several optimization based approaches exist 8 to solve this problem [13, 26], whose solution drives many 6 RMS error (in mm) (in error RMS 4 applications [7, 1, 8, 12]. Recently Faion et al [9] developed a 0 RMS error (in mm) 2 filtering based solution to this problem, which could perform 0 online estimation using a UKF to estimate the pose which 020 20 40 40 60 60 8080100 100 NumberNumber ofof points points is parameterized using axis-angle and Cartesian parameters. We compare our DQF to this UKF based estimation. We Fig. 3. (a) Initial position and estimated position of 100 points with added also develop an EKF based estimation using the measurement noise are shown against the CAD model of the “Stanford bunny”. DQF model described in [9] for a second comparison. estimates the registration parameters accurately even in the presence of noise. (b) A plot of the RMS error wrt number of points for DQF, EKF and UKF. DQF and EKF converge quickly, but UKF takes a while to converge. Overall, C. Simulation all the three filters converge closely to one another, with the DQF performing marginally better. The DQF converges with computation time an order of We first tested our algorithm with simulated data and then magnitude lower than the other two as shown in Table I. with data collected from real experiments. For the simulated case, we first generate 500 random poses for the tool tip, Ai (i = 1,..., 500). We then choose a ground truth SE(3) B. Sensor calibration element X to generate the corresponding poses for the EM The sensor calibration problem is as follows: given the tracker Bi. We initialize the filters to zero translation and qer pose of two bodies Ai and Bi, defined with respect to zero rotation with an initial covariance of Σ0 = 5I4 for t two different inertial frames: {1} and {2}, we would like to rotation and Σ0 = 100I3 for translation. We assume that estimate the between the two bodies, by the correspondence between the sensed poses is known. Such tracking Ai and Bi, where the index i denotes an instance an assumption is reasonable as the sensor measurements can of time. Fig. 4(a) shows the various frames described above. be easily time-synchronized. If this synchronization is not This problem can be described as AijX = XBij, where possible, correlation between the sensor measurements can be −1 −1 Aij = Ai Aj and Bij = Bi Bj. X is the rigid transfor- obtained as shown in [19]. mation between the two bodies which needs to be estimated The SE(3) elements estimated by DQF, EKF and UKF as shown in Fig. 4(b). are shown in Table II along with computation time for each If the measurements are noise-free, then X can be obtained algorithm and the error in position and orientation parameters. TABLE II DQF provides faster estimates and is more accurate, especially SENSOR CALIBRATION RESULTS in the translation estimation, compared to UKF and EKF. Following this, we perturb Bi that is computed from the Simulation: No noise in sensor measurements ground truth X. The pose is perturbed by a translation x y z θx θy θz Time uniformly sampled from the interval [−2 2]mm along each (mm) (mm) (mm) (deg) (deg) (deg) (s) axis and a rotation uniformly sampled from [−10 10]deg along Actual 5.73 8.59 11.46 10.00 -16.00 35.00 – each axis. The estimated parameters are shown in Table II. DQF 5.73 8.59 11.46 9.96 -15.95 34.91 0.25 EKF 3.38 1.82 5.25 10.09 -15.93 35.05 1.20 Once again DQF estimated faster and is more accurate than UKF 3.56 10.59 10.81 9.98 -15.98 35.05 3.11 UKF and EKF. Simulation: With noise in sensor measurements DQF 5.59 8.22 11.38 9.95 -15.95 34.81 0.24 D. Experimental validation EKF -3.48 4.22 8.36 10.14 -16.01 35.01 1.15 UKF 5.89 10.44 10.01 10.83 -16.81 34.81 3.20 In order to test our formulation with real data, we use an Robot experiments experimental setup as shown in Fig. 4(a), which consists of a Actual -4 -20 45 105 88 109 – R da Vinci surgical robot (Intuitive Surgical Inc., Mountain DQF -4.10 -17.50 -45.10 105.55 87.88 108.69 0.27 View, CA) and an electromagnetic (EM) tracking sensor EKF -3.60 -22.00 -45.10 105.97 86.15 107.08 1.39 (trakSTARTM from Ascension Technologies, Burlington,VT). UKF -4.40 -14.1 -47.50 132.11 87.05 135.01 3.70 The tracker is rigidly attached to a known point on the tool of the robot. The robot is then telemanipulated and the position and orientation of the tip of the robot is measured from the observe that it takes around 0.08s for the DQF estimate to kinematics. The position and orientation of the EM tracker converge which is roughly 5 times faster than EKF and 15 with respect to the inertial frame attached to the magnetic times faster than UKF, while being more accurate than both field generator is also simultaneously recorded. We then use EKF and UKF. Since the estimation is close to real time, DQF, EKF and UKF to estimate the transformation between we implement this algorithm in an online manner to estimate the frames of the tip of the robot and the frame of the EM SE(3) elements as needed. tracker. Last three rows of Table II shows the parameters as VII.CONCLUSION estimated by DQF, EKF and UKF. In this work, we have developed linear measurement models 1 to be used with Kalman filters for the estimation of SE(3) elements. This was possible due to our choice of using dual 0.5 quaternions to represent SE(3) elements and combining mul- (a) q q q q 0 1 2 3 tiple sensor measurements simultaneously. All the information 0 contained in the non-linear update model was encoded in the

Estimated values Estimated linear measurement model and its corresponding uncertainty, -0.5 0 50 100 150 200 250 300 350 400 450 500 550 600 which happens to be state dependent in this case. Since the Number of measurements dependence on the state was found to be linear, results from 0 stochastic theory were used to determine the exact expressions for the uncertainty. We show that the new linear measure- -20 ment model allows for decoupled estimation of rotation and

(b) -40 translation using independent Kalman filters. The decoupled estimation potentially has the advantage of running in-parallel -60 and accelerating the estimation process. t t t Estimated values (in mm) (in values Estimated x y z We have shown through simulations and experiments that -80 0 50 100 150 200 250 300 350 400 450 500 550 600 the dual quaternion-based linear filtering is capable of es- Number of measurements timating the SE(3) more accurately with less computation time compared to state-of-the-art filtering methods for SE(3) Fig. 5. (a) The plot shows the estimated value of the quaternion that estimation. These characteristics of the dual quaternion-based represents the rotation. The values converge at around 100 measurements. (b) The plot shows the estimated value of the translation vector. The values filter, make it an candidate to be used in applications converge at around 200 measurements. that require real-time estimation of SE(3) elements such as sensor calibration, localization and manipulation. Adapting the Fig. 5 shows the values of the quaternion and the translation method for applications that involve estimation of time varying vector as estimated by the dual quaternion filter. The estimated SE(3) elements, whose process model may not be linear, will values converge at around 100 measurements for rotation and be a subject of future work. 200 measurements for translation. Since the rotation estimation does not depend upon translation estimation, we can stop ACKNOWLEDGMENTS running the filter that estimates rotation after convergence and This work has been funded through the National Robotics continue to run the translation filter until convergence. We Initiative by NSF grant IIS-1426655. REFERENCES theory. Courier Corporation, 2007. [15] Rudolph Emil Kalman. A new approach to linear filtering [1] Martin Kendal Ackerman, Andrew Cheng, Bernard Shiff- and prediction problems. Journal of Fluids Engineering, man, Emad Boctor, and Gregory Chirikjian. Sensor 82(1):35–45, 1960. calibration with unknown correspondence: Solving AX= [16] Ben Kenwright. A beginners guide to dual-quaternions: XB using Euclidean-group invariants. In International what they are, how they work, and how to use them for Conference on Intelligent Robots and Systems (IROS), 3D character hierarchies.Vaclav´ Skala-UNION Agency, pages 1308–1313. IEEE, 2013. 2012. [2] IY Bar-Itzhack, J Deutschmann, and FL Markley. Quater- [17] Joseph J LaViola Jr. A comparison of unscented and nion normalization in additive EKF for spacecraft attitude extended Kalman filtering for estimating quaternion mo- determination. page 403, 1991. tion. In Proceedings of the American Control Conference, [3] Paul J Besl and Neil D McKay. Method for registration volume 3, pages 2435–2440. IEEE, 2003. of 3-D shapes. In Robotics-DL tentative, pages 586–606. [18] Tine Lefebvre, Herman Bruyninckx, and Joris De Schut- International Society for Optics and Photonics, 1992. ter. Nonlinear Kalman filtering for force-controlled robot [4] Homer H Chen. A screw motion approach to uniqueness tasks. Springer-Verlag Berlin, 2004. analysis of head-eye geometry. In Proceedings of the [19] Elmar Mair, Michael Fleps, Michael Suppa, and Darius IEEE Computer Society Conference on Computer Vision Burschka. Spatio-temporal initialization for IMU to cam- and Pattern Recognition (CVPR), pages 145–151. IEEE, era registration. In 2011 IEEE International Conference 1991. on Robotics and Biomimetics (ROBIO), pages 557–564. [5] D. Choukroun, I.Y. Bar-Itzhack, and Y. Oshman. Novel IEEE, 2011. quaternion Kalman filter. IEEE Transactions on [20] Joao˜ Lu´ıs Marins, Xiaoping Yun, Eric R Bachmann, Aerospace and Electronic Systems, 42(1):174–190, Jan Robert B McGhee, and Michael J Zyda. An extended 2006. ISSN 0018-9251. doi: 10.1109/TAES.2006. Kalman filter for quaternion-based orientation estimation 1603413. using MARG sensors. In International Conference on [6] Clifford. Preliminary Sketch of . Proceed- Intelligent Robots and Systems (IROS), volume 4, pages ings of the London Mathematical Society, s1-4(1):381– 2003–2011. IEEE, 2001. 395, 1871. doi: 10.1112/plms/s1-4.1.381. [21] Mehdi Hedjazi Moghari and Purang Abolmaesumi. [7] Yuchao Dai, Jochen Trumpf, Hongdong Li, Nick Barnes, Point-based rigid-body registration using an unscented and Richard Hartley. Rotation averaging with application Kalman filter. IEEE Transactions on Medical Imaging, to camera-rig calibration. In Proceedings of the 9th Asian 26(12):1708–1728, 2007. conference on Computer Vision-Volume Part II, pages [22] K.E. Nicewarner and A.C. Sanderson. A general repre- 335–346. 2009. sentation for orientational uncertainty using random unit [8] Fadi Dornaika and Radu Horaud. Simultaneous robot- quaternions. In Proceedings of the IEEE International world and hand-eye calibration. IEEE Transactions on Conference on Robotics and Automation (ICRA), pages Robotics and Automation, 14(4):617–622, 1998. 1161–1168 vol.2, May 1994. [9] F. Faion, P. Ruoff, A. Zea, and U.D. Hanebeck. Re- [23] Frank C Park and Bryan J Martin. Robot sensor cali- cursive Bayesian calibration of depth sensors with non- bration: solving AX= XB on the . IEEE overlapping views. In 15th International Conference Transactions on Robotics and Automation, 10(5), 1994. on Information Fusion (FUSION), pages 757–762, July [24] Xavier Pennec and Jean-Philippe Thirion. A framework 2012. for uncertainty and validation of 3-D registration methods [10] James Samuel Goddard and Mongi A Abidi. Pose and based on points and frames. International Journal of motion estimation using dual quaternion-based extended Computer Vision, 25(3):203–229, 1997. Kalman filtering. In Photonics West’98 Electronic Imag- [25] Szymon Rusinkiewicz and Marc Levoy. Efficient variants ing, pages 189–200. International Society for Optics and of the ICP algorithm. In Proceedings of the Third Photonics, 1998. International Conference on 3-D Digital Imaging and [11] Søren Hauberg, Franc¸ois Lauze, and Kim Steenstrup Modeling, pages 145–152. IEEE, 2001. Pedersen. Unscented Kalman filtering on Riemannian [26] Yiu Cheung Shiu and Shaheen Ahmad. Calibration of manifolds. Journal of Mathematical Imaging and Vision, wrist-mounted robotic sensors by solving homogeneous 46(1):103–120, 2013. transform equations of the form AX= XB. IEEE Trans- [12] Radu Horaud and Fadi Dornaika. Hand-eye calibration. actions on Robotics and Automation, 5(1):16–29, 1989. The International Journal of Robotics Research (IJRR), [27] Malcolm D Shuster. The quaternion in Kalman filtering. 14(3):195–210, 1995. Advances in the Astronautical Sciences, 85:25–37, 1993. [13] Berthold KP Horn. Closed-form solution of absolute [28] Davide Spinello and Daniel J Stilwell. Nonlinear esti- orientation using unit quaternions. Journal of the Optical mation with state-dependent Gaussian observation noise. Society of America, 4(4):629–642, 1987. IEEE Transactions on Automatic Control, 55(6):1358– [14] Andrew H Jazwinski. Stochastic processes and filtering 1366, 2010. [29] Rangaprasad Arun Srivatsan, Matthew Travers, and Howie Choset. Using Lie algebra for shape estimation of medical snake robots. In International Conference on Intelligent Robots and Systems (IROS), pages 3483–3488. IEEE, 2014. [30] Gary KL Tam, Zhi-Quan Cheng, Yu-Kun Lai, Frank C Langbein, Yonghuai Liu, David Marshall, Ralph R Mar- tin, Xian-Fang Sun, and Paul L Rosin. Registration of 3D point clouds and meshes: a survey from rigid to nonrigid. IEEE Transactions on Visualization and Computer Graphics, 19(7):1199–1217, 2013. [31] Shiuh-Ku Weng, Chung-Ming Kuo, and Shu-Kang Tu. Video object tracking using adaptive Kalman filter. Jour- nal of Visual Communication and Image Representation, 17(6):1190–1208, 2006. [32] Simon Winkelbach, Sven Molkenstruck, and Friedrich M Wahl. Low-cost laser range scanner and fast surface registration approach. In Pattern Recognition, pages 718– 728. Springer, 2006.