JACIII Vol.26 No.2 pp. 206-216
doi: 10.20965/jaciii.2022.p0206


High-Precision and Fast LiDAR Odometry and Mapping Algorithm

Qingshan Wang*,**, Jun Zhang**,†, Yuansheng Liu**, and Xinchen Zhang**

*CATARC (Tianjin) Automotive Engineering Research Institute Co., Ltd.
No.68 Xianfeng East Road, Dongli District, Tianjin 300300, China

**College of Robotics, Beijing Union University
No.97 Beisihuan East Road, Chao Yang District, Bejing 100101, China

Corresponding author

October 7, 2019
February 3, 2022
March 20, 2022
LiDAR SLAM, NDT, PLICP, localization, mapping

LiDAR SLAM technology is an important method for the accurate navigation of automatic vehicles and is a prerequisite for the safe driving of automatic vehicles in the unstructured road environment of complex parks. This paper proposes a LiDAR fast point cloud registration algorithm that can realize fast and accurate localization and mapping of automatic vehicle point clouds through a combination of a normal distribution transform (NDT) and point-to-line iterative closest point (PLICP). First, the NDT point cloud registration algorithm is applied for the rough registration of point clouds between adjacent frames to achieve a rough estimate of the pose of automatic vehicles. Then, the PLICP point cloud registration algorithm is adopted to correct the rough registration result of the point cloud. This step completes the precise registration of the point cloud and achieves an accurate estimate of the pose of the automatic vehicle. Finally, cloud registration is accumulated over time, and the point cloud information is continuously updated to construct the point cloud map. Through numerous experiments, we compared the proposed algorithm with PLICP. The average number of iterations of the point cloud registration between adjacent frames was reduced by 6.046. The average running time of the point cloud registration between adjacent frames decreased by 43.05156 ms. The efficiency of the point cloud registration calculation increased by approximately 51.7%. By applying the KITTI dataset, the computational efficiency of NDT-ICP was approximately 60% higher than that of LeGO-LOAM. The proposed method realizes the accurate localization and mapping of automatic vehicles relying on vehicle LiDAR in a complex park environment and was applied to a Small Cyclone automatic vehicle. The results indicate that the proposed algorithm is reliable and effective.

Cite this article as:
Qingshan Wang, Jun Zhang, Yuansheng Liu, and Xinchen Zhang, “High-Precision and Fast LiDAR Odometry and Mapping Algorithm,” J. Adv. Comput. Intell. Intell. Inform., Vol.26, No.2, pp. 206-216, 2022.
Data files:
  1. [1] K. M. Wurm, C. Stachniss, and G. Grisetti, “Bridging the Gap Between Feature- and Grid-based SLAM,” Robotics and Autonomous Systems, Vol.58, No.2, pp. 140-148, 2010.
  2. [2] A. Eliazar and R. Parr, “DP-SLAM: Fast, Robust Simultainous Localization and Mapping Without Predetermined Landmarks,” Proc. of the 18th Int. Joint Conf. on Artificial Intelligence (IJCAI-03), pp. 1135-1142, 2003.
  3. [3] S. Thrun, W. Burgard, and D. Fox, “Probabilistic Robotics,” MIT Press, 2005.
  4. [4] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the bayes tree,” The Int. J. of Robotics Research, Vol.31, pp. 217-236, 2012.
  5. [5] R. Zlot and M. Bosse, “Efficient large-scale 3D mobile mapping and surface reconstruction of an underground mine,” The 7th Int. Conf. on Field and Service Robots, 2012.
  6. [6] L. Hengjie, B. Hong, and X. Cheng, “Fast Closed-Loop SLAM based on the fusion of IMU and Lidar,” J. of Physics: Conf. Series, 2021 Int. Conf. on Electrical, Electronics and Computing Technology (EECT 2021), Vol.1914, Article No.012019, 2021.
  7. [7] S. Yuan, H. Wang, and L. Xie, “Survey on Localization Systems and Algorithms for Unmanned Systems,” Unmanned Systems, Vol.9, No.2, pp. 129-163, 2020.
  8. [8] P. Jiang, L. Chen, H. Guo et al., “Novel indoor positioning algorithm based on Lidar/inertial measurement unit integrated system,” Int. J. of Advanced Robotic Systems, Vol.18, No.2, doi: 10.1177/1729881421999923, 2021.
  9. [9] J. Li, X. Zhang, J. Li et al., “Building and optimization of 3D semantic map based on Lidar and camera fusion,” Neurocomputing, Vol.409, pp. 394-407, 2020.
  10. [10] X. Guo, Y. Liu, Q. Zhong et al., “Research on Moving Target Tracking Algorithm Based on Lidar and Visual Fusion,” J. Adv. Comput. Intell. Intell. Inform, Vol.22, No.5, pp. 593-601, doi: 10.20965/jaciii.2018.p0593, 2018.
  11. [11] Q. Zhong, Y. Liu, X. Guo et al., “Dynamic Obstacle Detection and Tracking Based on 3D Lidar,” J. Adv. Comput. Intell. Intell. Inform, Vol.22, No.5, pp. 602-610, doi: 10.20965/jaciii.2018.p0602, 2018.
  12. [12] K. Konolige, M. Agrawal, and J. Solà, “Large-Scale Visual Odometry for Rough Terrain,” The 13th Int. Symp. of Robotics Research (ISRR), 2010.
  13. [13] David Nistér, O. Naroditsky, and J. R. Bergen, “Visual odometry for ground vehicle applications,” J. of Field Robotics, Vol.23, No.1, pp. 3-20, 2006.
  14. [14] P. J. Besl and N. D. McKay, “A Method for Registration of 3D Shapes,” IEEE Trans. on Pattern Analysis and Machine Intelligence, Vol.14, No.2, pp. 239-256, 1992.
  15. [15] S. Rusinkiewicz and M. Levoy, “Efficient Variants of the ICP Algorithm,” Proc. of the 3rd Int. Conf. on 3-D Digital Imaging and Modeling, pp. 145-152, 2001.
  16. [16] A. Censi, “An ICP variant using a point-to-line metric,” 2008 IEEE Int. Conf. on Robotics and Automation, doi: 10.1109/ROBOT.2008.4543181, 2008.
  17. [17] Y. Chen and G. Medioni, “Object Modelling by Registration of Multiple Range Images,” Image and Vision Computing, Vol.10, No.3, pp. 145-155, 1992.
  18. [18] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” Proc. of Robotics: Science and Systems, 2009.
  19. [19] R. A. Newcombe, S. Izadi, O. Hilliges et al., “Kinect Fusion: Real-time Dense Surface Mapping and Tracking,” Proc. of the IEEE Int. Symp. on Mixed and Augmented Reality, pp. 127-136, 2011.
  20. [20] A. Nuchter, “Parallelization of Scan Matching for Robotic 3D Mapping,” Proc. of the 3rd European Conf. on Mobile Robots, 2007.
  21. [21] D. Qiu, S. May, and A. Nuchter, “GPU-Accelerated Nearest Neighbor Search for 3D Registration,” Proc. of the Int. Conf. on Computer Vision Systems, pp. 194-203, 2009.
  22. [22] D. Neumann, F. Lugauer, S. Bauer et al., “Real-time RGB-D mapping and 3-D modeling on the GPU using the random ball cover data structure,” 2011 IEEE Int. Conf. on Computer Vision Workshops (ICCV Workshops), 2011.
  23. [23] P. Biber and W. Strasser, “The Normal Distributions Transform: A New Approach to Laser Scan Matching,” Proc. 2003 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS 2003), 2003.
  24. [24] M. Magnusson, A. Lilienthal, and T. Duckett, “Scan registration for autonomous mining vehicles using 3D-NDT,” J. of Field Robotics, Vol.24, No.10, pp. 803-827, 2007.
  25. [25] R. B. Rusu, Z. C. Marton, N. Blodow, and M. Beetz, “Learning Informative Point Classes for the Acquisition of Object Model Maps,” Proc. of the IEEE Int. Conf. on Control, Automation, Robotics and Vision, pp. 643-650, 2008.
  26. [26] R. B. Rusu, G. Bradski, R. Thibaux, and J. Hsu, “Fast 3D Recognition and Pose Using the Viewpoint Feature Histogram,” Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 2155-2162, 2010.
  27. [27] M. Bosse and R. Zlot, “Keypoint Design and Evaluation for Place Recognition in 2D Lidar Maps,” Robotics and Autonomous Systems, Vol.57, No.12, pp. 1211-1224, 2009.
  28. [28] R. Zlot and M. Bosse, “Efficient Large-scale 3D Mobile Mapping and Surface Reconstruction of an Underground Mine,” Proc. of the 8th Int. Conf. on Field and Service Robotics, 2012.
  29. [29] J. Zhang and S. Singh, “LOAM: Lidar Odometry and Mapping in Real-time,” Proc. of Robotics: Science and Systems, 2014.
  30. [30] J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry and Mapping,” Autonomous Robots, Vol.41, No.2, pp. 401-416, 2017.
  31. [31] R. Dube, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, and C. Cadena, “SegMatch: Segment Based Place Recognition in 3D Point Clouds,” Proc. of the IEEE Int. Conf. on Robotics and Automation, pp. 5266-5272, 2017.
  32. [32] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain,” IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), pp. 4758-4765, 2018.
  33. [33] D. Zhu, “Point Cloud Library PCL Learning Course,” Beijing Aerospace University Press, 2012.
  34. [34] M. Quigley, B. Gerkey, K. Conley et al., “ROS: An open-source robot operating system,” Workshop on Open Source Software (Collocated with ICRA 2009), 2009.

*This site is desgined based on HTML5 and CSS3 for modern browsers, e.g. Chrome, Firefox, Safari, Edge, Opera.

Last updated on May. 20, 2022