Global Fine Registration of Point Cloud in LiDAR SLAM Based on Pose Graph

doi:10.11947/j.JGGS.2020.0203

Abstract

Abstract:

The laser scanning system based on Simultaneous Localization and Mapping (SLAM)technology has the advantages of low cost, high precision and high efficiency. It has drawn wide attention in the field of surveying and mapping in recent years. Although real-time data acquisition can be achieved using SLAM technology, the precision of the data can’t be ensured, and inconsistency exists in the acquired point cloud. In order to improve the precision of the point cloud obtained by this kind of system, this paper presents a hierarchical point cloud global optimization algorithm. Firstly, the “point-to-plane” iterative closest point (ICP) algorithm is used to match the overlapping point clouds to form constraints between the trajectories of the scanning system. Then a pose graph is constructed to optimize the trajectory. Finally, the optimized trajectory is used to refine the point cloud. The computational efficiency is improved by decomposing the optimization process into two levels, i.e. local level and global level. The experimental results show that the RMSE of the distance between the corresponding points in overlapping areas is reduced by about 50% after optimization, and the internal inconsistency is effectively eliminated.

Key words: point cloud refine; Simultaneous Localization and Mapping; global optimization; graph optimization; iterative closest point

Li YAN,Jicheng DAI,Junxiang TAN,Hua LIU,Changjun CHEN. Global Fine Registration of Point Cloud in LiDAR SLAM Based on Pose Graph[J]. Journal of Geodesy and Geoinformation Science, 2020, 3(2): 26-35.

Figures/Tables 9

Fig.1

Fig.2

Fig.3

Fig.4

Tab.1

Fig.5

Fig.6

Fig.7

Tab.2

References 30

[1]	MEI Wensheng, ZHOU Yanfang, ZHOU Jun. Fine topographic mapping based on ground three-dimensional laser scanning[J]. Bulletin of Surveying and Mapping, 2010(1):53-56.
[2]	YANG Bisheng, DONG Zhen, WEI Zheng, et al. Extracting complex building facades from mobile laser scanning data[J]. Acta Geodaetica et Cartographica Sinica, 2013,42(3):411-417.
[3]	TANG Jian, CHEN Yuwei, NIU Xiaoji, et al. LiDAR scan matching aided inertial navigation system in GNSS-denied environments[J]. Sensors, 2015,15(7):16710-16728. doi: 10.3390/s150716710 pmid: 26184206
[4]	WANG Heng, WANG Bin, LIU Bingbing, et al. Pedestrian recognition and tracking using 3D LiDAR for autonomous vehicle[J]. Robotics and Autonomous Systems, 2017,88:71-78. doi: 10.1016/j.robot.2016.11.014
[5]	LIANG Mingjie, MIN Huaqing, LUO Ronghua. Graph-based SLAM:a survey[J]. Robot, 2013,35(4):500-512. doi: 10.3724/SP.J.1218.2013.00500
[6]	LAUTERBACH H, BORRMANN D, HEβ R, et al. Evaluation of a backpack-mounted 3D mobile scanning system[J]. Remote Sensing, 2015,7(10):13753-13781. doi: 10.3390/rs71013753
[7]	BOSSE M, ZLOT R. Place recognition using keypoint voting in large 3D LiDAR datasets [C]//Proceedings of 2013 IEEE International Conference on Robotics and Automation. Karlsruhe, Germany:IEEE, 2013: 2677-2684.
[8]	ZLOT R, BOSSE M. Efficient large-scale three-dimensional mobile mapping for underground mines[J]. Journal of Field Robotics, 2014,31(5):758-779. doi: 10.1002/rob.21504
[9]	HESS W, KOHLER D, RAPP H, et al. Real-time loop closure in 2D LiDAR SLAM [C]//Proceedings of 2016 IEEE International Conference on Robotics and Automation. Stockholm, Sweden:IEEE, 2016.
[10]	CAO Jun, ZENG Bi, LIU Jianqi, et al. A novel relocation method for simultaneous localization and mapping based on deep learning algorithm[J]. Computers & Electrical Engineering, 2017,63:79-90.
[11]	ELSEBERG J, BORRMANN D, NüCHTER A. 6D of Semi-rigid slam for mobile scanning [C]//Proceedings of 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vilamoura, Portugal:IEEE, 2012: 1865-1870.
[12]	YAO Jili, MA Ning, JIA Xiangyang, et al. Auto-registration for terrestrial laser scanning multi-stations point clouds with bundle block adjustment method[J]. Acta Geodaetica et Cartographica Sinica, 2014,43(7):711-716, 723. DOI: 10.13485/j.cnki.11-2089.2014.0099.
[13]	YAO Jili, JIA Xiangyang, MA Ning, et al. Overall orientation adjustment model of terrestrial laser scanning multi-station point clouds[J]. Acta Geodaetica et Cartographica Sinica, 2014,43(8):835-841. DOI: 10.13485/j.cnki.11-2089.2014.0123.
[14]	YAN Li, TAN Junxiang, YANG Ronghao, et al. A method of globally optimal registration for multi-view point clouds constrained by closed-loop conditions[J]. Acta Geodaetica et Cartographica Sinica, 2016,45(4):418-424. DOI: 10.11947/j.AGCS.2016.20150018.
[15]	KELBE D, VAN AARDT J, ROMANCZYK P, et al. Marker-free registration of forest terrestrial laser scanner data pairs with embedded confidence metrics[J]. IEEE Transactions on Geoscience and Remote Sensing, 2016,54(7):4314-4330. doi: 10.1109/TGRS.2016.2539219
[16]	KELBE D, VAN AARDT J, ROMANCZYK P, et al. Multiview marker-free registration of forest terrestrial laser scanner data with embedded confidence metrics[J]. IEEE Transactions on Geoscience and Remote Sensing, 2017,55(2):729-741. doi: 10.1109/TGRS.2016.2614251
[17]	LI Cailin, GUO Baoyun, JI Zheng. Global optimization and whole registration algorithm of multi-view 3D laser point cloud[J]. Acta Geodaetica et Cartographica Sinica, 2015,44(2):183-189. DOI: 10.11947/j.AGCS.2015.20130737. doi: 10.11947/j.AGCS.2015.20130737
[18]	GRISETTI G, KUMMERLE R, STACHNISS C, et al. A tutorial on graph-based slam[J]. IEEE Intelligent Transportation Systems Magazine, 2010,2(4):31-43.
[19]	BORRMANN D, ELSEBERG J, LINGEMANN K, et al. Globally consistent 3D mapping with scan matching[J]. Robotics and Autonomous Systems, 2008,56(2):130-142. doi: 10.1016/j.robot.2007.07.002
[20]	BESL P J, MCKAY N D. A method for registration of 3D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992,14(2):239-256. doi: 10.1109/34.121791
[21]	KÜMMERLE R, GRISETTI G, STRASDAT H, et al. G2O:a general framework for graph optimization [C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation.[S.l.]:IEEE, 2011: 3607-3613.
[22]	CADENA C, CARLONE L, CARRILLO H, et al. Past, present, and future of simultaneous localization and mapping:toward the robust-perception age[J]. IEEE Transactions on Robotics, 2016,32(6):1309-1332. doi: 10.1109/TRO.2016.2624754
[23]	SHEPARD D P, HUMPHREYS T E. High-precision globally-referenced position and attitude via a fusion of visual SLAM, carrier-phase-based GPS, and inertial measurements [C]//Proceedings of 2014 IEEE/ION Position, Location and Navigation Symposium. Monterey, CA, USA:IEEE, 2014: 1309-1328.
[24]	MARQUARDT D W. An algorithm for least-squares estimation of nonlinear parameters[J]. Journal of the Society for Industrial and Applied Mathematics, 1963,11(2):431-441. doi: 10.1137/0111030
[25]	RUSU R B, MARTON Z C, BLODOW N, et al. Towards 3D point cloud based object maps for household environments[J]. Robotics and Autonomous Systems, 2008,56(11):927-941. doi: 10.1016/j.robot.2008.08.005
[26]	PAULY M, KEISER R, GROSS M. Multi-scale feature extraction on point-sampled surfaces[J]. Computer Graphics Forum, 2003,22(3):281-289. doi: 10.1111/cgf.2003.22.issue-3
[27]	POMERLEAU F, COLAS F, SIEGWART R, et al. Comparing ICP variants on real-world data sets[J]. Autonomous Robots, 2013,34(3):133-148. doi: 10.1007/s10514-013-9327-2
[28]	PULLI K. Multiview registration for large data sets [C]//Proceedings of the Second International Conference on 3D Digital Imaging and Modeling. Ottawa:IEEE, 1999: 160-168.
[29]	LOW K L. Linear least-squares optimization for point-to-plane ICP surface registration [R].[S.l.]:University of North Carolina at Chapel Hill, 2004,4(TR04).
[30]	WANG Yongbo, YANG Huachao, LIU Yanhua, et al. Linear-feature-constrained registration of LiDAR point cloud via quaternion[J]. Geomatics and Information Science of Wuhan University, 2013,38(9):1057-1062.

Data	Scanning time/s	Trajectory length/m	Number of point cloud frames
1	382	291.2	7559
2	1466	1300.5	21718

time interval t_s /s	Data 1 RMSE/cm		Data 2 RMSE/cm
time interval t_s /s	Before optimization	After optimization	Before optimization	After optimization
1	10.5	4.8	14.2	6.6
2	10.5	4.5	14.3	6.7
3	10.7	4.3	14.4	6.7
4	10.9	4.4	14.4	6.8
5	11.0	4.5	14.5	7.0
Mean	10.7	4.5	14.4	6.8