A flexible and scalable SLAM system with full 3D motion estimation
iSAM: Incremental Smoothing and Mapping