MM Montiel Dpto Informatica Ingenieria de Sistemas Uni ersidad de Zaragoza Spain Email josemariunizar es Ja vier Ci era Dpto Informatica Ingenieria de Sistemas Uni ersidad de Zaragoza Spain Email jci eraunizar es Andre J Da vison Department of Comput ID: 26297
Download Pdf The PPT/PDF document "Unied In erse Depth arametrization for M..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
UniedInverseDepthParametrizationforMonocularSLAMJ.M.M.MontielDpto.InformaticaeIngenieriadeSistemasUniversidaddeZaragoza.SpainEmail:josemari@unizar.esJavierCiveraDpto.InformaticaeIngenieriadeSistemasUniversidaddeZaragoza.SpainEmail:jcivera@unizar.esAndrewJ.DavisonDepartmentofComputingImperialCollegeLondon.UKEmail:ajd@doc.ic.ac.ukAbstractRecentworkhasshownthattheprobabilisticSLAMapproachofexplicituncertaintypropagationcansucceedinpermittingrepeatable3Dreal-timelocalizationandmappingeveninthe`purevision'domainofasingleagilecamerawithnoextrasensing.AnissuewhichhascauseddifcultyinmonocularSLAMhoweveristheinitializationoffeatures,sinceinformationfrommultipleimagesacquiredduringmotionmustbecombinedtoachieveaccuratedepthestimates.ThishasledalgorithmstodeviatefromthedesirableGaussianuncertaintyrepresentationoftheEKFandrelatedprobabilisticltersduringspecialinitializationsteps.InthispaperwepresentanewuniedparametrizationforpointfeatureswithinmonocularSLAMwhichpermitsefcientandaccuraterepresentationofuncertaintyduringundelayedinitialisationandbeyond,allwithinthestandardEKF(ExtendedKalmanFilter).Thekeyconceptisdirectparametrizationofin-versedepth,wherethereisahighdegreeoflinearity.Importantly,ourparametrizationcancopewithfeatureswhicharesofarfromthecamerathattheypresentlittleparallaxduringmotion,maintainingsufcientrepresentativeuncertaintythatthesepointsretaintheopportunityto`comein'frominnityifthecameramakeslargermovements.Wedemonstratetheparametrizationusingrealimagesequencesoflarge-scaleindoorandoutdoorscenes.I.INTRODUCTIONAmonocularcameraisaprojectivesensorwhichmeasuresthebearingofimagefeatures.Toinferthedepthofafeaturethecameramustobserveitrepeatedlyasittranslatesthroughthescene,eachtimecapturingarayoflightfromthefeaturetoitsopticcenter.Theanglebetweenthecapturedraysisthefeature'sparallaxthisiswhatallowsitsdepthtobeestimated.Incomputervision,thewell-knownconceptofapointatinnityisafeaturewhichexhibitsnoparallaxduringcameramotionduetoitsextremedepth.Astarforinstancewouldbeobservedatthesameimagelocationbyacamerawhichtranslatedthroughmanykilometerspointedupattheskywithoutrotating.Suchafeaturecannotbeusedforestimatingcameratranslationbutisaperfectbearingreferenceforestimatingrotation.Thehomogeneouscoordinatesystemsofvisualprojectivegeometryallowexplicitrepresentationofpointsatinnity,andtheyhaveproventoplayanimportantroleduringoff-lineoptimization-basedstructureandmotionestimationfromimagesequences.Recentresearchhasshownthatthewaytoimproveonoff-linesequenceestimationandachievesequential,repeatablemotionandstructureestimationwithamovingcameraistoadopttheprobabilisticSLAM(SimultaneousLocalizationandMapping)approachofexplicituncertaintypropagationfamiliarfrommobilerobotics.Davison[2]provedthatthestandardEKFformulationofSLAMcanbeverysuccessfulevenwhentheonlysourceofinformationisthevideofromanagilesinglecamera,demonstratingreal-time30Hzmotionandstructureestimationin3D.AsignicantlimitationofDavison'sapproach,however,wasthatitcouldonlymakeuseoffeatureswithincloserangeofthecamerawhichexhibitedsignicantparallax,andwasthereforepracticallylimitedtoroom-scalescenes.Theproblemwasininitialisinguncertaindepthestimatesfordistantfeatures.Acknowledgingthatfeaturedepthuncer-taintyduringinitialisationisnotwell-modelledbyastandardGaussiandistributioninEuclideanspace,Davisonusedaparticleapproachtorepresentafeature'sdepthcoordinateuntilconversiontoGaussianrepresentationwhenthedistributionhadcollapsedsufciently.Asidefrombeingabletodealonlywithfeaturedepthswithinthesmallpre-denedrangealongwhichparticleswerespread(around1to5meters),this`delayed'styleofinitialisationmeantthatobservationsoffeatureswerenotusedtoupdatethecameraposeestimateuntiltheirconversionintofullyinitialisedfeatures.ItwouldberelativelysimpletodealwithpointsatinnityinSLAMifitwereknowninadvancewhichfeatureswereatinnityandwhichwerenot.Thoseatinnitywouldbemodelledwithaspecial`direction'parametrization,ignoringtheirdepth,whilenitefeaturesmaintainedthestandardform.Montiel[8]showedthatinthespecialcasewhereallfeaturesareknowntobeinniteinverylargescaleoutdoorscenesorwhenthecamerarotatesonatripodSLAMinpureangularcoordinatesturnsthecameraintoareal-timevisualcompass.Inthemoregeneralcase,thedifcultyisthatwedonotknowinadvancewhichfeaturesareinniteandwhicharenot.Weshouldclarifythediscussionbydeningthemeaningof`innity'inthecurrentcontext.Ofcoursenoobservablefeatureistrulyinnitelyfarfromthecamera(evenastarofcoursehasanitedepth).Apointatinnityissimplyfarenoughawayrelativetothecameramotionsinceithasbeenobservedthatnoparallaxhasbeenobserved.Letusimagineacameramovingthrougha3Dscenewith observablefeaturesatarangeofdepths.Fromtheestimationpointofview,wecanthinkofallfeaturesstartingatinnityand`comingin'asthecameramovesfarenoughtomeasuresufcientparallax.Fornearbyindoorfeatures,onlyafewcentimetresofmovementwillbesufcient.Distantfeaturesmayrequiremanymetersorevenkilometersofmotionbeforeparallaxisobserved.Itisimportantthatthesefeaturesarenotpermanentlylabelledasinniteafeaturethatseemstobeatinnityshouldalwayshavethechancetoproveitsnitedepthgivenenoughmotion,ortherewillbetheseriousriskofsystematicerrorsinthescenemap.OurprobabilisticSLAMalgorithmmustbeabletorepresentthatuncertaintyindepthofseeminglyinnitefeatures.Observingnoparallaxforafeatureafter10metersofcameratranslationdoestellussomethingaboutitsdepthitgivesareliablelowerbound.Wefeelthatthisconsiderationofuncertainlyinlocationsofpointshasnotbeenpreviouslyrequiredinoff-linecomputervisionalgorithms,butthatnowwehaveamethodfordealingwithitinthemoredifculton-linecase.Ourcontributioninthispaperistoshowthatinfactthereisauniedandstraightforwardparametrizationforfeatureloca-tionswhichcanhandlebothinitialisationandstandardtrackingofbothcloseandverydistantfeatureswithinthestandardEKFframework.AnexplicitparametrizationoftheinversedepthallowsaGaussiandistributiontocoveruncertaintyindepthwhichspansadepthrangefromnearbytoinnity,andpermitsseamlesscrossingovertonitedepthestimatesoffeatureswhichhavebeenapparentlyinniteforlongperiodsoftime.Thefactisthattheprojectivenatureofacamerameansthattheimagemeasurementprocessisnearlylinearinthisinversedepthcoordinate.ThisisaprinciplewhichshouldperhapshavebeennotedsoonerinSLAM,becauseinversedepthisaconceptusedwidelyincomputervision:itappearsintherelationbetweentheimagedisparityandapointdepthinstereovision;itisinterpretedastheparallaxwithrespecttotheplaneatinnityin[4];inversedepthisalsousedtorelatethemotioneldinducedbyscenepointswiththecameravelocityinopticalowanalysis[5],andinStructurefromMotionerroranalysis[9],[1].Theuniedrepresentationmeansthatouralgorithmrequiresnospecialinitialisationprocessforfeatures.Theyaresimplytrackedrightfromthestart,immediatelycontributetoim-provedcameraestimatesandhavetheircorrelationswithallotherfeaturesinthemapcorrectlymodelled.ThatthiscanbeachievedwithinthestandardEKFmeansthatallthegreatbenetsitoffersaremaintainedintermsofhighlyefcientrepresentationofcorrelateduncertainty.WestronglybelievethatEKFmaps,ornetworksofEKFsubmaps,willcontinuetohaveacentralroleinSLAM.Whenparametrizationsarechosencarefully,thereisoftennoneedtouselteringtechniquesusingparticles(e.g.[7])forinstancewhichcanexplicitlyrepresentnon-Gaussiandistributionsbuthavetheirowndisadvantages.NotethatourparameterizationwouldbeequallycompatiblewithothervariantsofGaussianlteringsuchassparseinformationlters.Solaetal.[10]alsorecentlyproposedaninterestingnewapproachtomonocularfeatureinitialization.Intheirwork,anundelayedinitializationofnewpointswasbasedonmain-tainingseveraldepthhypothesesasGaussianvolumesforeachinitializedfeaturespreadinageometricsumadevelopmentoftheparticlemethodofDavisonbuttakingadvantagetosomeextentoftheinversedepthconcept.Astheestimationproceeds,thehypothesesareprunedandanapproximationtotheGaussianSumFilterisproposedkeepthecomputationaloverheadlow.Theirresultsarevalidatedwith2Dsimulationscombiningodometryandvisionandappearimpressive.How-ever,webelievethatourapproachhassignicantbenetsintermsofuniformity,clarityandsimplicity.Further,theymakenoclaimsaboutbeingabletocopewithfeaturesatverylarge`innite'depths.Inveryrecentwork,EadeandDrummondhavepresentedaninversedepthinitialisationschemewithinthecontextoftheirFastSLAM-basedsystemformonocularSLAM[3].Theirmethodwhichsharesmanysimilaritieswithourapproach,andtheyoffersomeofthesameargumentsaboutadvantagesinlinearity.Thepositionofeachnewpartiallyinitialisedfeatureaddedtothemapisparametrizedwiththreecoordinatesrepresentingitsdirectionandinversedepthrelativetothecameraposeattherstobservation,andestimatesofthesecoordinatesarerenedwithinasetofKalmanFiltersforeachparticleofthemap.Oncetheinversedepthestimationhascollapsed,thefeatureisconvertedtoafullyinitialisedstandardEuclideanrepresentation.Whileretainingthedifferentiationbetweenpartiallyandfully-initialisedfeatures,theygofurtherandareabletousemeasurementsofpartiallyinitialisedfeatureswithunknowndepthtoimproveestimatesofcameraorientationviaaspecialepipolarupdatestep.TheirapproachcertainlyappearsappropriatewithinaFast-SLAMimplementation.However,itlacksthesatisfyinguni-edqualityoftheparametrizationwepresentinthispaper,wherethetransitionfrompartiallytofullyinitialisedneednotbeexplicitlytackledandfulluseisautomaticallymadeofalloftheinformationavailableinmeasurements.ItisthiswhichmakesitsuitablefordirectuseinanEKFframeworkforsparsemapping,withalltheadvantagesthatoffersintermsofcompleteandcorrectrepresentationofuncertaintyandcorrelations.Besides,oursystemisabletocodeinthemapdistantpoints,inwhichtheinversedepthcodingnevercollapsesandcannotbecodedwiththestandardEuclideanrepresentation.SectionIIisdevotedtothecameramotionmodel,andtheparametrizationofinversedepthisdetailed.ThemeasurementequationisdescribedinsectionIII,andadiscussionaboutmeasurementequationlinearizationerrorsisincluded.Next,featureinitializationfromasinglefeatureobservationisde-tailedinSectionIV.Thepaperendswithexperimentalvalida-tion(SectionV)overrealimagesequencescapturedat30Hzinlargescaleenvironmentsbothindoorsandoutdoors;linkstomoviesdescribingthesystemperformanceareprovided. II.STATEVECTORDEFINITIONAconstantangularandlinearvelocitymodelisusedtocodethehand-heldcameramotion,sothecamerastatexviscom-posedoflocation:rWCcameraopticalcenter,qWCquaterniondeningorientation;velocityvWandangularvelocity!W:xv=0rWCqWCvW!W1:(1)AteverystepitisassumedanunknownlinearandangularaccelerationzeromeanGaussianprocesses,aWand®W,producinganimpulseoflinearandangularvelocity:n=µVWW¶=µaW¢t®W¢t¶:(2)Thestateupdateequationforthecamerais:fv=0rWCk+1qWCk+1vW+1!W+11=0rWCk+³vW+VW´¢tqWCk£q¡¡!W+W¢¢t¢vW+VW!W+W1(3)beingq¡¡!W+W¢¢t¢thequaterniondenedbythero-tationvector¡!W+W¢¢t.Ascene3Dpointiisdenedbythedimension6statevector(seeFig1):yi=¡xiyiziµiÁi½i¢�(4)whichmodelsa3Dpointlocatedat(seeFig1):0xiyizi1+1½im(µi;Ái):(5)Thestatecodestherayfortherstpointobservationas:xi;yi;zi,thecameraopticalcenterwherethe3Dpointwasrstobserved;andµi;Áiazimuthandelevation(codedintheabsolutereference)fortheraydirectionalvectorm(µi;Ái).Thepointdepthalongtheraydiiscodedbyitsinverse½i=1=di.Thefeaturesyiareconsideredasconstantalongtheesti-mate.Itisassumednounknowninputactingonthefeaturelocation.Thewholestatevectorxisthecomposedofthecameraandallthemapfeatures:x=¡x�;y�1;y�2;:::y�n¢�:(6)III.MEASUREMENTEQUATIONEachobservedfeatureimposesaconstraintbetweenthecameralocationandthecorrespondingmapfeature(seeFig1).TherotationiscodedintherotationmatrixRCW¡qWC¢,de-pendingonthecameraorientationquaternion.TheobservationFig.1.Featureparametrizationandmeasurementequation.ofapointyifromacameralocationdenesarayexpressedinthecameraframeashC=¡hxhyhz¢�:hC=RCW00xiyizi1+1½im(µi;Ái)¡rWC1(7)whichisalmostequivalenttothenextexpressionifcodedwithdi:hC=RCW00xiyizi1+dim(µi;Ái)¡rW1(8)Thedifferenceisthat(7)cancodeapointatinnityusing½i=0,eveninthatcase,(7)canberewrittenas:hC=RCW0½i00xiyizi1¡rWC1+m(µi;Ái)1;(9)analogously,(8)cancodeapointatzerodepthwhilenot(7)nor(9)can.ThecameradoesnotobservedirectlyhC,butitsprojectioninthetheimageaccordingtothepinholemodel.First,theprojectionismodeledonthenormalizedretina:À=hxhz(10)º=hyhz(11)andthenitisappliedthecameracalibrationtoproducethepixelcoordinatesfortheobservedpoint:h=µuv¶=Ãu0¡fdxÀv0¡fdyº!(12) Fig.2.Observationofapointbytwocameras.Thegeometryhasbeendenedwithrespecttotheepipolarplane.Bottomsubgureshowsthesamegeometryasobservedbythecameraswhere,u0,v0arethecameracenterinpixels,fisthefocallengthand,dxanddythepixelsize.Finally,aradialdistortionmodelhastobeappliedinordertodealwithrealcameralenses.Inthisworkwehaveusedthestandardphotogrammetrytwoparametersdistortionmodel[6].Itisworthnoting,thatthemeasurementequationhasasensitivedependencyontheparallaxangle®(seeFig.1).Inourcalibratedcameracontext,theparallaxistheangledenedbythetworaysdenedbythesamescenepointwhenobservedfromtwodifferentviewpoints.Atlowparallax,bothraysarealmostparalleland:½i00xiyizi1¡rWC1+m(µi;Ái)¼m(µi;Ái)whatimpliesthatequation(9)canbeapproximatedby:hC¼RCW(m(µi;Ái))andthemeasurementequationonlyprovidesinformationaboutthecameraorientationandaboutthedirectionalvectorm(µi;Ái).Thisparticularcasehasbeenexploitedin[8]tobuildavisualcompassbasedonSLAM.A.MeasurementequationlinearityWeareusingtheEKFtoestimatethestate.Themorelinearthemeasurementequationis,thebetterperformanceisexpectedfromtheKalmanlter.Next,weshowhowatlowparallaxangles,equation(7),codedin½,improvesthelinearizationwhencomparedwithequation(8),codedind.Becauseofthatweparameterizeontheinversedepth.Wefocusontheobservationofapointfromtwocameralo-cations(seeFig2)C1(absoluteframe)andC2.Thereferencesarealignedwithrespecttotheepipolarplane(denedbythescenepointandthetwocamerasopticalcenters,see[4]foradetailedexplanation)tosimplifythemeasurementequation.TheZaxisisalignedwiththeraydenedbytheopticalcenterandtheobservedpoint.TheYaxisisnormaltotheepipolarplane.GivenapointimagedinC1asxC1itsimageonC2,xC2isconstrainedtobe(ifinfrontofthecameras)ontheepipolarsegmentdenedbytheepipole(theimageofC1onC2)andx1(theimageonxC2ifthescenepointwhereatinnity).Hencethemeasurementequationisdenedby:y=µ0;0;0;0;0;1dc1¶T(13)RC2C1=0cos®0sin®010¡sin®0cos®1(14)rCW=(rx;0;rz):(15)Applyingequation(10)tothetwodifferentparameter-izations,(7)or(8)weobtaincorrespondingmeasurementequationsforthetwoparameterizations:À(½)andÀ(d).Weproposetocomparethetwoparameterizationsintermsoftheirlinearity,rstwefocusonÀ(½)thentheanalysisisextendedtoÀ(d)andnallyacomparisonismade.IfÀ(½)wereperfectlylinearin½,then@À@½shouldbeaconstant,modeling½asGaussian,itsvariationaroundthelinearizationpoint½0isexpectedtobeintheinterval[½0¡2¾½;½0+2¾½].Nextweanalyzetherstderivativechangeinthatinterval.Arstorderapproximationfortherstderivativeintheinterval[½0¡2¾½;½0¡2¾½]isgivenbytherstorderTaylorexpansionaround½0:@À@½(½0+¢½)¼@À@½¯½0+@2À@½2¯½0¢½:(16)Weproposetousethedimensionlessratiobetweenthederivativeincrementattheintervalextreme@2À@½2¯½02¾½andthederivativeinthelinearizationpoint@À@½¯½0asalinearitymeasurement.So:@2À@½22¾½@À@½¼0(17)inordertohaveanacceptablelinearization.Wecomputethedimensionlessratioforthe½parametriza-tion:2¾½½02µ1¡dC1dC2cos®¶¼0(18)Whichsaysthat,atlowparallax,andwhendC1dC2¼1,theterm³1¡dC1dC2cos®´¼0andlowlinearizationerrorcanbeachievedevenif2¾½½0À0.SohugeinitialuncertaintyregionscanbecodedGaussianly.Forexample,considering®=5±¾½=0:5;½0=0:5thecodedacceptanceregionextendsfrom[0:67;1],andtheratioisonly0:8%.Whentheparallaxangleincreases,³1¡dC1dC2cos®´alsoincreases,buttheuncertaintyin½reducesandhence2¾½½isreducedandcondition(18)isfullledevenwithmoderateorhighparallaxangles.Whenwecompute(17)forthedparametrization:2¾ddC2(2cos®)¼0(19) Fig.3.Simulationofapointreconstructionfromtwolowparallaxobservations.Itisshowhowthereconstructionerrorcodedin½;µisGaussianwhilecodedascartesianXZisnotGaussian.RedellipsesrepresentlinearuncertaintypropagationfromtheraysGaussianerrorso,atlowparallax,cos®¼1,andhenceagoodlinearizationcanbeachievedonlyif:2¾ddC2¼0)¾d¿dC2(20)whichmakesdifcultcodinghugeinitialuncertaintyregions.Forexample,®=5±;dC1=20;¾d=10codeanacceptanceinterval[0;40]andtheratiois200%.Asanexampleoftheimprovementinthemeasurementequationlinearization,gure3showsasimulationofalowparallax(0:5±)pointreconstructionwhenobservedbytwocamerasatknownlocations.ThecamerasobservetherayswithaGaussianerror,¾=0:1±.Itisshownthe3DpointreconstructionmodeledwithXZcartesiancoordinatesorwith½;µcoordinates.The95%uncertaintyregionpropagatedfromtheimageerrorisplottedaswell.ItisshowntheGaussianityin½;µbutnotinXZ.IV.FEATUREINITIALIZATIONItisaremarkablequalityofourproposalthatnewfeaturesareinitializedusingonlyoneimage,theimagewherethefeatureisrstobserved;theinitializationincludesboththefeaturestateinitialvaluesandthecovarianceassignment.Despitetheinitialuncertaintyregioncoversahugerangedepth([1;1]inourexperiments)becauseofthelowlinearizationerrors(18)theuncertaintyissuccessfullycodedasGaussian;onceinitialized,thefeatureisprocessedwiththestandardEKFprediction-updateloop.Itisworthnoting,thatthankstotheproposedparametriza-tion,whilethefeatureisobservedatlowparallax,thefeaturewillbeusedmainlytodeterminethecameraorientationbutthefeaturedepthwillbekeptquiteuncertain,includinginitsuncertaintyregiontheeveninnity;ifthecameratranslationisabletoproduceaparallaxbigenoughthenthefeaturedepthestimationwillbeimproved.Theinitiallocationfortheobservedfeatureisdenedas:^y³^rWC;^qWC;h;½0´=¡^xi^yi^zi^µi^Ái^½i¢�(21)fromthecameralocationestimateatstepk(thekindexeshavebeendroppedforsimplicity),andtheobservationofanewfeature:h=¡uv¢�and,theinitial½0.Theprojectionrayinitialpoint(seeFig1)isdirectlytakenfromthecurrentcameralocationestimate:0^xi^yi^zi1=^rWCkjk(22)Theprojectionraydirectionalvectoriscomputedfromtheobservedpoint,expressedintheabsoluteframe:hW=RWC³qWCkjk´hC0Àº11(23)beingÀandºtheimageinthenormalizedretina.DespitebeinghWanon-unitarydirectionalvector,theanglescanbederivedas:µµiÁi¶=0arctanµ¡hW;qhW2+hW2¶arctan³hW;hW´1(24)Thecovariancefor^xi;^yi;^zi;^µi,and^ÁiisderivedfromtheimagemeasurementerrorcovarianceRjandthestatecovarianceestimate^Pkjk.Theinitialvaluefor½0isderivedheuristicallytocoverinits95%acceptanceregionaworkingspacefrominnitytoapredenedclosedistance,dminexpressedasinversedepth:h1dmin;0i,so:^½0=½min2¾½=½min4½min=1dmin:(25)Inourexperimentsdmin=1;^½0=0:5;¾½=0:25.Thestatecovarianceafterfeatureinitializationis:^Pnewkjk=J0^Pkjk000Rj000¾2½1J�J=ÃI0@y@rWC;@y@qWC;0;:::;0;@y@h;@y@½!V.EXPERIMENTALRESULTSTheperformancehasbeentestedonrealimagesequencesacquiredwithhand-heldlowcostUnibrainIEEE1394camera,witha90±eldofviewand320£240resolutionmonochromeat30fps.OurcurrentexperimentsareruninMatlab;howeverwebelievethat30Hzperformancecouldbeachievedinrealtime.CurrentC++implementationsformonocularSLAMwithdimension3foreverypointfeaturecanrunat30Hz.formapsupto100features.Ourfeatureisdimensionsix.Howeveroursystemofferscomputationalloadadvantages:i)thesimplefeatureintializationischeaperthanthecurrentapproaches.ii)Severalfeaturescaninitializedfromaframeandrotationinformationisobtainedfromthesecondtimeafeatureis Fig.4.First(a)andlast((b)imagesofthesequence.Todisplayamapthatcontainsfeaturesatverydifferentdepths,twotopviewsatdifferentscalesareplotted.Thetopviewplottedatbottomleftsubguredisplaystheclosefeatures;thetopviewplottedatthebottomrightsubguredisplaysthedistantfeatures.BothtopviewscompareourinversedepthGaussianparametrizationwiththestandardXYXGaussianparametrizationbythecomparisonoftheiruncertaintyregions.TheGaussianinversedepthacceptanceregionsareplottedinXYZasacloudofblackdotsnumericallypropagatedfromtheGaussian6dimensionalsuperellipsoidalacceptanceregioncodedininversedepth.ThestandardGaussianXYZacceptanceellipsoidsarelinearlypropagatedfromthe6dimensionalGaussiancodedininversedepthbymeansoftheJacobian.Thecameratrajectoryanditsuncertaintyisshowninblue.Attheinitialstep(a),mostthefeaturesareatlowparallax.Atthenalstep(b),parallaxenoughhasbeengatheredforthemajorityofthefeaturesandthefeatureuncertaintyislow.observed,becauseofthatthesearchregionsformatchesarereducedandhencetheprocessingtimeisreduced.iii)whenthefeaturesareobservedwithamoderateparallax,thefeaturescanbecodedwithadimension3XYZstate.Soweexpecttoachieverealtimeperformanceat30Hz.forreasonablemapsizes.Therstexperiment,isa500framesmovieofalecturetheater.Thesecondexperimentis870framesmovieofanoutdoorsscenewherecloseobjectstemporarilyoccludedistantfeatures.IndoorsequenceThemovieshowingtheinputsequenceandtheestimationhistorycanbereachedathttp://webdiis.unizar.es/%7Ejosemari/in.aviThepurposeoftheexperimentwastoanalyzetheperfor-manceinanenvironmentwithfeaturesatdifferentdepths.Weparticularlyanalyzeinitializationforthreefeaturesinitializedinthesameframebutlocatedatdifferentdepths.Figure4showstheimagewheretheanalyzedfeaturesareinitialized(frame18inthesequence)andthelastimageinthesequence;thetopviewofthemapwiththefeaturecovarianceisplottedaswell.Todisplayamapthatcontainsfeaturesatverydifferentdepths,twotopviewsatdifferentscalesareplotted.Thetopviewplottedatbottomleftsubguredisplaystheclosefeatures;thetopviewplottedatthebottomrightsubguredisplaysthedistantfeatures.BothtopviewscompareourinversedepthGaussianparametrizationwiththestandardXYXGaussianparametrizationbythecomparisonoftheiruncertaintyregions.TheGaussianinversedepthac-ceptanceregionsareplottedinXYZasacloudofblackdotsnumericallypropagatedfromtheGaussian6dimensionalsuperellipsoidalacceptanceregioncodedininversedepth.ThestandardGaussianXYZacceptanceellipsoidsarelinearlypropagatedfromthe6dimensionalGaussiancodedininversedepthbymeansoftheJacobian.Atthebeginningofthesequence,thedepthuncertaintyishuge,evenincludingtheinnity,duetothesmalltranslation,noparallaxisobservedinthefeatures.ItisworthnotingthatGaussianityininversedepthisnotmappedtoaGaussianinXYZ,sotheredellipsoidsarefarfromrepresentingtheXYZdistributionerror,especiallyindepth.Asstatedbyequation(18),isatlowparallaxwhentheinversedepthparametrizationplaysakeyrole.Asthecameramoves,thetranslationproducesparallax,thefeaturesdepthestimateimproves,sointhelastimage,mostofthemapfeatureshavereducedtheiruncertainty.AsaresulttheboththeuncertaintyinXYZandininversedepthareGaussianandtheblackandthereduncertaintyregionsbecomecoincident.Figure5focusontheevolutionoftheestimatecorrespond-ingtofeatures11,12and13atframes1,10,25,50,100and200countedsincefeatureinitialization.IntopviewitisplottedboththeXYZGaussianuncertainty(redellipsoid)andtheregionininversedepth(blackdots);theparallaxforeachfeatureateverystepisalsodisplayed.Wheninitialized,the½Gaussian95%acceptanceregionincludes½=0sotheinniteisconsidered.Thecorrespondingacceptanceregionindepthisquiteasymmetric,excludinglowdepthsbutthatextendsathighdepthdowntoinnity,andevennegativedepthscorrespondingtonegative½(negativedepthsarenotrepresented).Asraysproducingbiggerparallaxaregathered,theuncertaintyin½becomesnarrowerbutstillmapstoanonGaussiandistributioninXYZ.Eventually,both½andXYZ Fig.5.Featureinitialization.Everyrowshowstheevolutionofafeatureestimationintopview.Pereachfeature,theestimationafter1,10,25,50,100and200framessinceinitializationareplotted;theparallaxbetweentheinitialobservationandthecurrentframeisdetailedontopofeverysubplot.Blackdotsareanumericalrepresentationforthe95%uncertaintyregiongaussianintheinversedepth.TheredellipsoidistheuncertaintyregioncodedasGaussianinXYZ.gionsbecamebothnarrowandGaussianbecauseenoughparallaxisavailable.Letusfocusonthedistantfeatures.Thecameratrans-latesafterinitializationbutthistranslationdoesnotproduceparallaxbecausethefeatureisdistant.Thisinformationiscodedin½shiftingitsvaluetowardszeroandnarrowingitsuncertainty;intheXYZspacethisimplieshavingstillanasymmetricalacceptanceregionbutthatnowexcludesthelowdepths.Intuitively,ifthecamerahastranslatedandnoparallaxhasbeendetected,thentheobservedfeaturecannotbeclose,soevenifthedepthcannotbeestimatedbecausethefeatureisdistant,someinformationaboutitsdepthhasbeencodedintheestimate.Astheestimationproceeds,whenenoughparallaxiseven-tuallyavailable,theestimationevolvestoanarrowGaussianin½thatwhentransformedtoXYZcutsdowntheprobabilitycorrespondingtohighdepthscollapsingnallytoaGaussianestimatebothininversedepthandinXYZ.B.OutdoorsequenceGiventhesystemabilitytodealwithbothcloseanddistantfeatures,ithasaniceperformanceoutdoors.Thewholeexper-imentsequencealongwiththeestimatedmapcanbereachedathttp://webdiis.unizar.es/%7Ejosemari/out.avi.Figure6showsthreeframesofthemovieillustratingtheperformance.Itdisplaysaswellthemapafterprocessingthewholemovie.AsinSectionV-A,themaprepresentedbytwotopviewsatdifferentscales.Twooftheproblemsthathavetobetackledoutdoorsaredistantfeaturesandpartialocclusionduetothefactthatthereareobjectsatquitedifferentdepthsdisplayingratherdifferentparallaxasthecameramoves.Formostofthefeatures,thecameraendsupgatheringenoughparallaxtoestimatetheirdepth.However,beingout-doors,thereareratherdistantfeaturesproducingnoparallax.Itshownhowdistantfeatures,e.g24or39,inthebuildingsatthebackgroundarepersistentlytrackedalongthesequence;howeverthedepthcannotbeestimated.TheestimationerrorcodedasgaussianininversedepthissuccessfullymanagedbytheEKF,andthefeaturesbehavesaspointsatinnity.ItcanbenoticedaswellthepoorerrorrepresentationifcodedasGaussianinXYZ.Regardingpartialocclusion,ThesignaledfeatureinFig6,labeledas36,showsthesystemabilitytoreobservefeatures,fromadifferentpointofviewafterlongpartialocclusion.VI.CONCLUSIONWehavepresentedaparametrizationformonocularSLAMwhichpermitsoperationbaseduniquelyonthestandardEKFprediction-updateprocedureateverystep,unifyinginitializa-tionwiththetrackingofknownfeatures.Ourinversedepthparametrizationfor3Dpointsallowsuniedmodellingandprocessingonforanypointinthescene,closeordistant,orevenat`innity'.Infact,close,distantorjust-initializedfeaturesareprocessedwiththeroutineEKFprediction-updateloopwithoutmakinganybinarydecisions.Thekeyfactoristhatduetotheinversedepthparametriza-tionourmeasurementequationhaslowlinearizationerror Fig.6.Subgures(a)and(b)displayframes197and454,showinghowsceneswithobjectsatquitedifferentdistancesarelikeytoproducepartialocclusion.Thesystemcannicelyreobservethemaftertheocclusionasshowninthesignaledfeature(labeledas36)onthetreebasis.Subgure(c)Showsthesystemabilitytotracksuccessfullydistantfeaturesalonghundredsofframes,beingGaussianinlambdabutnotGaussianinXYZ.Thelinespairstheimageofthefeatureswiththetopviewreconstruction.atlowparallax,andhencetheestimationuncertaintyisac-curatelymodeledasGaussianininversedepth.InSectionIII-Awepresentedasimpliedmodelwhichapproximatelyquantiesthelinearizationerror.ItprovidesatheoreticalunderstandingoftheimpressiveperformanceoftheEKFwiththeproposedparametrization.Theinversedepthparametrizationimpliesadimension6statevectorperfeaturecomparedtodimension3forEuclideanXYZcoding.Thisdoublesthethesizeofthemapstatevector,andhenceproducesa4-foldincreaseincomputationalcostifallfeaturesretainthenewparametrization.However,ourexperimentsshowthattheuncertaintiesinclosefeaturelocationscollapseafterseveralframestoaccurateGaussiandistributionsinEuclidean3Dspace,indicatingtheopportunitytosafelyconvertthesefeaturesbacktoanXYZparametriza-tionandreturntodimension3,meaningthatthelong-termcomputationalcostwouldnotsignicantlyincrease.Further,however,thevalueofimmediateinitializationthatthenewparametrizationprovidesmeansthatrightthroughtrackingtheamountofuncertaintyinthesystemwillbelower(removingjitterfromcameraposeestimation)andthiswillleadtocomputationalbenetsintermsofsmallersearchregionsandimprovedimageprocessingspeed.Theexperimentspresentedhavevalidatedthemethodwithrealimagery,usingahand-heldcameraastheuniquesensorbothindoorsandoutdoors.Ourcurrentexperimentshavebeenrunoff-lineprogrammedinMatlab,butwearecondentinachievingreal-timeperformanceinC++inthenearfuturefornumbersoffeaturesuptoperhaps100usingcurrentPChardwareenoughtomaplargeroomsorpartsofoutdoorscenesinpracticalscenarios.ACKNOWLEDGMENTThisresearchwassupportedbySpanishCICYTDPI2003-07986,EPSRCGR/T24685,EPSRCAdvancedResearchFellowshiptoAJD,andRoyalSocietyInternationalJointProjectgrantbetweenU.ofOxford,U.ofZaragozaandImperialCollege.WeareverygratefultoDavidMurray,IanReidandothermembersofOxford'sActiveVisionLaboratoryfordiscussionsandsoftwarecollaboration.REFERENCES[1]A.ChowdhuryandR.Chellappa.Stochasticapproximationandrate-distortionanalysisforrobuststructureandmotionestimation.IJCV,55(1):2753,2003.[2]A.Davison.Real-timesimultaneouslocalizationandmappingwithasinglecamera.InProc.InternationalConferenceonComputerVision,2003.[3]E.EadeandT.Drummond.ScalablemonocularSLAM.InInProceedingsoftheIEEEConferenceonComputerVisionandPatternRecognition,2006.[4]R.I.HartleyandA.Zisserman.MultipleViewGeometryinComputerVision.CambridgeUniversityPress,ISBN:0521540518,secondedition,2004.[5]D.HeegerandA.Jepson.Subspacemethodsforrecoveringrigidmotioni:Algorithmandimplementation.IJCV,pages95117,1992.[6]E.Mikhail,J.Bethel,andM.J.C.IntroductiontoModernPhotogram-metry.JohnWiley&Sons,2001.[7]M.Montemerlo,S.Thrun,D.Koller,andB.Wegbreit.FastSLAM:Afactoredsolutiontothesimultaneouslocalizationandmappingproblem.InProceedingsoftheAAAINationalConferenceonArticialIntelligence,Edmonton,Canada,2002.AAAI.[8]J.MontielandA.J.Davison.AvisualcompassbasedonSLAM.InProc.Intl.Conf.onRoboticsandAutomation,2006(accepted).[9]J.Oliensis.Amulti-framestructure-from-motionalgorithmunderperspectiveprojection.IJCV,34(2):163192,1999.[10]J.Sola,A.Monin,M.Devy,andT.Lemaire.UndelayedinitializationinbearingonlySLAM.In2005IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems,2005.