Inverse Depth Monocular SLAM

Transcription

1 Inverse Depth Monocular SLAM Javer Cvera, Andrew J. Davson, JMM Montel Javer Cvera JMM Montel A.J. Davson

2 Problem Statement Sequental smultaneous sensor locaton and map buldng at frame rate, 3Hz. Camera moves freel n 3D, 6dof camera moton Outdoors real scenes contans close and dstant, even at nfnt, features Man contrbuton codfng scene ponts wth ts nverse depth:. Deals wth low paralla cases. Deals wth both dstant and close ponts 3. Map features are ntalsed just from one mage Javer Cvera JMM Montel A.J. Davson

3 Camera Geometr: Pure Bearng-onl Sensor camera, optcal center O Z X [,] X[X,Y,Z] Y f Camera detects ras A ra s defned b the optcal center O and the observed pont The mages s used as the method to determned the detected ra Depth s not detected at mage of gallaes 9 ears lght far from the Earth Javer Cvera JMM Montel A.J. Davson

4 Ponts at Infnt projectve cameras do observe ponts at nfnt parallel lnes meet at nfnt, a projectve camera does observe ths ntersecton pont as vanshng pont we ntend to code and eplot ths ponts at nfnt n the monocular SLAM problem Javer Cvera JMM Montel A.J. Davson

5 Paralla no paralla geometres Camera rotaton Camera observng a scene plane low paralla cases Dstant features compared wth camera translaton Intal feature observaton Javer Cvera JMM Montel A.J. Davson

6 Stereo Vson. Senstvt Analss Eje óptco Plano de magen f Centro óptco C θ θ P b ra, = ra, = z X z tanθ f z tanθ + b b = tanθ tanθ C P The three angles n trangle C PC b z β β tanθ θ β Paralla angle tanθ θ P Z b z θ θ add up to π rad Geometr, dependng onl on: relatve camera locaton observed pont locaton Paralla, angle defned b the two ras correspondng to the two cameras for a scene pont. Javer Cvera JMM Montel A.J. Davson

7 σ o σ p Image errors, error standard devaton n pels as a rule of thumb error range s ± σ eample, for a clckng error ± pel, correspondng standard devaton σ p p =.5 pels σ p error analss s based on the ra orentaton error the standard devaton for the orentaton error, σ, n radans o f an appromate relaton between them σ o d σ f p, d, pel sze (mm.) f,lens focal lenght (mm.) Javer Cvera JMM Montel A.J. Davson

8 Stereo vson. Gaussan propagaton X P bσ o θ β + θ.5 Optcal As β Image Plane C b two ras drecton. θ + θ Ellposd wth major as orented n the two ras bsectrc drecton. C Error propagaton on X, Z drecton depends on σo b = β σo β b Depth error proportonal to baselne b nverse squared paralla Lateral error smaller than depth error, proportonal to baselne b σ β nverse paralla to the.5th power o σ β o.5 Javer Cvera JMM Montel A.J. Davson

9 Lneart lmts of the Gaussan propagaton. Inverse depth codng o α = z representaton.8.6 ρθrepresentaton z 5 4 ρ=/d o σ =. o σ = θ(deg) Smulaton: computng depth of a pont from vews at known camera locatons Non Gaussan n XZ Gaussan n /d, theta Javer Cvera JMM Montel A.J. Davson

10 State of the art I SLAM, ntall proposed b Smth and Cheesman, 986, wdespread usage n robotcs for multsensor fuson [Castellanos 999], [Feder 999] [Thrun et al. 5] Sequental approach Ablt to close loops, dentfng features prevousl observed as reobserved. Complet s lnked to the scene not to the number of observatons processed. SLAM used for computer vson, [Castellanos ], [Davson 998] combned wth odometr Monocular SLAM vson [Davson 3] Camera "followng the laws of mechancs" moton model Vson as the onl sensor, no odometr. Snergc usage of vson geometr and vson photometrc map Low paralla ponts avoded:» Ponts represented as XYZ, onl works wth ponts close to the camera» Delaed ntalzaton SFM,computer vson methods Javer Cvera JMM Montel A.J. Davson

11 State of the art II SLAM methods Photogrammetrc bundle adjustment, 6's Normall onl close ponts Computer vson geometr Hartle & Zsserman [Hartle 3] Robust statstcs Matchng between several shots enforcng a coherence wth a projectve camera model Appled to ndvdual shots, to sequences wth varng camera parameters Appled for robot navgaton [Nster 3, Mouragnon 6] Not sequental Wde-baselne performance Routne usage of ponts at nfnt Model selecton problem [Torr 998, 999 ] No paralla, homograph model Paralla eppolar geometr model Increasng the frame rate, the nterframe moton closes to a homograph Javer Cvera JMM Montel A.J. Davson

12 State of the art III Feature ntalzaton n Monocular SLAM Delaed approach: Image trackng untl safe Gaussan trangulaton Bale 3, Davson 3 Undelaed ntalzaton Multple hpotheses n depth, Kwok Dssanaake 4, Sola et al. 5, Inverse depth usage Concept used n dfferent domans Paralla wth respect plane at nfnt, Zsserman 3 Optcal flow Heeger & Jepson 99 Modfed polar coordnates n bearng onl TMA Adala & Hammel 983 Sequental structure estmaton from known moton Okutom&Kanade 993. Parwse frst and ndvdual EKF's Chowdhur&Chellappa 3 Recentl used for Monocular SLAM Trwan & Roumelots 5 Eade & Drummond 6, FastSLAM, a dstngushed ntalzaton stage. Javer Cvera JMM Montel A.J. Davson

13 Camera moton prors C C C C C C ω W v ( r WC, q WC ) Constant veloct moton model: Smooth camera moton Impulse acceleraton nose W Javer Cvera JMM Montel A.J. Davson

14 Javer Cvera JMM Montel A.J. Davson Scene pont codng n nverse depth. Measurement equaton ( ) ( ) θ ι φ ι ρ ρ φ θ ρ, a pont s observed as at low paralla, goes to zero, low baselne, close camera locatons, paralla goes to zero, dstant pont, paralla, the frst tme the feature s observed those of,,, m R h r r m CW = C WC WC z z z paralla angle α C z W r ρ W WC r = d ρ z m C h XYZ C ( ) WC q WC r, C h ρ z h h d f v v = z h h d f u u = ( ) + = = ι ι ι φ θ ρ ρ, m r R h CW WC C z z h h h

15 Javer Cvera JMM Montel A.J. Davson SLAM jont map+camera state vector The full estmate coded n a unque Gaussan dstrbuton. Camera state vector.. ALL the map features. 3. The jont Gaussan has proven to be useful n codng strong correlaton between the observatons. ( ) = = n n n n v n v n v v v v T T T T T n T T v P P P P P P P P P P L M O M M L L L

16 Actve search stored patch Estmaton at step k- Predcton at step k Javer Cvera JMM Montel stored patch s searched n all acceptance the regon Update at step k A.J. Davson

17 z ρ m( θ, φ ) ρ + σ New ponts are observed from just an observaton,, z, θ, φ and the correspondng covarance ρ at ρ Inverse depth feature ntalzaton pror are ntalzed from the frst feature observaton and ts covarnaceσ that the nterval [ ρ - σ, ρ + σ ] ρ covers a regon from d ρ ρ s ntalzed so mn and ncludng nfnte d mn = ρ + σ ρ ρ ρ σ ρ Javer Cvera JMM Montel A.J. Davson

18 Calbrated mage measurements Frame Rate Dmensonal Monocular SLAM z Δt,σ z Monocular SLAM Processng Camera Moton Prors Intal depth Pror σ, σ σ a ω ρ, σ camera locaton WC WC W C ( r, q, v, ω, L ) T α, σ ρ V scene map n So estmaton process can be consdered as a functon Beng the dmensons nvolved, tme, and length: Javer Cvera JMM Montel A.J. Davson

19 Dmensonless Monocular SLAM Calbrated mage measurements Frame Rate Π z, Π σ z Δt = Monocular SLAM Processng camera locaton scene map Camera Moton Prors Intal depth Pror Π Π ρ σ σ a ω, Π, Π =, σ α σ Π V σ ρ Δt, ρ are selected to defne the dmensonless coeffcents [Buckngham 94]: state vector splt n scale, d, and dmensonless coeffcents monocular camera cannot observe scale, d. If scale were known: Javer Cvera JMM Montel A.J. Davson

20 Interpretng Dmensonless Parameters as Image Quanttes dmensonless camera lnear acceleraton standard devaton dmensonless camera angular acceleraton standard devaton Javer Cvera JMM Montel A.J. Davson

21 Inverse depth estmaton hstor near feature (3), eventuall ecludes nfnte from the acceptance regon nfnt / zero nverse depth 3 (a) dstant feature (), nfnte alwas ncluded n the acceptance regon zero nverse depth - -. (b) Javer Cvera JMM Montel A.J. Davson

22 Loop Closng r r r z meters.6.4 meters.6.4 meters frame number 4 6 frame number 4 6 frame number ψ (rot) θ (rot) φ (rotz) deg deg deg frame number 4 6 frame number 4 6 frame number Javer Cvera JMM Montel A.J. Davson

23 Lneart Inde Javer Cvera JMM Montel A.J. Davson

24 Inverse depth lneart analss Javer Cvera JMM Montel at ntalzaton α cos α, L poor lneart d at ntalzaton α cosα, L lneart after paralla gatherng, cosα, but σ ρ lneart good performance along the whole estmaton ρ A.J. Davson

25 Inverse depth to XYZ converson Inverse depth good performance along the whole estmaton process Inverse depth codng needs 6 parameters XYZ codng good performance for reduced depth uncertant So, t s not mandator to swtch from nverse depth to XYZ, but computng overhead can be reduce Swtchng crtera based on the lneart nde Javer Cvera JMM Montel A.J. Davson

26 Inverse depth to XYZ converson threshold Javer Cvera JMM Montel A.J. Davson

27 Swtchng evoluton dmenson 6 4 state vector dmenson all map features n nverse depth swthchng nverse depth to XYZ % dmenson reducton for code swthchng % reducton 9 8 # map 3D ponts # map 3D ponts total # 3D ponts total # 3D ponts nverse depth total # 3D ponts n XYZ Javer Cvera JMM Montel A.J. Davson

28 Swtchng evoluton (a) (b) (c) (d) Inverse depth codng Depth codng Javer Cvera JMM Montel A.J. Davson

29 Bblografía J.M.M. Montel, Javer Cvera Andrew J. Davson: Unfed Inverse Depth Parametrzaton for Monocular SLAM. Robotcs: Scence and Sstems Conference 6. Javer Cvera, Andrew J. Davson and J.M.M. Montel,: Inverse Depth to Depth Converson for Monocular SLAM. IEEE Int Conf on Robotcs and Automaton Rome, Aprl 7. Javer Cvera, Andrew J. Davson, J. M. M. Montel. "Dmensonless Monocular SLAM". 3rd Iberan Conference on Pattern Recognton and Image Analss (IbPRIA), Grona, 7 J.M.M Montel and Andrew J. Davson: "A vsual compass based on SLAM". In Proc. Intl. Conf. on Robotcs and Automaton, pages 97--9, 6. J.M.M Montel home page: Anderw Davson home page Javer Cvera JMM Montel A.J. Davson