Simultaneous localization and mapping with infinite planes
iSAM: Incremental Smoothing and Mapping