Skip to main content
Log in

Efficient Velodyne SLAM with point and plane features

  • Published:
Autonomous Robots Aims and scope Submit manuscript


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.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Subscribe and save

Springer+ Basic
EUR 32.99 /Month
  • Get 10 units per month
  • Download Article/Chapter or Ebook
  • 1 Unit = 1 Article or 1 Chapter
  • Cancel anytime
Subscribe now

Buy Now

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12
Fig. 13
Fig. 14
Fig. 15
Fig. 16

Similar content being viewed by others


  • Agarwal, S., Mierle, K., & et al. (2016). Ceres solver.

  • Ainscough, T., Zanetti, R., Christian, J., & Spanos, P. D. (2014). Q-method extended kalman filter. Journal of Guidance, Control, and Dynamics, 38(4), 752–760.

    Article  Google Scholar 

  • Badino, H., Huber, D., Park, Y., & Kanade, T. (2011). Fast and accurate computation of surface normals from range images. In: 2011 IEEE international conference on robotics and automation (ICRA) (pp. 3084–3091). IEEE.

  • Behringer, R., Travis, W., Daily, R., Bevly, D., Kubinger, W., Herzner, W., et al. (2005). Rascal-an autonomous ground vehicle for desert driving in the darpa grand challenge 2005. In: Intelligent Transportation Systems, 2005. Proceedings (pp. 644–649). IEEE.

  • Blanco, J. L., & Rai, P. K. (2014). nanoflann: A C++ header-only fork of FLANN, a library for nearest neighbor (NN) wih kd-trees.

  • Borrmann, D., Elseberg, J., Lingemann, K., & Nüchter, A. (2011). The 3d hough transform for plane detection in point clouds: A review and a new accumulator design. 3D Research, 2(2), 1–13.

    Article  Google Scholar 

  • Ceriani, S., Sanchez, C., Taddei, P., Wolfart, E., & Sequeira, V. (2015). Pose interpolation slam for large maps using moving 3d sensors. In: 2015 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 750–757).

  • Choi, J., Lee, J., Kim, D., Soprani, G., Cerri, P., Broggi, A., et al. (2012). Environment-detection-and-mapping algorithm for autonomous driving in rural or off-road environment. IEEE Transactions on Intelligent Transportation Systems, 13(2), 974–982.

    Article  Google Scholar 

  • Davenport, P. B. (1968). A vector approach to the algebra of rotations with applications. National Aeronautics and Space Administration

  • Endres, F., Hess, J., Engelhard, N., Sturm, J., Cremers, D., & Burgard, W. (2012). An evaluation of the rgb-d slam system. In: 2012 IEEE International Conference on Robotics and Automation (ICRA) (pp. 1691–1696). IEEE.

  • Fitzgibbon, A. W. (2003). Robust registration of 2d and 3d point sets. Image and Vision Computing, 21(13), 1145–1153.

    Article  Google Scholar 

  • Grant, W. S., Voorhies, R. C., & Itti, L. (2013). Finding planes in lidar point clouds for real-time registration. In: 2013 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 4347–4354). IEEE.

  • Grant, W. S., Voorhies, R. C., & Itti, L. (2016). Ic3po data and source code. Accessed 3 February 2016.

  • Grisetti, G., Stachniss, C., & Burgard, W. (2005). Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling. In: Proceedings of the 2005 IEEE international conference on robotics and automation, 2005. ICRA 2005 (pp. 2432–2437). IEEE.

  • Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved techniques for grid mapping with rao-blackwellized particle filters. IEEE Transactions on Robotics, 23(1), 34–46.

    Article  Google Scholar 

  • Hahnel, D., Burgard, W., Fox, D., & Thrun, S. (2003) An efficient fastslam algorithm for generating maps of large-scale cyclic environments from raw laser range measurements. In: 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2003. (IROS 2003). Proceedings (Vol. 1, pp. 206–211). IEEE.

  • Henry, P., Krainin, M., Herbst, E., Ren, X., & Fox, D. (2012). Rgb-d mapping: Using kinect-style depth cameras for dense 3d modeling of indoor environments. The International Journal of Robotics Research, 31(5), 647–663.

    Article  Google Scholar 

  • Horn, B. K. (1987). Closed-form solution of absolute orientation using unit quaternions. JOSA A, 4(4), 629–642.

    Article  Google Scholar 

  • Itti, L., Voorhies, R. C., Grant, W. S., Parks, D., & Berg, D. (2016). Neuromorphic robotics toolkit. . Accessed 3 February 2016

  • Kaess, M. (2015). Simultaneous localization and mapping with infinite planes. In: 2015 IEEE International Conference on Robotics and Automation (ICRA) (pp. 4605–4611). IEEE.

  • Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J.J., & Dellaert, F. (2011). isam2: Incremental smoothing and mapping using the bayes tree. The International Journal of Robotics Research, 0278364911430419.

  • Markley, F. L., & Mortari, D. (2000). Quaternion attitude estimation using vector observations. Journal of the Astronautical Sciences, 48(2), 359–380.

    Google Scholar 

  • Moosmann, F., & Stiller, C. (2011). Velodyne slam. In: 2011 IEEE Intelligent Vehicles Symposium (IV) (pp. 393–398). IEEE.

  • Newcombe, R. A., Davison, A. J., Izadi, S., Kohli, P., Hilliges, O., Shotton, J., et al. (2011). Kinectfusion: Real-time dense surface mapping and tracking. In: 2011 10th IEEE international symposium on Mixed and augmented reality (ISMAR) (pp. 127–136). IEEE.

  • Pathak, K., Birk, A., Vaskevicius, N., Pfingsthorn, M., Schwertfeger, S., & Poppinga, J. (2010a). Online three-dimensional slam by registration of large planar surface segments and closed-form pose-graph relaxation. Journal of Field Robotics, 27(1), 52–84.

    Article  Google Scholar 

  • Pathak, K., Birk, A., Vaskevicius, N., & Poppinga, J. (2010b). Fast registration based on noisy planes with unknown correspondences for 3-d mapping. IEEE Transactions on Robotics, 26(3), 424–441.

    Article  Google Scholar 

  • Salas-Moreno, R. F., Glocken, B., Kelly, P. H., & Davison, A. J. (2014). Dense planar slam. In: 2014 IEEE international symposium on mixed and augmented reality (ISMAR) (pp. 157–164). IEEE.

  • Segal, A., Haehnel, D., & Thrun, S. (2009). Generalized-icp. In: Robotics: Science and Systems, Vol. 2.

  • Shuster, M. D. (2006). The generalized Wahba problem. The Journal of the Astronautical Sciences, 54(2), 245–259.

    Article  MathSciNet  Google Scholar 

  • Shuster, M. D., & Oh, S. (2012). Three-axis attitude determination from vector observations. Journal of Guidance, Control, and Dynamics.

  • Trevor, A. J., Rogers, J. G., & Christensen, H. I. (2014). Omnimapper: A modular multimodal mapping framework. In: 2014 IEEE International Conference on Robotics and Automation (ICRA) (pp. 1983–1990). IEEE.

  • Umeyama, S. (1991). Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence, 13(4), 376–380.

    Article  Google Scholar 

  • Urmson, C., Bagnell, J. A., Baker, C. R., Hebert, M., Kelly, A., Rajkumar, R., et al. (2007). Tartan racing: A multi-modal approach to the darpa urban challenge.

  • Weingarten, J., & Siegwart, R. (2005). Ekf-based 3d slam for structured environment reconstruction. In: 2005 IEEE/RSJ international conference on intelligent robots and systems, 2005 (IROS 2005) (pp. 3834–3839). IEEE.

  • Weingarten, J., & Siegwart, R. (2016) 3d slam using planar segments. In: 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp. 3062–3067). IEEE.

  • Whelan, T., Kaess, M., Fallon, M., Johannsson, H., Leonard, J., & McDonald, J. (2012). Kintinuous: Spatially extended kinectfusion. CSAIL Technical Reports

  • Wright, S., & Nocedal, J. (1999). Numerical optimization (Vol. 2). New York: Springer.

    MATH  Google Scholar 

  • Zhang, J., & Singh, S. (2014a). LOAM: Lidar odometry and mapping in real-time. In: Robotics: Science and Systems Conference (RSS). Berkeley, CA.

  • Zhang, J., & Singh, S. (2014b). Loam velodyne: A realtime method for state estimation and mapping using a 3d lidar.

Download references

Author information

Authors and Affiliations


Corresponding author

Correspondence to W. Shane Grant.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

This work was supported by the National Science Foundation (Grant Numbers CCF-1317433 and CNS-1545089), C-BRIC (one of six centers in JUMP, a Semiconductor Research Corporation (SRC) program sponsored by DARPA), the Army Research Office (W911NF-12-1-0433), the Office of Naval Research (N00014-13-1-0563), and the Intel Corporation. The authors affirm that the views expressed herein are solely their own, and do not represent the views of the United States government or any agency thereof.

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Grant, W.S., Voorhies, R.C. & Itti, L. Efficient Velodyne SLAM with point and plane features. Auton Robot 43, 1207–1224 (2019).

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: