done introduction to gtsam. probably needs optimization in non-euclidean spaces and coordinate systems