Nonlinear Least Squares Optimisation of Unit Quaternion Functions for Pose Estimation from

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

InProc.14thInt.Conf.PatternRecognition,Brisbane,Australia,pp.425-427,August1998.

NonlinearLeastSquaresOptimisationofUnitQuaternionFunctionsforPose

EstimationfromCorrespondingFeatures

AleˇsUde

JoˇzefStefanInstitute,DepartmentofAutomatics,BiocyberneticsandRobotics

Jamova39,1000Ljubljana,Slovenia,E-mail:ales.ude@ijs.si

Abstract

Poseestimationfromanarbitrarynumberof2-Dto3-Dfeaturecorrespondencesisoftendonebyminimisinganonlinearcriterionfunctionusingoneoftheminimalrep-resentationsfortheorientation.However,therearemanyadvantagesinusingunitquaternionstorepresenttheori-entation.Unfortunately,astraightforwardformulationoftheposeestimationproblembasedonquaternionsresultsinaconstrainedoptimisationproblem.Inthispaperweproposeanewmethodforsolvinggeneralnonlinearleastsquaresoptimisationproblemsinvolvingunitquaternionfunctionsbasedonunconstrainedoptimisationtechniques.Wedemonstratetheeffectivenessofourapproachforposeestimationfrom2-Dto3-Dlinesegmentcorrespondences.

1.Introduction

Theobjectposeisde nedasthedisplacementoftheco-ordinateframerigidlyattachedtotheobjectfromitsini-tialposition,whereitisalignedwiththeworldcoordinateframe,toitscurrentposition.Thereexistanalyticalandlin-earsolutionstotheproblemofposeestimationfrom2-Dto3-Dfeaturecorrespondences[1,2],buttheyaresensitivetonoise.Inthepresenceofnoise,whichisunavoidableinreal-worldapplications,algorithmsbasedonnonlinearop-timisationmethodsgivemoreaccurateresults.

Nonlinearoptimisationtechniqueshavebeenusedforposeestimationbymanyresearchersinthepast.Agoodoverviewisgivenin[1].Inmostoftheseapproaches,Eu-ler’sangleswereusedtoparameterisethegroupofrotationsSO(3)oftheEuclideanspace.However,itiswellknownthatSO(3),whichisathreedimensionalmanifold,cannotbegloballyembeddedinthethreedimensionalEuclideanspace.Itfollowsthatiftherotationgroupisrepresentedbythreerealparameters,theEuclideanmetrictopologyin

Currently,

theauthoriswiththeKawatoDynamicBrainProject,ER-ATO,JapanScienceandTechnologyCorporation,2-2HikaridaiSeika-cho,Soraku-gun,Kyoto619-0288,Japan,e-mail:ude@erato.atr.co.jp.

R3doesnotinduceaglobaltopologyandmetricstructureinSO(3).Thissuggeststhatcommonsolutionsusingminimalrepresentationsoftherotationgrouparenotideal.

Therepresentationoftherotationgroupbyunitquater-nions,whichformasphereS3inR4,hasmanyadvantagesoverminimalrepresentations.Methodsforposeestimationbasedonthequaternionrepresentationoftheorientationhavebeenproposedintheliteraturebefore[1],buttheposeestimationproblemhasbeenformulatedasanoptimisationprobleminR4ratherthanonS3intheseapproaches.

2.Preliminaries

3

Inthefollowingweshallneedtheexponentialmapexp:R→S3,which isgivenexp(r)=

by

cos( r ),sin( r )

r

,r=0.(1)(1,0,0,0),r=0

r

Theexponentialmaptransformsatangentvectorr∈R3T∈S3≡1(S3)intoq,whereqisapointatdistance r from1alongageodesiccurvestartingfrom1inthedirectionofr[3].Geodesicsarede nedasshortestpathsconnectinganytwopointsonamanifold3(sphereS3).Itturnsoutthatforanyotherpointq∈Sandforanyr∈T3S)3≡R3andtheexponentialq∈Tmapatq(S3),rq,expq qT(S3)1(S→,havingtheabovepropertiesisgivenby

q:qexpq(rq)=exp(rq q) q,

(2)

where denotesthequaternionmultiplication.

Letsconsidertheproblemofposeestimationfrom2-Dto3-Dlinesegmentcorrespondences.Letm(x1k,x2

k=

k),k=1,...,N,betheend-pointsofthek-th3-Dlinesegmentbelongingtotheobject’smodelandletAj,j=1,...,M,betheprojectivemappingontothej-thimageplane.Thesemappingsshouldbemadeavailablebyacameracalibrationprocedure.Letfdenotethemappingwhichtransformstheend-pointrepresentationofa2-Dlinesegmentintoitsmid-point f(v,vvTT

representation T12)=1+v22,arctan

y2 y1

x2 x1

,(3)

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) <ε.

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

Figure1.Stereoimagepairshowingthelineseg-mentsextractedfromoneimageandtheedgesofthelocalisedobjectsprojectedontotheotherimageOurgoalistodevelopamethodfortheminimisationof(5)over(t,q)∈R3×S3.Toachievethiswewrite(5)as

13 MN2hk(ti+d,exp(ω) qi)2=1

gi(d,ω)Tgi(d,ω).k=1

2(15)

Usingthesameapproachasinthecaseofpureunitquater-nionfunctions,theGauss-NewtoniterationonR3×S3canbeformulatedasfollows

ti+1

=ti+di,

qi+1=exp(ωi) qi,

(16)

dTi

ωi

T

T=

(JTiJi)

1JTigi

(0,0),whereJiistheJacobianofgiat(0,0).Thisiterationgen-eratesasequencewhichisguaranteedtolieinthesearch

spaceR3×S3.Sincetherearenoconversionsoforien-tationinsomeforeignform,suchasEuler’sangles,toaquaternionform,thenon-uniquenessofthequaternionrep-resentationdoesnotcauseanyproblems.SincethemetricstructureofSO(3)isthesameastheoneofS3,theaboveiterationmaybeviewedasaniterationinR3×SO(3).

4.Experimentalresultsandconclusions

Tovalidatetheproposedmethodexperimentally,weuseditforthecalculationofobjects’posesfromlineseg-mentcorrespondencesinasystemforobjectrecognitionandlocalisation(seeFig.1).TheconvergenceofthemethodisshowninTab.1.Evenwhenthestartingpointwasveryinaccurate,theGauss-Newtonmethodconvergedtothetrueobjectposeprovidedthefeaturecorrespondenceswerecorrect.HencethereisnoneedtousemorerobusttechniquesliketheLevenberg-Marquardtmethodforposeveri cationincalibratedstereoimages.Assumingthatfea-turecorrespondencesarecorrectandthemeasurementnoiseismoderate,thecriterionfunction(15)tendstozerointheneighbourhoodofthesolutionandtheconvergenceofthemethodisnearlyasgoodastheconvergenceoftheNewton-Raphsoniteration.

Table1.Convergenceofthemethod.TheEuclideanandtheangularmetricwereusedtomeasurethechangeinthepositionandorientation,respectively.

Step(trans.)Step(orien.)4.962116e+012.827351e-011.095884e+01

7.604011e-022.851983e-012.920441e-037.462760e-041.835331e-052.017427e-06

1.241607e-07

Furtherexperimentsshouldbecarriedouttotestthemethodforthecasewhenonlyonecameraisavailable.Thepresentedmethodisgeneralandworkswithanykindoffea-tures.Theusageoflineorpointcorrespondencesrequiresonlyarede nitionoffunctionf,whichisde nedforthecaseoflinesegmentsinEq.(3).

ComparingourapproachwiththeoneofPhongetal.[4],whoalsousedquaternionstorepresenttheorientation,ouriterationhastheadvantagethatitsearchesfortheoptimalorientationdirectlyinS3andnotinR4asthemethodofPhongetal.Phongetal.hadtointroduceapenaltytermtoforcetheminimumoftheircriteriontotendtowardsS3.However,thispenaltytermrequiresthesettingofauser-de nedparameterwhichisatbestarbitraryandcancauseproblemswiththeconvergenceoftheiteration.Ourmethoddoesnotsufferfromthisproblem.Moreover,itispossibletodeterminethesearchdirectionusingthetrustregionap-proachofPhongetal.inouriterationandthusmakeitlesssensitivetothequalityofastartingpointandwrongcorre-spondences.Thiswasnotnecessaryforourapplication.Acknowledgment:ThemeasurementsweretakenwhiletheauthorwaswiththeInstituteforReal-TimeComputerSystemsandRobotics,UniversityofKarlsruhe,Germany.

References

[1]R.L.CarceroniandC.M.Brown.Numericalmethodsfor

model-basedposerecovery.TechnicalReport659,ComputerScienceDepartment,TheUniversityofRochester,Rochester,NewYork,August1997.

[2]B.K.P.Horn.Closed-formsolutionofabsoluteorientation

usingunitquaternions.J.Opt.Soc.Am.A,4(4):629–642,1987.

[3]M.-H.Kyung,M.-S.Kim,andS.-J.Hong.Anewapproach

tothrough-the-lenscameracontrol.GraphicalModelsandImageProcessing,58(3):262–285,May1996.

[4]T.Q.Phong,R.Horaud,A.Yassine,andD.T.Pham.Object

put.Vis.,15:225–243,1995.

本文来源：https://www.bwwdw.com/article/lb21.html