Squareroot marginalization

Abstract

Implemented a square-root-based bundle adjustment framework optimized for real-time odometry use cases:

• Avoids explicit Hessian formation by updating square root priors via QR decomposition
Uses square root marginalization, algebraically equivalent to Schur complement with Moore-Penrose inverse
• Handles rank-deficient Jacobians robustly, preserving estimator accuracy
• chieves 36% speed improvement over the traditional Schur-complement-based baseline

Problem

• Traditional Schur complement methods can become computationally expensive due to full Hessian manipulation
• Marginalization steps can introduce numerical instability or performance bottlenecks
• Jacobians in real-world systems are often rank-deficient, requiring stable treatment to avoid divergence
• Integration of mathematical algorithms into real-time pipelines demands both numerical precision and computational efficiency

Contribution

• Re-implemented the algorithm from Square Root Marginalization for Sliding-Window Bundle Adjustment (Demmel et al., ICCV 2021)
• Developed a real-time compatible square-root BA module integrated with visual-inertial odometry systems
• Carefully handled QR decomposition and rank-deficient cases using robust linear algebra libraries (e.g., Eigen)

Result

• Achieved ~36% reduction in computation time compared to conventional Schur-based BA
• Contributed to low-latency, high-accuracy pose estimation in visual-inertial odometry systems
• Gained deep understanding and practical implementation of advanced estimation and optimization techniques