arxiv: v1 [cs.cv] 11 Jan 2019

Transcription

1 A General Opimizaion-based Framewor for Local Odomery Esimaion wih Muliple Sensors Tong Qin, Jie Pan, Shaozu Cao, and Shaojie Shen arxiv: v1 [cs.cv] 11 Jan 219 Absrac Nowadays, more and more sensors are equipped on robos o increase robusness and auonomous abiliy. We have seen various sensor suies equipped on differen plaforms, such as sereo cameras on ground vehicles, a monocular camera wih an IMU (Inerial Measuremen Uni) on mobile phones, and sereo cameras wih an IMU on aerial robos. Alhough many algorihms for sae esimaion have been proposed in he pas, hey are usually applied o a single sensor or a specific sensor suie. Few of hem can be employed wih muliple sensor choices. In his paper, we proposed a general opimizaion-based framewor for odomery esimaion, which suppors muliple sensor ses. Every sensor is reaed as a general facor in our framewor. Facors which share common sae variables are summed ogeher o build he opimizaion problem. We furher demonsrae he generaliy wih visual and inerial sensors, which form hree sensor suies (sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU). We validae he performance of our sysem on public daases and hrough real-world experimens wih muliple sensors. Resuls are compared agains oher sae-of-he-ar algorihms. We highligh ha our sysem is a general framewor, which can easily fuse various sensors in a pose graph opimizaion. Our implemenaions are open source 1. I. INTRODUCTION Real-ime 6-DoF (Degrees of Freedom) sae esimaion is a fundamenal echnology for roboics. Accurae sae esimaion plays an imporan role in various inelligen applicaions, such as robo exploraion, auonomous driving, VR (Virual Realiy) and AR (Augmened Realiy). The mos common sensors we use in hese applicaions are cameras. A large number of impressive vision-based algorihms for pose esimaion has been proposed over he las decades, such as [1] [5]. Besides cameras, he IMU is anoher popular opion for sae esimaion. The IMU can measure acceleraion and angular velociy a a high frequency, which is necessary for low-laency pose feedbac in real-ime applicaions. Hence, here are numerous research wors fusing vision and IMU ogeher, such as [6] [12]. Anoher popular sensor used in sae esimaion is LiDAR. LiDAR-based approaches [13] achieve accurae pose esimaion in a confined local environmen. Alhough a lo of algorihms have been proposed in he pas, hey are usually applied o a single inpu sensor or a specific sensor suie. Recenly, we have seen plaforms equipped wih various sensor ses, such as sereo cameras on ground vehicles, a monocular camera wih an IMU on mobile phones, sereo All auhors are wih he Deparmen of Elecronic and Compuer Engineering, Hong Kong Universiy of Science and Technology, Hong Kong, China. {ong.qin, jie.pan, shaozu.cao}@connec.us.h, eeshaojie@us.h. 1 hps://gihub.com/hkust-aerial-roboics/vins-fusion Fig. 1. An illusraion of he proposed framewor for sae esimaion, which suppors muliple sensor choices, such as sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU. Each sensor is reaed as a general facor. Facors which share common sae variables are summed ogeher o build he opimizaion problem. cameras wih an IMU on aerial robos. However, as mos radiional algorihms were designed for a single sensor or a specific sensor se, hey canno be pored o differen plaforms. Even for one plaform, we need o choose differen sensor combinaions in differen scenarios. Therefore, a general algorihm which suppors differen sensor suies is required. Anoher pracical requiremen is ha in case of sensor failure, an inacive sensor should be removed and an alernaive sensor should be added ino he sysem quicly. Hence, a general algorihm which is compaible wih muliple sensors is in need. In his paper, we propose a general opimizaion-based framewor for pose esimaion, which suppors muliple sensor combinaions. We furher demonsrae i wih visual and inerial sensors, which form hree sensor suies (sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU). We can easily swich beween differen sensor combinaions. We highligh he conribuion of his paper as follows: a general opimizaion-based framewor for sae esimaion, which suppors muliple sensors. a deailed demonsraion of sae esimaion wih visual and inerial sensors, which form differen sensor suies (sereo cameras, a monocular camera + an IMU, and sereo cameras + an IMU). an evaluaion of he proposed sysem on boh public daases and real experimens.

2 open-source code for he communiy. II. RELATED WORK Sae esimaion has been a popular research opic over he las decades. A large number of algorihms focus on accurae 6-DoF pose esimaion. We have seen many impressive approaches ha wor wih one ind of sensor, such as visual-based mehods [1] [5], LiDAR-based mehods [13], RGB-D based mehods [14]. and even-based mehods [15]. Approaches wor wih a monocular camera is hard o achieve 6-DoF pose esimaion, since absolue scale canno be recovered from a single camera. To increase he observabiliy and robusness, muliple sensors which have complemenary properies are fused ogeher. There are wo rends of approaches for muli-sensor fusion. One is filer-based mehods, he oher is opimizaionbased mehods. Filer-based mehods are usually achieved by EKF (Exended Kalman Filer). Visual and inerial measuremens are usually filered ogeher for 6-DoF sae esimaion. A high-rae inerial sensor is used for sae propagaion and visual measuremens are used for he updae in [9, 16]. MSCKF [6, 7] was a popular EKF-based VIO (Visual Inerial Odomery), which mainained several camera poses and leveraged muliple camera views o form he muli-consrain updae. Filer-based mehods usually linearize saes earlier and suffer from error induced by inaccurae linear poins. To overcome he inconsisency caused by linearized error, observabiliy consrained EKF [17] was proposed o improve accuracy and consisency. An UKF (Unscened Kalman Filer) algorihm was proposed in [18], where visual, Li- DAR and GPS measuremens were fused ogeher. UKF is an exernsion of EKF wihou analyic Jacobians. Filerbased mehods are sensiive o ime synchronizaion. Any lae-coming measuremens will cause rouble since saes canno be propagaed bac in filer procedure. Hence, special ordering mechanism is required o mae sure ha all measuremens from muliple sensors are in order. Opimizaion-based mehods mainain a lo of measuremens and opimize muliple variables a once, which is also nown as Bundle Adjusmen (BA). Compared wih filerbased mehod, opimizaion-based mehod have advanage in ime synchronizaion. Because he big bundle serves as a naure buffer, i can easily handle he case when measurmens from muliple sensors come in disorder. Opimizaion-based algorihms also ouperform he filer-based algorihms in erm of accuracy a he cos of compuaional complexiy. Early opimizaion solvers, such as G2O [19], leveraged he Gauss-Newon and Levenberg-Marquard approaches o solve he problem. Alhough he sparse srucure was employed in opimizaion solvers, he complexiy grown quadraically wih he number of saes and measuremens. In order o achieve real-ime performance, some algorihms have explored incremenal solvers, while ohers bounded he size of he pose graph. isam2 [2] was an efficien incremenal solver, which reused he previous opimizaion resul o reduce compuaion when new measuremens came. The opimizaion ieraion only updaed a small par of saes Fig. 2. A graphic illusraion of he pose graph. Each node represens saes (posiion, orienaion, velociy and so on) a one momen. Each edge represens a facor, which is derived by one measuremen. Edges consrain one sae, wo saes or muliple saes. insead of he whole pose graph. Aferward, an acceleraed solver was proposed in [21], which improved efficiency by reconsrucing dense srucure ino sparse blocs. Mehods, ha eep a fixed sized of pose graph, are called slidingwindow approaches. Impressive opimizaion-based VIO approaches, such as [8, 1, 12], opimized variables over a bounded-size sliding window. The previous saes were marginalized ino a prior facor wihou loss of informaion in [8, 12]. In his paper, we adop a sliding-window opimizaion-based framewor for sae esimaion. III. SYSTEM OVERVIEW The srucure of proposed framewor is shown in Fig. 1. Muliple inds of sensors can be freely combined. The mesuremen of each sensor is reaed as a general facor. Facors and heir relaed saes form he pose graph. An illusraion of pose graph is shown in Fig. 2. Each node represens saes (posiion, orienaion, velociy and so on) a one momen. Each edge represens a facor, which is derived by one measuremen. Facors consrain one sae, wo saes or muliple saes. For IMU facor, i consrains wo consecuive saes by coninuous moion resricion. For a visual landmar, is facor consrains muliple saes since i is observed on muliple frames. Once he graph is buil, opimizing i equals o finding he configuraion of nodes ha mach all edges as much as possible. In his paper, we specifically demonsrae he sysem wih visual and inerial sensors. Visual and inerial sensors can form hree combinaions for 6-DoF sae esimaion, which are sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU. A graphic illusraion of he proposed framewor wih visual and inerial sensors is shown in Fig. 3. Several camera poses, IMU measuremens and visual measuremens exis in he pose graph. The IMU and one of cameras are opional. A. Problem Definiion IV. METHODOLOGY 1) Saes: Main saes ha we need o esimae includes 3D posiion and orienaion of robo s cener. In addiion, we

3 Fig. 3. A graphic illusraion of he proposed framewor wih visual and inerial sensors. The IMU and one of cameras are opional. Therefore, i forms hree ypes (sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU). Several camera poses, IMU measuremens and visual measuremens exis in he pose graph. have oher opional saes, which are relaed o sensors. For cameras, dephs or 3D locaions of visual landmars need o be esimaed. For IMU, i produces anoher moion variable, velociy, Also, ime-varian acceleraion bias and gyroscope bias of he IMU are needed o be esimaed. Hence, for visual and inerial sensors, whole saes we need o esimae are defined as follows: X = [p, R, p 1, R 1,..., p n, R n, x cam, x imu ] x cam = [λ, λ 1,..., λ l ] x imu = [v, b a, b g, v 1, b a1, b g1,..., v n, b an, b gn ], where p and R are basic sysem saes, which correspond o posiion and orienaion of body expressed in world frame. x cam is camera-ralaed sae, which includes deph λ of each feaure observed in he firs frame. x imu is IMU-ralaed variable, which is composed of velociy v, acceleraion bias b a and gyroscope bias b g. x imu can be omied if we only use sereo camera wihou an IMU. The ranslaion from sensors cener o body s cener are assumed o be nown, which are calibraed offline. In order o simplify he noaion, we denoe he IMU as body s cener (If he IMU is no used, we denoe lef camera as body s cener). 2) Cos Funcion: The naure of sae esimaion is an MLE (Maximum Lielihood Esimaion) problem. The MLE consiss of he join probabiliy disribuion of robo poses over a period of ime. Under he assumpion ha all measuremens are independen, he problem is ypically derived as, X = arg max X (1) n p(z X ), (2) = S where S is he se of measuremens, which come from cameras, IMU and oher sensors. We assume he uncerainy of measuremens is Gaussian disribued, p(z X ) N ( z, Ω ). Therefore, he negaive log-lielihood of abovemenioned equaion is wrien as, X = arg max X = arg min X n exp( 1 z 2 h (X ) 2 ) Ω = S n z h (X ) 2. Ω = S (3) The Mahalanobis norm is defined as r 2 Ω = rt Ω 1 r. h( ) is he sensor model, which is deailed in he following secion. Then he sae esimaion is convered o a nonlinear leas squares problem, which is also nown as Bundle Adjusmen (BA). B. Sensor Facors 1) Camera Facor: The framewor suppors boh monocular and sereo cameras. The inrinsic parameers of every camera and he exrinsic ransformaion beween cameras are supposed o be nown, which can be easily calibraed offline. For each camera frame, corner feaures [22] are deeced. These feaures are raced in previous frame by KLT racer [23]. For he sereo seing, he racer also maches feaures beween he lef image and righ image. According o he feaure associaions, we consruc he camera facor wih per feaure in each frame. The camera facor is he reprojecion process, which projecs a feaure from is firs observaion ino following frames. Considering he feaure l ha is firs observed in he image i, he residual for he observaion in he following image is defined as: z l h l (X ) = z l h l (R i, p i, R, p, λ l ) [ ] [ ] u l = v l π c (T b 1 1 c T T i T b 1 u l c π c (λ l, i vi l )), (4) where [u l i, vl i ] is he firs observaion of he l feaure ha appears in he i image. [u l, v] l is he observaion of he same feaure in he image. π c and πc 1 are he projecion and bac-projecion funcions which depend on camera model (pinhole, omnidirecional or oher models). [ T] is he 4x4 R p homogeneous ransformaion, which is. We omi 1 some homogeneous erms for concise expression. T c b is he exrinsic ransformaion from body cener o camera cener, which is calibraed offline. The covariance marix Ω l of reprojecion error is a consan value in pixel coordinae, which comes from he camera s inrinsic calibraion resuls. This facor is universal for boh lef camera and righ camera. We can projec a feaure from he lef image o he lef image in emporal space, also we can projec a feaure from he lef image o he righ image in spaial space.

4 For differen cameras, a differen exrinsic ransformaion T c b should be used. 2) IMU Facor: We use he well-nown IMU preinegraion algorihm [11, 12] o consruc he IMU facor. We assume ha he addiive noise in acceleraion and gyroscope measuremens are Gaussian whie noise. The ime-varying acceleraion and gyroscope bias are modeled as a random wal process, whose derivaive is Gaussian whie noise. Since he IMU acquires daa a a higher frequency han oher sensors, here are usually muliple IMU measuremens exising beween wo frames. Therefore, we pre-inegrae IMU measuremens on he manifold wih covariance propagaion. The deailed preinegraion can be found a [12]. Wihin wo ime insans, 1 and, he preinegraion produces relaive posiion α 1, velociy β 1 and roaion γ 1. Also, he preinegraion propagaes he covariance of relaive posiion, velociy, and roaion, as well as he covariance of bias. The IMU residual can be defined as: z imu α 1 β 1 γ 1 h imu (X ) = R 1 1(p p gd2 v 1 d) R 1 1(v v 1 + gd) R 1 1R b a b a 1, b g b g 1 where is he minus operaion on manifold, which is specially used for non-linear roaion. d is he ime inerval beween wo ime insans. g is he nown graviy vecor, whose norm is around Every wo adjacen frames consruc one IMU facor in he cos funcion. 3) Oher Facors: Though we only specify camera and IMU facors, our sysem is no limied o hese wo sensors. Oher sensors, such as wheel speedomeer, LiDAR and Radar, can be added ino our sysem wihou much effor. The ey is o model hese measuremens as general residual facors and add hese residual facors ino cos funcion. C. Opimizaion In radiional, he nonlinear leas square problem of eq.3 is solved by Newon-Gaussian or Levenberg-Marquard approaches. The cos funcion is linearized wih respec o an iniial guess of saes, ˆX. Then, he cos funcion is equals o: arg min δx (5) n e + J δx 2, (6) Ω = S where J is he Jacobian marix of each facor wih respec o curren saes ˆX. Afer linearizaion approximaion, his cos funcion has closed-form soluion of δx. We ae Newon- Gaussian as example, he soluion is derived as follows, J T Ω 1 J δx = J T Ω 1 e. (7) } {{ } H } {{ } b Finally, curren sae ˆX is updaed wih ˆX δx, where is he plus operaion on manifold for roaion. This procedure ieraes several imes unil convergence. We adop Ceres solver [24] o solve his problem, which uilizes advanced mahemaical ools o ge sable and opimal resuls efficienly. D. Marginalizaion Since he number of saes increases along wih ime, he compuaional complexiy will increase quadraically accordingly. In order o bound he compuaional complexiy, marginalizaion is incorporaed wihou loss of useful informaion. Marginalizaion procedure convers previous measuremens ino a prior erm, which reserves pas informaion. The se of saes o be marginalized ou is denoed as X m, and he se of remaining saes is denoed as X r. By summing all marginalized facors (eq.7), we ge a new H and b. Afer rearrange saes order, we ge he following relaionship: [ ] [ ] Hmm H mr δxm = H rm H rr δx r [ ] bm. (8) b r The marginalizaion is carried ou using he Schur complemen [25] as follows: (H rr H rm H mm 1H mr ) }{{} H p 1 δx r = b r H rmh mm b }{{ m. } b p (9) We ge a new prior H p, b p for he remaining saes. The informaion abou marginalized saes is convered ino prior erm wihou any loss. To be specific, we eep en spacial camera frames in our sysem. When a new eyframe comes, we marginalize ou he visual and inerial facors, which are relaed wih saes of he firs frame. Afer we ge he prior informaion abou curren saes, wih Bayes rule, we could calculae he poserior as a produc of lielihood and prior: p(x z) p(z X )p(x ). The sae esimaion hen becomes a MAP (Maximum A Poseriori) problem. Denoe ha we eep saes from insan m o insan n in he sliding window. The saes before m are marginalized ou and convered o a prior erm. Therefore, he MAP problem is wrien as: X m:n = arg max X m:n = arg min X m:n n p(z X m:n ) p(x m:n ) =m S n z h (X m:n ) 2 Ω =m S + (H p δx m:n b p ). (1) Compared wih eq.3, he above-menioned equaion only adds a prior erm. I is solved as same as eq.3 by Ceres solver [24]. E. Discussion The proposed sysem is a general framewor. Various sensors can be easily added ino our sysem, as long as i can be derived as a general residual facor. Since our sysem is no specially designed for a cerain sensor, i is capable o handle sensor failure case. When sensor failure occurs, we jus remove facors of he inacive sensor and add new facors from oher alernaive sensors.

5 Translaion error [m] Roaion error [deg] Relaive Pose Error in MH Proposed (sereo) Proposed (mono+imu) Proposed (sereo+imu) OKVIS (sereo+imu) Fig. 4. Relaive pose error [26] in MH 5 difficul. Two plos are relaive errors in ranslaion and roaion respecively. Translaion error [m] Roaion error [deg] Relaive Pose Error in V Proposed (sereo) Proposed (mono+imu) Proposed (sereo+imu) OKVIS (sereo+imu) Fig. 5. Relaive pose error [26] in V2 2 medium. Two plos are relaive errors in ranslaion and roaion respecively. 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 in he large-scale oudoor environmen. The numerical analysis is generaed o show he accuracy of our sysem in deail. A. Daases We evaluae our proposed sysem using he EuRoC MAV Visual-Inerial Daases [27]. This daases are colleced onboard a micro aerial vehicle, which conain sereo images (Apina MT9V34 global shuer, 752x48 monochrome, 2 TABLE I RMSE[M] IN EUROC DATASET. Sequence Lengh Proposed RMSE OKVIS sereo mono+imu sereo+imu RMSE MH MH MH MH MH V V V x V V V x FPS), synchronized IMU measuremens (ADIS16448, 2 Hz), Also, he ground ruh saes are provided by VICON and Leica MS5. We run daases wih hree differen combinaions of sensors, which are sereo cameras, a monocular camera wih an IMU, sereo cameras wih an IMU separaely. In his experimen, we compare our resuls wih OKVIS [8], a sae-of-he-ar VIO ha wors wih sereo cameras and an IMU. OKVIS is anoher opimizaion-based sliding-window algorihm. OKVIS is specially designed for visual-inerial sensors, while our sysem is a more general framewor, which suppors muliple sensors combinaions. We esed he proposed framewor and OKVIS wih all sequences in EuRoC daases. We evaluaed accuracy by RPE (Relaive Pose Errors) and ATE (Absolue Trajecory Errors). The RPE is calculaed by ools proposed in [26]. The RPE (Relaive Pose Errors) plo of wo sequences, MH 5 difficul and V2 2 medium, are shown in Fig. 4 and Fig. 5 respecively. The RMSE (Roo Mean Square Errors) of ATE for all sequences in EuRoC daases is shown in Table. I. Esimaed rajecories are aligned wih he ground ruh by Horn s mehod [28]. The sereo-only case fails in V1 3 difficul and V2 3 difficul sequences, where he movemen is oo aggressive for visual racing o survive. Mehods which involves he IMU wor successfully in all sequences. I is a good case o show ha he IMU can dramaically improve moion racing performance by bridging he gap when visual racs fail due o illuminaion change, exureless area, or moion blur. From he relaive pose error and absolue rajecory error, we can see ha he sereo-only mehod performed wors in mos sequences. Posiion and roaion drif obviously grown along wih disance in sereo-only case. In oher words, he IMU significanly benefied vision in saes esimaion. Since he IMU measures graviy vecor, i can effecively suppress drifs in roll and pich angles. Sereo cameras wih an IMU didn always perform bes, because i requires more accurae calibraion han he case of a monocular camera wih an IMU. Inaccurae inrinsic and exrinsic calibraion will inroduce more noise ino he sysem. In general, muliple sensor fusion increase he robusness of he sysem. Our resuls ouperforms OKVIS in mos sequences.

6 Relaive Pose Error in Oudoor Daase 3 sereo mono+imu sereo+imu Translaion error [m] Fig. 8. Fig. 6. The self-developed sensor suie used in he oudoor environmen. I conains sereo cameras (mvbluefox-mlc2w, 2Hz) and DJI A3 conroller, which include inbuil IMU (2Hz) and GPS receiver. Oudoor Trajecory 2 Proposed(sereo) Proposed(mono+imu) Proposed(sereo+imu) GPS 15 1 y [m] Relaive pose error [26] in oudoor experimen. around on he oudoor ground. We run saes esimaion wih hree differen combinaions, which are sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU. For accuracy comparison, we waled wo circles on he ground and compared our esimaion wih GPS. The rajecory is shown in Fig. 7, and he RPE (Relaive Pose Error) is shown in Fig. 8. As same as daase experimen, noiceable posiion drifs occurred in he sereo-only scenario. Wih he assisance of he IMU, he accuracy improves a lo. The RMSE of more oudoor experimens is shown in Table. II. The mehod which involves he IMU always performs beer han he sereo-only case. VI. C ONCLUSION x [m] Fig. 7. Esimaed rajecories in oudoor experimen. B. Real-world experimen In his experimen, we used a self-developed sensor suie o demonsrae our framewor. The sensor suie is shown in Fig. 6. I conains sereo cameras (mvbluefox-mlc2w, 2Hz) and DJI A3 conroller2, which inculdes inbuil IMU (2Hz) and GPS receiver. The GPS posiion is reaed as ground ruh. We hold he sensor suie by hand and wal 2 hp:// TABLE II RMSE[ M ] IN OUTDOOR EXPERIMENT. Sequence Lengh oudoor1 oudoor2 oudoor sereo Proposed RMSE mono+imu sereo+imu In his paper we have presened a general opimizaionbased framewor for local pose esimaion. The proposed framewor can suppor muliple sensor combinaions, which is desirable in aspec of robusness and pracicabiliy. We furher demonsrae i wih visual and inerial sensors, which form hree sensor suies (sereo cameras, a monocular camera wih an IMU, and sereo cameras wih an IMU). Noe ha alhough we only show he facor formulaions for he camera and IMU, our framewor can be generalized o oher sensors as well. We validae he performance of our sysem wih muliple sensors on boh public daases and real-world experimens. The numerical resul indicaes ha our framewor is able o fuse sensor daa wih differen seings. In fuure wor, we will exend our framewor wih global sensors (e.g. GPS) o achieve locally accurae and globally aware pose esimaion.

7 REFERENCES [1] G. Klein and D. Murray, Parallel racing and mapping for small ar worspaces, 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. Mouriis 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. Mouriis, 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 ef-based approach, in Proc. of he IEEE/RSJ In. Conf. on Inell. Robos and Sys. IEEE, 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] C. Forser, L. Carlone, F. Dellaer, and D. Scaramuzza, On-manifold preinegraion for real-ime visual-inerial odomery, IEEE Trans. Robo., vol. 33, no. 1, pp. 1 21, 217. [12] 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. [13] J. Zhang and S. Singh, Loam: Lidar odomery and mapping in realime. in Roboics: Science and Sysems, vol. 2, 214, p. 9. [14] 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. [15] H. Rebecq, T. Horsschaefer, G. Gallego, and D. Scaramuzza, Evo: A geomeric approach o even-based 6-dof parallel racing and mapping in real ime, IEEE Roboics and Auomaion Leers, vol. 2, no. 2, pp , 217. [16] S. Lynen, M. W. Acheli, 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 [17] G. P. Huang, A. I. Mouriis, and S. I. Roumeliois, Observabiliybased rules for designing consisen ef slam esimaors, In. J. Robo. Research, vol. 29, no. 5, pp , 21. [18] S. Shen, Y. Mulgaonar, 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 [19] R. Kümmerle, G. Grisei, H. Srasda, K. Konolige, and W. Burgard, g2o: A general framewor for graph opimizaion, in Proc. of he IEEE In. Conf. on Robo. and Auom. IEEE, 211, pp [2] M. Kaess, H. Johannsson, R. Robers, V. Ila, J. J. Leonard, and F. Dellaer, isam2: Incremenal smoohing and mapping using he bayes ree, In. J. Robo. Research, vol. 31, no. 2, pp , 212. [21] 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 [22] J. Shi and C. Tomasi, Good feaures o rac, in Compuer Vision and Paern Recogniion,1994. IEEE Compuer Sociey Conference on, 1994, pp [23] B. D. Lucas and T. Kanade, An ieraive image regisraion echnique wih an applicaion o sereo vision, in Proc. of he Inl. Join Conf. on Arificial Inelligence, Vancouver, Canada, Aug. 1981, pp [24] S. Agarwal, K. Mierle, and Ohers, Ceres solver, hp://ceres-solver. org. [25] G. Sibley, L. Mahies, and G. Suhame, Sliding window filer wih applicaion o planeary landing, J. Field Robo., vol. 27, no. 5, pp , Sep. 21. [26] A. Geiger, P. Lenz, and R. Urasun, Are we ready for auonomous driving? he ii vision benchmar suie, in Proc. of he IEEE In. Conf. on Paern Recogniion, 212, pp [27] M. Burri, J. Niolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Acheli, and R. Siegwar, The euroc micro aerial vehicle daases, In. J. Robo. Research, 216. [28] B. K. Horn, Closed-form soluion of absolue orienaion using uni quaernions, JOSA A, vol. 4, no. 4, pp , 1987.