Graph Optimality-Aware Stochastic LiDAR Bundle Adjustment With Progressive Spatial Smoothing

Abstract

Large-scale LiDAR Bundle Adjustment (LBA) to refine sensor orientation and point cloud accuracy simultaneously for building navigation maps is a fundamental task in logistics, intelligent transportation, and robotics. In the context of autonomous delivery and smart mobility, the 3D map obtained by accurate and robust LBA plays a pivotal role in enabling reliable localization and navigation across complex, large-scale urban environments. Unlike pose-graph-based methods that rely solely on pairwise relationships between LiDAR frames, LBA leverages raw LiDAR correspondences to achieve more precise results, especially when initial pose estimates are unreliable for low-cost sensors. However, existing LBA methods face challenges such as simplistic planar correspondences, extensive observations, and dense normal matrices in the least-squares problem, which limit robustness, efficiency, and scalability. To address these issues, we propose a Graph Optimality-aware Stochastic Optimization scheme with Progressive Spatial Smoothing, namely PSS-GOSO, to achieve <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">robust</i>, <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">efficient</i>, and <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">scalable</i> LBA. The Progressive Spatial Smoothing (PSS) module extracts <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">robust</i> LiDAR feature association exploiting the prior structure information obtained by the polynomial smooth kernel. The Graph Optimality-aware Stochastic Optimization (GOSO) module first sparsifies the graph according to optimality for an <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">efficient</i> optimization. GOSO then utilizes stochastic clustering and graph marginalization to solve the large-scale state estimation problem for a <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">scalable</i> LBA. We validate PSS-GOSO across diverse scenes captured by various platforms, demonstrating its superior performance compared to existing methods. Moreover, the resulting point cloud maps are used for automatic last-mile delivery in large-scale complex scenes, showcasing the practical benefits of our method in modern intelligent transportation systems. The project page can be found at: <uri xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">https://kafeiyin00.github.io/PSS-GOSO/</uri>