Feature/gtsam bad

@mikebosse, take a look at this. I'm making progress.