Fast ICP-SLAM Method Based on Multi-resolution Search andMulti-density Point Cloud Matching
LI Xin1, ZHONG Xunyu1, PENG Xiafu1, GONG Zhaohui1, ZHONG Xungao2
1. Department of Automation, Xiamen University, Xiamen 361102, China; 2. School of Electrical Engineering and Automation, Xiamen University of Technology, Xiamen 361024, China
Abstract：For better real-time performance and localization accuracy of LiDAR SLAM (simultaneous localization and mapping), a fast ICP-SLAM (iterative closest point SLAM) method with hierarchical search and matching is proposed on the basis of classic ICP-SLAM, in order to overcome the influence of the increased search range and pose matching resolution on real-time performance when the initial pose is not accurate. Firstly, global search is performed from coarse to fine in the search range, and calculation for matching is carried out step by step through gradually increasing the density of the points to be matched. In the process of point cloud matching, the distance from the nearest neighbour to the matched points is calculated by constructing the distance map, and the calculation cost is reduced to O(1). Secondly, the non-optimal solutions are eliminated quickly by prioritizing and pruning the points matching results. Finally, half-globally optimal principle and full-number locally optimal principle are taken as the judgment conditions for the end of the search to improve search efficiency. The test results on the benchmark dataset show that the proposed method achieves smaller average error and square error compared with the popular LiDAR SLAM algorithm Cartographer, and the computation efficiency is about 2~5 times faster than Cartographer algorithm. Meanwhile, the practical application to the industrial AGV (automated guided vehicle) demonstrates that the real-time pose estimation and mapping can be realized without initial pose knowledge, and the repeated positioning accuracy is smaller than 1.5 cm. In conclusion, the proposed fast ICP-SLAM method can ensure accurate localization and good real-time performance.
[1] Thrun S, Burgard W, Fox D. Probabilistic robotics[M]. Cambridge, USA:MIT Press, 2005. [2] Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM:A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163. [3] Qin T, Li P L, Shen S J. VINS-Mono:A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4):1004-1020. [4] Forster C, Pizzoli M, Scaramuzza D. SVO:Fast semi-direct monocular visual odometry[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2014:15-22. [5] Kohlbrecher S, Stryk O V, Meyer J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//IEEE International Symposium on Safety, Security, and Rescue Robotics. Piscataway, USA:IEEE, 2011:155-160. [6] Hess W, Kohler D, Rapp H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2016:1271-1278. [7] Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1):34-46. [8] Li L, Yang M, Wang B, et al. An overview on sensor map based localization for automated driving[C]//2017 Joint Urban Remote Sensing Event. Piscataway, USA:IEEE, 2017. DOI:10.1109/JURSE.2017.7924575. [9] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2):239-256. [10] Trehard G, Alsayed Z, Pollard E, et al. Credibilist simultaneous localization and mapping with a LIDAR[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2014:2699-2706. [11] Arth C, Pirchheim C, Ventura J, et al. Instant outdoor localization and SLAM initialization from 2.5D maps[J]. IEEE Transactions on Visualization and Computer Graphics, 2015, 21(11):1309-1318. [12] Choudhary S, Indelman V, Christensen H I, et al. Information-based reduced landmark SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2015:4620-4627. [13] Mäkelä T, Clarysse P, Sipilä O, et al. A review of cardiac image registration methods[J]. IEEE Transactions on Medical Imaging, 2002, 21(9):1011-1021. [14] Newcombe R A, Izadi S, Hilliges O, et al. KinectFusion:Real-time dense surface mapping and tracking[C]//10th IEEE/ACM International Symposium on Mixed and Augmented Reality. Piscataway, USA:IEEE, 2011:127-136. [15] Schapire R E, Freund Y, Bartlett P, et al. Boosting the margin:A new explanation for the effectiveness of voting methods[J]. Annals of Statistics, 1998, 26(5):1651-1686. [16] Wang S F, Wu Z, Zhang W C. An overview of SLAM[M]//Lecture Notes in Electrical Engineering, Vol.528. Berlin, Germany:Springer, 2019:673-681. [17] Akca D, Gruen A. Fast correspondence search for 3D surface matching[C]//International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Vol.36.2005:186-191. [18] Zhang Z Y. Iterative point matching for registration of free-form curves and surfaces[J]. International Journal of Computer Vision, 1994, 13(2):119-152. [19] Liu Y H. Constraints for closest point finding[J]. Pattern Recognition Letters, 2008, 29(7):841-851. [20] Liu Y H. Improving ICP with easy implementation for free-form surface matching[J]. Pattern Recognition, 2004, 37(2):211-226. [21] Fitzgibbon A W. Robust registration of 2D and 3D point sets[J]. Image and Vision Computing, 2002, 21(13-14):1145-1153. [22] Breuel T M. Implementation techniques for geometric branch-and-bound matching methods[J]. Computer Vision and Image Understanding, 2003, 90(3):258-294. [23] Tiar R, Ouadah N, Azouaoui O, et al. ICP-SLAM methods implementation on a bi-steerable mobile robot[C]//11th IEEE International Workshop of Electronics, Control, Measurement, Signals and Their Application to Mechatronics. Piscataway, USA:IEEE, 2013. DOI:10.1109/ECMSM.2013.6648973. [24] Bustos A P, Chin T J, Eriksson A, et al. Fast rotation search with stereographic projections for 3D registration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2016, 38(11):2227-2240. [25] Yang J L, Li H D, Campbell D, et al. Go-ICP:A globally optimal solution to 3D ICP point-set registration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2016, 38(11):2241-2254. [26] Mellado N, Aiger D, Mitra N J. Super 4PCS fast global pointcloud registration via smart indexing[J]. Computer Graphics Forum, 2014, 33(5):205-215. [27] Olson E B. Real-time correlative scan matching[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2009:4387-4393. [28] Jost T, Hügli H. A multi-resolution scheme ICP algorithm for fast shape registration[C]//1st International Symposium on 3D Data Processing Visualization and Transmission. Piscataway, USA:IEEE, 2002:540-543. [29] Back J H, Kim S, Ho Y S. High-precision 3D coarse registration using RANSAC and randomly-picked rejections[C]//24th International Conference on Multimedia Modeling. Cham, Switzerland:Springer, 2018:254-266. [30] Kummerle R, Grisetti G, Strasdat H, et al. G2o:A general framework for graph optimization[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2011:3607-3613. [31] Kümmerle R, Steder B, Dornhege C, et al. On measuring the accuracy of SLAM algorithms[J]. Autonomous Robots, 2009, 27(4):387-407.
