arxiv: v1 [cs.cv] 11 Jan 2019

Transcription

1 A General Opimizaion-based Framework for Global Pose Esimaion wih Muliple Sensors arxiv: v1 [cs.cv] 11 Jan 219 Tong Qin, Shaozu Cao, Jie Pan, and Shaojie Shen Absrac Accurae sae esimaion is a fundamenal problem for auonomous robos. To achieve locally accurae and globally drif-free sae esimaion, muliple sensors wih complemenary properies are usually fused ogeher. Local sensors (camera, IMU, LiDAR, ec) provide precise pose wihin a small region, while global sensors (GPS, magneomeer, baromeer, ec) supply noisy bu globally drif-free localizaion in a largescale environmen. In his paper, we propose a sensor fusion framework o fuse local saes wih global sensors, which achieves locally accurae and globally drif-free pose esimaion. Local esimaions, produced by exising VO/VIO approaches, are fused wih global sensors in a pose graph opimizaion. Wihin he graph opimizaion, local esimaions are aligned ino a global coordinae. Meanwhile, he accumulaed drifs are eliminaed. We evaluae he performance of our sysem on public daases and wih real-world experimens. Resuls are compared agains oher sae-of-he-ar algorihms. We highligh ha our sysem is a general framework, which can easily fuse various global sensors in a unified pose graph opimizaion. Our implemenaions are open source1. Fig. 1. KITTI daase resuls of he proposed sensor fusion framework (VO + GPS). The op of his figure is a pair of sereo images. The lef boom par is he esimaed rajecory and feaure poins, while he righ boom par is he esimaed global rajecory aligned wih Google map. I. I NTRODUCTION Auonomous robo has become a popular research opic over he las decades. We have seen an increasing demand for robos in various applicaions, such as auonomous driving, inspecion, search and rescue. One of he fundamenal echnologies for auonomous asks is localizaion. Robos require precise 6-DoF (Degrees of Freedom) poses for navigaion and conrol. A lo of sesnors have been used for local pose esimaion. The Radar and LiDAR are widely used in he confined indoor environmen, while he camera and IMU are applicable in boh indoor and oudoor environmens. There are many impressive algorihms for local pose esimaion, such as visual-based mehod [1] [5], and visual-inerialbased mehod [6] [11]. These algorihms achieves incremenal and accurae sae esimaion wihin a local region. However, here are several drawbacks limiing he usage of hese algorihms in pracice. The firs drawback of local pose esimaion algorihems is ha hey produce pose esimaion in a local frame (wih respec o he saring poin) wihou a global coordinae. We may ge differen esimaions when we sar from differen poins even in he same environmen. Hence, hey are unfriendly o reuse wihou a fixed global coordinae. The second drawback is ha due o he lack of global measuremens, he local esimaions are prone o accumulaed drifs in he All auhors are wih he Deparmen of Elecronic and Compuer Engineering, Hong Kong Universiy of Science and Technology, Hong Kong, China. {ong.qin, shaozu.cao, jie.pan}@connec.us.hk, eeshaojie@us.hk. 1 hps://gihub.com/hkust-aerial-roboics/vins-fusion long run. Alhough some vision-based loop closure mehods were proposed o eliminae drifs, hey canno handle he large-scale environmen wih he mass daa. Compared wih local sensors, global sensors, such as GPS, baromeers, and magneomeers, have advanages in global localizaion wihin large-scale environmens. They provide global measuremens wih respec o he fixed earh frame, which is drif-free. However, heir measuremens are usually unsmooh and noisy, which canno be direcly used for precise conrol and navigaion. Taking GPS as an example, i can measure approximae locaion in meers, bu measuremens are disconinuous a a low rae. Also, i only measures 3D posiion wihou 3D orienaion. Therefore, only global sensors are insufficien for real-ime 6-DoF sae esimaion. Since local sensors (camera, IMU and LiDAR) achieves impressive performance in local accuracy and global sensors (GPS, magneomeer and baromeer) are drif-free, i is a smar way o fuse hem ogeher o achieve locally accurae and globally drif-free 6-DoF pose esimaion. In order o increase he robusness, we wan o fuse sensors as many as possible. Consequenly, a general framework which suppors muliple sensors is required. Alhough radiional EKF-based mehods can fuse he local esimaion ino he global frame gradually, an accurae iniial guess abou he ransformaion beween differen frames is required o guaranee convergence. Also, he EKF mehods are sensiive o ime synchronizaion. Any lae-coming measuremens will cause rouble since saes canno be propagaed back in filer procedure.

2 Fig. 2. An illusraion of he proposed framework srucure. The global esimaor fuses local esimaions wih various global sensors o achieve locally accurae and globally drif-free pose esimaion. To his end, we use an opimizaion-based mehod o solve his problem, which is suiable for muliple sensor fusion inherenly. In his paper, we propose an opimizaion framework o fuse local esimaions wih global sensor measuremens. Local esimaions come from exising sae-of-he-ar VO/VIO works. Global sensors are reaed as general facors in pose graph. Local facors and global sensor facors are summed up ogeher o build he opimizaion problem. Our sysem achieves locally accurae and globally drif-free sae esimaion. We highligh he conribuion of his paper as follows: a general framework o fuse various global sensors wih local esimaions, which achieves locally accurae and globally drif-free localizaion. an evaluaion of he proposed sysem on boh public daases and real experimens. open-source code for he communiy. II. RELATED WORK Recenly, muliple sensor fusion approach for sae esimaion has become a popular rend in order o improve boh accuracy and robusness. By he ype of sensors employed in he sysem, research works can be classified as local localizaion and global-aware localizaion. For local localizaion, cameras, IMU, LiDAR and RGB-D sensors are usually used for 6-DoF sae esimaion in small environmen. Impressive approaches over he las decades include visual-based mehods [1] [5], LiDAR-based mehods [12], RGB-D based mehods [13], and even-based mehods [14]. There are also some muli-sensor fusion mehods, such as visual-inerial fusion [6] [11, 15, 16] and visual-lidar fusion [17]. Among hese work, [6, 7, 9] are filer-based mehods while [8, 1, 11] are opimizaion-based mehods. In opimizaion-based framework, a lo of visual measuremens and inerial measuremens are kep in a bundle. The saes relaed o he observed measuremens are opimized ogeher. One advanage of opimizaion-based approaches over EKFbased ones is ha saes can be ieraively linearized o increase accuracy. Boh filer-based mehods and opimizaionbased mehods can achieve highly accurae sae esimaion. Due o he lack of global measuremen, he accumulaed drifs is unavoidable over ime. For global-aware localizaion, global sensors (GPS, magneomeer, baromeer, ec) are incorporaed in he sysem. Global sensor measures absolue quaniies wih respec o he earh frame, which are independen of he saring poin. Global measuremens are usually noisy and lowfrequency, so hey canno be used alone. Therefore, global sensors are usually fused wih local sensors for accurae and global-aware localizaion. [18] proposed an EKF-based algorihm o fuse visual measuremen wih inerial and GPS measuremen o ge drif-free esimaion. [19] used UKF (Unscened Kalman Filer) algorihm o fuse visual, LiDAR and GPS measuremens, which is an exension of EKF wihou analyic Jacobians. Filer-based mehods are sensiive o ime synchronizaion. Any lae-coming measuremens will cause rouble since saes canno be propagaed back in filer procedure. Hence, special ordering mechanism is required o make sure ha all measuremens from muliple sensors are in order. Compared wih filer-based mehod, opimizaion-based mehod have advanage in his aspec. Because he big bundle serves as a naure buffer, i can wai and sore measuremens for a long ime. [2] used an opimizaion-based framework o fuse local VIO (Visual Inerial Odomery) wih GPS measuremens, which produced more accurae resuls han mehod proposed in [18]. The ransformaion beween local coordinae and global coordinae was frequenly opimized in his approach. Few research works fuse sensors more han hree ypes. In his paper, we propose a more general opimizaion-based framework for global localizaion, which can suppor muliple global sensors. Each sensor serves as a general facor, which can be easily added ino he opimizaion problem. III. SYSTEM OVERVIEW According o he measuremen s reference frame, we caegory sensors ino local and global ypes. 1) Local Sensors: Camera, LiDAR, IMU (acceleromeer and gyroscope), ec. This kind of sensor is no globally referenced, hus a reference frame is usually needed. In

3 general, he firs pose of he robo is se as he origin in order o boo up he sensor. The esimaion of he robo s pose incremenally evolves from he saring poin. Therefore, accumulaed drif will grow wih he disance from saring poin. 2) Global Sensors: GPS, magneomeer, baromeer, ec. This kind of sensor is globally referenced. I always works under a fixed global frame, such as he earh frame. The origin of he reference frame is fixed and known in advance. Their measuremens are global-referenced bu noisy. The error is independen of raveled disance. For GPS, i measures absolue longiude, laiude and aliude wih respec o he earh. The longiude, laiude, and aliude can be convered o x, y and z coordinae. For magneomeer, i measures magneic field direcion and srengh, which can deermine he orienaion. For baromeer, i measures air pressure, which can be convered o heigh. The srucure of our framework is shown in Fig. 2. Local sensors (camera and IMU) are used in local esimaion. The exising VO/VIO approaches are adoped o produce local poses. Local resuls and global sensors are inpu ino a global pose graph. They are convered o unified facors o consruc he opimizaion problem. The global esimaor generaes locally accurae and globally aware 6-DoF pose resuls. A. Local Pose Esimaion IV. METHODOLOGY For local pose esimaion, we adop exising VO (Visual Odomery)/VIO (Visual-Inerial Odomery) algorihms. There are many impressive VO/VIO algorihms, such as [4, 6] [11]. Any of hem can be used as local pose esimaion in our framework, as long as i produces 6-DoF poses. This par is no he main conribuion of his paper. For he compleeness, we briefly inroduce our previous VIO algorihm [11], which is used in our open-source implemenaions. The VIO esimaes poses of several IMU frames and feaures deph wihin a sliding window. The saes are defined as: X l = [x, x 1, x n, λ, λ 1, λ m ] x k = [ p l b k, v l b k, q l b k, b a, b g ], k [, n], (1) where he k-h IMU sae x k consiss of he posiion p l b k, velociy vb l k, orienaion q l b k of IMU s cener wih respec o local reference frame l. We use quaernion o represen orienaion. The firs IMU pose is se as reference frame. b a and b g are acceleromeer bias and gyroscope bias respecively. Feaures are parameerized by heir inverse deph λ when firs observed in camera frame. The esimaion is formulaed as a nonlinear leas-squares problem, min X l { r p H p X 2 + r B (ẑ b k bk+1, X ) k B (l,j) C ρ( rc (ẑ cj l 2 + P b k bk+1, X ) 2 P c j l ), (2) Fig. 3. An illusraion of he global pose graph srucure. Every node represens one pose in world frame, which conains posiion and orienaion. The edge beween wo consecuive nodes is a local consrain, which is from local esimaion (VO/VIO). Oher edges are global consrains, which come from global sensors. where r B (ẑ b k bk+1, X ) and r C (ẑ cj l, X ) represen inerial and visual residuals respecively. The prior erm, {r p, H p }, conains informaion abou pas marginalized saes. ρ( ) represens robus huber norm [21]. The deailed explanaion can be found a [11]. The VIO achieves accurae real-ime 6-DoF pose esimaions in he local frame. B. Global Pose Graph Srucure The illusraion of he global pose graph srucure is shown in Fig. 3. Every pose, which conains posiion and orienaion in world frame, serves as one node in he pose graph. The densiy of nodes is deermined by he lowes-frequency sensor. The edge beween wo consecuive nodes is a local consrain, which is from local esimaion (VO/VIO). Tha edge consrains he relaive pose from one node o anoher node. Oher edges are global consrains, which come from global sensors. The naure of pose graph opimizaion is an MLE (Maximum Likelihood Esimaion) problem. The MLE consiss of he join probabiliy disribuion of robo poses over a ime period. Variables are global poses of all nodes, X = {x, x 1,..., x n }, where x i = {p w i, qw i }. pw and q w are posiion and orienaion under he global frame. Under he assumpion ha all measuremen probabiliies are independen, he problem is ypically derived as, X = arg max X n p(z k X ), (3) = k S where S is he se of measuremens, which includes local measuremens (VO\VIO) and global measurmens (GPS, magneomeer, baromeer and so on). We assume he uncerainy of measuremens are Gaussian disribuion wih mean and covariance, which is p(z k X ) N ( z k, Ω k ). Therefore, he above-menioned equaion is derived as, X = arg max X = arg min X n exp( 1 2 = k S z k h k (X ) 2 ) Ω k n z k h k (X ) 2. Ω k = k S The Mahalanobis norm is r 2 Ω = rt Ω 1 r. Then he sae esimaion is convered o a nonlinear leas squares problem, which is also known as Bundle Adjusmen (BA). (4)

4 C. Sensor Facors 1) Local Facor: Since he local esimaion (VO/VIO) is accurae wihin a small region, we ake advanage of he relaive pose beween wo frames. Considering wo sequenial frame 1 and frame, he local facor is derived as, z l h l (X ) = z l h l (x 1, x ) [ ] q l 1 = 1 (p l p l 1) 1 q l 1 q l [ q w 1 1 ] (p w p w 1) 1, q w 1 where (q l 1, p l 1) and (q l, p l ) are poses a ime 1 and in local frame from VO/VIO. is he minus operaion on he error sae of quaernion. The firs row represens relaive posiion error beween wo poses, and he second row represens relaive roaion error beween wo poses. If he VO/VIO algorihm produces he covariance marix of poses, we use i as he covariance of local measuremens. Oherwise, we use he unified covariance for all local measurmens. 2) GPS Facor: Raw measuremens of GPS are longiude, laiude, and aliude, which are no in x,y, and z-axis coordinaes. Generally, we can conver longiude, laiude and aliude ino ECEF (Earh Cenred Earh Fixed), ENU (local Eas Norh Up), and NED (local Norh Eas Down) coordinaes. Here, we ake ENU coordinae as he example. By seing he firs GPS measuremen as he origin poin, we ge GPS s measrumens in he ENU world frame, p GP S = [x w, y w, z w ] T. The GPS facor is derived as, z GP S h GP S (X ) = z GP S q w (5) h GP S (x ) = p GP S p w. (6) The GPS measuremens direcly consrain he posiion of every node. The covariance is deermined by he number of saellies when he measuremen is received. The more saellies i receives, he smaller covariance i is. 3) Magneomeer Facor: The magneomeer can measure a vecor of he magneic field inensiy. The direcion of his vecor can help o deermine orienaion in he world frame. We assume he magneomeer is calibraed offline wihou offse or bias. Firs of all, we lookup able o ge he magneic inensiy z w of he local region in he ENU coordinae. We assume ha he magneic inensiy z w is consan wihin his area. Our measuremen is denoed as z m. The orienaion of z m should mach z w if we pu he sensor coinciding wih he ENU coordinae. Inspired by his, he facor is derived as: z m h m (X ) = z m h m (x ) = zm z m qm b q w 1 z w z w, (7) where q m b is he ransformaion from robo s cener o he magneomeer s cener, which is known and calibraed offline. Since he magneic field is easily affeced by he environmen, we only use he normalized vecor wihou lengh. The lengh is used o deermine covariance. If he lengh of measuremen differs z w a lo, we se a big covariance. Oherwise, we use a small covariance. 4) Baromeer Facor: The baromeer measures he air pressure in an area. We assume ha he air pressure is consan a one aliude over a period of ime. So he air pressure can be convered o heigh linearly. As he same as GPS, we se he firs measuremen as he origin heigh. Then we ge he measuremen of heigh z m. Inuiively, he facor is a residual of heigh esimaion, which is wrien as: z m h m (X ) = z m h m (x ) = z m z. (8) Since his measuremen is noisy, we caculae he variance of several measurmens wihin a shor ime, and use i in he cos funcion. 5) Oher Global Facors: Though we only specify GPS, magneomeer, and baromeer facors in deail, our sysem is no limied o hese global sensors. Oher global sensors and even some arificial sensors, such as Moion Capure sysem, WiFi and Blueooh fingerprin, can be used in our sysem. The key is o model hese measuremens as residual facors under one global frame. D. Pose Graph Opimizaion Once he graph is buil, opimizing i equals o finding he configuraion of nodes ha mach all edges as much as possible. Ceres Solver [22] is used for solving his nonlinear problem, which uilizes Gaussian-Newon and Levenberg- Marquad approaches in an ieraive way. We run pose graph opimizaion a low frequency (1Hz). Afer every opimizaion, we ge he ransformaion for local frame o global frame. Therefore, We can ransform subsequen high-rae local poses (VO/VIO, 2Hz) by his ransformaion o achieve real-ime high-rae global poses. Since he pose graph is quie sparse, he compuaion complexiy increases linearly wih he number of poses. We can keep a huge window for pose graph opimizaion o ge accurae and globally drif-free pose esimaion. When he compuaion complexiy exceeds real-ime capabiliy, we hrow old poses and measuremens, and keep he window in a limied size. V. EXPERIMENTAL RESULTS We evaluae he proposed sysem wih visual and inerial sensors boh on daases and wih real-world experimens. In he firs experimen, we compare he proposed algorihm wih anoher sae-of-he-ar algorihm on public daases. We hen es our sysem wih self-developed sensor suie in he real-world oudoor environmen. The numerical analysis is generaed o show he accuracy of our sysem in deail. A. Daases We evaluae our proposed sysem using KITTI Daases [23]. The daases are colleced onboard a vehicle, which conains sereo images (Poin Grey Flea 2, 1382x512 monochrome, 1 FPS) and GPS. Images are synchronized and recified. The ransformaion beween sensors is calibraed. Also, he ground ruh saes are

5 Roaion Error [deg/m] Translaion Error [%] Relaive Pose Error in 1_3_drive_ Pah Lengh [m] Pah Lengh [m] Fig. 4. Roaion error and ranslaion error plo in 1 3 drive 42. Roaion Error [deg/m] Translaion Error [%] Relaive Pose Error in 9_3_drive_ Pah Lengh [m] Pah Lengh [m] Fig. 6. Trajecories of one KITTI sequence (9 3 drive 33) recovered from and he proposed algorihm. TABLE I RMSE [24] IN KITTI DATASETS IN METERS. Sequences Lengh[km] RMSE[m] 9 3 drive drive drive drive drive drive drive drive Fig. 5. Roaion error and ranslaion error plo in 9 3 drive 33. provided by he Inerial Navigaion Sysem (OXTS RT 33). We run daases wih sereo cameras, laiude, longiude, and aliude from raw GPS measuremens. The sereo cameras are used for local sae esimaion. The local resuls are fused wih GPS measuremens in global opimizaion. In his experimen, we compare our resuls wih ORB- SLAM [4], a sae-of-he-ar visual odomery mehod ha works wih sereo cameras. Orb-slam is an opimizaionbased algorihm wih powerful relocalizaion capabiliy. I mainains a map of keyframes and landmarks. We evaluae RPE (Relaive Pose Errors) and ATE (Relaive Trajecory Error) one resuls produced by proposed mehod and ORB- SLAM. The RPE is caculaed by he ool proposed in [23]. The posiion and roaion RPE of wo sequences are shown in Fig. 4 and Fig. 5 respecively. For ranslaion error, he proposed mehod is obviously lower han. I shows ha he posiion drif is effecively eliminaed by GPS measuremen. For roaion error, he proposed mehod is no beer han. Because GPS can direcly measure roaion angles, i canno improve local accuracy on roaion. The RMSE (Roo Mean Square Errors) of absolue rajecory error for more sequences in KITTI daases is shown in Table. I. Esimaed rajecories are aligned wih he ground ruh by Horn s mehod [25]. For all sequences, he proposed mehod ouperforms orb-slam, which demonsraes ha fusing GPS measuremens effecively increases he accuracy of he esimaed rajecory. Inuiively, GPS correcs accumulaed drifs in he long run. Trajecories of one KITTI sequence (9 3 drive 33) recovered from and he proposed algorihm are shown in Fig. 6. Trajecories are aligned wih Google map from he bird-eye view. From he picure, we can see ha he esimaed rajecory of drifs several meers a he end. The esimaed rajecory of proposed mehod maches he road nework well. This experimen demonsraes ha proposed sysem has advanage of pose esimaion in a long disance. B. Real-world experimen In his experimen, we used self-developed sensor suie which were equipped wih muliple sensors. The sensor suie is shown in Fig. 7. I conains sereo cameras (mvbluefox-

6 Fig. 7. The sensor suie we used for he oudoor experimen, which conains wo forward-looking global shuer cameras (MarixVision mvbluefox-mlc2w) wih 752x48 resoluion. We use he buil-in IMU, magneomeer, baromeer and GPS from he DJI A3 fligh conroller. Fig. 1. The rajecories of our large-scale oudoor experimen recovered from VIO and he proposed algorihm (VIO + GPS + magneomeer + baromeer) respecively. Oudoor Trajecory 2 VIO MSF RTK GPS 15 1 y [m] x [m] Fig. 8. The rajecories of our small-scale oudoor experimen recovered from VIO, he proposed algorihm and MSF respecively. We use RTK rajecory as ground ruh. Translaion error [m] Relaive Pose Error in Oudoor Daase VIO MSF Disance [m] Fig. 9. VI. C ONCLUSION Relaive pose error plo in our small scale oudoor experimen. TABLE II RMSE[ M ] IN OUTDOOR EXPERIMENT. Sequence Lengh[m] Oudoor1 Oudoor2 Oudoor VIO MLC2w, 2Hz) and DJI A3 conroller2, which inculdes buil-in IMU, magneomeer, baromeer and GPS receiver. We also equip i wih RTK (Real-Time Kinemaic)3 receiver for high-accurae localizaion. The RTK base saion is esablished on he op of a building. The RTK can provide precise posiioning of one cenimeer verically and horizonally, which is reaed as ground ruh. We run saes esimaion wih all available sensors. For accuracy comparison, we walked wo circles on he ground. We compare our resuls agains MSF [18], which fuses visual odomery, inerial measuremens, and GPS in an EKF-based framework. The rajecory comparison is shown in Fig. 8, and he RPE (Relaive Pose Error) is ploed in Fig. 9. We can see obvious ranslaion drif in he esimaed rajecory of VIO. From he relaive pose error, we can see ha he proposed sysem improved he accuracy of VIO a lo. Also, he proposed sysem ouperforms MSF [18]. The RMSE of more oudoor experimens is shown in Table. II. Our sysem achieved he bes performance in all sequence. We also perform a larger oudoor experimen and compared he resuls wih Google map. Esimaed rajecoies are shown in Fig. 1. The esimaed rajecory of VIO drifed along wih disance. Thanks o he global sensors, he rajecory of proposed sysem is almos drif-free, which maches he road map very well. RMSE[m] MSF In his paper, we propose a opimizaion-based framework o fuse local esimaions wih global sensors. Local esimaions came from previous VO/VIO work. Global sensors are reaed as general facors in pose graph opimizaion. This sysem achieves locally accurae and globally drif-free pose esimaion. We demonsrae he impressive performance of our sysem on public daases and wih real-world experimens. 2 hp:// 3 hps://

7 REFERENCES [1] G. Klein and D. Murray, Parallel racking and mapping for small ar workspaces, in Mixed and Augmened Realiy, 27. IEEE and ACM Inernaional Symposium on, 27, pp [2] C. Forser, M. Pizzoli, and D. Scaramuzza, SVO: Fas semi-direc monocular visual odomery, in Proc. of he IEEE In. Conf. on Robo. and Auom., Hong Kong, China, May 214. [3] J. Engel, T. Schöps, and D. Cremers, Lsd-slam: Large-scale direc monocular slam, in European Conference on Compuer Vision. Springer Inernaional Publishing, 214, pp [4] R. Mur-Aral, J. Moniel, and J. D. Tardos, Orb-slam: a versaile and accurae monocular slam sysem, IEEE Trans. Robo., vol. 31, no. 5, pp , 215. [5] J. Engel, V. Kolun, and D. Cremers, Direc sparse odomery, IEEE Transacions on Paern Analysis and Machine Inelligence, 217. [6] A. I. Mourikis and S. I. Roumeliois, A muli-sae consrain Kalman filer for vision-aided inerial navigaion, in Proc. of he IEEE In. Conf. on Robo. and Auom., Roma, Ialy, Apr. 27, pp [7] M. Li and A. Mourikis, High-precision, consisen EKF-based visualinerial odomery, In. J. Robo. Research, vol. 32, no. 6, pp , May 213. [8] S. Leuenegger, S. Lynen, M. Bosse, R. Siegwar, and P. Furgale, Keyframe-based visual-inerial odomery using nonlinear opimizaion, In. J. Robo. Research, vol. 34, no. 3, pp , Mar [9] M. Bloesch, S. Omari, M. Huer, and R. Siegwar, Robus visual inerial odomery using a direc ekf-based approach, in Proc. of he IEEE/RSJ In. Conf. on Inell. Robos and Sys., 215, pp [1] R. Mur-Aral and J. D. Tardós, Visual-inerial monocular slam wih map reuse, IEEE Roboics and Auomaion Leers, vol. 2, no. 2, pp , 217. [11] T. Qin, P. Li, and S. Shen, Vins-mono: A robus and versaile monocular visual-inerial sae esimaor, IEEE Trans. Robo., vol. 34, no. 4, pp , 218. [12] J. Zhang and S. Singh, Loam: Lidar odomery and mapping in realime. in Roboics: Science and Sysems, vol. 2, 214, p. 9. [13] C. Kerl, J. Surm, and D. Cremers, Dense visual slam for rgb-d cameras, in Proc. of he IEEE/RSJ In. Conf. on Inell. Robos and Sys. [14] H. Rebecq, T. Horsschaefer, G. Gallego, and D. Scaramuzza, Evo: A geomeric approach o even-based 6-dof parallel racking and mapping in real ime, IEEE Roboics and Auomaion Leers, vol. 2, no. 2, pp , 217. [15] G. P. Huang, A. I. Mourikis, and S. I. Roumeliois, Observabiliybased rules for designing consisen ekf slam esimaors, In. J. Robo. Research, vol. 29, no. 5, pp , 21. [16] H. Liu, M. Chen, G. Zhang, H. Bao, and Y. Bao, Ice-ba: Incremenal, consisen and efficien bundle adjusmen for visual-inerial slam, in Proc. of he IEEE In. Conf. on Paern Recogniion, 218, pp [17] J. Zhang and S. Singh, Visual-lidar odomery and mapping: Lowdrif, robus, and fas, in Proc. of he IEEE In. Conf. on Robo. and Auom. IEEE, 215, pp [18] S. Lynen, M. W. Achelik, S. Weiss, M. Chli, and R. Siegwar, A robus and modular muli-sensor fusion approach applied o mav navigaion, in Proc. of he IEEE/RSJ In. Conf. on Inell. Robos and Sys. IEEE, 213, pp [19] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, Muli-sensor fusion for robus auonomous fligh in indoor and oudoor environmens wih a roorcraf MAV, in Proc. of he IEEE In. Conf. on Robo. and Auom., Hong Kong, China, May 214, pp [2] R. Mascaro, L. Teixeira, T. Hinzmann, R. Siegwar, and M. Chli, Gomsf: Graph-opimizaion based muli-sensor fusion for robus uav pose esimaion, in Inernaional Conference on Roboics and Auomaion (ICRA 218). IEEE, 218. [21] P. Huber, Robus esimaion of a locaion parameer, Annals of Mahemaical Saisics, vol. 35, no. 2, pp , [22] S. Agarwal, K. Mierle, and Ohers, Ceres solver, hp://ceres-solver. org. [23] A. Geiger, P. Lenz, and R. Urasun, Are we ready for auonomous driving? he kii vision benchmark suie, in Proc. of he IEEE In. Conf. on Paern Recogniion, 212, pp [24] J. Surm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, A benchmark for he evaluaion of rgb-d slam sysems, in Proc. of he IEEE/RSJ In. Conf. on Inell. Robos and Sys., 212, pp [25] B. K. Horn, Closed-form soluion of absolue orienaion using uni quaernions, JOSA A, vol. 4, no. 4, pp , 1987.