From 358c978f0089a1e40caa978ca8e511324c7cb492 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 18 Aug 2014 20:40:52 -0400 Subject: [PATCH] added 3D version of readG2o with unit test --- examples/Data/pose3example.txt | 11 ++++++ gtsam/slam/dataset.cpp | 63 ++++++++++++++++++++++++----- gtsam/slam/dataset.h | 7 ++-- gtsam/slam/tests/testDataset.cpp | 68 +++++++++++++++++++++++++++++++- 4 files changed, 135 insertions(+), 14 deletions(-) create mode 100644 examples/Data/pose3example.txt diff --git a/examples/Data/pose3example.txt b/examples/Data/pose3example.txt new file mode 100644 index 000000000..189e23dbc --- /dev/null +++ b/examples/Data/pose3example.txt @@ -0,0 +1,11 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 +VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 +VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446 +VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488 +EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.125250 -0.534379 0.769122 0.327419 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 \ No newline at end of file diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b3c9b9557..92551c07a 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -250,7 +250,6 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, new BetweenFactor(id1, id2, l1Xl2, model)); graph->push_back(factor); } - // Parse measurements double bearing, range, bearing_std, range_std; @@ -358,12 +357,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, } /* ************************************************************************* */ -GraphAndValues readG2o(const string& g2oFile, +GraphAndValues readG2o(const string& g2oFile, const bool is3D, KernelFunctionType kernelFunctionType) { // just call load2D int maxID = 0; bool addNoise = false; bool smart = true; + + if(is3D) + return load3D(g2oFile); + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType); } @@ -374,11 +377,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, fstream stream(filename.c_str(), fstream::out); - // save poses + // save 2D & 3D poses BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { - const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.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; + } } // save edges @@ -406,10 +411,14 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -bool load3D(const string& filename) { +GraphAndValues load3D(const string& filename) { + ifstream is(filename.c_str()); if (!is) - return false; + throw invalid_argument("load2D: can not find file " + filename); + + Values::shared_ptr initial(new Values); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); while (is) { char buf[LINESIZE]; @@ -422,6 +431,17 @@ bool load3D(const string& filename) { int id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + Rot3 R = Rot3::ypr(yaw,pitch,roll); + Point3 t = Point3(x, y, z); + initial->insert(id, Pose3(R,t)); + } + if (tag == "VERTEX_SE3:QUAT") { + int id; + double x, y, z, qx, qy, qz, qw; + ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; + Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Point3 t = Point3(x, y, z); + initial->insert(id, Pose3(R,t)); } } is.clear(); /* clears the end-of-file and error flags */ @@ -438,13 +458,38 @@ bool load3D(const string& filename) { int id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; + Rot3 R = Rot3::ypr(yaw,pitch,roll); + Point3 t = Point3(x, y, z); Matrix m = eye(6); for (int i = 0; i < 6; i++) for (int j = i; j < 6; j++) ls >> m(i, j); + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + NonlinearFactor::shared_ptr factor( + new BetweenFactor(id1, id2, Pose3(R,t), model)); + graph->push_back(factor); + } + if (tag == "EDGE_SE3:QUAT") { + Matrix m = eye(6); + int id1, id2; + double x, y, z, qx, qy, qz, qw; + ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; + Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Point3 t = Point3(x, y, z); + for (int i = 0; i < 6; i++){ + for (int j = i; j < 6; j++){ + ls >> m(i, j); + if( j > i && m(i, j)!= 0) + throw invalid_argument("load3D: current version assumes diagonal noise!"); + } + } + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + NonlinearFactor::shared_ptr factor( + new BetweenFactor(id1, id2, Pose3(R,t), model)); + graph->push_back(factor); } } - return true; + return make_pair(graph, initial); } /* ************************************************************************* */ diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 8fd75269c..c27419a6e 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -110,11 +110,12 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, /** * @brief This function parses a g2o file and stores the measurements into a * NonlinearFactorGraph and the initial guess in a Values structure - * @param filename The name of the g2o file + * @param filename The name of the g2o file\ + * @param is3D indicates if the file describes a 2D or 3D problem * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, +GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false, KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** @@ -130,7 +131,7 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /** * Load TORO 3D Graph */ -GTSAM_EXPORT bool load3D(const std::string& filename); +GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfM_Measurement; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index d7adf9b51..603dc474b 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -116,13 +116,76 @@ TEST( dataSet, readG2o) EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } +/* ************************************************************************* */ +TEST( dataSet, readG2o3D) +{ + const string g2oFile = findExampleDataFile("pose3example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = true; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); + + Values expectedValues; + Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Point3 p0 = Point3(0.000000, 0.000000, 0.000000); + expectedValues.insert(0, Pose3(R0, p0)); + + Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Point3 p1 = Point3(1.001367, 0.015390, 0.004948); + expectedValues.insert(1, Pose3(R1, p1)); + + Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); + Point3 p2 = Point3(1.993500, 0.023275, 0.003793); + expectedValues.insert(2, Pose3(R2, p2)); + + Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323); + Point3 p3 = Point3(2.004291, 1.024305, 0.018047); + expectedValues.insert(3, Pose3(R3, p3)); + + Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933); + Point3 p4 = Point3(0.999908, 1.055073, 0.020212); + expectedValues.insert(4, Pose3(R4, p4)); + + EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); + + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0)); + NonlinearFactorGraph expectedGraph; + + Point3 p01 = Point3(1.001367, 0.015390, 0.004948); + Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + + Point3 p12 = Point3(0.523923, 0.776654, 0.326659); + Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); + expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); + + Point3 p23 = Point3(0.910927, 0.055169, -0.411761); + Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); + expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); + + Point3 p34 = Point3(0.775288, 0.228798, -0.596923); + Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); + expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); + + Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); + Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); + expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); + + Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); + Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); + expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); + + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); +} + /* ************************************************************************* */ TEST( dataSet, readG2oHuber) { const string g2oFile = findExampleDataFile("pose2example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER); + bool is3D = false; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); @@ -149,7 +212,8 @@ TEST( dataSet, readG2oTukey) const string g2oFile = findExampleDataFile("pose2example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY); + bool is3D = false; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);