iSAM: Incremental Smoothing and Mapping
A shortest augmenting path algorithm for dense and sparse linear assignment problems