From 78fe603933f949e6bc9ba7ce7948be03973162df Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:56:01 -0400 Subject: [PATCH] re-established previous function --- gtsam/slam/dataset.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9a1fb3303..1029d7615 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -258,8 +258,8 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, if (!factor) continue; - Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE2 " << factor->key1() << " " << factor->key2() << " " + Pose2 pose = factor->measured().inverse(); + stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;