Vol.13 No.4 J OUR NAL OF ELECTRONICS Oct. 1996

A N A L G O R I T H M OF MOTION ESTIMATION B A S E D ON U N I T Q U A T E R N I O N D E C O M P O S I T I O N OF

THE ROTATION MATRIX*

Huang Yu Yuan Baozong

( InJtitute of Information Science, Northern Jiaotong University, Beijing 100044)

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 methods

propo6ed by Horn(1987) and Su, et M.(1989). 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 worde Unit quaternion decomposition~ Space position vectors; Estimation of motion

parameters; Singular points

I . I n t r o d u c t i o n

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 mo-

tion est imation task; Furthermore, we can get the absolute value of translation vectors. So

the stereo vision-based method in motion analysis by now has been more emphasized [I],

in which the main problem is to determine the rotation matrix R, and then the transla-

tion vector can be obtained afterwards. Several methods had been proposed to infer R

directly, such as SVD-based method[Z], 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

est imated matr ix can not be guaranteed to be a rotat ion matrix. Thus, researchers s tar t

to find those est imation methods with R expressed by fewer parameters , such as rotat ion

axis and its angle, skew-symmetric matrix, three ELder's angles and unit quaternion, etc.

Because the rotat ion expression with the unit quaternlon not only does not confront singular

points but also describes the computat ion of several rotat ion matrixes briefly [5]. I t would

*Suppor ted by the "863" High Technology Research and Development Program of China

under Grant 863-306-03-01

290 JOURNAL OF ELECTRONICS Vol.13

be of more practical importance. Horn, et al.[s] and Faugeras, et al.[6l had respectively put

forward similar motion estimation method with the unit quateruion 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, so it runs faster. Eventually, uniqueness

analysis of its solution and results of simulation experiments are also given.

I I . R o t a t i o n R e p r e s e n t a t i o n w i t h U n i t Q u a t e r n i o n

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~, p~ respectively, i -- 1, 2 , . . - , N, then we have

p~=Rpi+T+N~, i = 1 , 2 , . - . , N (1)

where R is the rotation matrix, T is the tranAlation vector, Ni is the additive noise.

DOF of R, as a 3 x 3 orthogonal rotation matrix, is 3, so R can be expressed in terms

of three Euler's angles (the orientation a~gles about x, y, z axes respectively by the right-

hand rule), axis of rotation mad rotation angle, skew-symmetric matrix corresponding to the

Carly vector and unit quaternion. But it seems that when one Euler's 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. The detedled analysis is shown as follows:

Assume a unit quaternion Quater= (nT,q) T, with n as a 3 x 1 vector and q as a scalar

with the constraint that Ilnll 2 + q2 = 1. Then we have the following Lemma:

L e m m a 1 A 4 • 4 square matrix Q constructed with a unit quateruion Quater= (n T, q)T would be an orthogonal matrix, i. e.

nT I 0 --n3 n2 1

S n ---- n3 0 - h i

-n2 nl 0

(2)

where I m a 3 x 3 unit matrix, Sn m a skew-symmet~cal matrLx corresponding to the vector - -

Proof[ 7] QQT= [q2I-S2- ' t -nn T 0S• ]

01• ll,,ll2 + q2

Since q2I - San + nn T = q2I + 11,,1121 = I ( l l n l l 2 + q2 = 1), so QQT = / 4 .

T h e o r e m 1 For a rotation matrix JR, there are only two unit quaternions with the

contrary signs, i.e. (n T, q~T, (_nT, _q)T, satisfying the following decomposition:

[ R 01x3 03• 1 =Q' (3)

No.4 AN ALGORITHM OF MOTION ESTIMATION 291

P r o o f ffl Multiply the right-hand side of the Eq.(3) with (n T, q)T, we have

[0111~X3 0311] [q] ~(~Q [q] : [ : ] '::::~ [~1~ 00• [q]--~04• (4)

There must be no zero solution of (n r , q ) r for the above equation, so we can get at

least one unit quateruion to satisfy Eq.(3).

Now we assume that two unit quaternions (n T, q)T, (aT, q)T satisfy Eq.(3), and each

of them constructs respectively the corresponding square matr ix Q1, Q2-

(I) Let n I • 03X1, then ql = + i . Now R = I, Ol = 4-14, O2Oz = 14, so Q2 - QT, we

can find that n2 = 03xl, q2 = :t:1.

(2) Let n l ~ 03xl, then Q1Ol[n2T,q] T = [nT, q] T, and sT1 �9 n2 = 03xl, so n2 =

kn t ( k ~ 0), substitution into Eq.(3) can make sure that k = + l , so q2 = :t:ql, n2 -- 4-nl.

Theorem 1 gives the UQD of the rotation matrix, now we can infer the relationship

between the unit quaternion and R as below

Le m mA 2 A rotation matrix R is expressed with unit quaternion Quater= (n T, q)T =

(nl, n2, as, q)T as below

Proot~71

F ~2 + n2 .,2 n2 _ 2(nln3 + qn2) ] /I/ 1 - - ' " 2 - - 3 2(nln2 qna) R = / 2(r~1n2 + qn3) q2 + n~ - n 2 - n 2 2(nzn3 - qnl) / (5)

.2 + n2 . 2 n2 / L 2( nln3 - qn2) 2(n2r~3 + q n 3 ) ,1 3 - , '2 - 1 j

From Eq.(3) we can infer tha t

R : ( q I -~- 8n) 2 + nn T : (2q z - I ) I + 2nn T + 2 q S n

Representation with unit quaternion can substi tute for such representation as axis of

rotat ion and rotation angle (r , 0), the Carley vector (a, b, c) T as below:

n=rsin~ n=~/ l+a2 b2+e 2 0 '

q = cos q = F i J

(6)

From Eq.(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.

3 E s t i m a t i o n o f R o t a t i o n M a t r i x w i t h t h e U n i t Q u a t e r n i o n

Since the unit quaternion can represent rotation without any singular points, so re-

search on algorithm~ of motion estimation based on this representation is more emphasized.

Horn [5] and Faugeras [6] respectively put forward similar motion estimation methods with

unit quateruion representation, which is computed by an iterative algorithm of determiuln~

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 Ref.[6]: First the motion estimation problem is to infer R and T minimizing the sum of

292 J OUR NAL OF ELECTRONICS Vol.13

squared residues as follows:

N

rain J1 = E lip5 = R p , - TII 2 i=1

_ _ ! Let ~ = ~ i p i / N , ~' = ZipS~N, qi = Pi - if, q~ - Pi - ~ , then Eq.(7) is restated as

N rain J2 = ~ IIq[ - Rq, II 2

i--1

While R is obtained, T would be calculated by:

Now we define as below[S]:

P = [ q l , q 2 , ' " q N ] ,

S = M + M T,

(7)

(8)

u3 = ( m 3 2 -- m 2 3 , rn l3 -- rrt31, m21 -- m12) T (10 )

"where mi j axe all the elements of matr ix M, and the sign t r refers to trace of matr ix. Then

we give a symmetr ic matr ix as below:

so Eq.(8) can be changed to [sl

wT ] (~ + 7 ) I - s

rain Ja T (11) = Q, ,HQ, ,

Now solution to the problem is the 4-D unit eigenvector Q~ corresponding to the smallest

eigenvalue of H to minimize the quadratic form QTHQ, , .

Computa t ion 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. Its

details axe given as follows:

Substi tut ion of Eq.(3) into Eq.(8) generates

N m l . J4 = ~ IIQT (q~ , O) T, _Q(qT , 0)TII 2

i=1

I t can be restated as a simplified form as

N �9 . i . & = Y~( l lqv , - S=,nl l 2 + II~,"ll 2) (13 )

i=1 _ ~ _ - - ? with vi ~ - q/, S . , as a skew-symmetric matr ix corresponding to ui - qi + qi-

Now we discuss how to solve Eq.(13):

(1) When q # 0, Eq.(13) changes to the following form:

N rain ,/5 = ~'~( l lv , - S= ,n /q l l 2 + IIvTn/qll 2) ( 1 4 )

i---I

(12)

p l = i i [q l ,q2 , ' "qN] , M = p p r r ,

=HPH 2+HP~[[ 2, 7 = t r ( M ) ,

T = r - ~ (9)

No.4 AN ALGORITHM OF MOTION ESTIMATION 293

Eq.(14) is equivalent to solution of the following regular equation:

A. n /q = b (15)

T v with A = ~'~i(S~,S~, + vivT), b -- ~-~-i S~, i. Once n/q is obtained, we can determine two

solutions of n , q(llnll 2 + q2 _ 1) with contrary signs. From Theorem 1, each of them can

correspond to the same rotation matrix.

(2) When q = 0, then Eq.(13) change to the following form:

N

mln J5 --= ~-~(l lS~,-I I 2 + I l l 'n i l 2) (16) i = l

with Ilnll ~- = 1. Similarly Eq.(16) is equivalent to solution of an equation as below:

A . = 03 • I1.11 ~ = 1 (17)

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 Eq.(1) is chosen as origin of the coordinate system,

thus we can assume the z component of rotation axis n = (nl,n2,n3) T, n3 ~ 0, i. e. the

axis of rotat ion would intersect with x-y plane. Now let A = [A1, A2, A3], Eq.(17) can be

restated as: [n,/n3[" = - A a (18) [A1, A21 [n21n3

Once nl/na, n2/na are determined, only two solutions of n (l[n[I 2 = 1) with the contrary

signs can be obtained. Since the axis of rotation does not consider the sign, so we choose

one of them.

Uniqueness of solution about Eq.(15) and Eq.(18) is analyzed as follows: First let

V = [ v , , v 2 , ' " , V N ] = P t - P , U = [ u , , u 2 , ' " , U N ] = P ' + P } (19)

u . [stT,, ~ T = s : , , , . . . , s&,] , x = IV,, v] , ,~ = t r ( V V ~)

it yields A = X X r = UoU~rs + V V T = cxI - U U T + V V T. Besides, if the estimated unit

quaternion (nT,q) T corresponds to the rotat ion matr ix R, then from Lemma 2 we have

rank(R+l)={ 3'1, qq#0= 0 rank(R-l)={ 2'0, q#-l-lq = +1

and V = (R + X)P, v + (R - x)P.

(1) Consider Eq.(15) when q # 0: First we have ;.o,,,ma 3 r~k(U) _> 2 ~0 ranU(Uo) = 3.

Proof UU T is semi-positive-definite, where hi is its eigen value, )*i -> 0 with ~i hi = r So rank(U) _> 2 0 rank(UU T) > 2 o hi < r r rank(UoU T) ---- 3 r162 rank(Us) : 3.

T h e o r e m 2 H rank(P) > 2, then an unique rotation matrix can be obtained from

Eq.(15).

P r o o f I fq # O, rank(U)=rank(P) , then rank(A)=rank[Uo, V] > rank(Us), so rank(P)

294 JOURNAL OF ELECTRONICS Vol.13

>_ 2, and ranlc(A)=rank(Ue) = 3 by Lemm~ 3, thus the unique solution of n/q can be

guaranteed. With the constraint l[n[l 2 -- i, we can obtain two unit quaternions with the

contrary signs, i.e. the unique rotation matrix.

(2) Consider Eq.(18) when q = 0, we have

Theorem 3 If N _~ 2, an unique rotation matrix can be obtained from Eq.(18).

P r o o f rank(A)=rank[Uo, V] ~rank(Uo) _~ 2, and two axes of rotation with the con-

trary signs can be obtained from Eq.(17) when rank(A) = 2, so either of them is enough;

Unique LS solution of nl/n3, n2/n3 can also be determined from Eq.(18) when rank(A) = 3,

similarly we can infer unique rotation matrix.

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

Eq.(8).

IV. Resu l t s of S imula t ion E x p e r i m e n t s

Simulation data is generated from the following scenario: A set of 3-D feature points

(p~), i --- 1, 2 , . - . , 10 is obtained at random from (-2, -2 , 2) to (2,2,4), the axis of rotation

is (0,0,1), and the rotation angle 0 is generated from [0 ~ 180~ three components of the

translation vector is chosen randomly as real numbers between 1 and 3; With these motion

parameters above, it yields {p'~), i -- 1, 2 - . . , 10 by Eq.(1). Now the stereo vision system is

described as below: two lens centers on the z axis with their middle point at the origin of

the camera centered coordinate system, the two lens axes parallel coincident with the z axis,

both of the two image planes parallel to the z-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 {P~,]~ }, i = i, 2,--., I0, then add the Gaussian noise to the 2-D

image point pairs; Finally through the stereo triangulation, we reconstruct contaminated

3-D feature points.

0.08

I ~ CL04

Gu~

i ~ c)~1

0 JIO

, , I . . | , , i , , ,

B SO U 40 s~(d~)

! } .e

1.4

1.2

1

O,B

08

0.4

(t2

O r , , ~9

L i , , , �9 - -

SNR(dB)

Fig.l Translation vector estimation error Fig.2 Rotation angle estimation error

Since the algorithm of motion estimation based on the Carley theorem proposed in

No.4 AN ALGOPdTHM OF MOTION ESTIMATION 295

Ref.[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 errors of

bo th methods see Fig.1 and Fig.2. Fig.1 illustrates the estimation error of the translation

vector, with d T = l i t - T011; Fig.2 for the rotation angle (1~ with dO -- 118 - 001[. 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 a little more accurate.

(Since normalization of the components in the unit quaternion is convenient for numerical

computation, especially for the rotat ion angle more larger or smaller than 90~ What is

more, the method in Ref.[8] may confront the problem of singularity in representation of

rotat ion matr ix (see Section II in detail).

V . C o n c l u s i o n

In this paper, a non-iterative linear algorithm of motion estimation based on the theorem

of unit quaternion decomposition is proposed, which is simpler and faster than that in

l~fs.[5, 6]; Furthermore, the 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, ELder's angles, axis of rotation and rotation angle, etc.). Fin~Uy

solution's uniqueness snd results of simulation experiments axe given as well.

R e f e r e n c e s

[1] B. Sahara, J. K. Aggarwat, CVGIP: Image Understanding, 54(1991)3, 309-324.

[2] K. Arun, et at., IEEE Trans. on PAMI, PAMI-9(1987)5, 698-700.

[3] B. Horn, et al., J. Opt. Soc. Am. (A), 5(1988)7, U27-1135.

[4] R. J. Su, et at., Matrix decomposition and optimal estimation of rigid -body displacement, Proc. of the

Int. Syrup. MTNS-89, Netherlands: 1989, 513-520.

[5] B. Horn, J. Opt. Soc. Ame. (A), 4(1987)4, 829-642.

[6] O. D. FaugeraS, et al., Motion and structure from motion from point and line matches, Proc. of IEEE

Int. Conf. on Computer Vision, England: 1987, 25-34.

[7] Fan Hong, High accurate calibration in the 3-D computer vision system and accurate extraction of 3-D

motion information, Ph. D. Dissertation, Northern Jiaotong Uni., Beijing: 1992, 9, (in Chinese).

[8] Xu Wenli, Liu Wenhuang, Chinese J. o?Antomation, ,18(1992)4, 441--447, (in Chinese).