Square Root Marginalization for Sliding-Window Bundle Adjustment

TL;DR

This paper introduces a novel square root sliding-window bundle adjustment method for real-time odometry, improving computational efficiency and numerical stability over traditional approaches by using a square root formulation.

Contribution

The paper presents a new square root marginalization technique for bundle adjustment that is algebraically equivalent to Schur complement but more numerically stable and faster.

Findings

36% faster than baseline in real-world datasets

Prevents numeric failures common in Hessian-based methods

Maintains accuracy with single-precision computations

Abstract

In this paper we propose a novel square root sliding-window bundle adjustment suitable for real-time odometry applications. The square root formulation pervades three major aspects of our optimization-based sliding-window estimator: for bundle adjustment we eliminate landmark variables with nullspace projection; to store the marginalization prior we employ a matrix square root of the Hessian; and when marginalizing old poses we avoid forming normal equations and update the square root prior directly with a specialized QR decomposition. We show that the proposed square root marginalization is algebraically equivalent to the conventional use of Schur complement (SC) on the Hessian. Moreover, it elegantly deals with rank-deficient Jacobians producing a prior equivalent to SC with Moore-Penrose inverse. Our evaluation of visual and visual-inertial odometry on real-world datasets demonstrates that the proposed estimator is 36% faster than the baseline. It furthermore shows that in single precision, conventional Hessian-based marginalization leads to numeric failures and reduced accuracy. We analyse numeric properties of the marginalization prior to explain why our square root form does not suffer from the same effect and therefore entails superior performance.