Incremental Cholesky Factorization for Least Squares Problems in Robotics*

Online applications in robotics, computer vision, and computer graphics rely on efficiently solving the associated nolinear systems every step. Iteratively solving the non-linear system every step becomes very expensive if the size of the problem grows. This can be mitigated by incrementally updating the linear system and changing the linearization point only if needed. This paper proposes an incremental solution that adapts to the size of the updates while keeping the error of the estimation low. The implementation also differs form the existing ones in the way it exploits the block structure of such problems and offers efficient solutions to manipulate block matrices within incremental nonlinear solvers. In this work, in particular, we focus our effort on testing the method on simultaneous localization and mapping (SLAM) applications, but the applicability of the technique remains general. The experimental results show that our implementation outperforms the state of the art SLAM implementations on all tested datasets.