switchable_constraints Mini program for testing the algorithm in {Switchable constraints for robust pose graph SLAM}