[Paper Review] Adaptive Graduated Non-Convexity for Pose Graph Optimization

Seungwon Choi, Won‐Seok Kang+3 more|arXiv (Cornell University)|Aug 22, 2023

Robotics and Sensor-Based Localization4 citations

TL;DR

This paper proposes Adaptive Graduated Non-Convexity (AGNC) for pose graph optimization, using a B-spline-based adaptive shape function to dynamically adjust the robust kernel's convexity, reducing GNC iterations. The method achieves up to 18% faster runtime than riSAM across multiple datasets while maintaining or improving accuracy, especially under high outlier ratios.

ABSTRACT

We present a novel approach to robust pose graph optimization based on Graduated Non-Convexity (GNC). Unlike traditional GNC-based methods, the proposed approach employs an adaptive shape function using B-spline to optimize the shape of the robust kernel. This aims to reduce GNC iterations, boosting computational speed without compromising accuracy. When integrated with the open-source riSAM algorithm, the method demonstrates enhanced efficiency across diverse datasets. Accompanying open-source code aims to encourage further research in this area. https://github.com/SNU-DLLAB/AGNC-PGO

Motivation & Objective

• To address the inefficiency of traditional Graduated Non-Convexity (GNC) methods in pose graph optimization due to fixed or suboptimal kernel shape progression.
• To reduce the number of GNC iterations required for convergence by dynamically adapting the kernel shape based on residual statistics.
• To improve computational efficiency without sacrificing accuracy in the presence of outliers, especially in high-noise or high-outlier scenarios.
• To integrate the proposed method into the open-source riSAM framework for broader adoption and reproducibility.
• To provide a more robust and efficient alternative to existing GNC-based pose graph optimization techniques in SLAM systems.

Proposed method

• Introduces a B-spline-based adaptive shape function to parameterize the control parameter μ in GNC, enabling smooth and data-driven adjustment of the robust kernel's non-convexity.
• Defines the shape function such that μ varies non-linearly across iterations, with larger increments early on and smaller ones later, promoting faster convergence.
• Uses Mahalanobis distance thresholds (χ²(0.25) to χ²(0.9)) as input to the B-spline to adaptively control the kernel's shape based on residual distribution.
• Integrates the adaptive shape function into the riSAM algorithm, replacing its fixed μ progression rule with a dynamic, interpolation-based approach.
• Employs a non-convexity-aware optimization strategy that balances convergence speed and robustness by controlling the rate of kernel non-convexity increase.
• Leverages the Black-Rangarajan duality to maintain theoretical grounding while enabling efficient numerical optimization.

Experimental results

Research questions

• RQ1 Can an adaptive shape function for the GNC kernel reduce the number of iterations required for convergence in pose graph optimization?
• RQ2 Does the proposed B-spline-based shape function improve robustness to outliers compared to fixed or linear μ progression?
• RQ3 To what extent does the adaptive method enhance computational efficiency without degrading estimation accuracy?
• RQ4 How does the method perform under high outlier ratios and noisy loop closure measurements compared to baseline GNC methods?
• RQ5 Can the adaptive kernel shaping be effectively integrated into existing open-source SLAM back-ends like riSAM while preserving accuracy?

Key findings

• The proposed AGNC method reduced runtime by up to 18% compared to riSAM across all tested datasets, with the highest improvement observed at 50% outlier ratio.
• On the Sphere2500 dataset, the method achieved slightly better precision (1.0) and recall (0.998) at 50% outlier ratio compared to riSAM’s 0.998 and 0.985, respectively.
• The adaptive shape function enabled faster convergence: trajectory estimation accuracy matched riSAM but in fewer GNC steps, reducing total computation time.
• In the noisy Manhattan3500 experiment, the method with both adaptive shape and graduation outperformed the baseline mode (fixed α=0.5), showing improved robustness.
• The runtime ratio of the proposed method to riSAM remained consistently below 0.83 across all datasets and outlier levels, indicating stable efficiency gains.
• The method maintained identical or near-identical recall and precision to riSAM on most datasets, confirming that accuracy was not compromised by the optimization speedup.