Efficient Velodyne SLAM with point and plane features

This paper develops and tests a plane based simultaneous localization and mapping algorithm capable of processing the uneven sampling density of Velodyne-style scanning LiDAR sensors in real-time. The algorithm uses an efficient plane detector to rapidly provide stable features, both for localization and as landmarks in a graph-based SLAM. When planes cannot be detected or when they provide insufficient support for localization, a novel constraint tracking algorithm selects a minimal set of supplemental point features to be provided to the localization solver. Several difficult indoor and outdoor datasets, totaling 6981 scans, each with $$\sim $$∼ 70,000 points, are used to analyze the performance of the algorithm without the aid of any additional sensors. The results are compared to two competing state-of-the-art algorithms, GICP and LOAM, showing up to an order of magnitude faster runtime and superior accuracy on all datasets, with loop closure errors of 0.14–0.95 m, compared to 0.44–66.11 m.

[1]  Ji Zhang,et al.  LOAM: Lidar Odometry and Mapping in Real-time , 2014, Robotics: Science and Systems.

[2]  Michael Kaess,et al.  Simultaneous localization and mapping with infinite planes , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[3]  Malcolm D. Shuster,et al.  The generalized Wahba problem , 2006 .

[4]  Christoph Stiller,et al.  Velodyne SLAM , 2011, 2011 IEEE Intelligent Vehicles Symposium (IV).

[5]  Andreas Birk,et al.  Fast Registration Based on Noisy Planes With Unknown Correspondences for 3-D Mapping , 2010, IEEE Transactions on Robotics.

[6]  Wolfram Burgard,et al.  An efficient fastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements , 2003, Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453).

[7]  Andrew W. Fitzgibbon.  Robust registration of 2D and 3D point sets , 2003, Image Vis. Comput..

[8]  Henrik I. Christensen,et al.  OmniMapper: A modular multimodal mapping framework , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[9]  D K Smith,et al.  Numerical Optimization , 2001, J. Oper. Res. Soc..

[10]  Laurent Itti,et al.  Finding planes in LiDAR point clouds for real-time registration , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[11]  M. Shuster,et al.  Three-axis attitude determination from vector observations , 1981 .

[12]  Dongwook Kim,et al.  Environment-Detection-and-Mapping Algorithm for Autonomous Driving in Rural or Off-Road Environment , 2012, IEEE Transactions on Intelligent Transportation Systems.

[13]  Wolfram Burgard,et al.  Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters , 2007, IEEE Transactions on Robotics.

[14]  Wolfram Burgard,et al.  Improving Grid-based SLAM with Rao-Blackwellized Particle Filters by Adaptive Proposals and Selective Resampling , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[15]  John J. Leonard,et al.  Kintinuous: Spatially Extended KinectFusion , 2012, AAAI 2012.

[16]  S. Umeyama,et al.  Least-Squares Estimation of Transformation Parameters Between Two Point Patterns , 1991, IEEE Trans. Pattern Anal. Mach. Intell..

[17]  Wolfram Burgard,et al.  An evaluation of the RGB-D SLAM system , 2012, 2012 IEEE International Conference on Robotics and Automation.

[18]  F. Markley,et al.  Quaternion Attitude Estimation Using Vector Observations , 2000 .

[19]  Pol D. Spanos,et al.  Q-Method Extended Kalman Filter , 2015 .

[20]  Andreas Birk,et al.  Online three‐dimensional SLAM by registration of large planar surface segments and closed‐form pose‐graph relaxation , 2010, J. Field Robotics.

[21]  William Whittaker,et al.  Tartan Racing: A multi-modal approach to the DARPA Urban Challenge , 2007 .

[22]  Erik Wolfart,et al.  Pose interpolation SLAM for large maps using moving 3D sensors , 2015, 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[23]  Paul H. J. Kelly,et al.  Dense planar SLAM , 2014, ISMAR.

[24]  T. Kanade,et al.  Fast and accurate computation of surface normals from range images , 2011, 2011 IEEE International Conference on Robotics and Automation.

[25]  Frank Dellaert,et al.  iSAM2: Incremental smoothing and mapping using the Bayes tree , 2012, Int. J. Robotics Res..

[26]  Roland Siegwart,et al.  3D SLAM using planar segments , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[27]  Berthold K. P. Horn,et al.  Closed-form solution of absolute orientation using unit quaternions , 1987 .

[28]  Andrew W. Fitzgibbon,et al.  KinectFusion: Real-time dense surface mapping and tracking , 2011, 2011 10th IEEE International Symposium on Mixed and Augmented Reality.

[29]  K. Lingemann,et al.  The 3D Hough Transform for plane detection in point clouds: A review and a new accumulator design , 2011 .

[30]  Dieter Fox,et al.  RGB-D mapping: Using Kinect-style depth cameras for dense 3D modeling of indoor environments , 2012, Int. J. Robotics Res..

[31]  P. B. Davenport.  A vector approach to the algebra of rotations with applications , 1968 .

[32]  Andreas Birk,et al.  Online three-dimensional SLAM by registration of large planar surface segments and closed-form pose-graph relaxation , 2010 .

[33]  Roland Siegwart,et al.  EKF-based 3D SLAM for structured environment reconstruction , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[34]  W. Herzner,et al.  RASCAL - an autonomous ground vehicle for desert driving in the DARPA Grand Challenge 2005 , 2005, Proceedings. 2005 IEEE Intelligent Transportation Systems, 2005..