ICP-based pose-graph SLAM
iSAM2: Incremental smoothing and mapping using the Bayes tree