diff --git a/examples/Data/example_with_vertices.g2o b/examples/Data/example_with_vertices.g2o index 6c2f1a530..ca7cd86df 100644 --- a/examples/Data/example_with_vertices.g2o +++ b/examples/Data/example_with_vertices.g2o @@ -1,16 +1,16 @@ -VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 -VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 -VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 -VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 -VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 -VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 -VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 -VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 -VERTEX_TRACKXYZ 7782220156096217088 10 10 10 -VERTEX_TRACKXYZ 7782220156096217089 -10 10 10 -VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10 -VERTEX_TRACKXYZ 7782220156096217091 10 -10 10 -VERTEX_TRACKXYZ 7782220156096217092 10 10 -10 -VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10 -VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10 -VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10 +VERTEX_SE3:QUAT 0 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 +VERTEX_SE3:QUAT 1 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 +VERTEX_SE3:QUAT 2 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 +VERTEX_SE3:QUAT 3 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 +VERTEX_SE3:QUAT 4 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 +VERTEX_SE3:QUAT 5 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 +VERTEX_SE3:QUAT 6 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 +VERTEX_SE3:QUAT 7 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 +VERTEX_TRACKXYZ 0 10 10 10 +VERTEX_TRACKXYZ 1 -10 10 10 +VERTEX_TRACKXYZ 2 -10 -10 10 +VERTEX_TRACKXYZ 3 10 -10 10 +VERTEX_TRACKXYZ 4 10 10 -10 +VERTEX_TRACKXYZ 5 -10 10 -10 +VERTEX_TRACKXYZ 6 -10 -10 -10 +VERTEX_TRACKXYZ 7 10 -10 -10 diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 2aac939d9..9cc814245 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -53,7 +53,9 @@ using namespace std; namespace fs = boost::filesystem; -using namespace gtsam::symbol_shorthand; +using gtsam::symbol_shorthand::L; +using gtsam::symbol_shorthand::P; +using gtsam::symbol_shorthand::X; #define LINESIZE 81920 @@ -127,19 +129,16 @@ static void parseLines(const string &filename, Parser parse) { } /* ************************************************************************* */ -// Parse types T into a Key-indexed map +// Parse types T into a size_t-indexed map template -map parseToMap(const string &filename, Parser> parse, - Key maxKey) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse: can not find file " + filename); - - map result; - Parser> emplace = [&result, parse](istream &is, - const string &tag) { - if (auto t = parse(is, tag)) - result.emplace(*t); +map parseToMap(const string &filename, Parser> parse, + size_t maxIndex) { + map result; + Parser> emplace = [&](istream &is, const string &tag) { + if (auto t = parse(is, tag)) { + if (!maxIndex || t->first <= maxIndex) + result.emplace(*t); + } return boost::none; }; parseLines(filename, emplace); @@ -148,7 +147,6 @@ map parseToMap(const string &filename, Parser> parse, /* ************************************************************************* */ // Parse a file and push results on a vector -// TODO(frank): do we need this *and* parseMeasurements? template static vector parseToVector(const string &filename, Parser parse) { vector result; @@ -164,7 +162,7 @@ static vector parseToVector(const string &filename, Parser parse) { /* ************************************************************************* */ boost::optional parseVertexPose(istream &is, const string &tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - Key id; + size_t id; double x, y, yaw; is >> id >> x >> y >> yaw; return IndexedPose(id, Pose2(x, y, yaw)); @@ -174,16 +172,16 @@ boost::optional parseVertexPose(istream &is, const string &tag) { } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexPose, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexPose, maxIndex); } /* ************************************************************************* */ boost::optional parseVertexLandmark(istream &is, const string &tag) { if (tag == "VERTEX_XY") { - Key id; + size_t id; double x, y; is >> id >> x >> y; return IndexedLandmark(id, Point2(x, y)); @@ -193,9 +191,9 @@ boost::optional parseVertexLandmark(istream &is, } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexLandmark, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexLandmark, maxIndex); } /* ************************************************************************* */ @@ -287,10 +285,10 @@ boost::optional parseEdge(istream &is, const string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || (tag == "ODOMETRY")) { - Key id1, id2; + size_t id1, id2; double x, y, yaw; is >> id1 >> id2 >> x >> y >> yaw; - return IndexedEdge(pair(id1, id2), Pose2(x, y, yaw)); + return IndexedEdge({id1, id2}, Pose2(x, y, yaw)); } else { return boost::none; } @@ -323,7 +321,7 @@ template struct ParseFactor : ParseMeasurement { template <> struct ParseMeasurement { // The arguments boost::shared_ptr sampler; - Key maxKey; + size_t maxIndex; // For noise model creation bool smart; @@ -345,9 +343,9 @@ template <> struct ParseMeasurement { is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); // optional filter - Key id1, id2; + size_t id1, id2; tie(id1, id2) = edge->first; - if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + if (maxIndex && (id1 > maxIndex || id2 > maxIndex)) return boost::none; // Get pose and optionally add noise @@ -378,9 +376,11 @@ boost::shared_ptr createSampler(const SharedNoiseModel &model) { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey, - true, NoiseFormatAUTO, KernelFunctionTypeNONE}; + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, + maxIndex, true, NoiseFormatAUTO, + KernelFunctionTypeNONE}; return parseToVector>(filename, parse); } @@ -403,8 +403,9 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - auto poses = parseMeasurements(filename, model, maxKey); + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + auto poses = parseMeasurements(filename, model, maxIndex); std::vector> result; result.reserve(poses.size()); for (const auto &p : poses) @@ -417,8 +418,9 @@ parseMeasurements(const std::string &filename, template <> std::vector::shared_ptr> parseFactors(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseFactor parse({model ? createSampler(model) : nullptr, maxKey, + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseFactor parse({model ? createSampler(model) : nullptr, maxIndex, true, NoiseFormatAUTO, KernelFunctionTypeNONE, nullptr}); return parseToVector::shared_ptr>(filename, parse); @@ -429,7 +431,7 @@ using BearingRange2D = BearingRange; template <> struct ParseMeasurement { // arguments - Key maxKey; + size_t maxIndex; // The actual parser boost::optional> @@ -437,7 +439,7 @@ template <> struct ParseMeasurement { if (tag != "BR" && tag != "LANDMARK") return boost::none; - Key id1, id2; + size_t id1, id2; is >> id1 >> id2; double bearing, range, bearing_std, range_std; @@ -470,7 +472,7 @@ template <> struct ParseMeasurement { } // optional filter - if (maxKey && id1 >= maxKey) + if (maxIndex && id1 > maxIndex) return boost::none; // Create noise model @@ -484,30 +486,37 @@ template <> struct ParseMeasurement { /* ************************************************************************* */ GraphAndValues load2D(const string &filename, SharedNoiseModel model, - Key maxKey, bool addNoise, bool smart, + size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - auto graph = boost::make_shared(); + // Single pass for poses and landmarks. auto initial = boost::make_shared(); + Parser insert = [maxIndex, &initial](istream &is, const string &tag) { + if (auto indexedPose = parseVertexPose(is, tag)) { + if (!maxIndex || indexedPose->first <= maxIndex) + initial->insert(indexedPose->first, indexedPose->second); + } else if (auto indexedLandmark = parseVertexLandmark(is, tag)) { + if (!maxIndex || indexedLandmark->first <= maxIndex) + initial->insert(L(indexedLandmark->first), indexedLandmark->second); + } + return 0; + }; + parseLines(filename, insert); - // TODO(frank): do a single pass - const auto poses = parseVariables(filename, maxKey); - for (const auto &key_pose : poses) { - initial->insert(key_pose.first, key_pose.second); - } + // Single pass for Pose2 and bearing-range factors. + auto graph = boost::make_shared(); - const auto landmarks = parseVariables(filename, maxKey); - for (const auto &key_landmark : landmarks) { - initial->insert(L(key_landmark.first), key_landmark.second); - } - - // Create parser for Pose2 and bearing/range + // Instantiate factor parser ParseFactor parseBetweenFactor( - {addNoise ? createSampler(model) : nullptr, maxKey, smart, noiseFormat, + {addNoise ? createSampler(model) : nullptr, maxIndex, smart, noiseFormat, kernelFunctionType, model}); - ParseMeasurement parseBearingRange{maxKey}; + // Instantiate bearing-range parser + ParseMeasurement parseBearingRange{maxIndex}; + + // Combine in a single parser that adds factors to `graph`, but also inserts + // new variables into `initial` when needed. Parser parse = [&](istream &is, const string &tag) { if (auto f = parseBetweenFactor(is, tag)) { graph->push_back(*f); @@ -537,23 +546,24 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, return 0; }; - parseLines(filename, parse); + parseLines(filename, parse); return make_pair(graph, initial); } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, Key maxKey, +GraphAndValues load2D(pair dataset, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, + return load2D(dataset.first, dataset.second, maxIndex, addNoise, smart, noiseFormat, kernelFunctionType); } /* ************************************************************************* */ GraphAndValues load2D_robust(const string &filename, - noiseModel::Base::shared_ptr &model, Key maxKey) { - return load2D(filename, model, maxKey); + noiseModel::Base::shared_ptr &model, + size_t maxIndex) { + return load2D(filename, model, maxIndex); } /* ************************************************************************* */ @@ -596,10 +606,10 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D, return load3D(g2oFile); } else { // just call load2D - Key maxKey = 0; + size_t maxIndex = 0; bool addNoise = false; bool smart = true; - return load2D(g2oFile, SharedNoiseModel(), maxKey, addNoise, smart, + return load2D(g2oFile, SharedNoiseModel(), maxIndex, addNoise, smart, NoiseFormatG2O, kernelFunctionType); } } @@ -609,13 +619,16 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename) { fstream stream(filename.c_str(), fstream::out); + // Use a lambda here to more easily modify behavior in future. + auto index = [](gtsam::Key key) { return Symbol(key).index(); }; + // save 2D poses for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; const Pose2 &pose = p->value(); - stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + stream << "VERTEX_SE2 " << index(key_value.key) << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } @@ -627,7 +640,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const Pose3 &pose = p->value(); const Point3 t = pose.translation(); const auto q = pose.rotation().toQuaternion(); - stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " + stream << "VERTEX_SE3:QUAT " << index(key_value.key) << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } @@ -638,7 +651,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, if (!p) continue; const Point2 &point = p->value(); - stream << "VERTEX_XY " << key_value.key << " " << point.x() << " " + stream << "VERTEX_XY " << index(key_value.key) << " " << point.x() << " " << point.y() << endl; } @@ -648,26 +661,25 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, if (!p) continue; const Point3 &point = p->value(); - stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " " - << point.y() << " " << point.z() << endl; + stream << "VERTEX_TRACKXYZ " << index(key_value.key) << " " << point.x() + << " " << point.y() << " " << point.z() << endl; } // save edges (2D or 3D) for (const auto &factor_ : graph) { - boost::shared_ptr> factor = - boost::dynamic_pointer_cast>(factor_); + auto factor = boost::dynamic_pointer_cast>(factor_); if (factor) { SharedNoiseModel model = factor->noiseModel(); - boost::shared_ptr gaussianModel = - boost::dynamic_pointer_cast(model); + auto gaussianModel = boost::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); } Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " - << pose.x() << " " << pose.y() << " " << pose.theta(); + stream << "EDGE_SE2 " << index(factor->key1()) << " " + << index(factor->key2()) << " " << pose.x() << " " << pose.y() + << " " << pose.theta(); for (size_t i = 0; i < 3; i++) { for (size_t j = i; j < 3; j++) { stream << " " << Info(i, j); @@ -676,8 +688,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, stream << endl; } - boost::shared_ptr> factor3D = - boost::dynamic_pointer_cast>(factor_); + auto factor3D = boost::dynamic_pointer_cast>(factor_); if (factor3D) { SharedNoiseModel model = factor3D->noiseModel(); @@ -692,9 +703,10 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const Pose3 pose3D = factor3D->measured(); const Point3 p = pose3D.translation(); const auto q = pose3D.rotation().toQuaternion(); - stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() - << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() - << " " << q.y() << " " << q.z() << " " << q.w(); + stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " " + << index(factor3D->key2()) << " " << p.x() << " " << p.y() << " " + << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " + << q.w(); Matrix6 InfoG2o = I_6x6; InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation @@ -733,16 +745,16 @@ istream &operator>>(istream &is, Rot3 &R) { } /* ************************************************************************* */ -boost::optional> parseVertexPose3(istream &is, - const string &tag) { +boost::optional> parseVertexPose3(istream &is, + const string &tag) { if (tag == "VERTEX3") { - Key id; + size_t id; double x, y, z; Rot3 R; is >> id >> x >> y >> z >> R; return make_pair(id, Pose3(R, {x, y, z})); } else if (tag == "VERTEX_SE3:QUAT") { - Key id; + size_t id; double x, y, z; Quaternion q; is >> id >> x >> y >> z >> q; @@ -752,16 +764,16 @@ boost::optional> parseVertexPose3(istream &is, } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexPose3, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexPose3, maxIndex); } /* ************************************************************************* */ -boost::optional> parseVertexPoint3(istream &is, - const string &tag) { +boost::optional> parseVertexPoint3(istream &is, + const string &tag) { if (tag == "VERTEX_TRACKXYZ") { - Key id; + size_t id; double x, y, z; is >> id >> x >> y >> z; return make_pair(id, Point3(x, y, z)); @@ -770,9 +782,9 @@ boost::optional> parseVertexPoint3(istream &is, } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexPoint3, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexPoint3, maxIndex); } /* ************************************************************************* */ @@ -791,7 +803,7 @@ istream &operator>>(istream &is, Matrix6 &m) { template <> struct ParseMeasurement { // The arguments boost::shared_ptr sampler; - Key maxKey; + size_t maxIndex; // The actual parser boost::optional> operator()(istream &is, @@ -800,9 +812,9 @@ template <> struct ParseMeasurement { return boost::none; // parse ids and optionally filter - Key id1, id2; + size_t id1, id2; is >> id1 >> id2; - if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + if (maxIndex && (id1 > maxIndex || id2 > maxIndex)) return boost::none; Matrix6 m; @@ -847,8 +859,10 @@ template <> struct ParseMeasurement { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey}; + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, + maxIndex}; return parseToVector>(filename, parse); } @@ -872,8 +886,9 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - auto poses = parseMeasurements(filename, model, maxKey); + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + auto poses = parseMeasurements(filename, model, maxIndex); std::vector> result; result.reserve(poses.size()); for (const auto &p : poses) @@ -886,8 +901,9 @@ parseMeasurements(const std::string &filename, template <> std::vector::shared_ptr> parseFactors(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseFactor parse({model ? createSampler(model) : nullptr, maxKey}); + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseFactor parse({model ? createSampler(model) : nullptr, maxIndex}); return parseToVector::shared_ptr>(filename, parse); } @@ -896,21 +912,22 @@ GraphAndValues load3D(const string &filename) { auto graph = boost::make_shared(); auto initial = boost::make_shared(); - // TODO(frank): do a single pass - const auto factors = parseFactors(filename); - for (const auto &factor : factors) { - graph->push_back(factor); - } + // Instantiate factor parser. maxIndex is always zero for load3D. + ParseFactor parseFactor({nullptr, 0}); - const auto poses = parseVariables(filename); - for (const auto &key_pose : poses) { - initial->insert(key_pose.first, key_pose.second); - } - - const auto landmarks = parseVariables(filename); - for (const auto &key_landmark : landmarks) { - initial->insert(key_landmark.first, key_landmark.second); - } + // Single pass for variables and factors. Unlike 2D version, does *not* insert + // variables into `initial` if referenced but not present. + Parser parse = [&](istream &is, const string &tag) { + if (auto indexedPose = parseVertexPose3(is, tag)) { + initial->insert(indexedPose->first, indexedPose->second); + } else if (auto indexedLandmark = parseVertexPoint3(is, tag)) { + initial->insert(L(indexedLandmark->first), indexedLandmark->second); + } else if (auto factor = parseFactor(is, tag)) { + graph->push_back(*factor); + } + return 0; + }; + parseLines(filename, parse); return make_pair(graph, initial); } @@ -1187,8 +1204,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, dataValues.number_cameras()) { // we only estimated camera poses for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera - Key poseKey = symbol('x', i); - Pose3 pose = values.at(poseKey); + Pose3 pose = values.at(X(i)); Cal3Bundler K = dataValues.cameras[i].calibration(); Camera camera(pose, K); dataValues.cameras[i] = camera; @@ -1255,20 +1271,22 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) { return initial; } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -std::map parse3DPoses(const std::string &filename, Key maxKey) { - return parseVariables(filename, maxKey); -} - -std::map parse3DLandmarks(const std::string &filename, - Key maxKey) { - return parseVariables(filename, maxKey); -} - +// To be deprecated when pybind wrapper is merged: BetweenFactorPose3s parse3DFactors(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - return parseFactors(filename, model, maxKey); + const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) { + return parseFactors(filename, model, maxIndex); +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +std::map parse3DPoses(const std::string &filename, + size_t maxIndex) { + return parseVariables(filename, maxIndex); +} + +std::map parse3DLandmarks(const std::string &filename, + size_t maxIndex) { + return parseVariables(filename, maxIndex); } #endif } // namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 38ddfd1dc..7bdf6728d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -74,37 +74,41 @@ enum KernelFunctionType { }; /** - * Parse variables in 2D g2o graph file into a map. - * Instantiated in .cpp T equal to Pose2, Pose3 + * Parse variables in a line-based text format (like g2o) into a map. + * Instantiated in .cpp Pose2, Point2, Pose3, and Point3. + * Note the map keys are integer indices, *not* gtsam::Keys. This is is + * different below where landmarks will use be L(index) symbols. */ template -GTSAM_EXPORT std::map parseVariables(const std::string &filename, - Key maxKey = 0); +GTSAM_EXPORT std::map parseVariables(const std::string &filename, + size_t maxIndex = 0); /** - * Parse edges in 2D g2o graph file into a set of binary measuremnts. - * Instantiated in .cpp for T equal to Pose2, Pose3 + * Parse binary measurements in a line-based text format (like g2o) into a + * vector. Instantiated in .cpp for Pose2, Rot2, Pose3, and Rot3. The rotation + * versions parse poses and extract only the rotation part, using the marginal + * covariance as noise model. */ template GTSAM_EXPORT std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model = nullptr, - Key maxKey = 0); + const noiseModel::Diagonal::shared_ptr &model = nullptr, + size_t maxIndex = 0); /** - * Parse edges in 2D g2o graph file into a set of BetweenFactors. - * Instantiated in .cpp T equal to Pose2, Pose3 + * Parse BetweenFactors in a line-based text format (like g2o) into a vector of + * shared pointers. Instantiated in .cpp T equal to Pose2 and Pose3. */ template GTSAM_EXPORT std::vector::shared_ptr> parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model = nullptr, - Key maxKey = 0); + size_t maxIndex = 0); /// Return type for auxiliary functions -typedef std::pair IndexedPose; -typedef std::pair IndexedLandmark; -typedef std::pair, Pose2> IndexedEdge; +typedef std::pair IndexedPose; +typedef std::pair IndexedLandmark; +typedef std::pair, Pose2> IndexedEdge; /** * Parse TORO/G2O vertex "id x y yaw" @@ -130,19 +134,21 @@ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& GTSAM_EXPORT boost::optional parseEdge(std::istream& is, const std::string& tag); -/// Return type for load functions +/// Return type for load functions, which return a graph and initial values. For +/// landmarks, the gtsam::Symbol L(index) is used to insert into the Values. +/// Bearing-range measurements also refer tio landmarks with L(index). using GraphAndValues = std::pair; /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] - * @param maxKey if non-zero cut out vertices >= maxKey + * @param maxIndex if non-zero cut out vertices >= maxIndex * @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, Key maxKey = 0, + std::pair dataset, size_t maxIndex = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatAUTO, @@ -152,7 +158,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 maxKey if non-zero cut out vertices >= maxKey + * @param maxIndex if non-zero cut out vertices >= maxIndex * @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 @@ -160,13 +166,13 @@ GTSAM_EXPORT GraphAndValues load2D( * @return graph and initial values */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), Key maxKey = 0, bool addNoise = + SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 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, Key maxKey = 0); + noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -190,6 +196,11 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D * @param filename The name of the g2o file to write * @param graph NonlinearFactor graph storing the measurements * @param estimate Values + * + * Note:behavior change in PR #471: to be consistent with load2D and load3D, we + * write the *indices* to file and not the full Keys. This change really only + * affects landmarks, which get read as indices but stored in values with the + * symbol L(index). */ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); @@ -330,22 +341,24 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); +// To be deprecated when pybind wrapper is merged: +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s +parse3DFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + size_t maxIndex = 0); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 inline boost::optional parseVertex(std::istream &is, const std::string &tag) { return parseVertexPose(is, tag); } -GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - Key maxKey = 0); +GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, + size_t maxIndex = 0); -GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, - Key maxKey = 0); - -using BetweenFactorPose3s = std::vector::shared_ptr>; -GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( - const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model = nullptr, Key maxKey = 0); +GTSAM_EXPORT std::map +parse3DLandmarks(const std::string &filename, size_t maxIndex = 0); #endif } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 3e804b6b0..aad9124c5 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -82,7 +82,7 @@ TEST( dataSet, parseEdge) const auto actual = parseEdge(is, tag); EXPECT(actual); if (actual) { - pair expected(0, 1); + pair expected(0, 1); EXPECT(expected == actual->first); EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); } @@ -105,8 +105,9 @@ TEST(dataSet, load2D) { EXPECT(assert_equal(expected, *actual)); // Check binary measurements, Pose2 - auto measurements = parseMeasurements(filename, nullptr, 5); - EXPECT_LONGS_EQUAL(4, measurements.size()); + size_t maxIndex = 5; + auto measurements = parseMeasurements(filename, nullptr, maxIndex); + EXPECT_LONGS_EQUAL(5, measurements.size()); // Check binary measurements, Rot2 auto measurements2 = parseMeasurements(filename); @@ -132,14 +133,22 @@ TEST(dataSet, load2D) { } /* ************************************************************************* */ -TEST( dataSet, load2DVictoriaPark) -{ +TEST(dataSet, load2DVictoriaPark) { const string filename = findExampleDataFile("victoria_park.txt"); NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; + + // Load all boost::tie(graph, initial) = load2D(filename); - EXPECT_LONGS_EQUAL(10608,graph->size()); - EXPECT_LONGS_EQUAL(7120,initial->size()); + EXPECT_LONGS_EQUAL(10608, graph->size()); + EXPECT_LONGS_EQUAL(7120, initial->size()); + + // Restrict keys + size_t maxIndex = 5; + boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex); + EXPECT_LONGS_EQUAL(5, graph->size()); + EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well + EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]); } /* ************************************************************************* */ @@ -293,13 +302,13 @@ TEST(dataSet, readG2oCheckDeterminants) { } // Check determinants in initial values - const map poses = parseVariables(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(5, poses.size()); for (const auto& key_value : poses) { const Rot3 R = key_value.second.rotation(); EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); } - const map landmarks = parseVariables(g2oFile); + const map landmarks = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(0, landmarks.size()); } @@ -308,10 +317,13 @@ TEST(dataSet, readG2oLandmarks) { const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); // Check number of poses and landmarks. Should be 8 each. - const map poses = parseVariables(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, poses.size()); - const map landmarks = parseVariables(g2oFile); + const map landmarks = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, landmarks.size()); + + auto graphAndValues = load3D(g2oFile); + EXPECT(graphAndValues.second->exists(L(0))); } /* ************************************************************************* */ @@ -564,14 +576,12 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); Pose3 pose = poseChange.compose(readData.cameras[i].pose()); - value.insert(poseKey, pose); + value.insert(X(i), pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point - Key pointKey = P(j); Point3 point = poseChange.transformFrom( readData.tracks[j].p ); - value.insert(pointKey, point); + value.insert(P(j), point); } // Write values and readData to a file @@ -596,13 +606,11 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); - Key poseKey = symbol('x',0); - Pose3 actualPose = value.at(poseKey); + Pose3 actualPose = value.at(X(0)); EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Key pointKey = P(0); - Point3 actualPoint = value.at(pointKey); + Point3 actualPoint = value.at(P(0)); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 974806612..4249f5bc4 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -308,7 +308,7 @@ TEST(testNonlinearISAM, loop_closures ) { // check if edge const auto betweenPose = parseEdge(is, tag); if (betweenPose) { - Key id1, id2; + size_t id1, id2; tie(id1, id2) = betweenPose->first; graph.emplace_shared >(Symbol('x', id2), Symbol('x', id1), betweenPose->second, model);