Optimization of a SLAM graph-based implementation of an autonomous driving system

Scalability represents a fundamental challenge in pose graph optimization for SLAM systems. As new poses and constraints are continuously added during map creation, the underlying graph progressively increases in size and density. While global optimization is essential to ensure consistency, jointly optimizing all accumulated constraints can lead to prohibitive computational costs, particularly in large-scale and real-time applications. This work explores hierarchical initialization as a strategy to alleviate this limitation. By decomposing the global optimization problem into structured subgraphs, hierarchical methods provide a coarse but globally consistent estimate before refining the solution. The HiPE framework was implemented and evaluated using two decomposition strategies: Breadth-First Splitter (BFS) and Nested Dissection (ND). The approach was integrated within a nonlinear least-squares pipeline based on Ceres Solver. Experimental validation was performed on datasets of varying structural complexity, using Absolute Trajectory Error (ATE) and execution time as evaluation metrics. The results confirm that hierarchical initialization improves convergence behavior and computational efficiency compared to full pose graph optimization, particularly in dense graphs. BFS achieves the best performance under optimal parameter settings, while ND demonstrates greater robustness across configurations. The observed effectiveness of skeleton-only optimization further supports the potential of hierarchical approaches as scalable solutions for large-scale SLAM systems