https://www.cs.cmu.edu/~srini/papers/robotics/2015-Wang-iros/
Indoor Trajectory Identification: Snapping with Uncertainty - Srinivasan Seshan