Simultaneous localization and mapping with infinite planes
iSAM2: Incremental smoothing and mapping using the Bayes tree