An algorithm of motion estimation based on unit quaternion decomposition of the rotation matrix
Transcript of An algorithm of motion estimation based on unit quaternion decomposition of the rotation matrix
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).