STEAP: simultaneous trajectory estimation and planning
iSAM2: Incremental smoothing and mapping using the Bayes tree