第1页 / 共7页
第2页 / 共7页
第3页 / 共7页
第4页 / 共7页
第5页 / 共7页
第6页 / 共7页
第7页 / 共7页
Fast and Robust Initialization for Visual-Inertial SLAM.pdf
2019 International Conference on Robotics and Automation (ICRA)
Palais des congres de Montreal, Montreal, Canada, May 20-24, 2019
978-1-5386-6026-3/19/$31.00 ©2019 IEEE
1288
FastandRobustInitializationforVisual-InertialSLAMCarlosCampos,Jos´eM.M.MontielandJuanD.Tard´osAbstract—Visual-inertialSLAM(VI-SLAM)requiresagoodinitialestimationoftheinitialvelocity,orientationwithrespecttogravityandgyroscopeandaccelerometerbiases.InthispaperwebuildontheinitializationmethodproposedbyMartinelli[1]andextendedbyKaiseretal.[2],modifyingittobemoregeneralandefficient.Weimproveaccuracywithseveralroundsofvisual-inertialbundleadjustment,androbustifythemethodwithnovelobservabilityandconsensustests,thatdiscarderroneoussolutions.OurresultsontheEuRoCdatasetshowthat,whiletheoriginalmethodproducesscaleerrorsupto156%,ourmethodisabletoconsistentlyinitializeinlessthantwosecondswithscaleerrorsaround5%,whichcanbefurtherreducedtolessthan1%performingvisual-inertialbundleadjustmentaftertenseconds.I.INTRODUCTIONVisual-InertialSLAMstandsforthosetechniquesabletobuildamapandsimultaneouslylocateanagentinsidethemap,usingasinputacameraandanInertialMeasurementUnit(IMU)[3][4][5].Bothsensorsrequiretobeinitializedorcalibratedbeforeusingthem.Whilecameraiscalibratedjustonceasitdoesnotevolvewithtime,IMUhastobeinitializedbeforeeveryuse.Inertialinitializationaimstocomputevaluesfortheinitialvelocity,gravitydirectionandgyroscopeandaccelerometerbiasesthatcanbeusedasinitialseedfortheVisual-InertialSLAMestimationprocess,guaranteeingitsconvergence.Oncethesevaluesarefound,inertialmeasurementscanbeusedtoimprovetracking,makingitmorerobust,aswellastofindthetruescaleofthemap,whichcannotbeobtainedwithpuremonocularsystems.SometechniquesinitializefirstthemonocularSLAMpart,buildingamapup-to-scale,andthen,enoughtimelaterforhavinggoodobservabilityofallvariables,computescalefactor,gravitydirectionandIMUbiases.Thistechnique,firstproposedbyMur-ArtalandTard´os[5]andlatteradaptedin[6],givesveryaccurateresults.Inparticular,thetruescaleoftheenvironmentcanbeestimatedwithanerroraround1%.However,thisapproachhastwomaindrawbacks:•Dependencyonvisualinitialization.Itrequirestohaveaninitialmapup-to-scalewithenoughmap-points.Ifthevisualpartisnotabletoinitialize,orifitgetslostwithinashortperiodoftime,theinertialsystemwillnotinitialize.•Initializationtakestoolong.ThemethodrequirestorunmonocularSLAMforaroundfifteensecondstosafelyfindthecorrectinertialparametersforthevisual-inertialThisworkwassupportedinpartbytheSpanishgovernmentundergrantsDPI2015-67275andDPI2017-91104-EXP,theArag´ongovernmentundergrantDGAT45-17R,andbyHuaweiundergrantHF2017040003.TheauthorsarewithInstitutodeInvestigaci´oneningenier´ıadeArag´on(I3A),UniversidaddeZaragoza,Spaincampos@unizar.es;josemari@unizar.es;tardos@unizar.esbundleadjustment(BA).Thistimeistoolongformanyapplications.Martinelli[1]proposedaveryinterestingclosedformmethodthatonlyrequirestotrackafewpoints,andsolvesjointlyforthecameramotion,theenvironmentstructure,andtheinertialparameters.Thepaperincludedatheoreticalanalysisoftheconditionsfortheproblemtobesolvableandpresentedsimulatedresults.TherecentworkofKaiseretal.[2]extendsthemethodtoalsoestimategyroscopebias,andpresentsresultswithrealdatafromadrone,showingthatitispossibletoinitializeinlessthan3seconds,withrelativeerrorsaround15%onspeedandgravity.Thisinitializationmethodhasseverallimitations:•Itassumesthatallfeaturesaretrackedinallframes,andthatalltracksprovidedarecorrect.Incaseofspurioustracks,itcanprovidearbitrarilybadsolutions.•Theinitializationaccuracyislow,comparedto[5].Toimproveit,alotoftracksandframesareneeded,increasingitscomputationalcost,andmakingitunfea-sibleinrealtime.•Withnoisysensors,trajectoriesthatareclosetotheunsolvablecasesanalyzedin[1]giveweakobservabilityofsomeofthevariables.Themethodlacksrobustcriteriatodecidewhenaninitializationisaccurateenough.InthispaperwebuildontheMartinelli-Kaisersolution[1][2](orsimplyMK-solution),modifyingittobemoregeneral,efficient,robustandprecise.Themainnoveltiesofourinitializationalgorithmare:1)Generality:wegeneralizethemethodtousepartialtracksandtotakeintoaccountthecamera-IMUrelativepose.2)Efficiency:wereducetherunningtimebyusingafixednumberofmfeaturesandnkeyframescarefullycho-sen,andadoptingthepreintegrationmethodproposedin[7][8].3)Observabilitytest:afterMK-solution,weperformvisual-inertialBAwiththempointsandnkeyframes,andapplyanovelobservabilitytesttochecktheaccu-racyofthesolution.Ifthetestfails,theinitializationisdiscarded.4)Consensustest:wetrytoinitializeadditionaltrackedfeaturesbytriangulatingthe3Dpointandcheckingthereprojectionerror.Ifenoughconsensusisfound,weperformasecondvisual-inertialBAwithallpointsandkeyframes,andinitializetheSLAMmapwiththem.Otherwise,theinitializationisdiscarded.WepresentexhaustiveinitializationtestsontheEuRoC
1289
dataset[9]showingthatthemethodisabletoconsistentlyinitializeinlessthan2swithascaleerrorof5%.Thiserrorconvergesto1%performingvisual-inertialBAafter10s,whenthetrajectoryislongenoughtogivegoodobservabilityofallvariables.II.INITIALSOLUTIONA.FeatureextractionandtrackingWeusethemultiscaleORBextractor[10]tofindMuniformlydistributedpointstobetracked.Aspointswithhighresolutionarepreferable,weonlyextractORBpointsatthethreefinestimagescales,usingascalefactorof1.2.WehavefoundthattheextractionofFASTfeaturesandmatchingwithORBdescriptorsisnotstableenoughtoobtainlongtracks,asmostofthefeatureswerelostinsomeoftheframes.Hence,tosolvethisproblem,wehaveperformedfeaturetrackingusingthepyramidalimplementationoftheLucas-Kanadealgorithm[11]availableinOpenCV.However,simpletrackingisnotenoughsincetherecouldbesometrackswhicharenotcorrect,particularlyinareasofrepetitiveorlowtexture.Toattackthisproblem,thegeometricconsistencyofthesetracksischeckedbyfindingafundamentalmatrixusingRANSAC.ThecombinationofORBkeypoints,Lucas-Kanadetracking(KLT),andthefundamentalmatrixcheck,leadstolongtracksforahighnumberoffeatures.WhensometrackislostorrejectedbytheRANSACalgorithm,newORBpointsareextractedandstarttobetracked,inordertokeepaconstantnumberofMtracks.Eachtimewedetectthatthereareatleastmtrackedpointswithatracklengthofatleastlpixels,welaunchourinitializationmethod.Thisfirsttesttodecidewhethertoattemptinitializationornotiscalledtrack-lengthtest.B.ModifiedMartinelli-KaisersolutionInthispartweextendthemethodproposedin[1][2]tobeabletodealwithfeaturesnotseenbyallcameras,andcomputingtermsinanefficientwayusinginertialpre-integrationproposedby[7][8].Letmbethenumberoftrackedfeaturesusedforinitialization,placedat(x1...xm)intheglobalreferenceframe,andC={C1...Cn}thesetofncamerasindexesusedforinitialization,alsoreferredtoaskeyframes,whicharechosentobeuniformlydistributedalongtime.Let’salsocallCi={C1i...Cni}thesetofnicamerasindexesseeingfeaturei-th.Hereweremarkthat,incontrastto[1],inourformulationnotallfeatureshavetobeobservedbyallcameras,andthetransformationfromcameratobody(IMU),obtainedfromcalibration,istakenintoaccount.Withoutlossofgeneralityweuseasglobalreferencethefirstbodyposeusedforinitialization.Fromfigure1,wecanwritethesetofequalities:p1i+R1itBC+λi1iui1i=pj+RjtBC+λijuiji=1...m,∀j∈Ci\1i(1)where:•pj∈R3:positionofthej-thbodyintheglobalreferenceframe.Fig.1:RelationshipsbetweentwoBody(IMU)andCameraposesobservingthesamefeature.•Rj∈SO(3):rotationmatrixfromj-thbodytoglobalreferenceframe.•λij:distancebetweeni-thfeatureandj-thcamera.•uij:unitaryvectorfromj-thcameratoi-thfeatureintheglobalreferenceframe.Itcanbecomputedasuij=RjRBCcjuij,beingcjuijtheunitaryvectorinthej-thcamerareferenceframeforthei-thfeature,whichisdirectlyobtainedfromthetrackedimages.•[RBC|tBC]:transformationfromCameratoBody(IMU).Rjandpjcanbecomputedbyintegratingtheinertialmeasurements.Consideringconstantbiasesduringtheini-tializationtime,itleadsto[8]:R1,j(bg)=tj−1k=1Exp((ωωωmk−bg)∆t)(2)vj(b)=v1+g∆t1,j+tj−1k=1R1,k(amk−ba)∆t(3)pj(b)=p1+tj−1k=1vk∆t+12g∆t2+12R1,k(amk−ba)∆t2(4)where:•ExpstandsfortheexponentialmapExp:R3→SO(3)•amkandωωωmkarek-thaccelerationandangularvelocityIMUmeasurement•b=(ba,bg)aretheircorrespondingaccelerometerandgyrobiases•gstandsforgravityintheglobalframe•vjisthevelocityatthej-thbody.•∆ti,jdenotestimedifferencebetweeni-thandj-thposes.
1290
However,theaboveexpressionshaveanimportantdraw-back:eachtimethatbgorbaaremodified,alltheIMUintegrationrequirestoberecomputed,whichisverytimeconsuming.Tosolvethisproblemwehaveadoptedthelinearpreintegrationcorrectionfrom[7][8],splittingtheseexpressionsinbiasdependentandnon-dependentterms,usingdeltaterms∆R1,j,∆v1,j,∆p1,j,whicharedirectlycomputedfromIMUmeasurementsandwhicharedefinedasfollows:∆R1,jRT1Rj(5)∆v1,jRT1(vj−v1−g∆t1,j)(6)∆p1,jRT1(pj−p1−v1∆t1,j−12g∆t21,j)(7)Ifduringintermediatestepsbgchangesmorethan0.2rad/secfromthevalueusedforpreintegration,theprein-tegrationisrecomputedwiththisnewbias,otherwise,deltatermsaredirectlyupdatedusingtheirJacobiansw.r.t.biases(∂∆R1,j∂bg,∂∆v1,j∂bg,∂∆v1,j∂ba,∂∆p1,j∂bg,∂∆p1,j∂ba).Inthisway,werelinearizeeachtimewegettoofarfromthelinearizationpoint.TheseJacobianscanbefoundin[8].Werewriteeq.(1)usingexpressions(5),(6)and(7):λi1iui1i−λijuij−v1∆t1i,j−g∆t21,j−∆t21,1i2=∆p1,j−∆p1,1i+(∆R1,j−∆R1,1i)tBC∀i=1...m,∀j∈Ci\1i(8)Now,wecanaddthegravitymagnitudeinformation.Insteadofaddingitasaconstraintofthegravitymagnitudeasdonein[2],weprefertomodelthegravitybymeanofarotationmatrixparametrizedbyonlytwoangles(α,β)(rotationaroundz-axishasnoeffect)andthevectorgI=(0,0,−g),thusweremaininanunconstrainedproblem:g=Exp(α,β,0)gI(9)Equation(8)becomes:λi1iui1i−λijuij−v1∆t1i,j=s1i,j(bg,ba,α,β)(10)where:s1i,j(bg,ba,α,β)=Exp(α,β,0)gI∆t21,j−∆t21,1i2+∆p1,j−∆p1,1i+(∆R1,j−∆R1,1i)tBC(11)Now,eachtimebiasesareupdated,wedonotneedtopreintegrateagainallthemeasurements,butonlytoupdatethembymeansofJacobians.Neglectingaccelerometerbiasasin[2],theonlyunknownsareλij(∀i=1...m,j∈Ci\1i),v1,α,βandbg.Stackingequationsforallpossiblevaluesofiandjwebuildanoverdeterminedsparselinearsystem,withonlythreenon-zeroelementsperrow,suchas:A(bg)x=s(bg,α,β)(12)wherex=(v1,{λij})istheunknownvectorwithlineardependence.Tojointlyfindlinearandno-lineardependentparameters,wesolvethenextunconstrainedminimizationproblem:(bg,α,β)argminbg,α,βminxA(bg)x−s(bg,α,β)22(13)Costfunctionc(bg,α,β)=minxA(bg)x−s(bg,α,β)22isevaluatedforeach(bg,α,β)usingthefollowingscheme:1)Update∆R1,jand∆p1,j:Using[8],wedon’tneedtoreintegrateallIMUmeasurementseachtimethatbgchanges.WesimplyupdatedeltatermsusingtheirJacobiansw.r.t.bias.Thatsupposesanimportantcom-putationalsaving.2)ComputeA(bg)ands(bg,α,β)andbuildthelinearsystem.3)Solvex=A(bg)\s(bg,α,β)usingconjugategradient,whichissuitableforsparsesystems.4)Computec(bg,α,β)=A(bg)x−s(bg,α,β)22.Thecomputationalcostofevaluatingc(bg,α,β)comes,first,fromsolvingasparsesystemwithnomorethan3+n×munknownsand3×(n−1)×mequationsand,second,fromintegratinginertialmeasurementalongtheinitializationtime.However,usingformulationfrom[8],wecanavoidreintegration,integratingIMUmeasuresonlyonce,andupdatingpreintegratedtermsbymeansofalinearapproximation.Tooptimizec(bg,α,β)andfindthecorrectgyrobiasandgravitydirectionweuseLevenberg–Marquardtalgorithm,whereJacobiansofthecostfunctionarecomputednumeri-cally.Asresult,notonlyIMUinitializationparametersarefound(g,bgandv1)butalsothepositionoftrackedpoints(λijuij).WehighlightthatnotallMtrackedfeatureshavebeenusedduringthisinitialization,butonlyasmallsetofmfeatures,aimingtoreducecomputationalcomplexity.However,thesolutionsfoundafterthissteparenotaccurateenoughtolaunchthesystem,andfurtherintermediatestagesarerequired.III.IMPROVEDSOLUTIONA.FirstBAandobservabilitytestAfterfindingtheinitialparameters(g,bg,v1,{λij})webuildavisual-inertialBAproblemwiththesamenbodyposesandmpointsfromthepreviousstep(seefigure2).Wesetthezaxisintheestimatedgravitydirection.Allbodyposeshavesixoptimizablevariables(φ,t)∈se(3)exceptthefirstone,whichhasonlytwo(pitchandroll)sincetranslationandyawhavebeenfixedinordertoremovethefourgaugefreedomsinherenttothevisualinertialproblem(initialpositionandyaw).Bodyvelocitiesarealsoincludedintheoptimizationtask,andtheyevolveaccordingtotheinertialmeasurements.InitialestimationsforeachvertexareaddedusingresultsfromtheMK-solution.Inaddition,
1291
...IMU errorReprojection errorPrior biask-th velocityGyroscope biasAccelerometer biasInitial pitch and rollk-th poseInitialization framesTracked Pointsfor initializationFig.2:Graphforthefirstvisual-inertialBA.Thebodyposesandpointsincludedintheoptimizationarethesameusedintheinitialsolution.accelerometerbiasbaisincludedinthisoptimization,butsimilarlytobgitisassumedtobeconstantforallframes.PreviousbgestimationfromMK-solutionstepisincludedbymeansofaprior,aswellasbaisforcedtobeclosetozero.WecallthisoptimizationfirstBAorsimplyBA1.AnalyticexpressionforJacobians,foundin[8],areusedforIMUresiduals,whileJacobiansforthereprojectionerrorhavebeenderivedanalytically,takingintoaccountthatweareoptimizingbodyposeandnotcamerapose.Usuallythisoptimizationprovidesabetterinitializationsolution.However,ifthemotionperformedgiveslowobserv-abilityoftheIMUvariables,theoptimizationcanconvergetoarbitrarilybadsolutions.Forexamplethishappensincaseofpurerotationalmotionornon-acceleratedmotions[1].Inordertodetectthesefailurecasesweproposeanobserv-abilitytest,whereweanalyzetheuncertaintyassociatedtoestimatedvariables.Thiscouldbedonebyanalyzingthecovariancematrixoftheestimatedvariablesandcheckingifitssingularvaluesaresmallenough.However,thiswouldrequiretoinverttheinformationmatrix,i.e.theHessianmatrixfromfirstBA,whichhashighdimensions(3m+6+9n−4),beingcomputationallytooexpensive.Instead,weperformtheobservabilitytestimposingaminimalthresholdtoallsingularvaluesoftheHessianmatrixassociatedtoourfirstBA.TheHessiancanbebuiltfromtheJacobianmatricesassociatedtoeachedgeinthegraph,asexplainednext.Denote{x1...xp}thesetofpstates,and{e1...eq}thesetofqmeasurementswhichappearinthefirstBA.Let’scallEithesetofmeasurementwherestateiisinvolved.TheHessianblockmatrixforstatesiandj,takingafirstorderapproximation,canbebuiltasfollows:Hi,j≈e∈Ei∩EjJTi,eΩeJj,e(14)whereΩestandsfortheinformationmatrixoftheemea-surement,andJi,efortheJacobianoftheemeasurementw.r.t.i-thstate.Inordertohaveanon-zero(i,j)blockmatrix,theremusttobeanedgebetweeniandjnodeinthegraph(measurementdependingonbothvariables)asshownFig.3:ExampleofHessianmatrixforaninitialmapwith5keyframes(KF)and20mappoints(MP).Onecandistinguishdifferentblocks,outlinedwithdashedlines.Inthetop-leftpart,wehavethediagonalblocksofeachkeyframe(red),blocksrelatingconsecutivekeyframes,duetotheIMUmeasurements(blue),andblocksrelatingkeyframesandIMUbiases(pink).Inthebottom-rightpart,thereareonlythediagonalblocksofthemappoints(orange).Out-of-diagonaltermsrelatemappointswiththekeyframesthatobservethem(brown).Inthisexampleallcamerasobserveallfeatures.infigure3.ApplyingtheSVDdecompositiontoHandlookingatthesmallestsingularvalueonecandetermineiftheperformedmotionguaranteesobservabilityofalltheIMUvariables.Hence,wediscardallinitializationswherethesmallestHessiansingularvaluefallsbelowathresholddenotedbytobs.Ifthisobservabilitytestisnotpassed,wediscardtheinitializationattempt.Examplesofasuccessfulandarejectedcaseareshowninfigure4.B.ConsensustestandsecondBAAswehavenotedbefore,notallMtrackedfeatureshavebeenusedinMK-solutionandfirstBAsteps,butonlymfeatures.Totakeadvantageoftheseextraunusedtrackedpoints,weproposetoperformaconsensustestinordertodetectinitializationswhichhavebeenperformedusingspuriousdata,suchasbadtrackedfeatures.First,the3Dpointpositionofeachunusedtrackistriangulatedbetweenthetwomostdistantframeswhichsawthepoint,bymeanofLeast-SquarestriangulationusingaSVDdecomposition[12].Onlytrackswithparallaxgreaterthan0.01radiansareused.Thenwere-projecteach3Dpointintoalltheframeswhichobserveit,computetheresidualre-projectionerror,andperformaχ2(95%)testwith2ni−3degreesoffreedom,whereniisthenumberofframeswhichobservethispoint.Theconsensustestisperformedcountingthepercentageofinliers:ifitisbiggerthanathresholdtconsweconsiderthattheproposedsolutionisaccurate,ifnot,wediscardtheinitializationattempt.
1292
025507510010.07.55.02.50.02.55.07.510.0log10Successful caseSing. ValuestObs025507510010.07.55.02.50.02.55.07.510.0Failure caseSing. ValuestObsH singular values (log10)Singular Value ( )Singular Value indexSingular Value indexFig.4:SingularvaluesoftheinformationmatrixforasuccessfulinitializationandafailurecaseontheEuRoCV103sequence.ThesuccessfulcasehasaRMSEATEerrorof3.16%intheinitializationtrajectory,andcorrespondstoatranslationandrotationmotion.Thefailurecasehasanerrorof64.99%andcorrespondstoanalmostpurerotationalmotion.Wedrawtheobservabilitythresholdusedtobs=0.1Iftheconsensustestissuccessful,weperformasecondBA(orsimplyBA2)includingthempointsusedintheinitialsolutionplusallthepointswhichhavebeentriangulatedanddetectedasinliers,havingatotalofMpoints.ThegraphforthisoptimizationissimilarlybuiltthanincaseofBA1butwithmorepoints.C.MapinitializationAfterthissecondBA,thekeyframeposesareaccurateenough,butweonlyhaveafewpointstoinitializethemap.BeforelaunchingthewholeORB-SLAMvisual-inertialsystem,wetriangulatenewpointsaimingtodensifythepointcloudandtoeasetheposteriortrackingoperation.Sincewealreadyhavethekeyframeposes,weextractORBfeaturesineachkeyframeandperformanepipolarsearchineachother,usingtheORBdescriptor.Allthesenewpoints,togetherwiththeMpointsfromBA2,arepromotedtomappoints,andthenframesusedforinitializationarepromotedtomapkeyframes.Thecovisiblitygraph[13]ofthisnewmapisalsocreated,takingintoaccounttheobservationsofpoints.IV.EXPERIMENTSThemostimportantparametersofourmethodareshownintableI.OurimplementationusesORB-SLAMvisual-inertial[5]withitsthreethreadsfortracking,mappingandloopclosing.Initializationisperformedinaparallelthread,thusithasnoeffectintherealtimetrackingthread.ForMK-solutionweuseEigenC++library,whileforgraphoptimizationofBA1andBA2weuseg2oC++library[14].ExperimentshavebeenruninV1datasetfromEuRoC[9]usingaIntelCorei7-7700computerwith32GBofmemory.TABLEI:ParametersofourinitializationalgorithmTotalnumberoftracksM200Track-lengthtest(inpixels)l200TracksusedforMK-solutionm20KeyframesusedforMK-solutionn5Observabilitytest:Singularvaluethresholdtobs0.1Consensustest:Inlierthresholdtcons90%21012X (m)210123Y (m)Initializations Trajectories after observabilityandconsensustestGood InitializationBad InitializationGround-TruthFig.5:InitializationsfoundalongtheEuRoCV101trajec-tory,aftertheobservabilityandconsensustests.Inblue,groundtruthtrajectory;ingreen,estimatedinitializationtrajectorieswhoseRMSEATEerrorislowerthan5%;inred,thosewithabiggererror.Ourmethodwasabletofind511correctinitializationsalongthewholetrajectory,runninginrealtime.A.ResultsEuRoCdatasetprovidesstereoimagesandsynchronizedIMUmeasuresforthreedifferentindoorenvironments,withdifferentcomplexity.Wehavetestedourmethodforenvi-ronmentV1fromEuRoCatthreedifficultylevels.Weruntwodifferentexperiments.Inafirstexperiment,wetrytoinitializeasoftenaspossi-bleinrealtime.Alongthewholetrajectory,everytimethetrackingthreadhasmtrackswithlengthl,iftheinitializationthreadisidle,anewinitializationattemptislaunched.Figure5showstheinitializationsfoundfortrajectoryV101aftertheobservabilityandconsensustest.WeshowinredtrajectorieswhichhaveaRMSEATE[15]errorbiggerthan5%oftheinitializationtrajectorylength.Wecanseeinthefigurethatourinitializationalgorithmissuccessfulalmostalongallthetrajectory.Thepartswithoutinitializationsareduetorejectionfromobservabilityorconsensustest.IntableIIweshowthemainnumericalresultsoftheseexperimentswiththethreeV1sequences.RMSEATE[15]isexpressedinpercentageoverthelengthoftheinitializationtrajectory.Beloweachsequencenameweshowsuccessfulinitializationsoverthetotalnumber.Firstthingtonotice
1293
TABLEII:ResultsofexhaustiveinitializationtestsoverthethreeV1EuRoCsequences.V1EuRoCDatasetAftertrack-lengthtestAfterObserv+Cons.testSeq.NameRMSEATE(%)Scaleerror(%)RMSEATE(%)Scaleerror(%)CPUtime(ms)Trajectorytime(s)MK-solution9.17632.9987.74925.10495.0822.235V101(511/728)MK-solution+BA13.97710.7192.3526.471104.1142.235MK-solution+BA1&23.2708.8162.0365.496120.9832.235MK-solution12.025156.7516.76048.92660.2850.968V102(101/395)MK-solution+BA16.33825.2522.5417.19570.9630.968MK-solution+BA1&25.14920.3411.9355.49784.4430.968MK-solution47.928128.0086.63421.69162.1601.070V103(71/336)MK-solution+BA171.77428.1602.4756.83673.3011.070MK-solution+BA1&271.06824.5561.8705.25984.6761.070isthelargenumberofinitializationattempts.Forexample,insequenceV101whichlasts130seconds,upto728initializationsarecomputed,and511ofthemhavepassedtheobservabilityandconsensustest.ThetableshowsthattheoriginalMartinelli-Kaisersolutionobtainsaveragescaleerrorsbetween32.9%and156.7%onthesesequences.Thiserrorcanbereduceduntil8.8%to24.5%applyingthetworoundsofvisual-inertialBAproposedhere.Moreinterestingly,applyingthenovelobservabilityandconsensustests,inaccurateinitializationsareconsistentlyrejected,andtheaveragescaleerrorisreducedtoaround5%forallsequences,averysignificantimprovementovertheoriginalmethod.TheATEerrorisalsodrasticallyreducedafterbothtests.Consideringtheinitializationtimeweseeanevidentdif-ferencebetweenV101,thatrequiresinitializationtrajectoriesof2.2secondsinaverage,andV102andV103where1secondisenough.Inthesetwolastsequencesmotionisfasterandthetrack-lengthtestissatisfiedinlesstimethaninthefirstsequence,wherethequad-copterisflyingatlowspeed.Regardingthecomputationalcost,theaverageCPUre-quiredtosolvetheinitializationislessthan85msforsequencesV102andV103,andaround121msforV101,duetothelongerpreintegrationperiod.Inallcases,theMK-solutionsteptakesaround75%ofthetotalinitializationCPUtime.IntableIIIweshowcomputationaltimesforourmethodwhichusespreintegrationwithfirstorderbiascorrectionfrom[8].ComparedwithusingtheoriginalformulationfromMartinelliandKaiser,computingtimeisreducedby60%.Inasecondexperiment,welaunchvisual-inertialORB-SLAM[5]andweretrievetheRMSEATEandthescaleerrorjustaftertheproposedinitialization,andafterperformingfullvisual-inertialBAat5secondsand10secondsfromthefirstkeyframetimestamp.WecanseeintableIVthatallthreesequencesconvergetoscaleerrorsmallerthan1%after10seconds,confirmingthatourinitializationmethodisaccurateenoughtolaunchvisual-inertialSLAM.Anex-ampleofVisual-InertialORBSLAM[5]usingourproposedTABLEIII:ComparisonofrunningtimeforMKSolu-tion+BA1+BA2repeatingIMUintegrationineachiterationandusingpreintegrationwithfirstorderbiascorrection[8].V101EuRoCDatasetCPUtime(ms)MeanStdMaxReintegratingeachtime301.30291.974678.886Usingfirstordercorrection120.98327.609214.989TABLEIV:ResultsofVI-SLAMusingourinitialization(averageerrorsonfiveexecutionsareshown).V1EuRoCDatasetAfterinitializationAfterBA5sAfterBA10sSeq.NameRMSEATE(m)Scaleerror(%)RMSEATE(m)Scaleerror(%)RMSEATE(m)Scaleerror(%)V101easy0.01834.990.02001.850.01700.84V102medium0.03647.380.00763.670.01620.71V103difficult0.00434.800.01292.500.01200.27initializationcanbefoundintheaccompanyingvideo.Comparedwiththeinitializationmethodproposedin[5],ourmethodrequirestrajectoriesof1or2secondsinsteadof15seconds,useslessCPUtime,andisabletosuccessfullyinitializeinsequenceV103,wherethepreviousmethodfailed.V.CONCLUSIONSWehaveproposedafastjointmonocular-inertialinitial-izationmethod,basedontheworkofMartinelli[1]andKaiseretal.[2].Wehaveadaptedittobemoregeneral,allowingincompletefeaturetracks,andmorecomputation-allyefficientusingtheIMUpreintegrationmethodofForsteretal.[8].OurresultsshowthattheoriginalMartinelli-Kaisertechniquedoesnotprovideagoodenoughinitializationinmostpracticalscenarios,hencewehaveproposedtwovisual-inertialBAstepstoimprovethesolutionandtwonovelteststodetectbadinitializations.Thesetechniqueshaveproventobeworthit,reducingscaleerrordownto5%andrejectingbadinitializations.SolutionsfoundafterthosestepsisgoodenoughtolaunchVisual-InertialORBSLAM[5]andconvergetoveryaccuratemaps.Insummary,wehavedevelopedafastmethodforjointinitializationofmonocular-inertialSLAM,usingtrajectoriesof1to2seconds,thatismuchmoreaccurateandrobustthantheoriginaltechnique[2],withamaximumcomputationalcostof215ms.Asfutureworkwewouldliketoinvestigatetheadaptationoftheinitializationmethodtothestereocase,takingintoaccountthatscaleisdirectlyobservablefromtheimages.Wearealsointerestedintakingprofitofgyroscopereadingsfortracking,evenbeforetheinitializationhasbeenperformed.Finally,wewouldliketotesttheinitializationperformanceininmoredifficultscenarioswithourownacquiredsequences.
1294
REFERENCES[1]A.Martinelli,“Closed-formsolutionofvisual-inertialstructurefrommotion,”InternationalJournalofComputerVision,vol.106,no.2,pp.138–152,2014.[2]J.Kaiser,A.Martinelli,F.Fontana,andD.Scaramuzza,“Simultaneousstateinitializationandgyroscopebiascalibrationinvisualinertialaidednavigation,”IEEERoboticsandAutomationLetters,vol.2,no.1,pp.18–25,2017.[3]M.LiandA.I.Mourikis,“High-precision,consistentEKF-basedvisual-inertialodometry,”TheInternationalJournalofRoboticsRe-search,vol.32,no.6,pp.690–711,2013.[4]S.Leutenegger,S.Lynen,M.Bosse,R.Siegwart,andP.Furgale,“Keyframe-basedvisual–inertialodometryusingnonlinearoptimiza-tion,”TheInternationalJournalofRoboticsResearch,vol.34,no.3,pp.314–334,2015.[5]R.Mur-ArtalandJ.D.Tard´os,“Visual-inertialmonocularSLAMwithmapreuse,”IEEERoboticsandAutomationLetters,vol.2,no.2,pp.796–803,2017.[6]T.QinandS.Shen,“Robustinitializationofmonocularvisual-inertialestimationonaerialrobots,”inIEEE/RSJInternationalConferenceonIntelligentRobotsandSystems(IROS),2017,pp.4225–4232.[7]T.LuptonandS.Sukkarieh,“Visual-inertial-aidednavigationforhigh-dynamicmotioninbuiltenvironmentswithoutinitialconditions,”IEEETransactionsonRobotics,vol.28,no.1,pp.61–76,2012.[8]C.Forster,L.Carlone,F.Dellaert,andD.Scaramuzza,“IMUpreinte-grationonmanifoldforefficientvisual-inertialmaximum-a-posterioriestimation,”inRobotics:ScienceandSystems,2015.[9]M.Burri,J.Nikolic,P.Gohl,T.Schneider,J.Rehder,S.Omari,M.W.Achtelik,andR.Siegwart,“TheEuRoCmicroaerialvehicledatasets,”TheInternationalJournalofRoboticsResearch,vol.35,no.10,pp.1157–1163,2016.[10]E.Rublee,V.Rabaud,K.Konolige,andG.Bradski,“ORB:Aneffi-cientalternativetoSIFTorSURF,”inIEEEInternationalConferenceonComputerVision(ICCV),2011,pp.2564–2571.[11]B.D.Lucas,T.Kanade,etal.,“Aniterativeimageregistrationtechniquewithanapplicationtostereovision,”inInt.Joint.Conf.onArtificialIntelligence(IJCAI),1981,pp.674–679.[12]R.Szeliski,Computervision:algorithmsandapplications.SpringerVerlag,London,2011.[13]R.Mur-Artal,J.Montiel,andJ.D.Tard´os,“ORB-SLAM:aversa-tileandaccuratemonocularSLAMsystem,”IEEETransactionsonRobotics,vol.31,no.5,pp.1147–1163,2015.[14]R.K¨ummerle,G.Grisetti,H.Strasdat,K.Konolige,andW.Burgard,“g2o:Ageneralframeworkforgraphoptimization,”inIEEEInter-nationalConferenceonRoboticsandAutomation(ICRA),2011,pp.3607–3613.[15]J.Sturm,N.Engelhard,F.Endres,W.Burgard,andD.Cremers,“AbenchmarkfortheevaluationofRGB-DSLAMsystems,”inIEEE/RSJInternationalConferenceonIntelligentRobotsandSystems(IROS),2012,pp.573–580.
相关推荐
2023年江西萍乡中考道德与法治真题及答案.doc
2012年重庆南川中考生物真题及答案.doc
2013年江西师范大学地理学综合及文艺理论基础考研真题.doc
2020年四川甘孜小升初语文真题及答案I卷.doc
2020年注册岩土工程师专业基础考试真题及答案.doc
2023-2024学年福建省厦门市九年级上学期数学月考试题及答案.doc
2021-2022学年辽宁省沈阳市大东区九年级上学期语文期末试题及答案.doc
2022-2023学年北京东城区初三第一学期物理期末试卷及答案.doc
2018上半年江西教师资格初中地理学科知识与教学能力真题及答案.doc
2012年河北国家公务员申论考试真题及答案-省级.doc
2020-2021学年江苏省扬州市江都区邵樊片九年级上学期数学第一次质量检测试题及答案.doc
2022下半年黑龙江教师资格证中学综合素质真题及答案.doc
