Factor descent optimization for sparsification in graph SLAM
iSAM: Incremental Smoothing and Mapping