Pulled in gtsam_catkin merge