Reprinted from Journal of the Optical Society of America A, Vol, 4, page 629, April 1987Copyright © 1987 by the Optical Society of America and reprinted by permission of the copyright owner .

Closed-form solution of absolute orientation using uni tquaternions

Berthold K. P. Horn

Department of Electrical Engineering. University of Hawaii at Manisa, Honolulu. Hawaii 9672 0

Received August 6, 1986 : accepted November 25, 1986

Finding the relationship between two coordinate systems using pairs of measurements of the coordinates of anumber of points in both systems is a classic photogrammetric task . It finds applications i nstereophotogrammetryand in robotics . I present here a closed-form solution to the least-squares problem for three or more paints .Currently various empirical, graphical, and numerical iterative methods are in use. Derivation of the solution issimplified by use of unit quaternions to represent rotation . I emphasize a symmetry property that a solution to thi sproblem ought to possess. The best translational offset is the difference between the centroid of the coordinates i none system and the rotated and scaled centroid of the coordinates in the other system . The best scale is equal to th eratio of the root-mean-square deviations of the coordinates in the two systems from their respective centroids .These exact results are to be preferred to approximate methods based on measurements of a few selected points .The unit quaternion representing the best rotation is the eigenvector associated with the most positive eigenvalue o fa symmetric 4 X 4 matrix . The elements of this matrix are combinations of sums of products of correspondingcoordinates of the points .

1 . INTRODUCTION

Suppose that we are given the coordinates of a number o fpoints as measured in two different Cartesian coordinatesystems (Fig . 1.) . The photogrammetric problem of recover-ing the transformation between the two systems from thes emeasurements is referred to as that of absolut e orientation.1It occurs in several contexts, foremost in relating a stereomodel developed from pairs of aerial photographs to a geo-detic coordinate system . It also is of importance in robotics ,in which measurements in a camera coordinate system mus tbe related to coordinates in a system attached to a mechani -cal manipulator. Here one speaks of the determination o fthe hand-eye transform . 2

A. Previous Wor kThe problem of absolute orientation is usually treated in a nempirical, graphical, or numerical iterativ e fashion.1,3,4Thompson gives a solution to this problem when thre epoints are measured . His method, as well as the simpler oneof Schut,6 depends on selective neglect of the extra con-straints available when all coordinates of three points ar eknown. Schut uses unit quaternions and arrives at a set o flinear equations . I present a simpler solution to this specia lcase in Subsection 2 .A that does not require solution of asystem of linear equations. These methods all suffer fro mthe defect that they cannot handle more than three points .Perhaps more importantly, they do not even use all theinformation available from the three points .

Oswal and Balasubramanian7 developed a least-squaresmethod that can handle more than three points, but thei rmethod does not enforce the orthonormality of the rotatio nmatrix . An iterative method is then used to square up theresult--bringing it closer to being orthonormal . The meth-od for doing this is iterative, and the result is not the solutio nof the original least-squares problem .

0740-3232/87/040629-14$02 .00

I present a closed-form solution to the least-squares prob -lem in Sections 2 and 4 and show in Section 5 that it simpli -fies greatly when only three points are used . This is impor -tant, since at times only three points may be available . Thesolution is different from those described at the beginning o fthis section because it does not selectively neglect informa -tion provided by the measurements-it uses all of it .

The groundwork for the application of quaternions i nphotogrammetry was laid by Schut8 and Thompson . 9 Inrobotics, Salamin10 and Taylor" have been the main propa-gandists . The use of unit quaternions to represent rotatio nis reviewed in Section 3 and the appendixes (see also Ref. 2) .

B. Minimum Number of PointsThe transformation between two Cartesian coordinate sys-tems can be thought of as the result of a rigid-body motio nand can thus be decomposed into a rotation and a transla -tion. In stereophotogrammetry, in addition, the scale ma ynot be known . There are obviously three degrees of freedo mto translation . Rotation has another three (direction of theaxis about which the rotation takes place plus the angle o frotation about this axis) . Scaling adds one more degree o ffreedom. Three points known in both coordinate system sprovide nine constraints (three coordinates' each), more tha nenough to permit determination of the seven unknowns .

By discarding two of the constraints, seven equations i nseven unknowns can be developed that allow one to recove rthe parameters . I show in Subsection 2 .A how to find therotation in a similar fashion, provided that the three point sare not collinear . Two points clearly do not provide enoug hconstraint.

C. Least Sum of Squares of ErrorsIn practice, measurements are not exact, and so greate raccuracy in determining the transformation parameters wil lbe sought for by using more than three points. We no longe r

© 1987 Optical Society of America

630

J . Opt . Soc . Am . ANol . 4, No. 4/April 1987

Berthold K . P . Horn

C

itFig . 1 . The coordinates of a number of points is measured in tw odifferent coordinate systems . The transformation between the twosystems is to be found .

expect to be able to find a transformation that maps th emeasured coordinates of points in one system exactly intothe measured coordinates of these points in the other. In-stead, we minimize the sum of squares of residual errors .Finding the best set of transformation parameters is noteasy . In practice, various empirical, graphical, and numeri-cal procedures are in use . These are iterative in nature .That is, given an approximate solution, such a method leadsto a better, but still imperfect, answer . The iterative meth-od is applied repeatedly until the remaining error is negligi-ble .

At times, information is available that permits one t oobtain so good an initial guess of the transformation parame -ters that a single step of the iteration brings one close enoug hto the true solution of the least-squares problem to eliminat ethe need for further iteration in a practical situation .

D. Closed-Form SolutionIn this paper I present a closed-form solution to the least -squares problem of absolute orientation, one that does no trequire iteration. One advantage of a closed-form solutionis that it provides one in a single step with the best possibl etransformation, given the measurements of the points in th etwo coordinate systems . Another advantage is that oneneed not find a good initial guess, as one does when a niterative method is used .

I give the solution in a form in which unit quaternions ar eused to represent rotations . The solution for the desiredquaternion is shown to be the eigenvector of a symmetric 4 X4 matrix associated with the most positive eigenvalue. Theelements of this matrix are simple combinations of sums o fproducts of corresponding coordinates of the points . Tofind the eigenvalues, a quartic equation has to be solve dwhose coefficients are sums of products of elements of th ematrix . It is shown that this quartic is particularly simple ,since one ofits coefficients is zero . It simplifies even morewhen one or the other of the sets of measurements is copla-nar .

E. Orthonormal Matrice sWhile unit quaternions constitute an elegant representatio nfor rotation, most of us are more familiar with orthonormalmatrices with positive determinant . Fortunately, the ap-propriate 3 X 3 rotation matrix can be easily constructe dfrom the four components of the unit quaternions as is show nin Subsection 3 .E. Working directly with matrices is diffi-cult because of' the need to deal with six nonlinear con-st raints that ensure that the matrix is orthonormal . A

closed-form solution for the rotation matrix using manipula-tions of matrices will be presented in a subsequent paper .This closed-form solution requires the computation of thesquare root of a symmetric 3 X 3 matrix .

F. Symmetry of the SolutionLet us call the two coordinate systems " left " and "right ." Adesirable property of a solution method is that, when appliedto the problem of finding the best transformation from th eright to the left system, it gives the exact inverse of the bes ttransformation from the left to the right system . I show i nSubsection 2 .D that the scale factor has to be treated in aparticular way to guarantee that this will happen . Symme-try is guaranteed when one uses unit quaternions to repre-sent rotation .

2. SOLUTION METHOD S

As we shall see, the translation and the scale factor are eas yto determine once the rotation is known . The difficult par tof the problem is finding the rotation . Given three noncol-linear points, we can easily construct a useful triad in each o fthe left and the right coordinate systems (Fig. 2) . Let theorigin be at the first point . Take the line from the first t othe second point to be the direction of the new x axis . Placethe new y axis at right angles to the new x axis in the plan eformed by the three points . The new z axis is then made t obe orthogonal to the x and y axes, with orientation chosen tosatisfy the right-hand rule. This construction is carried outin both left and right systems . The rotation that takes oneof these constructed triads into the other is also the rotatio nthat relates the two underlying Cartesian coordinate sys-tems. This rotation is easy to find, as we show below .

A. Selective Discarding Constraint sLet the coordinates of the three points in each of the twocoordinate systems be r t,t , r t,2i rt , 3 and rr,l , r r,x, rr ,3, respec-tively . Construct

xr = rt,2 - r t.t .

Then

x i = x 1/ N i l!

is a unit vector in the direction of the new x axis in the left-hand-system . Now le t

37i (r 1.3 - r1,i) _ (( r1,3 - r 1,1) x 1~ x 1

Fig . 2. Three points define a triad . Such a triad can be construct -ed by using the left measurements . A second triad is then con -structed from the right measurements. The required coordinatetransformation can be estimated by finding the transformation tha tmaps one triad into the other . This method does not use theinformation about each of the three points equally .

Berthold K . P . Horn

Vol . 4, No. 4/April 1987/J . Opt. Soc. Am . A

63 1

be the component of ( rt.a - rt,1 ) perpendicular to X . Theunit vector

vt = y,111 y,11

is in the direction of the new y axis in the left-hand system .To complete the triad, we use the cross product

z t =x t Xy t .

This construction is now repeated in the right-hand syste mto obtain x r, yr, and zr . The rotation that we are looking fortakes x t into _s r, yt into y r , and zt into sr.

Now adjoin column vectors to form the matrices Mt andMr as follows :

yM = IxLytztl,

Mr = I x ryrzrl .

Given a vector r t in the left coordinate system, we see tha t

M 1 Tr t

gives us the components of the vector rt along the axes of th econstructed triad . Multiplication by Mr then maps thes einto the right-hand coordinate system, s o

rr = M,Mi Trt .

The sought-after rotation is given by

R = MA T.

The result is orthonormal since M r and Mt are orthonormal ,by construction . The above constitutes a closed-form solu-tion for finding the rotation, given three points . Note that i tuses the information from the three points selectively . In-deed, if we renumber the points, we get a different rotatio nmatrix, unless the data happen to be perfect . Also note tha tthe method cannot be extended to deal with more than thre epoints .

Even with just three points we should really attack thi sproblem by using a least-squares method, since there ar emore constraints than unknown parameters . The least -squares solution for translation and scale will be given i nSubsections 2 .C and 2 .E. The optimum rotation is found inSection 4 .

B. Finding the TranslationLet there be n points . The measured coordinates in the leftand right coordinate system will be denoted by

Irt,tl

and

Irr .,I ,

respectively, where i ranges from 1 ton . We are looking for atransformation of the for m

rr = sR(rt ) + r o

from the left to the right coordinate system . Here s is a scal efactor, ro is the translational offset, and R(r t) denotes th erotated version of the vector rt . We do not, for the moment ,use any particular notation for rotation . We use only th efacts that rotation is a linear operation and that it preserve slengths so that

IIR(r t )11 2 = 1101 2 ,

where II r 112 = r - r is the square of the length of the vector r .

Unless the data are perfect, we will not be able to find a

scale factor, a translation, and a rotation such that the trans -formation equation above is satisfied for each point . In-stead there will be a residual erro r

e t = rr ;-sR(rt .t )-To-

We will minimize the sum of squares of these errors

n111e ;11 2 .d- i

(I show in Appendix A that the measurements can be weight -ed without changing the basic solution method. )

We consider the variation of the total error first wit htranslation, then with scale, and finally with respect to rota-tion .

C. Centroids of the Sets of MeasurementsIt turns out to be useful to refer all measurements to th ecentroids defined by

n

nr t - n E rt r,

rr = n

rr,t .tai

r= t

Let us denote the new coordinates by

r t,t = rt .i - rt

r' r .i = rr.r - rr .

Note thatn

n

Now the error term can be rewritten as

e, = r'r.t - sR(r't .r) - r'o ,

where

rr'o = ro - r + sR(r t) .

The sum of squares of errors become s

r'r .E -SR(r't,i) -r D

Es t

orn

n

ll r'r .i - sR(r't.t) 112 - 2r'o • E I r`r i - sR(r ' t)] + n Ilr'oII 2 .

t=t

t = r

Now the sum in the middle of this expression is zero since themeasurements are referred to the centroid . So we are leftwith the first and third terms . The first does not depend onr'o, and the last cannot be negative . The total error i sobviously minimized with r'o = 0 o r

r o = r r - sR(r t ) .

That is, the translation is just the difference of the righ tcentroid and the scaled and rotated left centroid . We returnto this equation to find the translational offset once we hav efound the scale and rotation .

This method, based on all available information, is to b epreferred to one that uses only measurements of one or a fe wselected points to estimate the translation.

At this point we note that the error term can be written as

632

J. Opt. Soc . Am. A/Vol . 4, No . 4/April 1987

Berthold K . P . Horn

ei =r' ri -sR(r'ii )

since r' 0 = 0. So the total error to be minimized is jus tn1 IIr' r,i - R(r`!.i)II2 .

i= 1

D. Finding the Scal eExpanding the total error and noting tha t

II R(r' t,1)II 2 = IIr'l,i 11 2 ,

we obtainn

n

n21 Il r'r,i11 - 2s

R (r i .i)+ s2

IIr, ,,I1 2,

i= 1

which can be written in the form

S r - 2sD + s 2S i ,

where Sr and S i are the sums of the squares of the measure-ment vectors (relative to their centroids), while D is the su mof the dot products of corresponding coordinates in the righ tsystem with the rotated coordinates in the left system .Completing the square in s, we ge t

(sj; - D/ ) 2 + (S rSi - D 2 )/S i .

This is minimized with respect to scales when the first ter mis zero or s = D/S i , that is ,

s =

r'r,i R ( r' .i) / L II r'i,r.

i=1

i= 1

E. Symmetry in ScaleIf, instead of finding the best fit to the transformation ,

rr = sR(r i ) + re ,

we try to find the best fit to the inverse transformation ,

r i = sR(rr ) + ro ,

we might hope to get the exact inverse :

= 1/s,

re= - 1 R -1 (re),

R = R -1 .s

This does not happen with the above formulation. By ex -changing left and right, we find instead that s = DISr o r

n[[+'

= L r' i;EIIr'r,i II 2,

=1

i i- 1

which in general will not equal l/s, as determined above .(This is illustrated in an example in Appendix Al . )

One of the two asymmetrical results shown above may b eappropriate when the coordinates in one of the two system sare known with much greater precision than those in th eother . If the errors in both sets of measurements are similar ,it is more reasonable to use a symmetrical expression for th eerror term :

ei = -77 r' r.i - JR(r'J s

Then the total error becomes

n

n

s

II r i ~ I 2 - 2 E !Jr .; - R (r' i .i ) + si=1

i- 1

or

1 Sr -2D+sS1 .s

Completing the square in s, we ge t

(is, -

Sr 2 + 2(S i S r - D) .

This is minimized with respect to scale s when the first ter mis zero or s = Sr/Si, that is ,

n

n

1/2

s = E IIr'r,;11 2I1 IIr'iiII2 /i=1

i= 1

One advantage of this symmetrical result is that it allow sone to determine the scale without the need to know th erotation. Importantly, the determination of the rotation i snot affected by our choice of one of the three values of th escale factor. In each case the remaining error is minimize dwhen D is as large as possible . That is, we have to choose th erotation that makes

n

i- 1

as large as possible .

3 . REPRESENTATION OF ROTATION

There are many ways to represent rotation, including th efollowing : Euler angles, Gibbs vector, Cayley-Klein param-eters, Pauli spin matrices, axis and angle, orthonormal ma-trices, and Hamilton ' s quaternions . 12 .13 Of these represen-tations, orthonormal matrices have been used most often i nphotogrammetry and robotics . There are a number of ad -vantages, however, to the unit-quaternion notation . One ofthese is that it is much simpler to enforce the constraint tha ta quaternion have unit magnitude than it is to ensure that amatrix is orthonormal. Also, unit quaternions are closelyallied to the geometrically intuitive axis and angle notation .

Here I solve the problem of finding the rotation that maxi-mizes

n

r' , i R ( r`! i)

by using unit quaternions . If desired, an orthonormal ma-trix can be constructed from the components of the resultingunit quaternion, as is shown in Subsection 3 .E. We starthere by reviewing the use of unit quaternions in representin grotations . Further details may be found in Appendixes A6-A8 . The reader familiar with this material may wish to ski pahead to Section 4 .

A . QuaternionsA quaternion can be thought of as a vector with four compo-nents, as a composite of a scalar and an ordinary vector, or a sa complex number with three different imaginary parts.

) .

II r' i.i II 2i=1

Berthold K . P . Horn

Vol . 4, No. 4/April 1987/J . Opt. Soc. Am . A

633

Quaternions will be denoted here by using symbols wit hcircles above them. Thus, using complex number rotation ,we have

= q0 + lox + joy- + kqz ,

a quaternion with real part qo and three imaginary parts, q x ,q„ and qz .

Multiplication of quaternions can be defined in terms o fthe products of their components . Suppose that we let

i 2 = -1,

j2 = -1,

k2 = -1;

ij=k,

jk=i,

ki=j ;

C . Dot Products of QuaternionsThe dot product of two quaternions is the sum of products o fcorresponding components :

p . q = pock + px g x + p yg y + Pzgz .

The square of the magnitude of a quaternion is the do tproduct of the quaternion with itself :

114f1 2= q

A unit quaternion is a quaternion whose magnitude equal s1 . Taking the conjugate of a quaternion negates its imagi-nary part ; thus

and

q* = qo - iqx - jq ,. - kqz .ji=-k,

kj=-i,

ik= -j .

Then, if

=ro+irx +jry +krz ,

we get

rq = (ro40 - rxgx - ryg y - r z gz )

+ i(ro4x + rxgo + ryg z - rz gy)

+ j(rog y - rx g z + rygo + r z gz)

+ k(rog z + rx q y - ryg x + r z g o) •

The product qr has a similar form, but six of the signs arechanged, as can readily be verified . Thus rq ~ qr, in gener-al. In Subsection 3 .B we think of quaternions as columnvectors with four components.

B. Products of Quaternion sThe product of two quaternions can also be convenientl yexpressed in terms of the product of an orthogonal 4 X 4matrix and a vector with four components . One may choos eto expand either the first or the second quaternion in aproduct into an orthogonal 4 X 4 matrix as follows :

ro -r x -ry -rzr z

ro -rz

ryrq

=ry.

rz

ro -rx q

q

r z - ry

rx

ro

or

Note that lRR differs from IR in that the lower-right-han d3 X 3 submatrix is transposed . (This again illustrates thenoncommutative nature of multiplication of quaternions . )Note that the sum of squares of elements of each column (o rrow) of IR and IR equal s

rot + rx 2 + ry 2 + rz2 ,

which is just the dot product r - as we see below .

The 4 X 4 matrices associated with the conjugate of aquaternion are just the transposes of the matrices associatedwith the quaternion itself. Since these matrices are or-thogonal, the products with their transposes are diagonal :that is, QQ T = q • qI, where I is the 4 X 4 identity matrix .Correspondingly, the product of q and q* is real :

44 * _ (402+ 4x2+qy2+ 4z2 ) = q•q.

We immediately conclude that a nonzero quaternion has a ninverse

4-1 = (1 /q • q)4* .

In the case of a unit quaternion, the inverse is just th econjugate .

D . Useful Properties of ProductsDot products are preserved because the matrices associate dwith quaternions are orthogonal ; thus

(4P) • (qr) = (QP) (Qr) = (QP) T (Qr)

and

(Q);) T(Q r ) = pTQ T Q r = 13 T(4 •4)Ir.

We conclude that

{4P) • (qr) = (4 •4)(p • r) ,

which, in the case when q is a unit quaternion, is juste • r . Aspecial case follows immediately :

(1;4) • (134) = (P -1;)(4 - 4) ;

that is . the magnitude of a product is just the product of th emagnitudes . It also follows that

(p4) • r =

( r4 * ) ,

a result that we will use later .Vectors can be represented by purely imaginary quaterni-

ons . If r = (x, y, z) T, we can use the quaternio n

=0+ix+jy+kz.

(Similarly, scalars can be represented by using real quaterni -ons .) Note that the matrices IR and lei associated with apurely imaginary quaternion are skew symmetric . That is ,in this special case,

1R T = -1R and lEt T = -IR .

r0

rxqT =

ry, -rz

ror 0

rz

rz

ry -rx

r 0

-rx -ry -r z

-ryr

q=Sq.s

634

J . Opt. Soc . Am . A/Vol. 4, No. 4/April 1987

Berthold K. P . Horn

E. Unit Quaternions and Rotatio nThe length of a vector is not changed by rotation, nor is theangle between vectors . Thus rotation preserves dot prod- 'ucts. Reflection also preserves dot products but change sthe sense of a cross product-a right-hand triad of vectors ischanged into a left-hand triad . Only rotations and reflec-tions preserve dot products . Thus we can represent rota-tions by using unit quaternions if we can find a way ofmapping purely imaginary quaternions into purely imagi-nary quaternions in such a way that dot products are pre -served, as is the sense of cross products . (The purely imagi-nary quaternions represent vectors, of course . )

Now we have already established that multiplication by aunit quaternion preserves dot products between two quater-nions . That is,

Op) • (qr ) = p • r ,

provided that q • q = 1 . We cannot use simple multiplica-tion to represent rotation, however, since the product of aunit quaternion and a purely imaginary quaternion is gener-ally not purely imaginary .

What we can use, however, is the composite produc t

= 4r4 * ,

which is purely imaginary . We show this by expandin g

qrq' _ (Q r)4 * = QT(Qr) _ (QT Q)r' ,

where Q and Q are the 4 X 4 matrices corresponding to q .Then we note that

angle B about the axis defined by the unit vector w = ( w z , w }.,w 2 ) T can be represented by the unit quaternio n

q = cos 0 + sin2 (u + jai, + kw z ) .

Thus the imaginary part of the unit quaternion gives th edirection of the axis of rotation, whereas the angle of rota-tion can be recovered from the real part and the magnitudeof the imaginary part .

G. Composition of Rotation sConsider again the rotation r' = qrq* . Suppose that we nowapply a second rotation, represented by the unit quaternio np. We have

r* _p7'J* =

p(4r4 * ) p* •

It is easy to verify that (q*p*) (pq)* . So we can write

= (pq)r(p4)*.

The overall rotation is represented by the unit quaternionpq . Thus composition of rotations corresponds to multipli-cation of quaternions .

It may be of interest to note that it takes fewer arithmeticoperations to multiply two quaternions than it does to multi -ply two 3 x 3 matrices . Also, calculations are not carried ou twith infinite precision . The product of many orthonorma lmatrices may no longer be orthonormal, just as the productof many unit quaternions may no longer be a unit quaternio nbecause of the limitation of the arithmetic used . It is, how -

[

20

0

q

QT _ 0

(q0 + q x2 - qy2 - qz2)

2(gxgy - g o g z)

2 (4:4r + gn gy)Q -

0

2 (4,4x + 404 :)

(q 02 - 4x2 +qy2 _ qz 2 )

2 (4ygz - 404x )

q

2 ( q , gx - q.q )

2(4:4y + 4ngx)

(q A2 - qx 2 - qy2 + q , 2 )y

So is purely imaginary if r is . Now Q and Q are orthonor-mal if q is a unit quaternion . Then q - q = 1, and so th elower-right-hand 3 X 3 submatrix of Q T Q must also be ortho -normal . Indeed, it is the familiar rotation matrix R thattakes r into r' :

r' =dlr.

In Appendix AG it is shown that cross products are als opreserved by the composite product qrq*, so that we ar edealing with a rotation, not a reflection (equivalently, we ca ndemonstrate that the determinant of the matrix above is+1) .

Note that

(-4) 7 ( -4 * ) = 4 r4 *

so that -q represents the same rotation as doe s

F . Relationship to Other Representation sThe expansion of QTQ given above provides an explici tmethod for computing the orthonormal rotation matrix Rfrom the components of the unit quaternion q . I give inAppendix AS a method to recover the unit quaternion fro man orthonormal matrix . It may be helpful to relate thequaternion notation to others with which the reader may b efamiliar . It is shown in Appendix A7 that a rotation by an

ever, trivial to find the nearest unit quaternion, whereas it i squite difficult to find the nearest orthonormal matrix .

4 . FINDING THE BEST ROTATION

We now return to the problem of absolute orientation . Wehave to find the unit quaternion q that maximize s

(gr' t,iq*)

• r'r i-i= i

Using one of the results derived above, we can rewrite this i nthe form

( 4r' i.i )i= r

Suppose that

= (x ' t,i,

z 'j,i)T while

= (x r r,i, y ' r,i ,z ' r ,i) T., then

I 0

-x' t, i0

z'r -

irr

q = llt r,iq ,

q ri - = y'i .i -z' t.i

0

x'1,i

z' r.r

Y' r,i

-x' r,i

0

Berthold K . P. Horn

Vol. 4, No . 41April 19871J . Opt . Soc . Am . A

63 5

Thus the 10 independent elements of the real symmetri c4 X 4 matrix N are sums and differences of the nine elementsof the 3 X 3 matrix M . Note that the trace, sum of diagona lelements of the matrix N, is zero. (This takes care of the10th degree of freedom .)

gTN 4

is the eigenvector corresponding to the most positive eigen-value of the matrix N .

The eigenvalues are the solutions of the fourth-order poly -_-nomial in X that we obtain when we expand the equatio n

det(N - AI) = 0 ,

where I is the 4 X 4 identity matrix . We show in whatfollows how the coefficients of this polynomial can be com-puted from the elements of the matrix N.

Once we have selected the largest positive eigenvalue, say ,Am, we find the corresponding eigenvector e m by solving thehomogeneous equation

[N - X m.fle m = 0.

I show in Appendix A5 how the components of e m can befound by using the determinants of submatrices obtainedfrom (N - A ml) by deleting one row and one column at atime .

C. Nature of the Closed-Form SolutionWe can now summarize the algorithm . We first find thecentroids xi and r r of the two sets of measurements in the lef tand the right coordinate system . The centroids are sub-tracted from all measurements so that, from now on, we dealonly with measurements relative to the centroids . For eachpair of coordinates we compute the nine possible product sxi)L' r , x 'iy'r , . . . z iz 'r of the components of the two vectors .These are added up to obtain Sxx , Szy, . . ., S . These nin etotals contain all the information that is required to find th esolution .

We now compute the 10 independent elements of the 4 X 4symmetric matrix N by combining the stuns obtained above .From these elements, in turn, we calculate the coefficients ofthe fourth-order polynomial that has to be solved to obtai nthe eigenvalues of N. There are closed-form methods forsolving quartics, such as Ferrari ' s method, which splits thequartic into two quadratics .12,i4,15 We pick the most posi-tive root and use it to solve the four linear homogeneousequations to obtain the corresponding eigenvector . Thequaternion representing the rotation is a unit vector in th e

n

n

same direction . (If desired, an orthonormal 3 X 3 matrixSxx =

x' r,ixr r,i '

Sxy =

can now be constructed from the unit quaternion, using th eresult in Subsection 3.E . )

and so on . Then

At this point, we compute the scale by using one of th e

I(Sxx + Syy + S,r)

Syz - Szy

Szx - 'SSZ

Szy - Sy xSyx - Su

(Sxx - Syy - S,,)

Sxy

+Sx ,

Sxy + Syx

( -Sxx +Syy - Sz)

Syz + Su

Szx +Sxz

S yz + Su

(-Sxx -- Syy + Su)

r1 r,i4 =y'r

i

Zr r,i

o

'.S+

4

r, q. -

Note that lR n, , and s-IR I, ; are skew symmetric as well asorthogonal . The sum that we have to maximize can now b ewritten in the form

n

1(IR t, ,q) ' ( 1R r,,4)i=t

Int̀-+

1 q T R i., TR rd4 ;1LL=...1rrr

that is,

g rrlNi l(i = 4TN4,

where Ni = *1Ri,i T IRr.i and N = Ni. It is easy toverify that each of the Ni matrices is symmetric, so N mustalso be symmetric .

A. Matrix of Sums of ProductsIt is convenient at this point to introduce the 3 X 3 matrix

M =

r' j .;r rrT

.ii- 1

whose elements are sums of products of coordinates mea-sured in the left system with coordinates measured in th eright system. It turns out that this matrix contains all theinformation required to solve the least-squares problem forrotation . We may identify the individual elements by writ-ing M in the form

M ., Syx Syy Syz ,Sxx Su Szz

n

i=i

Sxx Sxy S x z

while

or

where

S. Eigenvector Maximizes Matrix ProductIt is shown in Appendix A3 that the unit quaternion tha tmaximizes

Szx - SxzSxy - Syx

636

J . Opt . Soc. Am. A/Vol. 4, No. 4/April 1987

Berthold K . P. Horn

three formulas given for that purpose in Subsections 2 .D an d2 .E. If we choose the symmetric form, we actually do no tneed to know the rotation for this step. The scale is thenjust the ratio of the root-mean-square deviations of the twosets of measurements from their respective centroids .

Finally, we compute the translation as the difference be- _tween the centroid of the right measurements and the scale dand rotated centroid of the left measurements.

While these calculations may be nontrivial, there is noapproximation involved-no need for iterative correction .(Also, the basic data extracted from the measurements ar ethe same as those used by existing iterative schemes .)

This always happens when there are only three points .Note that c l = -8 det(M), while co = det(N) .

5. POINTS IN A PLANE

If one of the two sets of measured points lies in a plane, thecorresponding centroid must also lie in the plane . Thi smeans that the coordinates taken relative to the centroid li ein a plane that passes through the origin . The componentsof the measurements then are linearly related . Suppose, fo rconcreteness, that it is the right set of measurements that i sso affected . Let n r be a vector normal to the plane . Then

D. Coefficients of the Quartic-Some DetailsSuppose that we write the matrix N in the form

rri•IIr = 0

o r

a e h j r

T(x r,i+Y r,i> z r,i)

'II r = 0 '

N= l eh

bfi

f iThis implies that

S. ,) T . nr = 0 ,c

gg d

where a = Sxx + Sy, + Szi , e = Syz - Szy, h = Sz S - Sxz, and s oon . Then

det(N - AI) = 0

can be expanded in the form

A 4 + c 3A3 + c2 A2 + c 1 A + co = 0 ,

where

c 3 =a+b+c+d ,

c2 = (ac - h 2 ) + (bc - f~) + (ad - j2 ) + (bd - i 2 )

+ (cd + g2 ) + (ab - e 2) ,

c 1 = [-b(cd - g2 ) + f(df - gi) - i(fg - ci)

- a(cd - g2) + h(dh - gj) - j(gh - cj)

- a(bd - i 2) + e(de - if) - j(ei - bj)

- a(bc - f2) + e(ce - fh) - h(ef - bh)] ,

co = (ab - e 2)(cd - g 2 ) + (eh - af)(fd - gi )

+ (ai - ej)(fg - ci) + (ef - bh)(hd - gj)

+ (bj - ei)(hg - cj) + (hi - fj) 2 .

(These expressions may be rewritten and simplified some -what .) It is easy to see that co = 0 since it is the trace of th ematrix N. (This makes it easier to solve the quartic, sinc ethe sum of the four roots is zero .) Furthermore ,

C2=

2-2(S zs

2+ s̀ zy

2+ Sxr

2+Syx

+ Syy2 + Sy.22 +S 21 + Sxy 2 +S 2 ) ,

and so c2 is always negative . (This means that some of th eroots must be negative while others are positive.) Note thatc = -2 Tr(MTM) . Next we find tha t

ct = 8(SxxS VZ S=, . + S,.,.S zx Sx2 + S12 SxyS,.x )

- 8(SZI S sy S z1 + S,.,Sz ,S xy + S_yS,.=SF) .

This may be either positive or negative . It turns out to b ezero when the points are coplanar . as we shall see below

( Sy:+ syy, sy= )T •IIr= 0 ,

(`S'zx, Szy , S22)T • n r = 0,

or

Mnr =0.

We conclude that M is singular. Now c l = -8 det(M), an dhence c l = 0. The same reasoning applies when all thepoints in the left set of measurements are coplanar .

This means that the quartic is particularly easy to solve .It has the simple form

A4 + c2 X 2 + co = O .

If we let µ = A2 , we obtain

+c2µ+co = 0 ,

and the roots are just

= ±'/2(c 22 - 4c0 ) 112 - c 2 .

Now co is negative, so we get two positive roots, say, µ+ andµ_ . The solutions for A are the n

A=f µ+, A =f,.

The most positive of the four solutions i s

A m = [ 1/2(c2 2 - 4c0)1/2 - c 2] 11 2

This simple method applies when the points all lie in a plane .In particular, if we are given three points this will always b ethe case . So we find a simple least-squares solution to thi scase that is normally treated in an approximate fashion byselectively discarding information, as we did in Subsectio n2 .A .

A. Special Case-Coplanar PointsWhen both sets of measurements are exactly coplanar (a salways happens when there are only three) the general solu-tion simplifies further . This suggests that this case may b edealt with more directly. We could attack it by using uni tquaternions or orthonormal matrices . For variety we argue

Berthold K . P. Horn

Vol . 4, No . 4/April 1987/J . Opt . Soc. Am . A

63 7

this case geometrically . (Also, we make use of a dual inter-pretation : we consider the measurements to represent tw osets of points in one coordinate system . )

First, the plane containing the left measurements has t obe rotated to bring it into coincidence with the plane con-taining the right measurements (Fig . 3) . We do this b yrotating the left measurements about the line of intersectionof the two planes . The direction of the line of intersection i sgiven by the cross product of the normals of the two planes .The angle of rotation is that required to bring the left norma lto coincide with the right normal, that is, the angle betwee nthe two normals .

At this point, the remaining task is the solution of a least -squares problem in a plane . We have to find the rotatio nthat minimizes the sum of squares of distances betweencorresponding left and (rotated) right measurements . Thissecond rotation is about the normal to the plane . The angleis determined by the solution of the least-squares problem .

The overall rotation is just the composition of the tw orotations found above . It can be found by multiplication o funit quaternions or multiplication of orthonormal vectors ,as desired. The solution is simpler than the general on ebecause the rotation to be found by the least-squares meth-od is in a plane, so it depends on only one parameter, notthree, as does rotation in the general case (see Fig. 4) .

We start by finding normals to the two planes . We canuse the cross products of any pair of nonparallel vectors i nthe plane :

n[ = r' 12 X r'11,

n r = r' r2 X r/rt •

(The normals can also readily be expressed in terms of thre ecross products of three of the original measurements rathe rthan two measurements relative to the centroid .) We nex tconstruct unit normals h i and fir by dividing n t and sir b ytheir magnitudes . The line of intersection of the two plane slies in both planes, so it is perpendicular to both normals . Itis thus parallel to the cross product of the two normals . Let

a =n 1 Xn r .

(This can be expanded either as a weighted sum of the tw ovectors r' i1 and r'!2 or as a weighted sum of the two vectorsr ' r1 and r' r2 .) We find a unit vector a in the direction of th eline of intersection by dividing a by its magnitude .

The angle 0 through which we have to rotate is the angl ebetween the two normals. So

cos=n t •a.r'

sink=

[Xnr ll •

(Note that O S 5 rr .)We now rotate the left measurements into the plane con-

taining the right measurements . Let r" [,; be the rotatedversion of The rotation can be accomplished by usin gRodrigues's formula, the unit quaternio n

qQ = cos 2 + sin2

(ia x + ja; , + ka = ) ,

or the corresponding orthonormal matrix, say, Re .

B . Rotation in the PlaneWe now have to find the rotation in the plane of the right -hand measurements that minimizes the sum of squares of

distances between corresponding measurements . That is ,we wish to minimize

" 2

i= I

Let a; be the angle between corresponding measurements .That is,

r.ir [ i cos a ; = r r,i ' r,.

t . i

and

r'r,ir"i sin a ; = (r'r i X r" I i) hr +

where

r' r .i = II r 'r.i lL

r" t i = Il r"[,i ll = Ilr'1.i ll

Note that r'r, i X r" [,i is parallel to hr, so the dot produc t

where

with ' r has a magnitude

11 r r,• X r", ill

but a sign that depends on the order of r' r,i and r"[ ,i in th eplane.

Using the cosine rule for triangles, or by expanding th eterm above, we find that the square of the distance betwee ncorresponding measurements is

(rdr,i )2 + (r"t .i)2 - 2r'r .ir"[,i cos a ;.

Fig . 3 . When the two sets of measurements each lie in a plane, th erotation decomposes conveniently into rotation about the line o fintersection of the two planes and rotation about a normal of one o fthe planes . (In this figure the two coordinate systems have bee naligned and superimposed . )

Fig . 4 . The second rotation, when both sets of measurements arecoplanar, is about the normal of one of the planes . The rotationthat minimizes the sum of errors is found easily in this special casesince there is only one degree of freedom .

638

J . Opt. Soc. Am. ANol . 4, No . 4/April 1987

Berthold K . P . Hor n

When the left measurements are rotated in the plan ethrough an angle 6, the angles a i are reduced by 0 . So tominimize the sum of squares of distances we need to maxi-mize

E

1 .i cos(oi -6)i= 1

or

CcosO+SsinO ,

wheren

n

C=

" 1,[

icos ; =

(r 'r,t

r,l[L=1

= 1

and

S =

r'r .ir"i,i sin ; i =

r' r i X r" 1 i).

i=i

i=1

l

This expression has extrema wher e

Csin0=Scos 6

or

sin g = tS

cos0=fC

,V52 + C2

VS2 + C2

The extreme values are ± ~/S 2 + C2. So for a maximum wechose the pluses . (Note that S and C may be positive o rnegative, however . )

The second rotation is then about the axis fi r by an angle O .It can be represented by the unit quaternion

q p = cos 20 + sin (in. + jny + kn 2 )

or the corresponding orthonormal matrix, say, R p .The overall rotation is the composition of the two rota-

tions; that is,

q = gp g o or R R1,Ra .

This completes the least-squares solution for the case inwhich both the left and the right sets of measurements ar ecoplanar. Note that, in practice, it is unlikely that th emeasurements would be exactly coplanar even when theunderlying points are, because of small measurement errors ,unless n = 3 .

The above provides a closed-form solution of the least-squares problem for three points . The special case of thre epoints is usually treated by selectively neglecting some of theinformation, as discussed earlier . The answer then dependson which pieces of information were discarded . The methoddescribed above, while more complex, uses all the informa-tion. (The number of arithmetic operations is, of course ,not a significant issue with modern computational tools . )

By the way, in the above it may have appeared that trigo-nometric functions needed to be evaluated in order for thecomponents of the unit quaternions to be obtained . In fact ,we only need to be able to compute sines and cosines of half-angles given sines and cosines of the angles . But for -a 5 0

cos 612 = [(1 + cos 6)/2] 112while

sin 0/2 = sin 01[2(1 + cos 0)1 1 '2.

(As usual, it isbest to know both the sine and the cosine of a nangle . )

While unit quaternions constitute an elegant notation fo rrepresenting rotation, much of the previous work has bee nbased on orthonormal matrices. An orthonormal rotatio nmatrix can be constructed easily from the components of agiven unit quaternion, as shown in Subsection-3 .E.

6 . CONCLUSION

I have presented a closed-form solution for the least-squaresproblem of absolute orientation . It provides the best rigid -body transformation between two coordinate systems givenmeasurements of the coordinates of a set of points that ar enot collinear. The solution given uses unit quaternions torepresent rotations . The solution simplifies when there ar eonly three points. I also gave an alternative method for thisspecial case .

I showed that the best scale is the ratio of the root-mean-square deviations of the measurements from their respectiv ecentroids . The best translation is the difference betweenthe centroid of one set of measurements and the scaled androtated centroid of the other measurements . These exactresults are to be preferred to ones based only on measure-ments of one or two points . The unit quaternion represent-ing the rotation is the eigenvector associated with the mos tpositive eigenvalue of a symmetric 4 X 4 matrix . The corre-sponding orthonormal matrix can be easily found .

The solutions presented here may seem relatively com-plex. The ready availability of program packages for solv-ing polynomial equations and finding eigenvalues and eigen -vectors of symmetric matrices makes implementatio nstraightforward, however .

APPENDIXES

Al. The Optimum Scale Factor Depends on the Choiceof the Error TermConsider a case in which there is no translation or rotatio n(Fig . 5) . We are to find the best scale factor relating the set sof measurements in the left coordinate system to those in theright . If we decide to scale the left measurements beforematching them to the right on, we get an error term of theform

Ert = 2(c - sa)2 + 2(d - sb) 2 ,

where s is the scale factor . This error term is minimize dwhen

ac + bdS = Srt = a2

+ b2. .

If, instead, we decide to scale the right measurements beforematching them to the left ones, we get an error term of theform

E = 2(a - ic ) 2 + 2(b - id) 2 ,

Berthold K . P . Horn

Vol. 4, No. 41Apri1 19871J . Opt . Soc . Am. A

63 9

'Fig. 5 . Three different optimal scale factors can be obtained bychoosing three different forms for the error term. In general, asymmetric form for the error is to be preferred .

where s is the new scale factor . This error term is minimizedwhen

ac + bds= -

c2 + d2

The equivalent scaling of the left coordinate system is lls o r

__ c 2 + d 2_s-sir ac+bd

In general, sir � sri, so these two methods violate the symme -try property sought for . If we use the symmetric form

E

2(---l c - _,rsay +

d - ‘ric)2 ,■Fs

we obtain instead

C2 +d'.

SSgy=a 2 +b2

Note that Sn is the geometric mean of Sir and Sri and thatthe expression for S n, does not depend on products of mea-surements in the left coordinate system with measurementin the right coordinate system . If there is no strong reasonto do otherwise, the symmetric form is to be preferred, al -though the method presented in this paper for determinin gthe best rotation applies no matter how the scale factor i schosen .

A2. Weighting the ErrorsThe expected errors in measurements are not always equal .In this case it is convenient to introduce weights into the su mof errors so that one minimize s

w i llei ll 2 ,i= 1

where wi is some measure of confidence in the measurementsof the ith point. The method presented in this paper caneasily accommodate this elaboration. The centroids be -come weighted centroids:

R

R

E wiri,`E

w `i

and

rr= E

wi ry, / w ,

al

i= 1

The translation is computed by using these centroids, asbefore . Computation of the best scale is also changed onlyslightly by the inclusion of weighting factors :

,

f n

\112w i ll r',,11 2S =

The only change in the method for recovering rotation i sthat the products in the sums are weighted, that is,

s0

M = .wirri,i

Tr r,iim l

Sxx =

nflli .x [,iX r

i=1

and so on . Once the elements of the matrix M are found, thesolution proceeds exactly as it did before the introduction o fweights .

A3. Maximizing the Quadratic Form q-TNqTo find the rotation that minimizes the sum of squares oferrors, we have to find the quaternion q that maximizesq- TNq subject to the constraint that q - q = 1 . The symmetri c4 X 4 matrix N will have four real eigenvalues, say, A l , A2, . . . ,A4. A corresponding set of orthogonal unit eigenvectors e l ,e 2 , . . . e 4 can be constructed such that

Ne 1 =Hi

for i=I,2, . . .,4 .

The eigenvectors span the four-dimensional space, so a narbitrary quaternion q can be written as a linear combina-tion in the form

q = a le, + a 2e 2 + a3 e 3 + a4e 4 .

Since the eigenvectors are orthogonal, we find that

4 . 4 =a 12+a22+a32+a 4 2.

We know that this has to equal one, since q is a unit quaterni -on . Next, note that

Nq = a 1 X1e 1 + a2A2 e 2 + a3 A 3e 3 + ca 4 A 4e 4

since e l , e 2i . . ., e 4 are eigenvectors of N. We conclude that

g TNq = . (Na) = a12A1 + a22A2 + a32 A3 + a 42 A4 .

Now suppose that we have arranged the eigenvalues in order ,

so that

A l ? A 2 X 2 A 4 -

Then we see that

4 TNq ~ a12A1 + a22A 1 + a 32A 1 + a4 2A1 = A l

so that the quadratic form cannot become larger than th emost positive eigenvalue. Also, this maximum is attained

when we chose a l = 1 and a2 = re;; = a 4 = 0, that is, q = e l .We conclude that the unit eigenvector corresponding to th e

640

J . Opt. Soc. Am. A/Vol . 4, No. 4/April 1987

Berthold K. P . Hor n

most positive eigenvalue maximizes the quadratic formg TNq. . {The solution is unique if the most positive eigenval-ue is distinct . )

A4. Finding the Eigenvecto rThere are iterative methods available for finding eigen-values and eigenvectors of arbitrary symmetric matrices .One can certainly apply these algorithms to the proble mpresented here . We are, however, dealing only with a 4 X 4matrix, for which noniterative methods are available . Wehave already mentioned that the eigenvalues may be foun dby applying a method such as that of Ferrari to the quarti cequation obtained by expanding det(N - Al) = 0 .

Once the most positive eigenvalue, say, A,,,, has been deter -mined, we obtain the corresponding eigenvector e m by solv-ing the homogeneous equation

(N - A m l)em = 0 .

This can be accomplished by arbitrarily setting one compo-nent of em to 1, solving the resulting inhomogeneous set o fthree equations in three unknowns, and then scaling theresult.

If the component of em chosen happens to be small, nu-merical inaccuracies will tend to corrupt the answer . Forbest results, this process is repeated by setting each of th ecomponents of e m to 1 in turn and choosing the largest of thefour results so obtained . Alternatively, the four results ca nbe added up, after changing signs if necessary, to ensure thatthey all have the same sense . In either case, the result maybe scaled so that its magnitude is 1 . The inhomogeneou sequations can be solved by using Gaussian elimination (o reven Cramer's rule) .

A5. Using the Matrix of CofactorsThe method for finding an eigenvector (given an eigenvalue )described in the previous appendix can be implemented i nan elegant way by using the cofactors of the matrix (N - Al) .A cofactor of a matrix is the determinant of the submatrixobtained by striking a given row and a given column . Thereare as many cofactors as there are elements in a matrix sinc ethere are as many ways of picking rows and columns t ostrike . The cofactors can be arranged into a matrix . Theelement in the ith row and jth column of this new matrix i sthe determinant of the submatrix obtained by striking th eith row and jth column of the original matrix .

The matrix of cofactors has an intimate connection withthe inverse of a matrix : the inverse is the transpose of th ematrix of cofactors, divided by the determinant . Let

Na =N - AI;

then

det(N,,)N,, ' _ (Na,)T,

where Na is the matrix of cofactors ofN . Next, we note thatthe determinant is the product of the eigenvalues and tha tthe eigenvalues of (N - Al) are equal to the eigenvalues of Nminus A . So

det(N,) = 1 (A1 - A) .j- 1

We will also need the result that the eigenvalues of theinverse of a matrix are the (algebraic) inverses of the eigen-values of the matrix itself. So, if we write for an arbitraryquaternion

=Ar e ; ,

t= 1

we obtainn a ,

N~` 1 q=~

e .i

1 (A - A )

Combining these results, we see thatn n

(A1 - A)arer = (Na)Tq .j� 1

We remove all terms but one from the sum on the left-han dside by setting A = Am . This gives us

n (A1 - A m )ame m = (Nx m ) Tq .

) rn

The left-hand side is a multiple of the sought after eigenvec-tor em . This will be nonzero as long as the arbitrary quater-nion q has some component in the direction e m , that is, am0 (and provided that the most positive eigenvalue is dis-tinct) . It follows from consideration of the right-hand sideof the expression above that each column of (N,,,,)T, andhence each each row of N A,,, , must be parallel to em.

All that we need to do then is to compute the matrix o fcofactors of (N - A mn . Each row of the result is parallel t othe desired solution . We may pick the largest row or add u pthe rows in the fashion described at the end of the previou sappendix. Note that we could have saved ourselves som ecomputation by generating only one of the rows. It mayhappen, however, that this row is zero or very small .

The matrix of cofactors, by the way, can be computedeasily by using a modified Gaussian elimination algorith mfor inverting a matrix.

A6. Quaternions as Sums of Scalar and Vector PartsWe found it convenient to treat quaternions as quantitieswith three different imaginary parts (and occasionally a svectors with four components) . At other times it is useful t othink of quaternions as composed of a scalar and a vecto r(with three components) .

Thus

q = qo + iq s + jqy + kq =

can be represented alternatively in the for m

q

+ q ,

where q = qo and q = (qr, qy, qz) T. The rules for multiplica-tion of quaternions given earlier can then be written in th emore compact for m

p=rs-r-s,

p=rs+sr+rXs,

when p = rs and

r=r+r,

s=s+s,

p = p+p.

Berthold K . P . Horn

Vol . 4, No. 4/April 1987/J . Opt . Soc. Am . A

64 1

The results simplify if ;and s are purely imaginary, that is, if

The relationships presented here allow us to convert easil yr = 0 and s = 0 . In this special case

between axis and angle and unit-quaternion notations fo rrotations . We hav ep=-r•s,

p=r Xs .

Now let us apply the composite product with a unit quater-nion q to r, s, and p . That is, let

= qrq*,

s' = qsq*,

=gP4 *

Clearly

r's ' _ (qrq*)(4s4*) = (4r)(q*q)(sg*) = q (rs )4 ` ,

so consequently

-r'-s'=-r •s .

= q /IIgI I

and

sin 9 = 2q kill,

cos 9 = ( q2 - II g II 2) .

A8. Unit Quaternion from Orthonormal MatrixWe have shown in Subsection 3 .E that the orthonormalmatrix corresponding to the unit quaternio n

q =q0+ iqx+Jqy+ hq :

Also, r' X s' is the result of applying the composite product

i s

(40+

4x2 _qy 2 _ qZ2)

2 ( gxg y - 4o4 Z)

2 ( g x q , + gogy)

R =

2 ( g yg x + go g z )

((l 02 - qx2 + q y 2 - q 2 2)

2(gyq, - q 0q)

2 (q :g x - q 0q )

2 ( q :g y + g o g z)

(q02 _ gx 2 - qy2 + (722 )

with the unit quaternion to r X s . Thus dot products an dcross products are preserved . Consequently, the compositeproduct with a unit quaternion can be used to represen trotation .

A7 . Unit Quaternion from Axis and Angl eSuppose that a vector r is taken into r' by a rotation of angl e0 about an axis (through the origin) given by the unit vector

= (wx, wv, wz) T. Analysis of the diagram (Fig. 6) leads tothe celebrated formula of Rodrigues :

r' = cos Or + sin Ow X r + (1 - cos O)((e-

We would like to show that this transformation can also bewritten in the form

=qrq* ,

where the quaternions, expressed in terms of scalar an dvector parts, are

r=0+r,

r`=0+r',

and

q = cos(9/2) + sin(8/2)w .

Straightforward manipulation shows that, if the scalar par tof r is zero and is a unit quaternion, then the compositeproduct qrq* has zero scalar part and vector part

r'= (q 2 - q •q)r + 2qq X r+2(q•r)q .

If we now substitut e

q = cos(9/2),

q = sin(9/2) m

and use the identitie s

2 sin(O/2)cos(0/2) = sin 9 ,

cos 2 (912) - sin 2 (9I2) = cos 8 ,

we obtain the formula of Rodrigues .

At times it is necessary to obtain the unit quaternion corre-sponding to a given orthonormal matrix . Let ri, be theelement of the matrix R in the ith row and the jth column .

The following combinations of the diagonal terms prov euseful :

1 + r 11 + r22 + r 33 = 4 (102 ,

1 + rll - r22 - r 33 = 4g x2 ,

1 - r 11 + r22 - r 33 = 4gy 2 ,

1-r11 -r 22 +r33 = 4g2 2 .

We evaluate these four expressions to ascertain which of qo,qx , . . ., q z is the largest . This component of the quaternionis then extracted by taking the square root (and dividing b y2) . We may choose either sign for the square root since -qrepresents the same rotation as +q .

Next, consider the three differences of corresponding off -diagonal elements

Fig . 6 . The rotation of a vector r to a vector r' can be understood i nterms of the quantities appearing in this diagram. Rodrigues'sformula follows.

642

J . Opt. Soc . Am. ANol . 4, No . 4/April 198 7

r32 - r23 - 4g 0q: ,

r13 - r31 = 4goq y ,

rat - r i g = 4g oq y •

and the three sums of corresponding off-diagonal element s

r2t + r12= 4g x g y ,

r 32 + r 23 = 4g y g r ,

rl3 + r31 = 4g z gg .

Three of these are used to solve for the remaining threecomponents of the quaternion using the one componen talready found . For example, if qo turned out to be thelargest, then q zf qy, and qz can be found from the threedifferences yielding 4gog x , 4goqy, and 4goq= .

We select the largest component to start with in order t oensure numerical accuracy . If we chose to solve first for qo ,for example, and it turned out that qo were very small, the nq x , qy, and q 2 would not be found with high precision.

ACKNOWLEDGMENTS

I would like to thank Shahriar Negahdaripour and Hugh M .Hilden, who helped me with the least-squares solution usin gorthonormal matrices that will be presented in a subsequen tjoint paper . Jan T . Galkowski provided me with a consider -able number of papers on related subjects . Thomas Poikerand C . J . Standish made several helpful suggestions . ElaineShimabukuro uncomplainingly typed several drafts of thi spaper . This research was supported in part by the Nationa lScience Foundation under grant no . DMC85-11966 .

Berthold K . P . Hor n

B . K . P . Horn is on leave from the Artificial Intelligenc eLaboratory, Massachusetts Institute of Technology, Cam-bridge, Massachusetts .

REFERENCES

1. C . C . S igma, C . Theurer, and S . W. Henrikson, eds ., Manual ofPhotogrammetry (American Society of Photogrammetry, Fall sChurch, Va ., 1980) .

2. B. K . P . Horn, Robot Vision (MIT/McGraw-Hill, New York ,1986) .

3. P . R . Wolf, Elements of Photogrammetry (McGraw Hill, Ne wYork, 1974) .

4. S . K . Gosh, Theory of Stereophotogramametry (Ohio U . Book -stores, Columbus, Ohio, 1972) .

5. E . H . Thompson, "A method for the construction of orthogonalmatrices," Photogramm . Record 3, 55-59 (1958) .

6. G . H . Schut, "On exact linear equations for the computation o fthe rotational elements of absolute orientation," Photogramme -tria 16, 34-37 (1960) .

7. H . L . Oswal and S . Balasubramanian, "An exact solution o fabsolute orientation," Photogramm . Eng . 34, 1079-1083 (1968) .

8. G . H. Schut, "Construction of orthogonal matrices and theirapplication in analytical photogrammetry, " Photogrammetri a15, 149-162 (1959) .

9. E . H . Thompson, "On exact linear solution of the problem o fabsolute orientation," Photogrammetria 15, 163-179 (1959) .

10. E . Salamin, "Application of quaternions to computation wit hrotations , " Internal Rep . (Stanford University, Stanford, Cali -fornia, 1979) ,

11. R. H . Taylor, "Planning and execution of straight line manipu-lator trajectories," in Robot Motion : Planning and Control, M.Bradey, J . M . Mollerbach, T . L. Johnson, T. Lozano-Perez, an dM . T . Mason, eds. (MIT, Cambridge, Mass ., 1982) .

12. G . A . Korn and T . M. Korn, Mathematical Handbook for Scien -tists and Engineers (McGraw Hill, New York, 1968) .

13. J . H . Stuelpnagle, "On the parameterization of the three-di -mensional rotation group," SIAM Rev . 6, 422-430 (1964) .

14. G . Birkhoff and S . MacLane, A Survey of Modern Algebr a(Macmillan, New York, 1953) .

15. P . H . Winston and B . K . P . Horn, LISP (Addison-Wesley, Read -ing, Mass ., 1984) .