From e62d04ce2715f9fa913ce0979447334c3eb27855 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 13 Aug 2020 20:38:58 -0400 Subject: [PATCH] Templated parse methods --- gtsam/nonlinear/Values.h | 2 +- gtsam/sam/BearingRangeFactor.h | 15 +- gtsam/sfm/BinaryMeasurement.h | 93 ++-- gtsam/slam/dataset.cpp | 763 +++++++++++++++++-------------- gtsam/slam/dataset.h | 79 ++-- gtsam/slam/tests/testDataset.cpp | 26 +- timing/timeFrobeniusFactor.cpp | 2 +- 7 files changed, 530 insertions(+), 450 deletions(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index aadbcf394..b49188b68 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -397,7 +397,7 @@ namespace gtsam { template size_t count() const { size_t i = 0; - for (const auto& key_value : *this) { + for (const auto key_value : *this) { if (dynamic_cast*>(&key_value.value)) ++i; } diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index af7b47446..b8e231915 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -42,12 +42,19 @@ class BearingRangeFactor public: typedef boost::shared_ptr shared_ptr; - /// default constructor + /// Default constructor BearingRangeFactor() {} - /// primary constructor - BearingRangeFactor(Key key1, Key key2, const B& measuredBearing, - const R& measuredRange, const SharedNoiseModel& model) + /// Construct from BearingRange instance + BearingRangeFactor(Key key1, Key key2, const T &bearingRange, + const SharedNoiseModel &model) + : Base({key1, key2}, model, T(bearingRange)) { + this->initialize(expression({key1, key2})); + } + + /// Construct from separate bearing and range + BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, + const R &measuredRange, const SharedNoiseModel &model) : Base({key1, key2}, model, T(measuredBearing, measuredRange)) { this->initialize(expression({key1, key2})); } diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index c4b4a9804..c525c1b9e 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -15,78 +15,69 @@ * @file BinaryMeasurement.h * @author Akshay Krishnan * @date July 2020 - * @brief Binary measurement represents a measurement between two keys in a graph. - * A binary measurement is similar to a BetweenFactor, except that it does not contain - * an error function. It is a measurement (along with a noise model) from one key to - * another. Note that the direction is important. A measurement from key1 to key2 is not - * the same as the same measurement from key2 to key1. + * @brief Binary measurement represents a measurement between two keys in a + * graph. A binary measurement is similar to a BetweenFactor, except that it + * does not contain an error function. It is a measurement (along with a noise + * model) from one key to another. Note that the direction is important. A + * measurement from key1 to key2 is not the same as the same measurement from + * key2 to key1. */ -#include - #include -#include -#include - +#include #include #include +#include +#include + namespace gtsam { -template -class BinaryMeasurement { - // Check that VALUE type is testable - BOOST_CONCEPT_ASSERT((IsTestable)); - - public: - typedef VALUE T; +template class BinaryMeasurement : public Factor { + // Check that T type is testable + BOOST_CONCEPT_ASSERT((IsTestable)); +public: // shorthand for a smart pointer to a measurement - typedef typename boost::shared_ptr shared_ptr; + using shared_ptr = typename boost::shared_ptr; - private: - Key key1_, key2_; /** Keys */ +private: + T measured_; ///< The measurement + SharedNoiseModel noiseModel_; ///< Noise model - VALUE measured_; /** The measurement */ +public: + BinaryMeasurement(Key key1, Key key2, const T &measured, + const SharedNoiseModel &model = nullptr) + : Factor(std::vector({key1, key2})), measured_(measured), + noiseModel_(model) {} - SharedNoiseModel noiseModel_; /** Noise model */ - - public: - /** Constructor */ - BinaryMeasurement(Key key1, Key key2, const VALUE &measured, - const SharedNoiseModel &model = nullptr) : - key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) { - } - - Key key1() const { return key1_; } - - Key key2() const { return key2_; } + /// @name Standard Interface + /// @{ + Key key1() const { return keys_[0]; } + Key key2() const { return keys_[1]; } + const T &measured() const { return measured_; } const SharedNoiseModel &noiseModel() const { return noiseModel_; } - /** implement functions needed for Testable */ + /// @} + /// @name Testable + /// @{ - /** print */ - void print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BinaryMeasurement(" - << keyFormatter(this->key1()) << "," + void print(const std::string &s, + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } - /** equals */ bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const { - const BinaryMeasurement *e = dynamic_cast *> (&expected); - return e != nullptr && key1_ == e->key1_ && - key2_ == e->key2_ - && traits::Equals(this->measured_, e->measured_, tol) && - noiseModel_->equals(*expected.noiseModel()); + const BinaryMeasurement *e = + dynamic_cast *>(&expected); + return e != nullptr && Factor::equals(*e) && + traits::Equals(this->measured_, e->measured_, tol) && + noiseModel_->equals(*expected.noiseModel()); } - - /** return the measured value */ - VALUE measured() const { - return measured_; - } -}; // \class BetweenMeasurement -} \ No newline at end of file + /// @} +}; +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index af8904e49..11473d084 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -108,11 +108,94 @@ string createRewrittenFileName(const string &name) { } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, Key maxKey, - bool addNoise, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, - noiseFormat, kernelFunctionType); +// Parse a file by calling the parse(is, tag) function for every line. +template +using Parser = + std::function(istream &is, const string &tag)>; + +template +static void parseLines(const string &filename, Parser parse) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse: can not find file " + filename); + while (!is.eof()) { + string tag; + is >> tag; + parse(is, tag); // ignore return value + is.ignore(LINESIZE, '\n'); + } +} + +/* ************************************************************************* */ +// Parse types T into a Key-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); + return boost::none; + }; + parseLines(filename, emplace); + return result; +} + +/* ************************************************************************* */ +// 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; + Parser add = [&result, parse](istream &is, const string &tag) { + if (auto t = parse(is, tag)) + result.push_back(*t); + return boost::none; + }; + parseLines(filename, add); + return result; +} + +/* ************************************************************************* */ +boost::optional parseVertexPose(istream &is, const string &tag) { + if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { + Key id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + return IndexedPose(id, Pose2(x, y, yaw)); + } else { + return boost::none; + } +} + +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexPose, maxKey); +} + +/* ************************************************************************* */ +boost::optional parseVertexLandmark(istream &is, + const string &tag) { + if (tag == "VERTEX_XY") { + Key id; + double x, y; + is >> id >> x >> y; + return IndexedLandmark(id, Point2(x, y)); + } else { + return boost::none; + } +} + +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexLandmark, maxKey); } /* ************************************************************************* */ @@ -199,107 +282,6 @@ createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat, } } -/* ************************************************************************* */ -boost::optional parseVertexPose(istream &is, const string &tag) { - if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - Key id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - return IndexedPose(id, Pose2(x, y, yaw)); - } else { - return boost::none; - } -} - -/* ************************************************************************* */ -boost::optional parseVertexLandmark(istream &is, - const string &tag) { - if (tag == "VERTEX_XY") { - Key id; - double x, y; - is >> id >> x >> y; - return IndexedLandmark(id, Point2(x, y)); - } else { - return boost::none; - } -} - -/* ************************************************************************* */ -// Parse types T into a Key-indexed map -template -static map parseIntoMap(const string &filename, Key maxKey = 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 (maxKey && indexed_t->first >= maxKey) - 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 maxKey) { - return parseIntoMap(filename, maxKey); -} - -/* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional &indexed) { - string tag; - is >> tag; - indexed = parseVertexLandmark(is, tag); - return is; -} - -map parse2DLandmarks(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); -} - -/* ************************************************************************* */ -// Parse a file by first parsing the `Parsed` type and then processing it with -// the supplied `process` function. Result is put in a vector evaluates to true. -// Requires: a stream >> operator for boost::optional -template -static vector -parseToVector(const string &filename, - const std::function process) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse: can not find file " + filename); - - vector result; - string tag; - while (!is.eof()) { - boost::optional parsed; - is >> parsed; - if (parsed) { - Output factor = process(*parsed); // can return nullptr - if (factor) { - result.push_back(factor); - } - } - is.ignore(LINESIZE, '\n'); - } - return result; -} - /* ************************************************************************* */ boost::optional parseEdge(istream &is, const string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || @@ -315,29 +297,74 @@ boost::optional parseEdge(istream &is, const string &tag) { } /* ************************************************************************* */ -struct Measurement2 { - Key id1, id2; - Pose2 pose; - Vector6 v; // 6 noise model parameters for edge -}; - -istream &operator>>(istream &is, boost::optional &edge) { - string tag; - is >> tag; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || - (tag == "ODOMETRY")) { - Key id1, id2; - double x, y, yaw; - Vector6 v; - is >> id1 >> id2 >> x >> y >> yaw >> // - v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); - edge.reset({id1, id2, Pose2(x, y, yaw), v}); - } - return is; -} +// Measurement parsers are implemented as a functor, as they has several options +// to filter and create the measurement model. +template struct ParseMeasurement; /* ************************************************************************* */ -// Create a sampler +// Converting from Measurement to BetweenFactor is generic +template struct ParseFactor : ParseMeasurement { + ParseFactor(const ParseMeasurement &parent) + : ParseMeasurement(parent) {} + + // We parse a measurement then convert + typename boost::optional::shared_ptr> + operator()(istream &is, const string &tag) { + if (auto m = ParseMeasurement::operator()(is, tag)) + return boost::make_shared>( + m->key1(), m->key2(), m->measured(), m->noiseModel()); + else + return boost::none; + } +}; + +/* ************************************************************************* */ +// Pose2 measurement parser +template <> struct ParseMeasurement { + // The arguments + boost::shared_ptr sampler; + Key maxKey; + + // For noise model creation + bool smart; + NoiseFormat noiseFormat; + KernelFunctionType kernelFunctionType; + + // If this is not null, will use instead of parsed model: + SharedNoiseModel model; + + // The actual parser + boost::optional> operator()(istream &is, + const string &tag) { + auto edge = parseEdge(is, tag); + if (!edge) + return boost::none; + + // parse noise model + Vector6 v; + is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); + + // optional filter + Key id1, id2; + tie(id1, id2) = edge->first; + if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + return boost::none; + + // Get pose and optionally add noise + Pose2 &pose = edge->second; + if (sampler) + pose = pose.retract(sampler->sample()); + + // emplace measurement + auto modelFromFile = + createNoiseModel(v, smart, noiseFormat, kernelFunctionType); + return BinaryMeasurement(id1, id2, pose, + model ? model : modelFromFile); + } +}; + +/* ************************************************************************* */ +// Create a sampler to corrupt a measurement boost::shared_ptr createSampler(const SharedNoiseModel &model) { auto noise = boost::dynamic_pointer_cast(model); if (!noise) @@ -347,81 +374,75 @@ boost::shared_ptr createSampler(const SharedNoiseModel &model) { } /* ************************************************************************* */ -// Small functor to process the edge into a BetweenFactor::shared_ptr -struct ProcessPose2 { - // The arguments - Key maxKey; - SharedNoiseModel model; - boost::shared_ptr sampler; - - // Arguments for parsing noise model - bool smart; - NoiseFormat noiseFormat; - KernelFunctionType kernelFunctionType; - - ProcessPose2(Key maxKey = 0, SharedNoiseModel model = nullptr, - boost::shared_ptr sampler = nullptr, bool smart = true, - NoiseFormat noiseFormat = NoiseFormatAUTO, - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE) - : maxKey(maxKey), model(model), sampler(sampler), smart(smart), - noiseFormat(noiseFormat), kernelFunctionType(kernelFunctionType) {} - - // The actual function - BetweenFactor::shared_ptr operator()(const Measurement2 &edge) { - // optional filter - if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey)) - return nullptr; - - // Get pose and optionally add noise - Pose2 T12 = edge.pose; - if (sampler) - T12 = T12.retract(sampler->sample()); - - // Create factor - // If model is nullptr we use the model from the file - return boost::make_shared>( - edge.id1, edge.id2, T12, - model - ? model - : createNoiseModel(edge.v, smart, noiseFormat, kernelFunctionType)); - } -}; - -/* ************************************************************************* */ -BetweenFactorPose2s -parse2DFactors(const string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise, - Key maxKey) { - ProcessPose2 process(maxKey, nullptr, - corruptingNoise ? createSampler(corruptingNoise) - : nullptr); - - return parseToVector::shared_ptr>(filename, - process); +// Implementation of parseMeasurements for Pose2 +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}; + return parseToVector>(filename, parse); } /* ************************************************************************* */ -static void parseOthers(const string &filename, Key maxKey, - NonlinearFactorGraph::shared_ptr graph, - Values::shared_ptr initial) { - // Do a pass to get other types of measurements - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("load2D: can not find file " + filename); +// Implementation of parseMeasurements for Rot2, which converts from Pose2 - Key id1, id2; - while (!is.eof()) { - string tag; - if (!(is >> tag)) - break; +// Extract Rot2 measurement from Pose2 measurement +static BinaryMeasurement convert(const BinaryMeasurement &p) { + auto gaussian = + boost::dynamic_pointer_cast(p.noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose2 measurements " + "with Gaussian noise models."); + const Matrix3 M = gaussian->covariance(); + return BinaryMeasurement(p.key1(), p.key2(), p.measured().rotation(), + noiseModel::Isotropic::Variance(1, M(2, 2))); +} - // Parse measurements - bool haveLandmark = false; +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + auto poses = parseMeasurements(filename, model, maxKey); + std::vector> result; + result.reserve(poses.size()); + for (const auto &p : poses) + result.push_back(convert(p)); + return result; +} + +/* ************************************************************************* */ +// Implementation of parseFactors for Pose2 +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, + true, NoiseFormatAUTO, KernelFunctionTypeNONE, + nullptr}); + return parseToVector::shared_ptr>(filename, parse); +} + +/* ************************************************************************* */ +using BearingRange2D = BearingRange; + +template <> struct ParseMeasurement { + // arguments + Key maxKey; + + // The actual parser + boost::optional> + operator()(istream &is, const string &tag) { + if (tag != "BR" && tag != "LANDMARK") + return boost::none; + + Key id1, id2; + is >> id1 >> id2; double bearing, range, bearing_std, range_std; - // A bearing-range measurement if (tag == "BR") { - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; + is >> bearing >> range >> bearing_std >> range_std; } // A landmark measurement, converted to bearing-range @@ -429,7 +450,7 @@ static void parseOthers(const string &filename, Key maxKey, double lmx, lmy; double v1, v2, v3; - is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; + is >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range bearing = atan2(lmy, lmx); @@ -442,90 +463,93 @@ static void parseOthers(const string &filename, Key maxKey, bearing_std = sqrt(v1 / 10.0); range_std = sqrt(v1); } else { + // TODO(frank): we are ignoring the non-uniform covariance bearing_std = 1; range_std = 1; - if (!haveLandmark) { - cout << "Warning: load2D is a very simple dataset loader and is " - "ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this " - "file." - << endl; - haveLandmark = true; - } } } - // Do some common stuff for bearing-range measurements - if (tag == "LANDMARK" || tag == "BR") { + // optional filter + if (maxKey && id1 >= maxKey) + return boost::none; - // optional filter - if (maxKey && id1 >= maxKey) - continue; + // Create noise model + auto measurementNoise = noiseModel::Diagonal::Sigmas( + (Vector(2) << bearing_std, range_std).finished()); - // Create noise model - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas( - (Vector(2) << bearing_std, range_std).finished()); - - // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, - measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(L(id2))) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing) * range, sin(bearing) * range); - Point2 global = pose.transformFrom(local); - initial->insert(L(id2), global); - } - } - is.ignore(LINESIZE, '\n'); + return BinaryMeasurement( + id1, L(id2), BearingRange2D(bearing, range), measurementNoise); } -} +}; /* ************************************************************************* */ -GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxKey, - bool addNoise, bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(const string &filename, SharedNoiseModel model, + Key maxKey, bool addNoise, bool smart, + NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - Values::shared_ptr initial(new Values); + auto graph = boost::make_shared(); + auto initial = boost::make_shared(); - const auto poses = parse2DPoses(filename, maxKey); + // 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); } - const auto landmarks = parse2DLandmarks(filename, maxKey); + + const auto landmarks = parseVariables(filename, maxKey); for (const auto &key_landmark : landmarks) { - initial->insert(key_landmark.first, key_landmark.second); + initial->insert(L(key_landmark.first), key_landmark.second); } - // Parse the pose constraints - ProcessPose2 process(maxKey, model, addNoise ? createSampler(model) : nullptr, - smart, noiseFormat, kernelFunctionType); - auto factors = parseToVector::shared_ptr>( - filename, process); + // Create parser for Pose2 and bearing/range + ParseFactor parseBetweenFactor( + {addNoise ? createSampler(model) : nullptr, maxKey, smart, noiseFormat, + kernelFunctionType, model}); + ParseMeasurement parseBearingRange{maxKey}; - // Add factors into the graph and add poses if necessary - auto graph = boost::make_shared(); - for (const auto &f : factors) { - graph->push_back(f); + Parser parse = [&](istream &is, const string &tag) { + if (auto f = parseBetweenFactor(is, tag)) { + graph->push_back(*f); - // Insert vertices if pure odometry file - Key id1 = f->key1(), id2 = f->key2(); - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) - initial->insert(id2, initial->at(id1) * f->measured()); - } + // Insert vertices if pure odometry file + Key key1 = (*f)->key1(), key2 = (*f)->key2(); + if (!initial->exists(key1)) + initial->insert(key1, Pose2()); + if (!initial->exists(key2)) + initial->insert(key2, initial->at(key1) * (*f)->measured()); + } else if (auto m = parseBearingRange(is, tag)) { + Key key1 = m->key1(), key2 = m->key2(); + BearingRange2D br = m->measured(); + graph->emplace_shared>(key1, key2, br, + m->noiseModel()); - // Do a pass to parse landmarks and bearing-range measurements - parseOthers(filename, maxKey, graph, initial); + // Insert poses or points if they do not exist yet + if (!initial->exists(key1)) + initial->insert(key1, Pose2()); + if (!initial->exists(key2)) { + Pose2 pose = initial->at(key1); + Point2 local = br.bearing() * Point2(br.range(), 0); + Point2 global = pose.transformFrom(local); + initial->insert(key2, global); + } + } + return 0; + }; + + parseLines(filename, parse); return make_pair(graph, initial); } +/* ************************************************************************* */ +GraphAndValues load2D(pair dataset, Key maxKey, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, + noiseFormat, kernelFunctionType); +} + /* ************************************************************************* */ GraphAndValues load2D_robust(const string &filename, noiseModel::Base::shared_ptr &model, Key maxKey) { @@ -540,7 +564,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, fstream stream(filename.c_str(), fstream::out); // save poses - for (const Values::ConstKeyValuePair &key_value : config) { + for (const Values::ConstKeyValuePair key_value : config) { const Pose2 &pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; @@ -586,7 +610,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, fstream stream(filename.c_str(), fstream::out); // save 2D poses - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -596,7 +620,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } // save 3D poses - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -609,7 +633,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } // save 2D landmarks - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -619,7 +643,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } // save 3D landmarks - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -709,44 +733,46 @@ istream &operator>>(istream &is, Rot3 &R) { } /* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional> &indexed) { - string tag; - is >> tag; +boost::optional> parseVertexPose3(istream &is, + const string &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})}); + return make_pair(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; + return make_pair(id, Pose3(q, {x, y, z})); + } else + return boost::none; } -map parse3DPoses(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexPose3, maxKey); } /* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional> &indexed) { - string tag; - is >> tag; +boost::optional> parseVertexPoint3(istream &is, + const string &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; + return make_pair(id, Point3(x, y, z)); + } else + return boost::none; } -map parse3DLandmarks(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexPoint3, maxKey); } /* ************************************************************************* */ @@ -761,97 +787,126 @@ istream &operator>>(istream &is, Matrix6 &m) { } /* ************************************************************************* */ -struct Measurement3 { - Key id1, id2; - Pose3 pose; - SharedNoiseModel model; +// Pose3 measurement parser +template <> struct ParseMeasurement { + // The arguments + boost::shared_ptr sampler; + Key maxKey; + + // The actual parser + boost::optional> operator()(istream &is, + const string &tag) { + if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT") + return boost::none; + + // parse ids and optionally filter + Key id1, id2; + is >> id1 >> id2; + if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + return boost::none; + + Matrix6 m; + if (tag == "EDGE3") { + double x, y, z; + Rot3 R; + is >> x >> y >> z >> R >> m; + Pose3 T12(R, {x, y, z}); + // optionally add noise + if (sampler) + T12 = T12.retract(sampler->sample()); + + return BinaryMeasurement(id1, id2, T12, + noiseModel::Gaussian::Information(m)); + } else if (tag == "EDGE_SE3:QUAT") { + double x, y, z; + Quaternion q; + is >> x >> y >> z >> q >> m; + + Pose3 T12(q, {x, y, z}); + // optionally add noise + if (sampler) + T12 = T12.retract(sampler->sample()); + + // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: + 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); + + return BinaryMeasurement( + id1, id2, T12, noiseModel::Gaussian::Information(mgtsam)); + } else + return boost::none; + } }; -istream &operator>>(istream &is, boost::optional &edge) { - 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 >> m; - edge.reset( - {id1, id2, Pose3(R, {x, y, z}), noiseModel::Gaussian::Information(m)}); - } - if (tag == "EDGE_SE3:QUAT") { - Key id1, id2; - double x, y, z; - Quaternion q; - is >> id1 >> id2 >> x >> y >> z >> q >> m; - - // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: - 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); - - edge.reset({id1, id2, Pose3(q, {x, y, z}), - noiseModel::Gaussian::Information(mgtsam)}); - } - return is; +/* ************************************************************************* */ +// Implementation of parseMeasurements for Pose3 +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey}; + return parseToVector>(filename, parse); } /* ************************************************************************* */ -// Small functor to process the edge into a BetweenFactor::shared_ptr -struct ProcessPose3 { - // The arguments - Key maxKey = 0; - SharedNoiseModel model; - boost::shared_ptr sampler; +// Implementation of parseMeasurements for Rot3, which converts from Pose3 - // The actual function - BetweenFactor::shared_ptr operator()(const Measurement3 &edge) { - // optional filter - if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey)) - return nullptr; +// Extract Rot3 measurement from Pose3 measurement +static BinaryMeasurement convert(const BinaryMeasurement &p) { + auto gaussian = + boost::dynamic_pointer_cast(p.noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose3 measurements " + "with Gaussian noise models."); + const Matrix6 M = gaussian->covariance(); + return BinaryMeasurement( + p.key1(), p.key2(), p.measured().rotation(), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); +} - // Get pose and optionally add noise - Pose3 T12 = edge.pose; - if (sampler) - T12 = T12.retract(sampler->sample()); - - // Create factor - return boost::make_shared>(edge.id1, edge.id2, T12, - edge.model); - } -}; +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + auto poses = parseMeasurements(filename, model, maxKey); + std::vector> result; + result.reserve(poses.size()); + for (const auto &p : poses) + result.push_back(convert(p)); + return result; +} /* ************************************************************************* */ -BetweenFactorPose3s -parse3DFactors(const string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise, - Key maxKey) { - ProcessPose3 process; - process.maxKey = maxKey; - if (corruptingNoise) { - process.sampler = createSampler(corruptingNoise); - } - return parseToVector::shared_ptr>(filename, - process); +// Implementation of parseFactors for Pose3 +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}); + return parseToVector::shared_ptr>(filename, parse); } /* ************************************************************************* */ GraphAndValues load3D(const string &filename) { - const auto factors = parse3DFactors(filename); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + 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); } - Values::shared_ptr initial(new Values); - - const auto poses = parse3DPoses(filename); + const auto poses = parseVariables(filename); for (const auto &key_pose : poses) { initial->insert(key_pose.first, key_pose.second); } + const auto landmarks = parse3DLandmarks(filename); for (const auto &key_landmark : landmarks) { initial->insert(key_landmark.first, key_landmark.second); @@ -1200,4 +1255,20 @@ 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); +} + +BetweenFactorPose3s +parse3DFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + return parseFactors(filename, model, maxKey); +} +#endif } // namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 61eb9256e..38ddfd1dc 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -72,6 +73,34 @@ enum KernelFunctionType { KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY }; +/** + * Parse variables in 2D g2o graph file into a map. + * Instantiated in .cpp T equal to Pose2, Pose3 + */ +template +GTSAM_EXPORT std::map parseVariables(const std::string &filename, + Key maxKey = 0); + +/** + * Parse edges in 2D g2o graph file into a set of binary measuremnts. + * Instantiated in .cpp for T equal to Pose2, Pose3 + */ +template +GTSAM_EXPORT std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + Key maxKey = 0); + +/** + * Parse edges in 2D g2o graph file into a set of BetweenFactors. + * Instantiated in .cpp T equal to Pose2, Pose3 + */ +template +GTSAM_EXPORT std::vector::shared_ptr> +parseFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + Key maxKey = 0); + /// Return type for auxiliary functions typedef std::pair IndexedPose; typedef std::pair IndexedLandmark; @@ -90,7 +119,6 @@ GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, * @param is input stream * @param tag string parsed from input stream, will only parse if vertex type */ - GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& is, const std::string& tag); @@ -102,23 +130,6 @@ 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, - Key maxKey = 0); - -/// Parse vertices in 2D g2o graph file into a map of Pose2s. -GTSAM_EXPORT std::map parse2DPoses(const std::string &filename, - Key maxKey = 0); - -/// Parse landmarks in 2D g2o graph file into a map of Point2s. -GTSAM_EXPORT std::map parse2DLandmarks(const string &filename, - Key maxKey = 0); - /// Return type for load functions using GraphAndValues = std::pair; @@ -183,21 +194,6 @@ 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); -/// 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, - Key maxKey = 0); - -/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. -GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - Key maxKey = 0); - -/// Parse landmarks in 3D g2o graph file into a map of Point3s. -GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, - Key maxKey = 0); - /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); @@ -335,14 +331,21 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -/** - * Parse TORO/G2O vertex "id x y yaw" - * @param is input stream - * @param tag string parsed from input stream, will only parse if vertex type - */ 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 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); + #endif } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index bc65dc351..864f42162 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -104,8 +104,16 @@ TEST(dataSet, load2D) { boost::dynamic_pointer_cast>(graph->at(0)); EXPECT(assert_equal(expected, *actual)); - // Check factor parsing - const auto actualFactors = parse2DFactors(filename); + // Check binary measurements, Pose2 + auto measurements = parseMeasurements(filename, nullptr, 5); + EXPECT_LONGS_EQUAL(4, measurements.size()); + + // Check binary measurements, Rot2 + auto measurements2 = parseMeasurements(filename); + EXPECT_LONGS_EQUAL(300, measurements2.size()); + + // // Check factor parsing + const auto actualFactors = parseFactors(filename); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(graph->at(i)), @@ -113,13 +121,13 @@ TEST(dataSet, load2D) { } // Check pose parsing - const auto actualPoses = parse2DPoses(filename); + const auto actualPoses = parseVariables(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); + const auto actualLandmarks = parseVariables(filename); EXPECT_LONGS_EQUAL(0, actualLandmarks.size()); } @@ -202,7 +210,7 @@ TEST(dataSet, readG2o3D) { } // Check factor parsing - const auto actualFactors = parse3DFactors(g2oFile); + const auto actualFactors = parseFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(expectedGraph[i]), @@ -210,7 +218,7 @@ TEST(dataSet, readG2o3D) { } // Check pose parsing - const auto actualPoses = parse3DPoses(g2oFile); + const auto actualPoses = parseVariables(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } @@ -277,7 +285,7 @@ TEST(dataSet, readG2oCheckDeterminants) { const string g2oFile = findExampleDataFile("toyExample.g2o"); // Check determinants in factors - auto factors = parse3DFactors(g2oFile); + auto factors = parseFactors(g2oFile); EXPECT_LONGS_EQUAL(6, factors.size()); for (const auto& factor : factors) { const Rot3 R = factor->measured().rotation(); @@ -285,7 +293,7 @@ TEST(dataSet, readG2oCheckDeterminants) { } // Check determinants in initial values - const map poses = parse3DPoses(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(); @@ -300,7 +308,7 @@ TEST(dataSet, readG2oLandmarks) { const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); // Check number of poses and landmarks. Should be 8 each. - const map poses = parse3DPoses(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, poses.size()); const map landmarks = parse3DLandmarks(g2oFile); EXPECT_LONGS_EQUAL(8, landmarks.size()); diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index 8bdda968f..44bb084cc 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) { // Read G2O file const auto factors = parse3DFactors(g2oFile); - const auto poses = parse3DPoses(g2oFile); + const auto poses = parseVariables(g2oFile); // Build graph NonlinearFactorGraph graph;