For online robots, incremental SLAM algorithms offer huge potential
computational savings over batch algorithms. The dominant incremental
algorithms are iSAM and iSAM2 which offer radically different approaches to
computing incremental updates, balancing issues like 1) the need to
relinearize, 2) changes in the desirable variable marginalization order,
and 3) the underlying conceptual approach (i.e. the "matrix" story versus
the "factor graph" story).
In the paper, we propose a new incremental algorithm that computes
solutions with lower absolute error and generally provides lower error
solutions for a fixed computational budget than either iSAM or iSAM2. Key
to AprilSAM's performance are a new dynamic variable reordering algorithm
for fast incremental Cholesky factorizations, a method for reducing the
work involved in backsubstitutions, and a new algorithm for deciding
between incremental and batch updates.