Incremental Real-Time Multibody VSLAM with Trajectory Optimization Using Stereo Camera

Transcription

1 2016 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems (IROS) Daejeon Conventon Center October 9-14, 2016, Daejeon, Korea Incremental Real-Tme Multbody VSLAM wth Trajectory Optmzaton Usng Stereo Camera N Dnesh Reddy 1,2, Iman Abbasnejad 2,3,4, Sheetal Reddy 1, Amt Kumar Mondal 5 and Vndhya Devalla 5 Abstract Real-tme outdoor navgaton n hghly dynamc envronments s an crucal problem. The recent lterature on real-tme statc SLAM don t scale up to dynamc outdoor envronments. Most of these methods assume movng objects as outlers or dscard the nformaton provded by them. We propose an algorthm to jontly nfer the camera trajectory and the movng object trajectory smultaneously. In ths paper, we perform a sparse scene flow based moton segmentaton usng a stereo camera. The segmented objects moton models are used for accurate localzaton of the camera trajectory as well as the movng objects. We explot the relatonshp between movng objects for mprovng the accuracy of the poses. We formulate the poses as a factor graph ncorporatng all the constrants. We acheve exact ncremental soluton by solvng a full nonlnear optmzaton problem n real tme. The evaluaton s performed on the challengng KITTI dataset wth multple movng cars.our method outperforms the prevous baselnes n outdoor navgaton. I. INTRODUCTION Outdoor navgaton n dynamc envronments s a challengng task for autonomous drvng assstance systems (ADAS). It has wde applcatons n varous areas lke collson avodance, path plannng and scene understandng. Inference of hghly dynamc scenes accurately s crucal for such systems. A robot navgatng n such envronments needs a fast and accurate localzaton of the movng objects and ther trajectores. In the past decade, a lot of lterature s avalable for statc SFM or SLAM [1], [2] ppelne, where they utlze the statc landmarks to buld an accurate map and trajectory. These methods treat the movng objects as outlers. These methods fal when there are multple movng objects n the scene lke a congested urban scene due to wrong nler fttng. Dynamc object segmentaton and trajectory optmzaton s relatvely new feld of research wth sparse lterature. The few solutons to ths problem present n the lterature can be categorzed nto Decoupled and Jont Methods. Jont approach lke [3] use monocular cameras to jontly estmate the depth maps, do moton segmentaton and moton estmaton of multple bodes. Decoupled approaches lke [4], [5] have a sequental ppelne where they segment moton and ndependently reconstruct the movng and statc scenes. Our approach s a real-tme ncremental approach, and dffers from the other methods due to the smultaneous optmzaton 1 Internatonal Insttute of Informaton Technology, Hyderabad, Inda 2 Max Planck Insttute For Intellgent Systems, Tübngen, Germany 3 The Robotcs Insttute, Pttsburgh, PA, USA 4 Queensland Unversty of Technology, Brsbane, QLD, Australa 5 Unversty of Petroleum and Energy Studes, Dehradun, Inda Fg. 1. The top mages depcts the real tme segmentaton of movng objects. The top vew of the trajectores of movng objects and the camera odometry are depcted. The Red trajectory represents the camera odometry. The blue and green represents the movng car odometry.best vewed n color. of multple movng cars. The algorthm s easly scalable to multple cars and hghly traffc scenaros. Our approach emphass on a real tme approach for movng object trajectory optmzaton. We obtan our real tme moton detecton and segmentaton from the sparse moton segmentaton algorthm [6]. The movng object trajectores are ntalzed usng the trangulaton of the movng objects n the current frame and then transformng the trajectores wth respect to the world coordnate frame. These trajectores are ntalzed as poses of a factor graph. The factor graph s ncluded wth addtonal constrants lke the relatonshp between the camera moton and movng object. We also ncorporate the moton model of the movng objects for more accurate localzaton. The man contrbuton n ths paper s the optmzaton for the movng object trajectory and the camera trajectory smultaneously n real-tme. We ntroduce the concept of anchor nodes for movng object trajectory estmaton. The anchor nodes ntalze each movng object as a new pose optmzaton problem and solves for the complete trajectory of the movng object. II. RELATED WORK A lot of research n the area of computer vson and robotcs has been focused on moton segmentaton and SLAM, but lmted lterature focuses on mprovng the localzaton of movng objects and ther reconstructon usng vson based feedback. Moton segmentaton has been approached usng geometrc prors mostly from a vdeo. General paradgm nvolves usng geometrc constrants [12], /16/$ IEEE 4505

2 Fg. 2. Illustraton of the proposed method.the system takes a sequence of rectfed stereo mages from the trackng dataset of KITTI (A).Our formulaton computes the sparse scene flow (D) usng dsparty map(b) and optcal flow(c).these are used to segment the multple movng cars n the scene(e).for each segmented car, we estmate the trajectory separately and put them nto a jont formulaton. The optmzed movng object trajectores are dsplayed n world coordnate frame. Best vewed n color. [14], reducng the model to affne to cluster the trajectores nto subspaces [15] or semantc constrants [13]. A varety of approaches have been proposed to recover the Trajectores of statc ndoor and outdoor scenes. In contrast, here we propose a method that s able to extract accurate 3d nformaton by reasonng jontly about statc and dynamc scene elements as well as ther complex nterplay usng semantc nformaton. Recent advances n statc SFM nvolve addng semantc and geometrc constrants to Bundle Adjustment [16]. Janxong et al. have shown results on ndoor sequences wth very hgh accuracy. Factor graphs[10] have shown consderable mprovement n statc robot localzaton. An ncremental verson of the smoothng and mappng [9] has been shown a mprovement n the Statc ncremental SLAM algorthms. The movng object localzaton has recently been a wdely researched area for Automated Drver Assstance Systems (ADAS). Song et al. [17] use the object detecton and SFM cues for mprovng the 3D object localzaton. Dnesh et al.[19] have used semantc constrants for accurate localzaton of movng objects. The accurate localzaton of the movng objects n dynamc envronments helps n better understandng for outdoor navgaton. Our work closely resembles [8] n problem formulaton. They explot the relatonshp between two movng robots n the envronment and solve the SLAM problem n an ncremental formulaton. We dffer from the above method n terms of the constrants we explot. We formulate our problem as a factor graph over movng object trajectores. Ths allows us to model smooth trajectores wthout employng hard geometrc constrants. We also use these trajectores to fuse dynamc and statc objects whch can be used n robot navgaton. III. OUR APPROACH We present a probablstc formulaton of the mult-body SLAM problem based on pose graphs. Pose graphs are a common soluton for sngle robot localzaton and mappng, n whch all current and past robot poses form a Markov chan connected by odometry measurements. We have mplemented the ncremental smoothng and mappng (SAM) for optmzng the pose graph because t provdes an effcent soluton wthout need for approxmatons and allows effectve access to the estmaton uncertantes. The mplementaton explots constrants between the dfferent poses. A. Moton Detecton and segmentaton We consder a sequence of mages from a stereo camera rg. Interest ponts are detected n two consecutve and rectfed stereo mages and checked for mutual consstency. The nterest ponts are generated usng the SIFT feature detecton algorthm. Each feature s matched wth ther stereo rg and the dsparty s computed for each nterest pont. The 3d locaton of each Interest pont s computed from the dsparty. Snce,the mountng and ptchng of the stereo rg s unknown, we detect the ground plane for better understandng of the envronment. The ground plane computaton gves a good pror for the computaton of movng and statonary pont clusters. All the cluster of ponts on the ground plane are segmented as statonary. These nterest ponts are tracked over multple frames and the scene 4506

3 Fg. 3. The results for the moton segmentaton algorthm on the KITTI2 sequence s depcted n the mage.each movng car s segmented and labelled wth dfferent color, the red pont represent the statonary landmarks used for odometry computaton. Best vewed n color. Fg. 4. Ths depcts the formulaton of the dynamc SAM problem. The red represents the camera odometry, whle the green and blue represent the movng objects predcted from the III-A. flow s computed usng fnte dfference approxmaton to yeld dervatves. The scene flow stores the nformaton of the moton of the nterest pont n the world frame. A graph-lke structure connectng all detected nterest ponts n the mage plane s generated usng Delaunay trangulaton. The resultng edges are removed accordng to scene flow dfferences exceedng a certan threshold wth respect to the uncertanty of the computed 3D poston of every nterest pont. We have added addtonal geometrc constrants for accurate segmentaton as proposed n the [14]. We use the ground plane computed earler for excludng the false postve solutons. The remanng connected components of the graph descrbe movng objects n the scene. Detected objects are tracked over tme usng a global nearest neghbor (GNN) approach. The GNN algorthm searches the closest object n dstance from ts locaton and tracks the object over multple frames. B. Movng object trajectores n global frame We frst formulate the problem of the multbody mappng problem usng one pose graph for each movng object trajectory. We show a typcal movng object scenaro wth two movng objects n front of the camera n Fg.4. The pose varables are shown as coloured crcles, and measurements as small black dscs. In Fg.4 each movng object s represented wth dfferent color. For M movng objects, the trajectory of the movng object m {0...M 1} n the scene s gven by N m + 1 pose varables {z m}nm =0. As each movng object trajectory s computed from the stereo trangulaton, the trajectores by themselves are underconstraned. We fx the gauge freedom by ntroducng a pror P m for each trajectory m. Measurements between poses of a sngle trajectory are of two types. Where the successve poses are connected based on the camera readngs, the other knd of measurements s the connecton of arbtrary poses.e readngs of the poses between the cars from the depth nformaton of the stereo cameras. We have dscussed about the movng car trajectory as an ndependent entty, We now ntroduce the relatonshps between the movng objects and the camera trajectores. An encounter e between the movng object and the cam- Fg. 5. Here we depct the relatonshp between the movng object pose and the camera trajectory. The pose of the movng object and the camera s exploted by ths constrant. Fg. 6. Here we depct the relatonshp between the movng object pose m 1 and another movng object m 2. Ths explots the relatonshp between the movng object motons. era odometry explots the relatonshp between the camera moton and the movng object moton,depcted n Fg. 5.Smlarly, an encounter e between two movng objects m 1 and m 2 s a measurement that connects the two movng objects n a scene z m1 and z m2. An example s shown n the Fg. 6, wth the relaton between the measurements and poses. Snce we have a setup where the observatons are the same tme nstance, all our measurements e connect poses taken at the same nstance. We take a probablstc approach for estmatng the movng object trajectory based on all the measurements.we formulate a jont probablty for all the poses Z = {z m }Nm =0,m=0,measurements and prors Y = {p m } Nm =0 and L encounters E = {em }Nm P (Z, Y, E) = ( M 1 m=0 P (z m 0 p m 0 ) N m =1 P (z m z m 1, v ) =0,m=0 : ) L j=1 {vm }Nm P (z mj j =0 z m j, e j ) j The number of encounters L s equal to to the number of frames n the sequence. The data assocaton between the encounters j, j, m j, m j s computed for each frame (1) 4507

4 from the moton segmentaton and localzaton algorthm as dscussed n secton III-A. The nose s assumed to be gaussan as proposed n multple SLAM systems. z m = f (z m 1, v m ) + w m (2) Ths descrbes the movng object localzaton n the current frame of reference computed from the dsparty computaton. The w m s a normally dstrbuted nose wth covarance matrx ξ m. We model the encounters wthn the movng objects and between the movng objects and the camera as : z mj j = h j z m j, e j + k j (3) j Smlar to the successve poses nteractons the nose for the measurement model of the movng objects s computed from the dsparty computaton. Here, k j s a normally dstrbuted nose wth covarance of Γ j. We formulate the problem as a maxmum a posteror (MAP) estmate for the movng object trajectory. Ths leads to the followng nonlnear least squares problem: Z = arg mn { M 1 r=0 ( p m z m 0 2 Σ N m ) + f (z 1, m v m ) + w m 2 ξ m =1 } L + h j z m j 2, e j + z rj j j=1 Here a 2 Σ = at Σ 1 a s the squared Mahalanobs dstance wth covarance matrx Σ. We solve the non-lnear least squares problem usng the ncremental smoothng and mappng (SAM) algorthm. Snce the error n object localzaton of the movng object s modelled as a gaussan and the constrant functons are nonlnear, nonlnear optmzaton methods are used. We can use the methods lke gauss-newton, Levenberg-marquardt or the Powell s Dog-leg algorthm, whch use successon of lnear approxmatons to reach a mnmum.all the components can be wrtten n a standard least squares problem of the form : j Γ j (4) Θ = argmn Θ AΘ b 2 (5) where the vector Θ R n conssts of all the movng object poses and the robot pose, where n s the number of varables. The matrx A R mxn s a large, but sparse measurement jacoban, wth m the number of measurements, and b R m s the rght-hand sde vector. We solve ths usng the method of QR factorzaton of the A matrx. [ ] H A = Q (6) 0 where H R nxn s an nxn upper trangulaton matrx, 0 s an (m n)xn zero matrx, Q s an mxn orthogonal matrx. The vector b s modfed accordngly durng the QR decomposton to obtan d R n. The soluton s obtaned by back substtuton. RΘ = d (7) To avod refactorng an ncreasngly large measurement jacoban each tme a new measurement s computed, we have followed the method of SAM to update the new measurement rows. The key to effcency s to keep the square root nformaton matrx sparse, whch requres choosng a sutable varable orderng. SAM perodcally reorders the varables accordng to some heurstc and performs a batch factorzaton that also ncludes relnearzaton of the measurement equatons. The ntalzaton of the movng object trajectores s done usng the stereo trangulaton of all the ponts n the segmentaton. The pose s ntalzed as the transformaton between the set of 2d ponts. The moton detecton algorthm s used for accurate predcton of the pror p m, therefore we have very less gauss freedom and a very good ntalzaton of the movng object trajectores. IV. EXPERIMENTAL RESULTS We have used the KITTI trackng dataset for evaluaton of the algorthm, as the ground truth localzaton of movng objects per camera frame s avalable. It conssts of several sequences collected by a car-mounted camera n urban,resdental and hghway envronments, makng t a vared and challengng real world dataset. We have taken two sequences consstng of 30 mages and 212 mages for evaluatng our algorthm. The frst sequence contans two cars whch are over-takng the current car and the second sequence s a hghway sequence of multple movng cars. We chose these 2 sequences as these sequences pose challenge to moton segmentaton algorthm as the movng cars le n the subspace as the camera. These sequences also has a mx of multple cars vsble for short duraton and whole sequence tracked cars whch allows us to test our robustness of localzaton and trajectory reconstructon on both short and long sequences. A. Trajectory Evaluaton We compare the estmated trajectores of the movng objects to the extended Kalman flter based object trackng VISO2 (Stereo). VISO2 S(Stereo) has reported error of 2.44% on the KITTI odometry dataset, makng t a good baselne algorthm to compare wth. As proposed by Sturm et al. [11], we compare the two sequences based on ATE for root mean square error (RMSE), mean, medan and ARE. We use ther evaluaton algorthm whch algns the 2 trajectores usng SVD. We show the three statstcs as mean and medan are robust to outlers whle RMSE shows the exact devaton from the ground truth. We also evaluate RE whch s the relatve error % of RMSE: (AT E V AT E O )/AT E V and sgnfes the error change relatve to VISO2. The trajectory for each movng object s computed usng the KITTI trackng dataset s Ground truth. The locaton of each movng object n ndvdual frame s transformed to the world reference frame usng the odometry of the VISO

5 Car Num P VISO2 OUR APPROACH ATE R(m) ATE M(m) ATE Me(m) ARE (deg) ATE R(m) ATE M(m) ATE Me(m) ARE (deg) RE(%) Cam TABLE I STATISTICS OF VISO2 AND OUR APPROACH FOR KITTI 2 DATASET. P IS #POSES. ATE R IS ABSOLUTE TRAJECTORY ERROR RMSE, ATE M IS ABSOLUTE TRAJECTORY ERROR MEAN,ATE ME IS ABSOLUTE TRAJECTORY ERROR MEDIAN, ARE IS AVERAGE ROTATION ERROR AND RE IS RELATIVE POSE ERROR. Car Num P VISO2 OUR APPROACH ATE R(m) ATE M(m) ATE Me(m) ARE (deg) ATE R(m) ATE M(m) ATE Me(m) ARE (deg) RE(%) Cam % TABLE II STATISTICS OF VISO2 AND OUR APPROACH FOR KITTI 1 DATASET. P IS #POSES. ATE R IS ABSOLUTE TRAJECTORY ERROR RMSE, ATE M IS ABSOLUTE TRAJECTORY ERROR MEAN,ATE ME IS ABSOLUTE TRAJECTORY ERROR MEDIAN, ARE IS AVERAGE ROTATION ERROR AND RE IS RELATIVE POSE ERROR. Car 1 Car 2 Car 3 Car 4 Car 5 OURS VISO2 Fg. 7. Comparson plots for 5 movng cars n the KITTI 1. The black plot represents the ground truth trajectory of the movng car n the world frame. Blue plot represents the estmated trajectory of the movng car. Red lnes represent the error n the estmate wth respect to ground truth for the trajectores. The error comparson s computed between OUR method and VISO2. KITTI 1 Sequence s a 212 mage sequence wth n total 13 movng cars. We have showed results for 5 movng cars (due to space constrants). The car n front of the camera s tracked for 212 mages whle others are 5-10 mages long. As seen from Table II we can clearly observe that VISO 2 accumulates drft leadng to hgher RMSE and Medan error over the long sequence. Our approach shows consderable robustness to drft and has average reducton error of %. Car 2-5 show on small sequences average error reducton of 42.5 % whch s due to the smoothness constrants. KITTI 2 Sequence s a 28 mage sequence wth 2 cars overtakng the camera, ths poses challenge to moton segmentaton leadng to nosy ntal estmates. Our approach here too does better than VISO2 as shown n Table I wth average error reducton of 43.57%. Ths shows our method s able to handle both long and short sequences. Fg III-B shows the comparson of the trajectores relatve to VISO2. The KITTI 2 sequence s an good example of localzaton error of the robot. The moton of the cars le n the flow vector drecton leadng to error propagaton nto the camera localzaton. Our formulaton ncorporates the moton of the movng objects nto the formulaton causng mprovement n the localzaton of the camera concurrently more accurate localzaton of landmarks and movng objects. Usng our current formulaton, we propose an mprovement n the odometry and movng object localzaton when the ransac based SLAM systems fal. We can attrbute ths to the jont formulaton of the odometry and movng object trajectores. The pror from the movng object adds to the localzaton of the camera. V. CONCLUSIONS Ths paper presents an approach for accurate localzaton of movng objects n a hghly dynamc envronment. Ths 4509

6 VISO2 OURS Car 1 Car 2 Fg. 8. Comparson plots for 2 movng cars n the KITTI 2 sequence. The black plot represents the ground truth trajectory of the movng car n the world frame. Blue plot represents the estmated trajectory of the movng car. Red lnes represent the error n the estmate wth respect to ground truth for the trajectores. The error comparson s computed between OUR method and VISO2. s an ncremental real tme algorthm and can be used n both ndoor and outdoor envronments. The algorthm solves the full multbody VSLAM optmzaton algorthm n real tme. We have proposed a new algorthm for movng object localzaton. We show an mprovement n the camera trajectory computaton compared to the standard camera trajectory computatons. An extensve evaluaton of trajectores for long sequences has been compared. We propose a novel method of evaluaton of movng object trajectores. The accurate localzaton of the movng objects s useful n ADAS systems. We plan on releasng the GT trajectores for the movng objects and the evaluaton scrpt for cross comparson. We plan on mprovng the moton segmentaton usng addtonal semantc constrants for better localzaton of the movng objects. Incorporatng addtonal constrants nto the Factor graph wll be exploted. We are also nvestgatng the trajectory plannng for the robot navgaton n dynamc envronments. [7] Mchael Kaess and Frank Dellaert. Covarance Recovery from a Square Root Informaton Matrx for Data Assocaton, n Journal of Robotcs and Autonomous Systems (RAS),2009. [8] Been Km and Mchael Kaess and Luke Fletcher and John Leonard and Abe Bachrach and Ncholas Roy and Seth Teller, Multple Relatve Pose Graphs for Robust Cooperatve Mappng,n IEEE Intl. Conf. on Robotcs and Automaton (ICRA),2010. [9] Mchael Kaess and Hordur Johannsson and Rchard Roberts and Vorela Ila and John J. Leonard and Frank Dellaert,SAM2: Incremental Smoothng and Mappng wth Flud Relnearzaton and Incremental Varable Reorderng, In IEEE Intl. Conf. on Robotcs and Automaton (ICRA), 2011 [10] Frank Dellaert,Square Root SAM: Smultaneous Locaton and Mappng va Square Root Informaton Smoothng,In Robotcs: Scence and Systems (RSS), [11] J. Sturm and N. Engelhard and F. Endres and W. Burgard and D. Cremers. A Benchmark for the Evaluaton of RGB-D SLAM Systems, Internatonal Conference on Intellgent Robot Systems (IROS), [12] R.K. Namdev, K.M. Krshna, and C. V jawahar. Moton segmentaton of multple objects from a freely movng monocular camera, Internatonal Conference on Robotcs and Automaton (ICRA), [13] N.Dnesh Reddy, Prateek Snghal and K. Madhava Krshna,Semantc Moton Segmentaton Usng Dense CRF Formulaton,Indan Conference on Computer Vson, Graphcs and Image Processng (ICVGIP),2014. [14] Vctor Romero-Cano and Juan I. Neto,Stereo-based moton detecton and trackng from a movng platform,intellgent Vehcles Symposum(IV),2013. [15] E.Elhamfar and R.Vdal. Sparse subspace clusterng, Internatonal Conference on Computer Vson and Pattern Recognton (CVPR), [16] Janxong Xao, Andrew Owens, Antono Torralba, SUN3D: A Database of Bg Spaces Reconstructed Usng SfM and Object Labels, Internatonal Conference on Computer Vson(ICCV), 2013,pp [17] S. Song and M.K. Chandraker, Jont SFM and Detecton Cues n 3D Object Localzaton for Autonomous Drvng,In Internatonal Conference on Computer Vson and Pattern Recognton(CVPR) [18] N. Dnesh Reddy, Prateek Snghal, Vsesh Char, K. Madhava Krshna, Dynamc Body VSLAM wth Semantc Constrants, In Internatonal Conference on Intellgent Robot Systems (IROS) [19] Chhaya, Falak and Reddy, N Dnesh and Upadhyay, Sarthak and Char, Vsesh and Za, M Zeeshan and Krshna, K Madhava, Monocular Reconstructon of Vehcles: Combnng SLAM wth Shape Prors, In Internatonal Conference on Robotcs and Automaton (ICRA) [20] Cheh-chh Wang and Charles Thorpe and Martal Hebert and Sebastan Thrun and Hugh Durrant-whyte,Smultaneous localzaton, mappng and movng object trackng,in Internatonal Journal of Robotcs Research,2004. REFERENCES [1] C. Kerl and J. Sturm and D. Cremers,Dense Vsual SLAM for RGB- D Cameras, Internatonal Conference on Intellgent Robot Systems (IROS), [2] J. Engel and T. Schops and D. Cremers,LSD-SLAM: Large-Scale Drect Monocular SLAM,In Internatonal Conference on computer vson (ICCV), [3] Roussos, Anastasos and Russell, Chrs and Garg, Rav and Agapto, Lourdes,Dense multbody moton estmaton and reconstructon from a handheld camera, IEEE Internatonal Symposum on Mxed and Augmented Realty (ISMAR), 2012 [4] Chang Yuan and Gerard G. Medon,3D Reconstructon of Background and Objects Movng on Ground Plane Vewed from a Movng Camera, Conference on Computer Vson and Pattern Recognton (CVPR), [5] Kundu, Abhjt and Krshna, K. Madhava and Jawahar, C.V.,Realtme Multbody Vsual SLAM wth a Smoothly Movng Monocular Camera, IEEE Internatonal Conference on Computer Vson (ICCV), [6] Phlp Lenz and Julus Zegler and Andreas Geger and Martn Roser,Sparse Scene Flow Segmentaton for Movng Object Detecton n Urban Envronments,Intellgent Vehcles Symposum (IV),