diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 3a2d04127..4f56d3afe 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -105,10 +105,10 @@ string createRewrittenFileName(const string& name) { } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, int maxID, +GraphAndValues load2D(pair dataset, Key maxNr, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart, + return load2D(dataset.first, dataset.second, maxNr, addNoise, smart, noiseFormat, kernelFunctionType); } @@ -226,6 +226,54 @@ boost::optional parseVertexLandmark(istream& is, const string& } } +/* ************************************************************************* */ +// Parse types T into a Key-indexed map +template +static map parseIntoMap(const string &filename, Key maxNr = 0) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse: can not find file " + filename); + + map result; + string tag; + while (!is.eof()) { + boost::optional> indexed_t; + is >> indexed_t; + if (indexed_t) { + // optional filter + if (maxNr && indexed_t->first >= maxNr) + continue; + result.insert(*indexed_t); + } + is.ignore(LINESIZE, '\n'); + } + return result; +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional &indexed) { + string tag; + is >> tag; + indexed = parseVertexPose(is, tag); + return is; +} + +map parse2DPoses(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional &indexed) { + string tag; + is >> tag; + indexed = parseVertexLandmark(is, tag); + return is; +} + +map parse2DLandmarks(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + /* ************************************************************************* */ boost::optional parseEdge(istream& is, const string& tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") @@ -241,49 +289,27 @@ boost::optional parseEdge(istream& is, const string& tag) { } /* ************************************************************************* */ -GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, - bool addNoise, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { +GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + + Values::shared_ptr initial(new Values); + + const auto poses = parse2DPoses(filename, maxNr); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + const auto landmarks = parse2DLandmarks(filename, maxNr); + for (const auto& key_landmark : landmarks) { + initial->insert(key_landmark.first, key_landmark.second); + } ifstream is(filename.c_str()); if (!is) throw invalid_argument("load2D: can not find file " + filename); - Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - string tag; - - // load the poses and landmarks - while (!is.eof()) { - if (!(is >> tag)) - break; - - const auto indexed_pose = parseVertexPose(is, tag); - if (indexed_pose) { - Key id = indexed_pose->first; - - // optional filter - if (maxID && id >= maxID) - continue; - - initial->insert(id, indexed_pose->second); - } - const auto indexed_landmark = parseVertexLandmark(is, tag); - if (indexed_landmark) { - Key id = indexed_landmark->first; - - // optional filter - if (maxID && id >= maxID) - continue; - - initial->insert(id, indexed_landmark->second); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - // If asked, create a sampler with random number generator std::unique_ptr sampler; if (addNoise) { @@ -302,6 +328,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, bool haveLandmark = false; const bool useModelInFile = !model; while (!is.eof()) { + string tag; if (!(is >> tag)) break; @@ -315,7 +342,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, kernelFunctionType); // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) + if (maxNr && (id1 >= maxNr || id2 >= maxNr)) continue; if (useModelInFile) @@ -375,7 +402,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (tag == "LANDMARK" || tag == "BR") { // optional filter - if (maxID && id1 >= maxID) + if (maxNr && id1 >= maxNr) continue; // Create noise model @@ -404,8 +431,8 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, /* ************************************************************************* */ GraphAndValues load2D_robust(const string& filename, - noiseModel::Base::shared_ptr& model, int maxID) { - return load2D(filename, model, maxID); + noiseModel::Base::shared_ptr& model, Key maxNr) { + return load2D(filename, model, maxNr); } /* ************************************************************************* */ @@ -448,10 +475,10 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D, return load3D(g2oFile); } else { // just call load2D - int maxID = 0; + Key maxNr = 0; bool addNoise = false; bool smart = true; - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + return load2D(g2oFile, SharedNoiseModel(), maxNr, addNoise, smart, NoiseFormatG2O, kernelFunctionType); } } @@ -516,8 +543,8 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " << pose.x() << " " << pose.y() << " " << pose.theta(); - for (int i = 0; i < 3; i++){ - for (int j = i; j < 3; j++){ + for (size_t i = 0; i < 3; i++){ + for (size_t j = i; j < 3; j++){ stream << " " << Info(i, j); } } @@ -545,13 +572,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << " " << q.y() << " " << q.z() << " " << q.w(); Matrix InfoG2o = I_6x6; - InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation - InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation - InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal - InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal + InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation + InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation + InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(0, 3); // off diagonal + InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(3, 0); // off diagonal - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + for (size_t i = 0; i < 6; i++){ + for (size_t j = i; j < 6; j++){ stream << " " << InfoG2o(i, j); } } @@ -562,126 +589,157 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -static Rot3 NormalizedRot3(double w, double x, double y, double z) { +// parse quaternion in x,y,z,w order, and normalize to unit length +istream &operator>>(istream &is, Quaternion &q) { + double x, y, z, w; + is >> x >> y >> z >> w; const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm; - return Rot3::Quaternion(f * w, f * x, f * y, f * z); + q = Quaternion(f * w, f * x, f * y, f * z); + return is; } /* ************************************************************************* */ -std::map parse3DPoses(const string& filename) { +// parse Rot3 from roll, pitch, yaw +istream &operator>>(istream &is, Rot3 &R) { + double yaw, pitch, roll; + is >> roll >> pitch >> yaw; // notice order ! + R = Rot3::Ypr(yaw, pitch, roll); + return is; +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional> &indexed) { + string tag; + is >> tag; + if (tag == "VERTEX3") { + Key id; + double x, y, z; + Rot3 R; + is >> id >> x >> y >> z >> R; + indexed.reset({id, Pose3(R, {x, y, z})}); + } else if (tag == "VERTEX_SE3:QUAT") { + Key id; + double x, y, z; + Quaternion q; + is >> id >> x >> y >> z >> q; + indexed.reset({id, Pose3(q, {x, y, z})}); + } + return is; +} + +map parse3DPoses(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional> &indexed) { + string tag; + is >> tag; + if (tag == "VERTEX_TRACKXYZ") { + Key id; + double x, y, z; + is >> id >> x >> y >> z; + indexed.reset({id, Point3(x, y, z)}); + } + return is; +} + +map parse3DLandmarks(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + +/* ************************************************************************* */ +// Parse BetweenFactor shared pointers into a vector +template +static vector>> +parseIntoVector(const string &filename, Key maxNr = 0) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("parse3DPoses: can not find file " + filename); + throw invalid_argument("parse: can not find file " + filename); - std::map poses; + vector>> result; + string tag; 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(NormalizedRot3(qw, qx, qy, qz), {x, y, z})); + boost::shared_ptr> shared; + is >> shared; + if (shared) { + // optional filter + if (maxNr && (shared->key1() >= maxNr || shared->key1() >= maxNr)) + continue; + result.push_back(shared); } + is.ignore(LINESIZE, '\n'); } - return poses; + return result; } /* ************************************************************************* */ -std::map parse3DLandmarks(const string& filename) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse3DLandmarks: can not find file " + filename); - - std::map landmarks; - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "VERTEX_TRACKXYZ") { - Key id; - double x, y, z; - ls >> id >> x >> y >> z; - landmarks.emplace(id, Point3(x, y, z)); - } +// Parse a symmetric covariance matrix (onlyupper-triangular part is stored) +istream &operator>>(istream &is, Matrix6 &m) { + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++){ + is >> m(i, j); + m(j,i) = m(i,j); } - return landmarks; + return is; } /* ************************************************************************* */ -BetweenFactorPose3s parse3DFactors( - const string& filename, - const noiseModel::Diagonal::shared_ptr& corruptingNoise) { - ifstream is(filename.c_str()); - if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); +istream &operator>>(istream &is, + boost::shared_ptr> &shared) { + string tag; + is >> tag; + + Matrix6 m; + if (tag == "EDGE3") { + Key id1, id2; + double x, y, z; + Rot3 R; + is >> id1 >> id2 >> x >> y >> z >> R; + Pose3 pose(R, {x, y, z}); + + is >> m; + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + shared.reset(new BetweenFactor(id1, id2, pose, model)); + } + if (tag == "EDGE_SE3:QUAT") { + Key id1, id2; + double x, y, z; + Quaternion q; + is >> id1 >> id2 >> x >> y >> z >> q; + Pose3 pose(q, {x, y, z}); + + // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: + is >> m; + Matrix6 mgtsam; + 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); + + shared.reset(new BetweenFactor(id1, id2, pose, model)); + } + return is; +} + +/* ************************************************************************* */ +BetweenFactorPose3s +parse3DFactors(const string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise, + Key maxNr) { + auto factors = parseIntoVector(filename, maxNr); - boost::optional sampler; if (corruptingNoise) { - sampler = Sampler(corruptingNoise); - } - - std::vector::shared_ptr> factors; - 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); - auto R12 = NormalizedRot3(qw, qx, qy, qz); - if (sampler) { - R12 = R12.retract(sampler->sample()); - } - - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(R12, {x, y, z}), model)); + Sampler sampler(corruptingNoise); + for (auto factor : factors) { + auto pose = factor->measured(); + factor.reset(new BetweenFactor(factor->key1(), factor->key2(), + pose.retract(sampler.sample()), + factor->noiseModel())); } } + return factors; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 30fa913ba..caef96cf4 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -23,10 +23,8 @@ #include #include #include -#include -#include +#include #include -#include #include #include #include @@ -114,18 +112,35 @@ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& GTSAM_EXPORT boost::optional parseEdge(std::istream& is, const std::string& tag); +using BetweenFactorPose2s = + std::vector::shared_ptr>; + +/// Parse edges in 2D g2o graph file into a set of BetweenFactors. +GTSAM_EXPORT BetweenFactorPose2s parse2DFactors( + const std::string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr); + +/// Parse vertices in 2D g2o graph file into a map of Pose2s. +GTSAM_EXPORT std::map parse2DPoses(const std::string &filename, + Key maxNr = 0); + +/// Parse landmarks in 2D g2o graph file into a map of Point2s. +GTSAM_EXPORT std::map parse2DLandmarks(const string &filename, + Key maxNr = 0); + /// Return type for load functions -typedef std::pair GraphAndValues; +using GraphAndValues = + std::pair; /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] - * @param maxID if non-zero cut out vertices >= maxID + * @param maxNr if non-zero cut out vertices >= maxNr * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT GraphAndValues load2D( - std::pair dataset, int maxID = 0, + std::pair dataset, Key maxNr = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatAUTO, @@ -135,7 +150,7 @@ GTSAM_EXPORT GraphAndValues load2D( * Load TORO/G2O style graph files * @param filename * @param model optional noise model to use instead of one specified by file - * @param maxID if non-zero cut out vertices >= maxID + * @param maxNr if non-zero cut out vertices >= maxNr * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model * @param noiseFormat how noise parameters are stored @@ -143,13 +158,13 @@ GTSAM_EXPORT GraphAndValues load2D( * @return graph and initial values */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise = + SharedNoiseModel model = SharedNoiseModel(), Key maxNr = 0, bool addNoise = false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, - noiseModel::Base::shared_ptr& model, int maxID = 0); + noiseModel::Base::shared_ptr& model, Key maxNr = 0); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -179,14 +194,18 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Parse edges in 3D TORO graph file into a set of BetweenFactors. using BetweenFactorPose3s = std::vector::shared_ptr>; -GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, - const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr); +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( + const std::string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, + Key maxNr = 0); /// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. -GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); +GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, + Key maxNr = 0); /// Parse landmarks in 3D g2o graph file into a map of Point3s. -GTSAM_EXPORT std::map parse3DLandmarks(const string& filename); +GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, + Key maxNr = 0); /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b6b1776d2..6ee44cfd6 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -89,20 +89,38 @@ TEST( dataSet, parseEdge) } /* ************************************************************************* */ -TEST( dataSet, load2D) -{ +TEST(dataSet, load2D) { ///< The structure where we will save the SfM data const string filename = findExampleDataFile("w100.graph"); NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; boost::tie(graph, initial) = load2D(filename); - EXPECT_LONGS_EQUAL(300,graph->size()); - EXPECT_LONGS_EQUAL(100,initial->size()); - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); - BetweenFactor expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model); - BetweenFactor::shared_ptr actual = boost::dynamic_pointer_cast< - BetweenFactor >(graph->at(0)); + EXPECT_LONGS_EQUAL(300, graph->size()); + EXPECT_LONGS_EQUAL(100, initial->size()); + auto model = noiseModel::Unit::Create(3); + BetweenFactor expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381), + model); + BetweenFactor::shared_ptr actual = + boost::dynamic_pointer_cast>(graph->at(0)); EXPECT(assert_equal(expected, *actual)); + + // // Check factor parsing + // const auto actualFactors = parse2DFactors(filename); + // for (size_t i : {0, 1, 2, 3, 4, 5}) { + // EXPECT(assert_equal( + // *boost::dynamic_pointer_cast>(graph->at(i)), + // *actualFactors[i], 1e-5)); + // } + + // Check pose parsing + const auto actualPoses = parse2DPoses(filename); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(initial->at(j), actualPoses.at(j), 1e-5)); + } + + // Check landmark parsing + const auto actualLandmarks = parse2DLandmarks(filename); + EXPECT_LONGS_EQUAL(0, actualLandmarks.size()); } /* ************************************************************************* */