diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4a327b8e1..3e2ed51c7 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,33 +162,96 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ +using BetweenFactors = std::vector::shared_ptr>; +using Poses = std::map; +std::pair parseG2o3D(const std::string&); + +#include +#define LINESIZE 81920 + +/* ************************************************************************* */ +std::pair parseG2o3D(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("load3D: can not find file " + filename); + + BetweenFactors factors; + Poses poses; + + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "VERTEX3") { + Key id; + double x, y, z, roll, pitch, yaw; + ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); + } + if (tag == "VERTEX_SE3:QUAT") { + Key id; + double x, y, z, qx, qy, qz, qw; + ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); + } + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "EDGE3") { + Key id1, id2; + double x, y, z, roll, pitch, yaw; + ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); + } + if (tag == "EDGE_SE3:QUAT") { + Key id1, id2; + double x, y, z, qx, qy, qz, qw; + ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { + double mij; + ls >> mij; + m(i, j) = mij; + m(j, i) = mij; + } + } + Matrix mgtsam(6, 6); + + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal + + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); + } + } + return make_pair(factors, poses); +} + 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); - - // Initialize 5 poses with quaternion/point3 values: - const std::vector poses = { - {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, - {{0.854230, 0.190253, 0.283162, -0.392318}, - {1.001367, 0.015390, 0.004948}}, - {{0.421446, -0.351729, -0.597838, 0.584174}, - {1.993500, 0.023275, 0.003793}}, - {{0.067024, 0.331798, -0.200659, 0.919323}, - {2.004291, 1.024305, 0.018047}}, - {{0.765488, -0.035697, -0.462490, 0.445933}, - {0.999908, 1.055073, 0.020212}}, - }; - - // Check values - for (size_t i : {0, 1, 2, 3, 4}) { - EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); - } + auto model = noiseModel::Isotropic::Precision(6, 10000); // Initialize 6 relative measurements with quaternion/point3 values: - const std::vector measurements = { + const std::vector relative_poses = { {{0.854230, 0.190253, 0.283162, -0.392318}, {1.001367, 0.015390, 0.004948}}, {{0.105373, 0.311512, 0.656877, -0.678505}, @@ -203,21 +266,61 @@ TEST(dataSet, readG2o3D) { {-0.623267, 0.086928, 0.773222}}, }; - // Specify connectivity: - std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, - {3, 4}, {1, 4}, {3, 0}}; + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - // Create expected factor graph - auto model = noiseModel::Isotropic::Precision(6, 10000); - NonlinearFactorGraph expectedGraph; + // Specify connectivity: + using KeyPair = pair; + std::vector edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}}; + + // Created expected factor graph size_t i = 0; - for (const auto& keys : pairs) { + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { expectedGraph.emplace_shared>( - keys.first, keys.second, measurements[i++], model); + keys.first, keys.second, relative_poses[i++], model); } - // Check if actual graph is correct + // Call parse version + BetweenFactors actualFactors; + Poses actualPoses; + std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); + + // Check factors + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(expectedGraph[i]), + *actualFactors[i], 1e-5)); + } + + // Check poses + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + + // Call graph version + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = true; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); + + // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + + // Check values + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */