Robust and efficient visual odometry using colored point cloud maps via dual-sparsity and hierarchical optimization
摘要
Reliable localization is fundamental to navigation in GNSS-challenged environments, yet monocular visual odometry (VO) inevitably suffers from drift without global constraints. Point cloud maps can provide global corrections to mitigate this drift. However, existing heterogeneous feature registration methods fail to fully exploit the shared information between vision and point clouds. This limitation results in suboptimal localization accuracy, reduced robustness, and increased computational overhead. To address these issues, we propose a visual odometry system based on colored point cloud maps, which leverages both the global point cloud map as a constraint and the shared modality (color) between the colored point cloud map and the camera to ensure localization accuracy and robustness. The system consists of two main components: sparse colored point cloud construction and visual localization. In the sparse colored point cloud construction module, we introduce a map sparsification strategy that associates visual gradients with point clouds, ensuring that the retained sparse point cloud preserves salient visual gradient information, thereby reducing computational costs. This strategy is further incorporated into the vision-to-map matching stage, forming a “dual-sparsity matching” scheme. In the localization stage, we propose a hierarchical optimization-based iterative Kalman filtering algorithm, which performs multi-level iterative optimization over multi-resolution images to prevent localization from getting trapped in local optima while enhancing accuracy. Experiments on public and self-collected sequences demonstrate significant gains in both accuracy and efficiency. Compared with a representative map-based method (DSL), the proposed approach reduces ATE (RMSE) by