Planar Pose Estimation using a Camera and Single-Station Ranging Measurements

Transcription

1 Planar Pose Estmaton usng a Camera and Sngle-Staton Rangng Measurements Chen Zhu 1, Gabrele Gorg 1, and Chrstoph Günther 1,2 1 Insttute for Communcatons and Navgaton, Department of Electrcal and Computer Engneerng, Technsche Unverstät München, Munch, Germany Emal: {chen.zhu, gabrele.gorg}@tum.de 2 Insttute of Communcatons and Navgaton, German Aerospace Center (DLR), Oberpfaffenhofen, Germany Emal: chrstoph.guenther@dlr.de BIOGRAPHY Mr. Chen Zhu s currently pursung hs Ph.D. at Technsche Unverstät München as a full-tme researcher at the Insttute for Communcatons and Navgaton. He receved hs B.Sc. n Automaton Engneerng from Tsnghua Unversty, n Bejng, Chna n 2009, and hs M.Sc. n Communcatons Engneerng n 2011 from Technsche Unverstät München, n Munch, Germany. Hs research nterests nclude vsual navgaton, robotc swarm navgaton, and mult-sensor fuson n autonomous vehcle navgaton. Dr. Gabrele Gorg s a researcher at the Insttute for Communcatons and Navgaton, Technsche Unverstät München, n Munch, Germany. He obtaned a Ph.D. followng hs wor on Global Navgaton Satellte System (GNSS) for aerospace applcatons from the Delft Insttute of Earth Observaton and Space Systems (DEOS), Delft Unversty of Technology, n Delft, The Netherlands. He holds a M.Sc. degree n space engneerng from the Unversty of Rome La Sapenza and a B.Sc. degree n aerospace engneerng from the same unversty. Hs man research focuses on satellte navgaton, vsual navgaton and mult-sensor fuson. Prof. Chrstoph Günther studed theoretcal physcs at the Swss Federal Insttute of Technology n Zurch, Swtzerland. He receved hs dploma n 1979 and completed hs Ph.D. n He wored on research n cryptography, codng, communcaton, and nformaton theory wth Asea Brown Bover, Ascom and Ercsson. Snce 2003, he s the Drector of the Insttute of Communcaton and Navgaton at the German Aerospace Center (DLR). The nsttute employs around 120 scentsts. Snce 2004, Günther s addtonally holdng the Char of Communcaton and Navgaton at Technsche Unverstät München (TUM). The focus of hs research wor s on navgaton. At TUM, he and hs team are developng algorthms for achevng a hgh accuracy. At DLR, the focus s on achevng hgh levels of ntegrty. ABSTRACT Cameras are wdely used for localzaton and navgaton n GNSS-dened envronments. By explotng VSLAM (Vsual Smultaneous Localzaton and Mappng) technques, vehcles equpped wth cameras are capable of estmatng ther own trajectores and smultaneously buldng a map of the surroundng envronment. Due to constrants on payload sze, weght, and costs, many VSLAM applcatons must be based on a sngle camera. However, the assocated monocular estmaton of the vehcle trajectory and the map s ambguous by a scale factor. The purpose of ths wor s to show how the correct scale factor can be estmated n planar moton cases by explotng range measurements from a sngle staton. The proposed method s ndependent of the VSLAM algorthm used for ego-moton estmaton of the vehcle.

2 Fgure 1: Statc staton and dynamc rover INTRODUCTION Autonomous navgaton of ground vehcles often reles on several sensors such as moble recevers, Inertal Measurement Unts (IMUs), laser scanners and cameras [1]. By applyng VSLAM (Vsual Smultaneous Localzaton and Mappng) technques, a vehcle can estmate ts ego-moton and smultaneously buld a map usng onboard cameras. Due to constrants on sze, weght, accommodaton and cost, a stereo camera rg cannot be mplemented n many cases. Moreover, f the baselne length of a stereo rg s very short, t does not provde sgnfcant advantage over a sngle camera due to the resoluton lmts. As a result, several approaches usng monocular cameras have been developed. Klen and Murray developed the Parallel Tracng and Mappng (PTAM) algorthm [2], whch dvdes the tracng and mappng nto separate threads to accelerate the computaton. Later on, Strasdat, Montel and Davson proposed a scale-aware loop closure method to deal wth the relatve scene scale drft [3]. Based on the PTAM framewor, Mur-Artal, Montel and Tardo s [4] proposed the state-of-the-art ORB-SLAM approach, whch provdes a robust real-tme monocular SLAM soluton. Engel, Scho ps and Cremers proposed a large scale dense SLAM algorthm (LSD-SLAM) usng monocular cameras [5], whch mnmzes the photometrc error nstead of the feature reprojecton error for mprovng the performance. However, all these algorthms estmate the moton only up to a global scale. A number of approaches have been consdered for resolvng the global scale ambguty. Many of them use IMUs, see for example Achtel et al. [6] and Nu tz et al. [7]. The fuson of camera and IMUs can sgnfcantly reduce the relatve scale drfts. However, the nherent drft of IMUs s prone to ntroducng nconsstency n global scale estmaton. Tabbazar and Basr proposed an approach usng range measurements from cellular networs [8]. Ther method requres at least 3 rangng lns for estmatng the robot s poston before ntegratng the result wth vson-based estmates. Three lns are often not avalable, e.g., moble networs do not typcally provde threefold coverage. Zhu, Gorg, and Gu nther proposed a scale and relatve poston estmaton method n [9] usng two dynamc rovers equpped wth monocular cameras and rangng capablty between them. It provdes a soluton for mult-agent based applcatons, e.g., robotc swarm navgaton. Nevertheless, the cooperaton of two dynamc vehcles s not always feasble. Therefore, we developed a method for estmatng the global scale n monocular VSLAM by explotng range measurements on a sngle ln to a statc staton. As a result, the The method developed n ths paper does not depend on the method of rangng, as long as t s performed wth respect to a gven statc locaton. SYSTEM MODEL The measurement scenaro addressed n ths wor s shown n Fg. 1. A rover equpped wth a monocular camera and a rangng devce, e.g., a wreless rado recever, executes SLAM tass on the ground. The moton of the vehcle s constraned to be planar. In the proposed scheme, a rado ln s avalable between the dynamc rover and a statc staton. The statc staton can be another robotc rover n statonary mode, a base staton, or a wreless hotspot wth transmtter for the specfc rado sgnal. The dynamc rover estmate ts ego-moton usng an onboard camera, and a rado ln s establshed to execute base-to-rover rangng. The range measurements can be obtaned by usng plot sgnals for synchronzaton. Because a satsfactory cloc synchronzaton between the rover and the staton cannot be acheved n many cases, round-trp-delay (RTD) technques s a favorable choce for slow-movement scenaros to elmnate the mpact of any cloc offset. The detals of rangng usng RTD for navgaton purposes are dscussed n [10]. We defne a navgaton frame (N) as a fxed coordnate frame for the rover wth ts orgn at the startng locaton of rover. The navgaton frame s related to the world reference frame by a specfc transformaton dependent on the ntal poston and

3 atttude of the vehcle. Moreover, we use () to express the camera s local coordnate frame at eyframe, whch vares as the camera moves. Let c (W) R 2 be the poston of the robot n world frame (W) at tme. In the remander of ths paper, we use a superscrpt wth parentheses ( ) to denote the coordnate frame n whch the vector s represented. Tme s measured at eyframes,.e., the tme reference nstances n whch both the range measurements and the trajectory estmaton are avalable. The transformaton between two arbtrary coordnate frames (P) and (Q) follows X (Q) = R (P Q) X (P) + t (P Q), (1) where X (P) and X (Q) denote the coordnates of an arbtrary 3D pont X expressed n the correspondng (P) and (Q) frames, R (P Q) SO(3) denotes the orthonormal rotaton matrx, and t (P Q) denotes the translaton vector from the orgn of (P) to the orgn of (Q). MOTION ESTIMATION USING MONOCULAR CAMERAS Accordng to perspectve projecton, a vsble pont wth 3D coordnates n the navgaton frame X (N) two-dmensonal (2D) pont u () n the measurement set Ω at -th eyframe as u () R 3 s projected to a = π( X (N), c (N),R (N ) ) Ω R 2. (2) Ω s the set of 2D coordnates of all the ponts of nterest on the mage plane. By tracng features n consecutve mage sequences, the essental matrx E ( +1) can be estmated usng eppolar geometry constrants [11]. The essental matrx can be decomposed nto a rotaton R ( +1) and a unt vector e ( +1) representng the translaton drecton as: E ( +1) = [ e ( +1) ] R ( +1), where [ ] denotes the 3 3 sew symmetrc matrx bult as e 1 e 2 e 3 The translaton n true scale s related to the monocular estmaton by = 0 e 3 e 2 e 3 0 e 1. (3) e 2 e 1 0 t ( +1) = s g l ( +1) e ( +1). (4) In ths equaton l ( +1) e ( +1) s the estmated translaton from monocular vson, n whch l ( +1) R + denotes the estmated norm of the translaton from tme to + 1, and e ( +1) reflects the drecton of the moton. s g R + s the true global scale n the world frame, whch cannot be obtaned n the monocular-only case [12]. Wthout loss of generalty, one can assume l (1 2) = 1, snce one can map the value nto s g. To start the tracng, usng the estmated moton from the frst two frames, the 3D coordnates of the traced ponts can be estmated by trangulaton as ˆX (N) = π 1 ( u (1), u (2),E (1 2) ). Consequently, the camera postons at the followng tme nstances 2 can be obtaned by mnmzng the re-projecton resdual π( X (N) ĉ (N) +1 = argmn c (N) +1 u (+1) Ω +1, c (N) +1 ) u(+1) 2 Σ 1,+1, (5) where where Σ 1 denotes the Mahalanobs dstance wth Σ as the measurements covarance matrx. Usng the estmated pose, the 3D poston of the new features detected n frame +1 can be updated usng π 1 ( ). As a result, the tracng can be contnued as long as suffcent features can be traced n consecutve frames. However, the estmated poston of the camera and the map ponts are all ambguous by a scale factor s g. Durng the consstent tracng, eyframes are selected f the mage has sgnfcant change whle suffcent relable features are traced. Snce the obtaned moton estmates follow dead-reconng prncple, the estmaton error wll accumulate over tme. In order to mprove the accuracy of the estmaton result, a global optmzaton for both 3D pont poston and the vehcle poses s performed usng K eyframes and N p map ponts: { ˆX (N) },{ĉ (N) },{ ˆR (N ) } = arg mn { X (N) N p K, c (N),R (N ) } =1 =1 v u () π( X (N), c (N),R (N ) ) 2 Σ 1,, (6) where v s a bnary vsblty mas, whch assumes v = 1 f feature s vsble to the camera at tme nstant, otherwse v = 0. Therefore, by executng the optmzaton n Eqn. (6), the rover obtans a set of up-to-scale egomoton estmates expressed n ts own navgaton frame,.e., {ĉ (N) }.

4 Fgure 2: Geometry of the statc staton and the dynamc rover SCALE ESTIMATION USING MONOCULAR CAMERA AND SINGLE RANGING LINK Fg. 2 llustrates the basc geometry between the statc staton and the dynamc rover. Wthout loss of generalty, the reference frame orgn s set to be the poston of the base staton. The drecton of the x-axs of the reference frame (W) can be arbtrarly chosen. The ntal headng of the camera s defned as the y -axs n the navgaton frame (N). As a result, the ntal poston of the rover can be parameterzed by the ntal radus r 1 and the ntal polar angle α n the world frame (W). The ntal atttude of the rover s descrbed by the angle α + θ π 2,.e., c (W) 1 = t (N W) = r 1 R(α)[1,0] T, (7) R (1 W) = R (N W) = R(α + θ π ), (8) 2 where R( ) SO(2) s a 2D rotaton matrx. Usng the mages from the monocular cameras, the ego-moton of the rover n ts navgaton frame (N) can be ndependently estmated up-to-scale as {ĉ (N) } by explotng a VSLAM algorthm. However, due to the lac of global scale nowledge, the poses n real-world metrc are unavalable. In the world reference frame (W), the poston of the rover at -th eyframe can be expressed as c (W) = s g R (N W) c (N) + c (W) 1 (9) As a result, the coordnates of the trajectory n world frame and navgaton frame can be related by a smlarty transformaton T Sm(2) R 3 3. The transformaton n Eq. (9) can be parameterzed by 4 parameters as: c (W) ( = R(α) r 1 [1,0] T + s g R(θ π 2 ) c(n) Although the monocular camera tself can only estmate the moton up-to-scale, wth the addtonal help of a sparse set of nosy range measurements {ρ }, where ρ = r + η = c (W) + η, (11) a method for estmatng the global scalng factor s g can be devsed by explotng consecutve rangng measurements at eyframes. ) (10)

5 Table 1: Transformaton on the results from unconstraned optmzaton. If Transformaton ŝ g < 0 ˆr 1 > 0 ŝ g ŝ g ˆθ ˆθ + π ŝ g > 0 ˆr 1 < 0 ˆr 1 ˆr 1 ˆθ ˆθ + π ŝ g < 0 ˆr 1 < 0 ŝ g ŝ g ˆr 1 ˆr 1 Usng the range measurements, the global scale s g, ntal atttude headng θ, and ntal radus r 1 can be estmated by least-squares optmzaton. Stacng the K range measurements and the three parameters nto vectors ρ = [ρ 1,ρ 2,...,ρ K ] T and F(ξ ) = [ c (W) 1, c (W) 2,..., c (W) K ]T wth ξ = [s g,θ,r 1 ] T, the problem can be formulated as ˆξ = argmn ρ F(ξ ) 2 Q 1 s.t. Bξ > 0. (12) ξ [ ] The nequalty constrants are due to the postveness of both the scale s g and the ntal true range r 1. B = s a selecton matrx used to set the constrants. Q s the covarance matrx of the nose η = [η 1,η 2,...,η K ] T. Due to the presence of several local mnma and the bounded search space, t s challengng to solve the nonlnear nequalty constraned optmzaton n Eq. (12). However, not all mnma volatng the constrants represent erroneous soluton, due to the symmetrc propertes of the objectve functon. Defne A (s g,θ,r 1 ) = (r (ξ ) ρ ) 2. For any s g,θ and r 1, the value of object functon s nvarant to the followng parameter change: A (s g,θ,r 1 ) = A ( s g,θ + π,r 1 ) =A (s g,θ + π, r 1 ) = A ( s g,θ, r 1 ). (13) Consequently, we can obtan the estmates of the parameters by solvng the correspondng unconstrant problem and transform the results obtaned wth the relatons gven n Table 1. The unconstraned optmzaton can be obtaned teratvely by solvng a lnearzed problem as ˆξ = argmn ρ J ξ ξ 2 (14) ξ Q 1 ˆξ +1 = ˆξ ( + J ξ ( ˆξ ) T Q 1 J ξ ( ˆξ 1 )) Jξ ( ˆξ ( ) T Q 1 ρ F( ˆξ ) ) where J ξ s the Jacoban matrx assocated to the functon F(ξ ). In order to solve the problem n Eq. (14), K 3 range measurements are requred. Due to the hgh nonlnearty of the objectve functon, the Levenberg-Marquardt algorthm [13] s appled, nstead of a Gauss-Newton approach [14], n order to explot ts better global mnmzaton capabltes. As a result, the global scale factor s g s estmated by combnng rangng and vsual measurements. Consequently, based on the global scale, the poston of the rover {ĉ (W) } and the coordnates of the map ponts { X (W) } can be obtaned wthout scale ambguty. In addton, the ntal headng of the rover θ, whch refers to the radal drecton between the rover and the staton, can be obtaned from the estmaton. It should be mentoned that the ranges are nvarant to the change of the ntal polar angle α. Hence wth a sngle rado ln, the absolute poston of the rover n reference frame (W) s ambguous by the angle. The ambguty can be resolved only f addtonal set-ups are avalable, e.g., by connectng to a second base staton, or by usng an antenna array to estmate the angle of arrval. SIMULATION RESULTS The proposed scale estmaton method usng sparse range measurements s tested n smulaton usng KITTI benchmar datasets [15]. We use the odometry dataset wth provded ground truth to verfy the result. The range measurements {ρ } are smulated from the true ranges wth addtve Gaussan nose. Fg. 3 shows the estmaton result at eyframe 100 from KITTI odometry dataset 07. The mage at the current eyframe s shown n the upper-left plot and the estmated trajectory n the navgaton frame untl current tme nstant s at upper-rght. The uncertanty of the range measurements s 1 [m]. The fgure at bottom-left compares the whole trajectory n estmated global scale wth the ground truth data. The true trajectory s plotted n blue color, and the estmated one s n red. It can be observed that the scaled trajectory usng the estmated ŝ g algns well wth the ground truth. The bottom-rght curve reflects the change the scale estmaton error over tme. The result converges qucly and the error remans low after 20 eyframes. As a comparson, Fg. (15)

6 Fgure 3: Scale estmaton usng KITTI odometry dataset 07 Fgure 4: True scale and the ntal guess for KITTI odometry dataset 07

7 Fgure 5: Impact of rangng error on the scale estmaton error 4 llustrates the dfference between the true global scale and the ntal guess wthout any scale estmaton. Snce a monocular camera cannot provde any global scale nformaton, the red trajectory s scaled usng s g = 1. To analyze the mpact of the rangng nose on the estmaton result, we execute the smulaton by addng dfferent rangng nose on the measurements. Fg. 5 vsualzes the result of the global scale estmaton error wth respect to the rangng nose level. The mages used n the smulaton are taen from KITTI dataset 07. The curve shows the root-mean-square error (RMSE) of global scale estmaton wth respect to the standard devaton of the range measurements. It can be seen that the scale estmaton error s superlnearly dependent on the range measurements error. The curve s generated usng KITTI dataset 07 wth ground truth scale s g = The relatve error s less than 0.8% when the rangng uncertanty s below 1 meter. In practce, dependent on the requrements and constrants for the SLAM scenaro, hgher rangng accuracy than 1 meter s feasble through sgnal desgn. Fg. 6, Fg. 7 and Fg. 8 shows the estmated poses of the rover usng mages from KITTI odometry dataset 04, 03 and 07 by applyng the jontly estmated parameters ˆξ. The dataset 04 contans mages taen from a lnear trajectory wthout any drecton change. The three datasets represent dfferent typcal motons of a dynamc rover. The dataset 03 s a forward trajectory wth a few turns, and the dataset 07 conssts of mages taen from motons n a closed loop. In the fgures, the red trajectores are the ground truth, and the blue ones are the estmated trajectores. In the smulaton, the standard devaton of the rangng nose s 1 meter. It can be conclude from the results that usng the proposed method, the poses of the rover can be well estmated wth only a sngle camera and sparse rangng measurements from a sngle base staton. CONCLUSION For VSLAM applcatons based on monocular cameras, the estmated postons of the camera and the map ponts have a global scale ambguty. We propose a method that explots rangng measurements from a sngle staton to estmate the scale factor n planar moton cases. Applyng the symmetry property of the cost functon, one can solve an unconstraned optmzaton whle preserves the postveness of the scale by usng a transformaton. The proposed method s verfed on real mages from benchmar datasets, and t s shown that the camera poses can be accurately estmated wthout global scale ambguty. ACKNOWLEDGMENT The project VaMEx-CoSMC s supported by the Federal Mnstry for Economc Affars and Energy on the bass of a decson by the German Bundestag, grant 50NA1521 admnstered by DLR Space Admnstraton. References [1] M. Mamone, Y. Cheng, and L. Matthes, Two years of vsual odometry on the mars exploraton rovers, Journal of Feld Robotcs, vol. 24, no. 3, pp , 2007.

8 Fgure 6: Pose estmaton usng KITTI odometry dataset 04 Fgure 7: Pose estmaton usng KITTI odometry dataset 03

9 Fgure 8: Pose estmaton usng KITTI odometry dataset 07 [2] G. Klen and D. Murray, Parallel tracng and mappng for small ar worspaces, n Mxed and Augmented Realty, ISMAR th IEEE and ACM Internatonal Symposum on, Nov 2007, pp [3] H. Strasdat, J. Montel, and A. J. Davson, Scale drft-aware large scale monocular slam. n Robotcs: Scence and Systems, vol. 2, no. 3, 2010, p. 5. [4] R. Mur-Artal, J. M. M. Montel, and J. D. Tardos, Orb-slam: A versatle and accurate monocular slam system, IEEE Transactons on Robotcs, vol. 31, no. 5, pp , Oct [5] J. Engel, T. Schöps, and D. Cremers, Lsd-slam: Large-scale drect monocular slam, n Computer Vson - ECCV 2014, ser. Lecture Notes n Computer Scence, D. Fleet, T. Pajdla, B. Schele, and T. Tuytelaars, Eds. Sprnger Internatonal Publshng, 2014, vol. 8690, pp [6] M. Achtel, M. Achtel, S. Wess, and R. Segwart, Onboard mu and monocular vson based control for mavs n unnown n- and outdoor envronments, n Robotcs and Automaton (ICRA), 2011 IEEE Internatonal Conference on, May 2011, pp [7] G. Nütz, S. Wess, D. Scaramuzza, and R. Segwart, Fuson of mu and vson for absolute scale estmaton n monocular slam, Journal of Intellgent and Robotc Systems, vol. 61, no. 1-4, pp , [8] A. Tabbazar and O. Basr, Rado-vsual sgnal fuson for localzaton n cellular networs, n Multsensor Fuson and Integraton for Intellgent Systems (MFI), 2010 IEEE Conference on, Sept 2010, pp [9] C. Zhu, G. Gorg, and C. Günther, Scale and 2d relatve pose estmaton of two rovers usng monocular cameras and range measurements, n Proceedngs of the 29th Internatonal Techncal Meetng of The Satellte Dvson of the Insttute of Navgaton (ION GNSS+ 2016), Portland, Oregon. Insttute of Navgaton, 2016, pp [10] E. Staudnger, S. Zhang, A. Dammann, and C. Zhu, Towards a rado-based swarm navgaton system on mars - ey technologes and performance assessment, n Wreless for Space and Extreme Envronments (WSEE), 2014 IEEE Internatonal Conference on, Oct 2014, pp [11] R. Hartley and A. Zsserman, Multple vew geometry n computer vson. Cambrdge unversty press, [12] D. Scaramuzza and F. Fraundorfer, Vsual odometry [tutoral], Robotcs & Automaton Magazne, IEEE, vol. 18, no. 4, pp , 2011.

10 [13] J. J. Moré, The levenberg-marquardt algorthm: mplementaton and theory, n Numercal analyss. Sprnger, 1978, pp [14] Y. Wang, Gauss newton method, Wley Interdscplnary Revews: Computatonal Statstcs, vol. 4, no. 4, pp , [15] A. Geger, P. Lenz, and R. Urtasun, Are we ready for autonomous drvng? the tt vson benchmar sute, n Conference on Computer Vson and Pattern Recognton (CVPR), 2012.