Nonlinear Least Squares Optimisation of Unit Quaternion Func(2)

Pose estimation from an arbitrary number of 2-D to 3-D feature correspondences is often done by minimising a nonlinear criterion function using one of the minimal representations for the orientation. However, there are many advantages in using unit quatern

wherevl=[xl,yl]T.Thelengthofalinesegmentisex-cludedfromthismappingbecauseitismorenoisythanothermid-pointparameters.3Weencodetheposepbya3-Dvectort∈Randbyaquaternionq∈S3.Letgjk:R3×S3→R3bethefollowingmappingsgjk(p)=f(Aj(q

x1k

q+t),Aj(q

x2k

q+t)).(4)

Eachgjkmapsthek-th3-Dmodelsegmentontothe2-D

imagesegmentobtainedbymovingthemodelsegmentbyp=(q,t)andbyprojectingtheresultingsegmentontothej-thimageplane.

Weassumenowthatthecorrespondencesbetweenmeasuredimagesegmentsyjk=[uTT

the

andthemjk, jk]

modelsegmentsobjectposekaregiven.Theoptimalestimateforthecurrentcanbecalculatedbyminimisingthefollowingnonlinearcriterionfunction

1 M N

2y jk gjk(t,q) Σ jk

1

yjk g jk(t,q)j=1

k=1=13

MN

2

hk(t,q)2,(5)k=1

whereΣjkarethecovariancesofthemeasuredsegments.Theminimisationof(5)overtandqwouldbeaclassicnonlinearleastsquaresoptimisationproblemifwecouldtreatqasanelementofR4andnotofS3.Sincethisisnotthecase,theclassicapproachwouldbetoaddthecon-straint|q|=1totheabovecriterion.Inthenextsectionweproposeabettersolution.

3.Leastsquaresoptimisationonunitsphere

Letsconsidertheminimisationofsumofsquaresofgen-eralunitquaternion functions

1

n qminF(q)=fk(q)2=1f(q)Tf(q).(6)∈S3

2k=1

2Denotingthen×4Jacobianmatrixoff(q)asJ(q),the

gradient F(q)andtheHessian 2F(q)aregivenby F(q)=J(q)Tf(q), 2

F(q)=

J(q)T

J(q)+

n(7)

fk(q) 2

fk(q).

(8)

k=1

Letqibethecurrentapproximationfortheminimumof(6).

TheTaylorseriesexpansionforthevectorfunction F(q)around F(qi)isgivenby

F(q)≈ F(qi)+ 2

F(qi)(q qi)

(9)

Usingtheaboveexpansionandthefactthat F(q)=0attheminimumofF,thenextapproximationforthemini-mumcanbecalculatedasfollows

qi+1=qi ( 2F(qi)) 1 F(qi).

(10)

IfweassumethatthevalueofFissmallforallqbelongingtotheneighbourhoodofthesolution,i.e.fk,weobtainthefollowingapproximationfork(theq)≈Hessian0forallintheneighbourhoodofthesolutionbasedonEq.(8)

2F(q)≈J(q)TJ(q).

(11)

WritingthisapproximationfortheHessianin(10),wear-rivetotheGauss-Newtoniteration,whichisveryeffectiveprovidedagoodstartingpointisknown

qi+1=qi (J(qi)TJ(qi)) 1JT(qi)f(qi).

(12)

Unfortunately,qi+1givenbyiteration(12)doesnotnec-essarilylieontheunitsphere.Thisisduetothefactthatthisiterationsearchesfortheminimumof(6)intheR4-neighbourhoodandnotintheS3-neighbourhoodofqde neaniterationontheunitsphereweobservethati.TotheneighbouringpointsofqqiinS3aregivenbyexp(ω) i,ω∈R3.Hencewecanwritecriterion(6)asn

n

Gi(ω)

=1 2fk(exp(ω) qi)2

=1 ik=1

2gk(ω)2

=

1k=1

2

gi

(ω)Tgi(ω).(13)

GicanbeviewedasamappingfromR3toR.Eq.(2)

guaranteesusthatinthiswaywecoverthewholetangentspaceTqi(S3)or,inotherwords,alldirectionsstartingfromqasqi.Wewanttocalculatethenextapproximateqωthepropertiesofithe+1exponentiali+1=exp(map,i) qsuchqi.Becauseofthecurrentapproximatei+1liesalongthegeodesiccurvestartingatqexponentialmappreservesiinthedirectionofωi qi.Sincethedistances,thelengthofthesteponthesphereisequaltothenormofωTodetermineωwetakezeroasaninitialapproxima-i.tionfortheminimumi,ofcriterion(13).Wedenotethen×3Jacobianmatrixofgiatω=0asJi.Inthesecircum-stances,onestepoftheGauss-Newtoniteration(12)resultsinthefollowingapproximationfortheminimumofcrite-rion(13)

ωi= (JTiJi) 1JTigi

(0).(14)ThustheGauss-Newtoniterationontheunitspherecanbe

summarisedasfollows:(i)Initialisation:

q0←initialapproximation.

(ii)Loop:

q

i+1=exp

(JT 1JT

iJi)

igi

(0) qi.

(iii)Convergencetest:

Gi(0) = JTigi

(0) <ε.