diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index 0ea84e450..56a0ed327 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -51,6 +51,13 @@ class Assignment : public std::map { public: using std::map::operator=; + // Define the implicit default constructor. + Assignment() = default; + + // Construct from initializer list. + Assignment(std::initializer_list> init) + : std::map{init} {} + void print(const std::string& s = "Assignment: ", const std::function& labelFormatter = &DefaultFormatter) const { diff --git a/gtsam/discrete/DiscreteValues.cpp b/gtsam/discrete/DiscreteValues.cpp index 5d0c8dd3d..b0427e91b 100644 --- a/gtsam/discrete/DiscreteValues.cpp +++ b/gtsam/discrete/DiscreteValues.cpp @@ -17,6 +17,7 @@ #include +#include #include using std::cout; @@ -26,6 +27,7 @@ using std::stringstream; namespace gtsam { +/* ************************************************************************ */ void DiscreteValues::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << ": "; @@ -34,6 +36,44 @@ void DiscreteValues::print(const string& s, cout << endl; } +/* ************************************************************************ */ +bool DiscreteValues::equals(const DiscreteValues& x, double tol) const { + if (this->size() != x.size()) return false; + for (const auto values : boost::combine(*this, x)) { + if (values.get<0>() != values.get<1>()) return false; + } + return true; +} + +/* ************************************************************************ */ +DiscreteValues& DiscreteValues::insert(const DiscreteValues& values) { + for (const auto& kv : values) { + if (count(kv.first)) { + throw std::out_of_range( + "Requested to insert a DiscreteValues into another DiscreteValues " + "that already contains one or more of its keys."); + } else { + this->emplace(kv); + } + } + return *this; +} + +/* ************************************************************************ */ +DiscreteValues& DiscreteValues::update(const DiscreteValues& values) { + for (const auto& kv : values) { + if (!count(kv.first)) { + throw std::out_of_range( + "Requested to update a DiscreteValues with another DiscreteValues " + "that contains keys not present in the first."); + } else { + (*this)[kv.first] = kv.second; + } + } + return *this; +} + +/* ************************************************************************ */ string DiscreteValues::Translate(const Names& names, Key key, size_t index) { if (names.empty()) { stringstream ss; @@ -60,6 +100,7 @@ string DiscreteValues::markdown(const KeyFormatter& keyFormatter, return ss.str(); } +/* ************************************************************************ */ string DiscreteValues::html(const KeyFormatter& keyFormatter, const Names& names) const { stringstream ss; @@ -84,6 +125,7 @@ string DiscreteValues::html(const KeyFormatter& keyFormatter, return ss.str(); } +/* ************************************************************************ */ string markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter, const DiscreteValues::Names& names) { return values.markdown(keyFormatter, names); diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h index 72bb24081..6a1f550b6 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -27,21 +27,16 @@ namespace gtsam { -/** A map from keys to values - * TODO(dellaert): Do we need this? Should we just use gtsam::DiscreteValues? - * We just need another special DiscreteValue to represent labels, - * However, all other Lie's operators are undefined in this class. - * The good thing is we can have a Hybrid graph of discrete/continuous variables - * together.. - * Another good thing is we don't need to have the special DiscreteKey which - * stores cardinality of a Discrete variable. It should be handled naturally in - * the new class DiscreteValue, as the variable's type (domain) +/** + * A map from keys to values * @ingroup discrete */ class GTSAM_EXPORT DiscreteValues : public Assignment { public: using Base = Assignment; // base class + /// @name Standard Constructors + /// @{ using Assignment::Assignment; // all constructors // Define the implicit default constructor. @@ -50,14 +45,44 @@ class GTSAM_EXPORT DiscreteValues : public Assignment { // Construct from assignment. explicit DiscreteValues(const Base& a) : Base(a) {} + // Construct from initializer list. + DiscreteValues(std::initializer_list> init) + : Assignment{init} {} + + /// @} + /// @name Testable + /// @{ + + /// print required by Testable. void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /// equals required by Testable for unit testing. + bool equals(const DiscreteValues& x, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** Insert all values from \c values. Throws an invalid_argument exception if + * any keys to be inserted are already used. */ + DiscreteValues& insert(const DiscreteValues& values); + + /** For all key/value pairs in \c values, replace values with corresponding + * keys in this object with those in \c values. Throws std::out_of_range if + * any keys in \c values are not present in this object. */ + DiscreteValues& update(const DiscreteValues& values); + + /** + * @brief Return a vector of DiscreteValues, one for each possible + * combination of values. + */ static std::vector CartesianProduct( const DiscreteKeys& keys) { return Base::CartesianProduct(keys); } + /// @} /// @name Wrapper support /// @{ diff --git a/gtsam/discrete/tests/testDiscreteValues.cpp b/gtsam/discrete/tests/testDiscreteValues.cpp index c8a1fa168..6cfc11531 100644 --- a/gtsam/discrete/tests/testDiscreteValues.cpp +++ b/gtsam/discrete/tests/testDiscreteValues.cpp @@ -27,12 +27,25 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +static const DiscreteValues kExample{{12, 1}, {5, 0}}; + +/* ************************************************************************* */ +// Check insert +TEST(DiscreteValues, Insert) { + EXPECT(assert_equal({{12, 1}, {5, 0}, {13, 2}}, + DiscreteValues(kExample).insert({{13, 2}}))); +} + +/* ************************************************************************* */ +// Check update. +TEST(DiscreteValues, Update) { + EXPECT(assert_equal({{12, 2}, {5, 0}}, + DiscreteValues(kExample).update({{12, 2}}))); +} + /* ************************************************************************* */ // Check markdown representation with a value formatter. TEST(DiscreteValues, markdownWithValueFormatter) { - DiscreteValues values; - values[12] = 1; // A - values[5] = 0; // B string expected = "|Variable|value|\n" "|:-:|:-:|\n" @@ -40,16 +53,13 @@ TEST(DiscreteValues, markdownWithValueFormatter) { "|A|One|\n"; auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; }; DiscreteValues::Names names{{12, {"Zero", "One", "Two"}}, {5, {"-", "+"}}}; - string actual = values.markdown(keyFormatter, names); + string actual = kExample.markdown(keyFormatter, names); EXPECT(actual == expected); } /* ************************************************************************* */ // Check html representation with a value formatter. TEST(DiscreteValues, htmlWithValueFormatter) { - DiscreteValues values; - values[12] = 1; // A - values[5] = 0; // B string expected = "
\n" "\n" @@ -64,7 +74,7 @@ TEST(DiscreteValues, htmlWithValueFormatter) { ""; auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; }; DiscreteValues::Names names{{12, {"Zero", "One", "Two"}}, {5, {"-", "+"}}}; - string actual = values.html(keyFormatter, names); + string actual = kExample.html(keyFormatter, names); EXPECT(actual == expected); }