近年來與自動駕駛載具 (Self-Driving Vehicle) 相關的技術蓬勃發展，而在這眾多技術背後其中一個重要的層面是一套完整基於光達感測器的同步定位與地圖構建演算法。本研究提出了三個新的方法，改善同步定位與地圖構建演算法的框架，能夠優化並加強點雲定位的精度，進而改善點雲稠密地圖的準確性。第一種方法主要針對輸入點雲資料的單幀物件移除進行處理。該方法包括地面點分割、反射率過濾法、DBSCAN 聚類、高度資訊過濾和K-Nearest Neighbor (KNN) 演算法。這些演算法被結合成一個物件移除的架構，特別設計用於實現從點雲資料的單幀中移除物件的目標。第二，我們對於LeGO-GSEF 架構的定位方法做出改良，增加了姿態航向參考系統 (AHRS) 的預補償，此方法能增加演算法對高速旋轉場景的強健性，以得到一個良好的初始位姿；第三，本研究提出一個位姿圖優化 (Pose Graph Optimization) 演算法的權重策略，其權重對錯誤的閉環檢測 (Loop Detection) 有強健性，達到消除定位時產生累積誤差的目標。本研究在中國鋼鐵 88 號倉庫資料集、KITTI 資料集、虛擬場景之虛幻引擎 (Unreal Engine) 資料集，以及高速旋轉場景的 VLP-16 航空太空工程學系 (DAA) 資料集，都有良好的表現，並對後端優化部分與經典的位姿圖優化方法做比較，以證明本研究在優化位姿上的研究成果。

LiDAR-based Simultaneous Localization and Mapping (SLAM) algorithms have become instrumental in the advancement of self-driving vehicle technologies. This study introduces three innovative methods specifically designed to enhance the precision of point cloud localization.
The first method primarily aims at single-frame object removal in the input point cloud data. It employs various algorithms, including ground point segmentation, intensity filtering, Density-Based Spatial Clustering of Applications with Noise (DBSCAN), height information filtering, and the K-Nearest Neighbor (KNN) algorithm. These algorithms are combined to form a framework for object removal, specifically designed to achieve the goal of removing objects from individual frames of the point cloud data.
The second method involves enhancements to the LeGO-GSEF framework by incorporating the pre-compensation of attitude using the Attitude Heading Reference System (AHRS). This modification improves the algorithm's robustness in high-speed rotation scenarios, enabling more accurate initial pose estimation.
Furthermore, a novel weighting strategy for Pose Graph Optimization (PGO) is proposed to address error accumulation during frame-to-global mapping. This strategy demonstrates robustness to incorrect loop detection, thus improving the overall accuracy of the localization process.
To validate the performance of the proposed algorithms, several datasets were used. The China Steel Corporation 88th Warehouse Dataset was employed to evaluate the object removal capabilities. The VLP-16 DAA Dataset was utilized to test the robustness of the AHRS pre-compensation method under high-speed rotation scenarios. The KITTI Dataset and the Unreal Engine Dataset were utilized to compare the loop closure results with standard PGO and ground truth, confirming the effectiveness of the proposed methods in optimizing the poses.

[1] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics: The kitti dataset,” The International Journal of Robotics Research, vol. 32, no. 11, pp. 1231–1237, 2013.
[2] 蔡子正et al., “三維點雲特徵萃取於高精地圖構建演算法開發,” Master’s thesis, National Cheng Kung University, 2022.
[3] D. Reiser, M. Vázquez-Arellano, D. S. Paraforos, M. Garrido-Izard, and H. W. Griepentrog, “Iterative individual plant clustering in maize with assembled 2d lidar data,” Computers in Industry, vol. 99, pp. 42–52, 2018.
[4] Y. Yenıaydin and K. W. Schmidt, “Sensor fusion of a camera and 2d lidar for lane detection,” in 2019 27th Signal Processing and Communications Applications Conference (SIU), pp. 1–4, 2019.
[5] G. Vosselman, B. G. Gorte, G. Sithole, and T. Rabbani, “Recognising structure in laser scanner point clouds,” International archives of photogrammetry, remote sensing and spatial information sciences, vol. 46, no. 8, pp. 33–38, 2004.
[6] S. Manivasagam, S. Wang, K. Wong, W. Zeng, M. Sazanovich, S. Tan, B. Yang, W.-C. Ma, and R. Urtasun, “Lidarsim: Realistic lidar simulation by leveraging the real world,” in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 11167–11176, 2020.
[7] X. Chen, A. Milioto, E. Palazzolo, P. Giguere, J. Behley, and C. Stachniss, “Suma++: Efficient lidar-based semantic slam,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4530–4537, 2019.
[8] X. Zhang, A. B. Rad, and Y.-K. Wong, “Sensor fusion of monocular cameras and laser rangefinders for line-based simultaneous localization and mapping (slam) tasks in autonomous mobile robots,” Sensors, vol. 12, no. 1, pp. 429–452, 2012.
[9] Y. Su, T. Wang, S. Shao, C. Yao, and Z. Wang, “Gr-loam: Lidar-based sensor fusion slam for ground robots on complex terrain,” Robotics and Autonomous Systems, vol. 140, p. 103759, 2021.
[10] Z. Li, J. Zheng, Z. Zhu, and S. Wu, “Selectively detail-enhanced fusion of differently exposed images with moving objects,” IEEE Transactions on Image Processing, vol. 23, no. 10, pp. 4372-4382, 2014.
[11] J. Wang and J. Kim, “Semantic segmentation of urban scenes with a location prior map using lidar measurements,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 661–666, 2017.
[12] J. Yan, D. Chen, H. Myeong, T. Shiratori, and Y. Ma, “Automatic extraction of moving objects from image and lidar sequences,” in 2014 2nd International Conference on 3D Vision, vol. 1, pp. 673–680, 2014.
[13] M. Keller, D. Lefloch, M. Lambers, S. Izadi, T. Weyrich, and A. Kolb, “Real-time 3d reconstruction in dynamic scenes using point-based fusion,” in 2013 International Conference on 3D Vision-3DV 2013, pp. 1–8, 2013.
[14] J. Schauer and A. Nüchter, “The peopleremover—removing dynamic objects from 3-d point cloud data by traversing a voxel occupancy grid,” IEEE robotics and automation letters, vol. 3, no. 3, pp. 1679–1686, 2018.
[15] C. Wang, M. Ji, J. Wang, W. Wen, T. Li, and Y. Sun, “An improved dbscan method for lidar data segmentation with automatic eps estimation,” Sensors, vol. 19, no. 1, p. 172, 2019.
[16] M. Ester, H.-P. Kriegel, J. Sander, X. Xu, et al., “A density-based algorithm for discovering clusters in large spatial databases with noise.,” in kdd, vol. 96, pp. 226–231, 1996.
[17] L. E. Peterson, “K-nearest neighbor,” Scholarpedia, vol. 4, no. 2, p. 1883, 2009.
[18] Y. Wu, Y. Wang, S. Zhang, and H. Ogai, “Deep 3d object detection networks using lidar data: A review,” IEEE Sensors Journal, vol. 21, no. 2, pp. 1152–1171, 2020.
[19] C. R. Qi, H. Su, K. Mo, and L. J. Guibas, “Pointnet: Deep learning on point sets for 3d classification and segmentation,” in Proceedings of the IEEE conference on computer vision and pattern recognition, pp. 652–660, 2017.
[20] C. R. Qi, L. Yi, H. Su, and L. J. Guibas, “Pointnet++: Deep hierarchical feature learning on point sets in a metric space,” Advances in neural information processing systems, vol. 30, 2017.
[21] B. Li, T. Zhang, and T. Xia, “Vehicle detection from 3d lidar using fully convolutional network,” 2016.
[22] X. Chen, H. Ma, J. Wan, B. Li, and T. Xia, “Multi-view 3d object detection network for autonomous driving,” 2017.
[23] Y. Wang, T. Shi, P. Yun, L. Tai, and M. Liu, “Pointseg: Real-time semantic segmentation based on 3d lidar point cloud,” arXiv preprint arXiv:1807.06288, 2018.
[24] A. Kendall and Y. Gal, “What uncertainties do we need in bayesian deep learning for computer vision?,” Advances in neural information processing systems, vol. 30, 2017.
[25] G. P. Meyer, A. Laddha, E. Kee, C. Vallespi-Gonzalez, and C. K. Wellington, “Lasernet: An efficient probabilistic 3d object detector for autonomous driving,” in Proceedings of the IEEE/CVF conference on computer vision and pattern recognition, pp. 12677–12686, 2019.
[26] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” in Sensor fusion IV: control paradigms and data structures, vol. 1611, pp. 586–606, Spie, 1992.
[27] K.-L. Low, “Linear least-squares optimization for point-to-plane icp surface registration,” Chapel Hill, University of North Carolina, vol. 4, no. 10, pp. 1–3, 2004.
[28] A. Censi, “An icp variant using a point-to-line metric,” in 2008 IEEE International Conference on Robotics and Automation, pp. 19–25, 2008.
[29] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.,” in Robotics: science and systems, vol. 2, p. 435, Seattle, WA, 2009.
[30] J. Serafin and G. Grisetti, “Nicp: Dense normal based point cloud registration,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 742–749, 2015.
[31] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.,” in Robotics: Science and Systems, vol. 2, pp. 1–9, Berkeley, CA, 2014.
[32] J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-based state estimation problems,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 809–816, 2016.
[33] J. Zhang and S. Singh, “Low-drift and real-time lidar odometry and mapping,” Autonomous Robots, vol. 41, pp. 401–416, 2017.
[34] J. Zhang and S. Singh, “Enabling aggressive motion estimation at low-drift and accurate mapping in real-time,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 5051–5058, 2017.
[35] J. Zhang and S. Singh, “Visual-lidar odometry and mapping: Low-drift, robust, and fast,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 2174–2181, 2015.
[36] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, 2018.
[37] M. Oelsch, M. Karimi, and E. Steinbach, “R-loam: Improving lidar odometry and mapping with point-to-mesh features of a known 3d reference object,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 2068–2075, 2021.
[38] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ international conference on intelligent robots and systems (IROS), pp. 5135–5142, 2020.
[39] G. T. Flitton, T. P. Breckon, and N. M. Bouallagu, “Object recognition using 3d sift in complex ct volumes.,” in BMVC, vol. 1, pp. 1–12, 2010.
[40] P. Dollár, V. Rabaud, G. Cottrell, and S. Belongie, “Behavior recognition via sparse spatiotemporal features,” in 2005 IEEE international workshop on visual surveillance and performance evaluation of tracking and surveillance, pp. 65–72, 2005.
[41] Y. Zhong, “Intrinsic shape signatures: A shape descriptor for 3d object recognition,” in 2009 IEEE 12th international conference on computer vision workshops, ICCV workshops, pp. 689–696, 2009.
[42] F. Tombari, S. Salti, and L. Di Stefano, “Unique signatures of histograms for local surface description,” in Computer Vision–ECCV 2010: 11th European Conference on Computer Vision, Heraklion, Crete, Greece, September 5-11, 2010, Proceedings, Part III 11, pp. 356–369, Springer, 2010.
[43] R. B. Rusu, N. Blodow, Z. C. Marton, and M. Beetz, “Aligning point cloud views using persistent feature histograms,” in 2008 IEEE/RSJ international conference on intelligent robots andsystems, pp. 3384–3391, 2008.
[44] R. B. Rusu, N. Blodow, and M. Beetz, “Fast point feature histograms (fpfh) for 3d registration,” in 2009 IEEE international conference on robotics and automation, pp. 3212–3217, 2009.
[45] L. He, X. Wang, and H. Zhang, “M2dp: A novel 3d point cloud descriptor and its application in loop closure detection,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 231–237, 2016.
[46] G. Kim and A. Kim, “Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4802–4809, 2018.
[47] G. Kim, S. Choi, and A. Kim, “Scan context++: Structural place recognition robust to rotation and lateral variations in urban environments,” IEEE Transactions on Robotics, vol. 38, no. 3, pp. 1856–1874, 2021.
[48] R. Dubé, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, and C. Cadena, “Segmatch: Segment based place recognition in 3d point clouds,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 5266–5272, 2017.
[49] R. Dubé, A. Cramariuc, D. Dugas, J. Nieto, R. Siegwart, and C. Cadena, “Segmap: 3d segment mapping using data-driven descriptors,” arXiv preprint arXiv:1804.09557, 2018.
[50] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous robots, vol. 4, pp. 333–349, 1997.
[51] G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard, “A tutorial on graph-based slam,” IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31–43, 2010.
[52] L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert, “Initialization techniques for 3d slam: A survey on rotation estimation and its use in pose graph optimization,” in 2015 IEEE international conference on robotics and automation (ICRA), pp. 4597–4604, 2015.
[53] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g 2 o: A general framework for graph optimization,” in 2011 IEEE International Conference on Robotics and Automation, pp. 3607–3613, 2011.
[54] S. Agarwal and K. Mierle, “Ceres solver: Tutorial & reference,” Google Inc, vol. 2, no. 72, p. 8, 2012.
[55] M. Kaess, A. Ranganathan, and F. Dellaert, “isam: Incremental smoothing and mapping,” IEEE Transactions on Robotics, vol. 24, no. 6, pp. 1365–1378, 2008.
[56] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012.
[57] F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” tech. rep., Georgia Institute of Technology, 2012.
[58] L. Carlone and G. C. Calafiore, “Convex relaxations for pose graph optimization with outliers,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 1160–1167, 2018.
[59] A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in 2012 IEEE conference on computer vision and pattern recognition, pp. 3354–3361, 2012.
[60] 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.
[61] S. Wold, K. Esbensen, and P. Geladi, “Principal component analysis,” Chemometrics and intelligent laboratory systems, vol. 2, no. 1-3, pp. 37–52, 1987.
[62] J. Shlens, “A tutorial on principal component analysis,” arXiv preprint arXiv:1404.1100, 2014.
[63] 高翔，張濤et al., 視覺SLAM 十四講：從理論到實踐. 電子工業出版社, 2017.
[64] C.-C. Peng, J.-J. Huang, and H.-Y. Lee, “Design of an embedded icosahedron mechatronics for robust iterative imu calibration,” IEEE/ASME Transactions on Mechatronics, vol. 27, no. 3, pp. 1467–1477, 2021.
[65] P. J. Huber, “Robust estimation of a location parameter,” Breakthroughs in statistics: Methodology and distribution, pp. 492–518, 1992.
[66] D. F. Andrews, “A robust method for multiple linear regression,” Technometrics, vol. 16, no. 4, pp. 523–531, 1974.
[67] F. Mosteller and J. W. Tukey, “Data analysis and regression. a second course in statistics,” Addison-Wesley series in behavioral science: quantitative methods, 1977.
[68] P. W. Holland and R. E. Welsch, “Robust regression using iteratively reweighted least-squares,” Communications in Statistics-theory and Methods, vol. 6, no. 9, pp. 813–827, 1977.
[69] R. C. Fair, “On the robust estimation of econometric models,” in Annals of Economic and Social Measurement, Volume 3, number 4, pp. 667–677, NBER, 1974.
[70] S. M. Stigler, “Studies in the history of probability and statistics. xxxiii cauchy and the witch of agnesi: An historical note on the cauchy distribution,” Biometrika, pp. 375–380, 1974.
[71] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of rgb-d slam systems,” in 2012 IEEE/RSJ international conference on intelligent robots and systems, pp. 573–580, 2012.