Simultaneous Vehicle Localization and Roadside Tree Inventory Using Integrated LiDAR-Inertial-GNSS System

Abstract

Autonomous driving systems rely on a comprehensive understanding of the surrounding environment, and trees, as important roadside features, have a significant impact on vehicle positioning and safety analysis. Existing methods use mobile LiDAR systems (MLS) to collect environmental information and automatically generate tree inventories based on dense point clouds, providing accurate geometric parameters. However, the use of MLS systems requires expensive survey-grade laser scanners and high-precision GNSS/IMU systems, which limits their large-scale deployment and results in poor real-time performance. Although LiDAR-based simultaneous localization and mapping (SLAM) techniques have been widely applied in the navigation field, to the best of my knowledge, there has been no research conducted on simultaneous real-time localization and roadside tree inventory. This paper proposes an innovative approach that uses LiDAR technology to achieve vehicle positioning and a roadside tree inventory. Firstly, a front-end odometry based on an error-state Kalman filter (ESKF) and a back-end optimization framework based on factor graphs are employed. The updated poses from the back-end are used for establishing point-to-plane residual constraints for the front-end in the local map. Secondly, a two-stage approach is adopted to minimize global mapping errors, refining accumulated mapping errors through GNSS-assisted registration to enhance system robustness. Additionally, a method is proposed for creating a tree inventory that extracts line features from real-time LiDAR point cloud data and projects them onto a global map, providing an initial estimation of possible tree locations for further tree detection. This method uses shared feature extraction results and data pre-processing results from SLAM to reduce the computational load of simultaneous vehicle positioning and roadside tree inventory. Compared to methods that directly search for trees in the global map, this approach benefits from fast perception of the initial tree position, meeting real-time requirements. Finally, our system is extensively evaluated on real datasets covering various road scenarios, including urban and suburban areas. The evaluation metrics are divided into two parts: the positioning accuracy of the vehicle during operation and the detection accuracy of trees. The results demonstrate centimeter-level positioning accuracy and real-time automatic creation of a roadside tree inventory.