Eureka moments! Slam Implementation using ESDF

Simultaneous localization and mapping (SLAM) is a method to help robots explore, navigate, and map an alien environment. It is easily known that traditional methods for SLAM based on the extended Kalman filter (EKF) suffer computational complexity problems when dealing with large scale environments, as good as inconsistencies for non-linear SLAM problems.

To incorporate non-linear measurements, current research has been focused on smoothing approaches using Graph SLAM techniques. The Exactly Sparse Delayed-State Filter (ESDF) was exploited by Eustice et al. to map the RMS titanic using pose constraints between camera views. We use the same expression as a start place for our Graph SLAM implementation. A lot of the influence on ESDF was done by my team mate Tom.Pseudo code for our ESDF We ran our algorithms on two data sets - Manhattan3500 CMU Newell-Simon Hall, A LevelManhattan3500 Simulated DatasetCMU Newell-Simon Hall, A Level Dataset Manhattan 3500 is a simulated data set of a robot driving about the streets of Manhattan. It contains 3500 pose constraints and covariances associated with each constraint. The CMU Newell-Simon Hall dataset contains odometry from an IMU and range and bearin returns for a 180 degree SICK laser. Manhattan 3500: This dataset was exploited to assert the operation of the ESDF before fusing it with the correlative Scan-Matcher to complete our system. Figures below show the ESDF performance both with and without loop closures. We can keep the natural sparsity of the SLAM information matrix. Only 0.182 percent of the elements of the information matrix are non-zero.Manhattan open LoopManhattan Close Loop CMU Newell-Simon Hall: This dataset was first used to control the functioning of the correlative Scan-Matcher in an open loop scenario. Once it was confirmed that the Scan-Matcher was returning reliable rigid body constraints, we used the strict body constraints from the Scan-Matcher in alignment with the ESDF. Figures below shows the map generated using just the pose constraints from the correlative Scan-Matcher. The organisation runs open loop until the last thirteen pose constraints, which are loop closing events. The final map recovered is visually quite like to the map recovered by iSAM. For our final implementation we would care to use iSAM given its computational benefits.ESDF on CMU DatasetiSAM open loopiSAM close loop