From 95e59d7c5d73bb9cb312cc445a80af1ce810e518 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 1 Nov 2014 19:23:07 +0100 Subject: [PATCH] Fixed the last failing unit test --- .../pose3example-offdiagonal-rewritten.txt | 2 ++ examples/Data/pose3example-rewritten.txt | 5 ++++ gtsam/nonlinear/Values-inl.h | 1 + gtsam/slam/dataset.cpp | 25 ++++++++----------- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/examples/Data/pose3example-offdiagonal-rewritten.txt b/examples/Data/pose3example-offdiagonal-rewritten.txt index a855eff4d..b99d266aa 100644 --- a/examples/Data/pose3example-offdiagonal-rewritten.txt +++ b/examples/Data/pose3example-offdiagonal-rewritten.txt @@ -1 +1,3 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000 diff --git a/examples/Data/pose3example-rewritten.txt b/examples/Data/pose3example-rewritten.txt index d445fa96c..6d342fcb0 100644 --- a/examples/Data/pose3example-rewritten.txt +++ b/examples/Data/pose3example-rewritten.txt @@ -1,3 +1,8 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 +VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446 +VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 87b2a51cc..49ea03e9f 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -33,6 +33,7 @@ namespace gtsam { + /* ************************************************************************* */ template struct _ValuesKeyValuePair { diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db85c65bf..388d712e7 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1,5 +1,4 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved @@ -330,8 +329,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, fstream stream(filename.c_str(), fstream::out); // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { - const Pose2& pose = dynamic_cast(key_value.value); + const Pose2& pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } @@ -373,25 +373,22 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D, /* ************************************************************************* */ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const string& filename) { - fstream stream(filename.c_str(), fstream::out); // save 2D & 3D poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + Values::ConstFiltered viewPose2 = estimate.filter(); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose2) { + stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " + << key_value.value.y() << " " << key_value.value.theta() << endl; + } - const Pose2* pose2D = dynamic_cast(&key_value.value); - if(pose2D){ - stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " " - << pose2D->y() << " " << pose2D->theta() << endl; - } - const Pose3* pose3D = dynamic_cast(&key_value.value); - if(pose3D){ - Point3 p = pose3D->translation(); - Rot3 R = pose3D->rotation(); + Values::ConstFiltered viewPose3 = estimate.filter(); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose3) { + Point3 p = key_value.value.translation(); + Rot3 R = key_value.value.rotation(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << endl; - } } // save edges (2D or 3D)