Continuous-time Gaussian process motion planning via probabilistic inference
iSAM2: Incremental smoothing and mapping using the Bayes tree