Incremental Block Cholesky Factorization for Nonlinear Least Squares in Robotics

• http://roboticsproceedings.org/rss09/p42.html

SLAM, Nonlinear least squares, Cholesky Factorization, Incremental Factorization, Sparse block matrix

The paper presents an extension to the matrix factorization methods, a resumed factorization. It is suitable for incremental applications where the size of the system grows with time, but the rest remains without change. The implementation yields excellent results, comparable and except for a couple singularities, exceeding the ones of methods working on graph factorizations. The proposed method is, however, much simpler, and as such is suitable for hardware acceleration.

Efciently solving nonlinear least squares (NLS) problems is crucial for many applications in robotics. In online applications, solving the associated nolinear systems every step may become very expensive. This paper introduces online, incremental solutions, which take full advantage of the sparse-block structure of the problems in robotics. In general, the solution of the nonlinear system is approximated by incrementally solving a series of linearized problems. The most computationally demanding part is to assemble and solve the linearized system at each iteration. In our solution, this is mitigated by incrementally updating the factorized form of the linear system and changing the linearization point only if needed. The incremental updates are done using a resumed factorization only on the parts affected by the new information added to the system at every step. The sparsity of the factorized form directly affects the efciency. In order to obtain an incremental factorization with persistent reduced ll-in, a new incremental ordering scheme is proposed. Furthermore, the implementation exploits the block structure of the problems and offers efcient solutions to manipulate block matrices, including a highly efcient Cholesky factorization on sparse block matrices. In this work, we focus our efforts on testing the method on 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.