First gtsam tests