modified 05 for GTSAM v3.0