Fast Pose Graph Optimization via Krylov-Schur and Cholesky Factorization

Gabriel Moreira, Manuel Marques, Joao Paulo CosteiraWACV 2021

TL;DR

This paper introduces MAKS, a novel algorithm for Pose Graph Optimization (PGO) that significantly speeds up 3D mapping and localization tasks. By cleverly combining Krylov-Schur methods for spectral decomposition with Cholesky factorization, MAKS achieves high scalability, low computational cost, and high precision simultaneously, outperforming existing state-of-the-art methods.

What Problem Does This Paper Address?

Accurately reconstructing 3D environments and tracking the movement of sensors (like robots or self-driving cars) within them is crucial for many applications, from autonomous navigation to virtual reality. A core challenge in this is "Pose Graph Optimization" (PGO). Imagine a robot moving through an environment, taking snapshots of its surroundings. PGO aims to find the most accurate sequence of transformations (poses) that describe the robot's path, given noisy measurements of how its current pose relates to previous ones.

Traditional methods for PGO often rely on iterative optimization techniques. While these can find good solutions, they can be computationally expensive and may get stuck in suboptimal solutions, especially for large-scale problems. Some recent approaches have reframed PGO as a "matrix completion" problem, showing promise, but often at the cost of optimality or efficiency. The paper addresses the need for a PGO method that is simultaneously scalable, efficient, and accurate, particularly for large-scale real-world applications where data is sparse and noisy.

Methods

The authors propose a two-stage approach called MAKS (Motion Averaging via Krylov-Schur).

  1. Rotation Averaging (The "R" in SE(3)): This is the more complex part. The core idea is to leverage spectral decomposition. They formulate the problem of finding the optimal rotations as finding the eigenvectors of a specific symmetric block matrix derived from the pairwise rotation measurements.
  2. Krylov-Schur Algorithm: For large, sparse matrices (common in PGO), directly computing all eigenvectors is infeasible. The Krylov-Schur method is used to efficiently compute only the eigenvectors corresponding to the smallest eigenvalues. This is crucial for scalability.
  3. Cholesky LDL Factorization: To speed up the Krylov-Schur iterations, they use Cholesky LDL factorization. This is particularly efficient because the same factorization can be reused later for translation optimization.
  4. Optimality Check: The method includes a check for optimality based on the smallest eigenvalue. If this eigenvalue is below a certain threshold, the solution is considered a good approximation of the global optimum.
  5. Translation Optimization (The "T" in SE(3)): Once the optimal rotations are estimated, finding the optimal translations becomes a much simpler, convex least-squares problem. This is solved efficiently using Cholesky LDL factorization, leveraging the factorization already computed for the rotation step.

Additionally, they embed this core algorithm within an Iteratively Reweighted Least Squares (IRLS) framework. This is important for handling outliers in the pairwise measurements, which are common in real-world data (e.g., from ICP algorithms). IRLS iteratively assigns weights to measurements, down-weighting those that are inconsistent with the current estimate, thus improving robustness.

Datasets

The paper evaluates MAKS on several types of datasets:

  • Simulated Graphs: They create synthetic graphs with varying connectivities and noise levels to empirically study the optimality of their rotation averaging solution.
  • Real-World Rotation Averaging Datasets: They use 12 datasets from Wilson et al. [24] which contain noisy relative rotations and ground-truth information from Bundle Adjustment.
  • RGB-D Datasets for 3D Reconstruction: They test their full pipeline on four datasets (Burghers, Stonewall, Sports car, Lounge) from Zhou et al. [25] to demonstrate its application in dense 3D scene reconstruction.

Key Insights and Ideas

  • Spectral Decomposition for Global Optimality: The paper highlights how spectral decomposition, specifically focusing on the smallest eigenvalues, can provide a strong approximation to the global optimum in PGO, especially under high Signal-to-Noise Ratio (SNR) conditions.
  • Synergy of Krylov-Schur and Cholesky: The combination of Krylov-Schur for efficient eigenvector computation and Cholesky factorization for solving linear systems is a key algorithmic innovation. The fact that both steps can reuse the same factorization pattern leads to significant speedups.
  • Optimality Assessment via Eigenvalues: Using the smallest eigenvalue as a direct indicator of solution quality is an elegant way to assess how close the estimated solution is to the true global optimum.
  • IRLS for Robustness: The integration of IRLS is crucial for practical applications, as it effectively handles the outliers that plague real-world sensor data, making the algorithm more reliable.

Key Results

  • High Scalability and Efficiency: MAKS significantly outperforms existing methods in terms of computational speed. For instance, on the Grid3D dataset, it's nearly 600 times faster than Gauss-Newton while achieving comparable results.
  • Accuracy: The algorithm provides solutions that are either optimal or very close to the global optimum, especially under high SNR. The angular RMSE between MAKS estimates and ground truth is consistently small.
  • Robustness: The IRLS-enhanced MAKS performs well on datasets with a high percentage of outlier transformations, outperforming other state-of-the-art methods like Chatterjee et al. [8] and Arrigoni et al. [2] in many cases, particularly in median error.
  • Successful 3D Reconstruction: The pipeline successfully reconstructs complex 3D scenes from RGB-D data, demonstrating the practical utility of the MAKS algorithm in real-time applications.

Limitations and Future Work

  • High SNR Assumption: The paper notes that the optimality of MAKS is particularly strong under a high SNR. While the IRLS component helps with outliers, performance might degrade with extremely noisy or poorly conditioned data.
  • Sphere Dataset Anomaly: The authors observed that MAKS performed less optimally on the "Sphere" dataset. They hypothesize this is due to the specific graph structure and nearly constant relative rotations, suggesting that the applicability domain might be sensitive to certain graph configurations.
  • Future Work: The paper implicitly suggests that further exploration of robust estimation techniques and analysis of performance on more challenging graph structures could be valuable. The authors also provide a C++ implementation, encouraging further research and adoption.

Summary updated

BibTeX
@inproceedings{moreira2021fast,
  author = {Gabriel Moreira and Manuel Marques and Joao Paulo Costeira},
  title = {{Fast Pose Graph Optimization via Krylov-Schur and Cholesky Factorization}},
  booktitle = {IEEE/CVF Winter Conference on Applications of Computer Vision},
  year = {2021},
}
Figure