diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 34370d4e2..b7d1b04d6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, /* ************************************************************************* */ GraphAndValues readG2o(const string& g2oFile, const bool is3D, - KernelFunctionType kernelFunctionType) { - // just call load2D - int maxID = 0; - bool addNoise = false; - bool smart = true; - - if(is3D) + KernelFunctionType kernelFunctionType) { + if (is3D) { return load3D(g2oFile); - - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, - NoiseFormatG2O, kernelFunctionType); + } else { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); + } } /* ************************************************************************* */ @@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { - +std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load3D: can not find file " + filename); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + throw invalid_argument("parse3DPoses: can not find file " + filename); + std::map poses; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) { Key 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)); + 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; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + 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); + return poses; +} +/* ************************************************************************* */ +std::vector::shared_ptr> parse3DFactors(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); + + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) { Key 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 = I_6x6; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + 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); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = I_6x6; Key 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++){ + 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 = I_6x6; + 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 + 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); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); } } + return factors; +} + +/* ************************************************************************* */ +GraphAndValues load3D(const string& filename) { + const auto factors = parse3DFactors(filename); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + for (const auto& factor : factors) { + graph->push_back(factor); + } + + const auto poses = parse3DPoses(filename); + Values::shared_ptr initial(new Values); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7c9b54a6f..9706bace0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -153,9 +153,13 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); -/** - * Load TORO 3D Graph - */ +/// Parse edges in 3D TORO graph file into a set of BetweenFactors. +GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); + +/// Parse vertices in 3D TORO graph file into a map of Pose3s. +GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); + +/// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 3e2ed51c7..0e131af32 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,90 +162,6 @@ 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"); auto model = noiseModel::Isotropic::Precision(6, 10000); @@ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) { keys.first, keys.second, relative_poses[i++], model); } - // Call parse version - BetweenFactors actualFactors; - Poses actualPoses; - std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); - - // Check factors + // Check factor parsing + const auto actualFactors = parse3DFactors(g2oFile); 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 + // Check pose parsing + const auto actualPoses = parse3DPoses(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } - // Call graph version + // Check 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)); }