12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

re-established previous function

release/4.3a0
Luca 2014-05-20 16:56:01 -04:00
parent 9a2f282e4b
commit 78fe603933
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/slam/dataset.cpp
View File

@ -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;