Motion Estimation Based on Unit Quaternion Decomposition of the

Motion Estimation Based on Unit Quaternion Decomposition of the Rotation Matrix Huang Yu,

Yuan Baozong

(Institute of Information Science, Northern Jiaotong University, Beijing 100044, PR China Abstract Based on the unit quaternion decomposition of rotation matrix, this paper puts forward an algorithm to estimate motion parameters from the space position vectors of 3D feature points. Rotation matrix's representation with the unit quaternion has no singular points, so the unit quaternion-based estimation method is of more practical importance, and the algorithm in this paper does not need iteration computation compared to those unit quaternion-based method proposed by Horn (1987) and Faugeras et.al (1987). Solution's uniqueness analysis of the algorithm and simulation experiment results are also presented, it can be seen that performance of our method is satisfactory. Key words Unit quaternion decomposition, estimation of motion parameters, singular points.

1 Introduction Discerning the motion of objects from the sequence of images is important for computer vision applications. If we extract the depth information directly, i.e. the distance from the cameras to the objects by the stereo vision system, then as opposed to a sequence of 2-D images, this additional depth information greatly reduces the complexity of the motion estimation task; Otherwise, we get the absolute value of translation vectors. So the stereo vision-based method in motion analysis by now has been more emphasized[1], in which the main problem is how to determine the rotation matrix R, where the translation vector can be obtained afterwards. Several methods had been proposed to infer R directly such as SVDbased method[2], orthogonal decomposition-based method[3] and RS decomposition-based method[4] etc., but we know that R has only 3 degrees of freedom (DOF), so more numerical computation may decrease the estimation's accuracy and the estimated matrix can not be guaranteed to be a rotation matrix. Thus, researchers start to find those estimation methods with R expressed by such fewer parameters as rotation axis and its angle, skew-symmetric matrix, three Euler angles and unit quaternion etc. Because rotation expression with the unit quaternion not only does not confront singular points but also describes the computation of several rotation matrixes briefly[5], it would be of more practical importance. Horn et.al[5] and Faugeras et.al[6] had respectively put forward similar motion estimation methods with unit quaternion representation, which is computed by an iterative algorithm of determining the eigenvectors corresponding to the smallest eigenvalue to solve a minimization problem of a quadratic form. Here based on the Theorem of Unit Quaternion Decomposition (UQD), a linear least squares algorithm of motion estimation without iterations is proposed, then it runs faster. Eventually uniqueness analysis of its solution and results of simulation experiments are also given.

2 Rotation Representation with Unit Quaternion By the kinematics theory, the object motion can be divided into a rotation component and a translation component. Let 3-D position vectors of the object's feature points in time instant t and t + δt be p i , p ' i respectively, i=1, 2, ..., N, then we have i=1,2,...N (1) p ' i = Rp i + T + N i , where R is the rotation matrix, T is the translation vector, N i is the additive noise. DOF of R, as an orthogonal rotation matrix, is 3, so R can be expressed in terms of such as three Euler angles (orientation angles about x, y, z axes respectively by the right-hand rule), axis of rotation and rotation angle, skew-symmetric matrix corresponding to the Carley vector and unit quaternion. But it seems that: when one Euler angle is equal to 90 , the other two can not be determined uniquely; when the rotation angle is equal to 0 , the rotation axis can take any direction; there is no Carley vector corresponding to the rotation matrix when rotation angle is equal to 180 . At the same time, representation with unit quaternion does not have singular points, detailed analysis sees below: 1

Assume a unit quaternion Quater= ( n T , q ) T , with n as a 3 × 1 vector and q as a scalar with the constraint that n

2

+ q 2 = 1. Then we have the following Lemma:

Lemma 1 A 4 4 square matrix Q constructed with a unit quaternion Quater= ( n T , q ) T , would be an orthogonal matrix, i.e.  0 −n3 n2  qI + S n n   = Q= T S , 0 − n1   n  n3 − n q   −n2 n1 0  (2) where I is the 3 × 3 unit matrix, S n is a skew-symmetric matrix corresponding to the vector n = ( n1 , n 2 , n 3 ) T . Proof[7] q 2 I − S n2 + nn T 0 3×1  QQ T =   2 n + q 2  01× 3  Since

q 2 I − S 2n + nn T = q 2 I + n I = I , n 2

2

+ q 2 = 1 ,so

QQ T = I 4 .

Theorem 1 For a rotation matrix R, there are only two unit quaternions with the contrary signs, i.e. ( n T , q ) T , ( − n T , − q ) T , satisfying the following decomposition:  R 0  1× 3

Proof[7]

0 3×1  = Q2 1 

Multiply the right-hand side of the equation (3) with ( n T , q ) T , we have  R 0 3×1  n n n  R − I 0 3×1  n  = QQ   =  , ⇒  = 0 4 ×1 0    1  q  0  q   1× 3 q  q   01× 3

(3)

(4)

There must be no zero solution of ( n T , q ) T for the above equation, so we can get at least one unit quaternion to satisfy the equation (3). Now we assume that two unit quaternions ( n 1T , q ) T , ( n T2 , q ) T satisfy the equation (3), and each of them constructs respectively the corresponding square matrix Q 1 , Q 2 . (1) Let n 1 = 0 3×1 , then q 1 = ±1. Now R=I, Q 1 = ± I 4 , Q 2 Q 2 = I 4 , so Q 2 = Q T2 , we can find that n 2 = 0 3×1 , q 2 = ±1.

n 1 ≠ 0 3×1 , then Q 1 Q 1 [ n T2 , q ] T = [ n T2 , q ] T , and S Tn1 ⋅ n 2 = 0 3×1 , so n 2 =k n 1 (k ≠ 0), substitution into the equation (3) can make sure that k= ± 1. So q 2 = ± q 1 , n 2 = ± n 1 . Theorem 1 gives the unit quaternion decomposition (UQD) of the rotation matrix, now we can infer the relationship between the unit quaternion and R as below: Lemma 2 A rotation matrix R is expressed with unit quaternion Quater = ( n T , q ) T = ( n 1 , n 2 , n 3 , q ) T as below (2)Let

q 2 + n12 − n22 − n32  R =  2(n1 n2 + qn3 )  2(n n − qn ) 1 3 2 

2(n1 n2 − qn3 ) q + n22 − n12 − n32 2(n 2 n3 + qn3 ) 2

2(n1 n3 + qn2 )   2(n2 n3 − qn1 )  q 2 + n32 − n22 − n12 

(5) Proof[7] From the equation (3) we can infer that R= ( qI + S n ) 2 + nn T = ( 2 q 2 − I ) I + 2 nn T + 2 qS n . Representation with unit quaternion can substitute for such representation as axis of rotation and rotation angle ( r , θ ) , the Carley vector ( a , b , c ) T as below:  [ a , b , c] T = n  θ n = r sin  1+ a 2 + b 2 + c 2  2 , 1  θ  q= q = cos  2 1+ a + b 2 + c 2  2 (6) From the equation (6), singular points can be found: when q=0, there is none of ( a , b , c ) T corresponding to the rotation matrix; when n =0, the rotation axis r can take any value. 2

3 Estimation of Rotation Matrix with the Unit Quaternion Since the unit quaternion can represent rotation without any singular points, so research on algorithms of motion estimation based on this representation is more emphasized. Horn[5] and Faugeras[6] respectively put forward similar motion estimation methods with unit quaternion representation, which is computed by an iterative algorithm of determining the eigenvectors (i.e. unit quaternion) corresponding to the smallest eigenvalue to solve a minimization problem of the quadratic form. Here we only simply describe the method in the reference [6]: First the motion estimation problem is to infer R, T minimizing the sum of squared residues as follows,

min J 1 =

N

∑

p ' i − Rp i − T

2

(7)

i =1

Let p = ∑i pi / N , p ' = ∑i p 'i / N , q i = pi − p , q 'i = p 'i − p ' , then the equation (7) is restated as

min J 2 =

N

∑ q'

i

− Rq i

2

(8)

i =1

While R is obtained, T would be calculated by: T = p ' − Rp Now we define as below[8]: P = [ q 1 , q 2 ,... q N ], P ' = [ q '1 , q ' 2 ,... q ' N ], M = PP ' T ,

S = M + MT ,β = P

2

(9)

+ P ' , γ = tr ( M ) , 2

(10) ω = ( m 32 − m 23 , m13 − m 31 , m 21 − m12 ) T . where m ij is all the elements of matrix M, and the sign tr refers to trace of matrix. Then we give a symmetric matrix as below, β − γ H=  ω

 ωT  ( β + γ ) I − S  ,

so the problem (8) can be changed to[6]

min J 3 = QTu HQu

(11)

Now solution to the problem is the 4-D unit eigenvector Q u corresponding to the smallest eigenvalue of H to minimize the quadratic form Q Tu HQ u . Computation of the eigenvector is an iterative process, here we change the problem to be a linear LS minimization based on UQD, then infer a non-iterative linear algorithm, below is details: Substitution of equation (3) into equation (8) generates N

min J 4 = ∑ QT ( q 'Ti , 0 )T − Q ( q Ti , 0 ) T

2

i =1

(12)

it can be restated as a simplified form as N

2

2

min J 5 = ∑ ( qv i − S u i n + vi n ) i =1

with v i = q ' i − q i , S u i as a skew-symmetric matrix corresponding to u i = q ' i + q i . Now we discuss how to solve the problem (13): (1) When q ≠ 0 , the problem (13) change to the following form N

min J 5 = ∑ ( vi − Su i n / q i =1

2

2

+ vTi n / q )

The problem (14) is equivalent to solution of the following regular equation: 3

(13)

(14)

with A =

∑

( S Tu i S u i i

+

v i v Ti

), b = ∑

ST i ui

(15) A⋅n/ q = b v i . Once n/q is obtained, we can determine two solutions of n,

q ( n + q 2 = 1) with contrary signs. From Theorem 1, each of them can correspond to the same rotation matrix. (2) When q=0, then problem (13) change to the following form 2

N

2

min J 5 = ∑ ( S u i n + vTi n )

(16)

i =1

with n

2

= 1. Similarly the problem (16) is equivalent to solution of an equation as below:

(17) A n = 03×1 , n = 1 Here the (parallel) stereo vision system lets z axis' direction of the coordinate system be the optical axis' direction, and position of the object is limited always in the front of cameras,; since the rotation center in equation (1) is chosen as origin of the coordinate system, thus we can assume the z component of rotation axis n = ( n 1 , n 2 , n 3 ) T , n 3 ≠ 0 , i.e. the axis of rotation would intersect with x-y plane. Now let A = [ A 1 , A 2 , A 3 ] , the equation (17) can be restated as: 2

n / n  [ A1 , A2 ] 1 3  = − A3 n2 / n3 

(18)

Once n 1 / n 3 , n 2 / n 3 are determined, only two solutions of n with the contrary signs ( n = 1) can be obtained. Since the axis of rotation does not consider the sign, so we choose any one of them, either positive or negative. Uniqueness of solution about equation (15) and equation (18) is analyzed as below: First let 2

V = [ v1 , v2 ,... v N ] = P ' − P , U = [ u1 , u2 ,... u N ] = P ' + P , Us = [ STu1 , STu 2 ,... STu N ], X = [ Us , V ], α = tr ( UU T ).

(19) It yields A = XX T = U s U Ts + VV T = αI − UU T + VV T . Besides, if the estimated unit quaternion ( n T , q ) T corresponds to the rotation matrix R, then from Lemma 2 we have 3, q ≠ 0; 2, q ≠ ±1; rank(R - I) =  rank ( R + I ) =  1, q = 0; 0, q = ±1; and U = ( R + I ) P , V = ( R − I ) P . (1) Consider the equation (15) when q ≠ 0 : First we have Lemma 3 rank ( U ) ≥ 2 ⇔ rank ( U s ) = 3. Proof

UU T is semi-positive-definite, where λ i is its eigen value, λ i ≥ 0 with

∑i λ i = α . So

rank ( U ) ≥ 2 ⇔ rank ( UU ) ≥ 2 ⇔ λ i < α ⇔ ) = 3 ⇔ rank ( U s ) = 3. Theorem 2 If rank ( P ) ≥ 2 , then an unique rotation matrix can be obtained from the equation (15). Proof If q ≠ 0 , rank(U)=rank(P), then rank(A)=rank[ U s ,V] ≥ rank ( Us ) , so rank ( P ) ≥ 2 , and rank(A)= rank( U s )=3 by Lemma 3, thus the unique solution of n/q can be guaranteed. With the constraint T

rank ( U s U Ts

= 1, we can obtain two unit quaternions with the contrary signs, i.e. the unique rotation matrix. (2) Consider the equation (18) when q=0, we have: Theorem 3 If N 2, unique rotation matrix can be obtained from the equation (18). Proof rank(A)=rank[ U s ,V] ≥ rank ( Us ) ≥ 2 , and two axes of rotation with the contrary signs can be obtained from the equation (17) when rank(A)=2, so either of them is enough; Unique LS solution of n 1 / n 3 , n 2 / n 3 can also be determined from equation (18) when rank(A)=3, similarly we can infer an unique rotation matrix. n

2

The final solution of R is chosen from the two solutions of the equation (15) and the equation (18) to minimize the equation (8).

4 Results of simulation experiments 4

Simulation data is generated from the following scenario: A set of 3-D feature points { pi }, i=1, 2, ...,10, is obtained at random from (-2,-2,2) to (2,2,4), the axis of rotation n is (0,0,1), and the rotation angle θ is generated from [ 0 , 180 ], three components of the translation vector is chosen randomly as a real number between 1 and 3; With these motion parameters above, it yields { p 'i } , i=1,2,...10, by the equation (1). Now the stereo vision system is described as below: two lens centers on the x axis with their middle point at the origin of the camera centered coordinate system, the two lens axis parallel coincident with the z axis, both of the two image planes parallel to the x-y plane with the focal length f =1 and the baseline length B =5. Secondly, through projective imaging model, we can obtain left and right image point pairs {Pil , Pir }, i=1, 2, ...,10, then add the Gaussian noise to the 2-D image point pairs ; Finally through the stereo triangulation, we reconstruct contaminated 3-D feature points. Since the algorithm of motion estimation based on the Carley Theorem proposed in the reference [8] has some similarity with that in this paper, here we compare estimation result of the latter with that of the former. SNR is varied from 20dB to 40dB, estimation error of both methods see Fig. 1 and Fig. 2: Fig. 1 illustrates the estimation error of the translation vector, with dT= T − T0 ; Fig. 2 for the rotation angle (1 ), with dθ = θ − θ0 . From the estimation results, we can find that accuracy of the former method is close very much to that of the latter one, sometimes estimation of our method is more accurate a little (Since normalization of the components in the unit quaternion is convenient for numerical computation, especially for the rotation angle more larger or smaller than 90 ). What is more, the method in the Reference [8] may confront the problem of singularity in representation of rotation matrix (see Chapter 2 in detail).

5 Conclusion In this paper, a non-iterative linear algorithm of motion estimation based on the Theorem of Unit Quaternion Decomposition (UQD) is proposed, which is simpler and faster than that in reference [5, 6]; Otherwise, representation method with unit quaternion does not have singular points, so it is more important than the other representation methods (such as skew-symmetric matrix, Euler angles, axis of rotation and rotation angle etc.). Finally solution's uniqueness and results of simulation experiments are given as well.

Reference [1] Sabata B, Aggarwal J K, CVGIP: Image Understanding, 1991, 54(3): 309-324. [2] Arun K et.al, IEEE Trans. on PAMI, 1987, PAMI-9(5):698-700. [3] Horn B et.al, 1988,5(7): 1127-1135. [4] Su R J et.al, Proc. of the Int. Symp. MTNS-89, Netherlands: 1989, 513-520. [5] Horn B, J. Opt. Soc. Ame. (A), 1987, 4(4): 629-642 [6] Faugeras O D et.al, Proc. of IEEE Int. Conf. on Computer Vision, England: 1987, 25-34. [7] Fan Hong, Ph. D. Dissertation, Northern Jiaotong Uni., Beijing: 1992,9. [8] Xu Wenli, Liu Wenhuang, Acta Automatica Sinica, 1992, 18(4): 441-447 (in Chinese). Carley

Carley UQD

UQD

SNR(dB)

SNR(dB)

Fig. 1 Translation vector estimation error

Fig. 2 Rotation angle estimation error

5