Switchable constraints for robust pose graph SLAM
G2o: A general framework for graph optimization