Work in progress with GTSAM