Merge branch 'develop' into hybrid/elimination

gtsam/discrete/Assignment.h

@@ -51,6 +51,13 @@ class Assignment : public std::map<L, size_t> {
  public:
   using std::map<L, size_t>::operator=;
 
+  // Define the implicit default constructor.
+  Assignment() = default;
+
+  // Construct from initializer list.
+  Assignment(std::initializer_list<std::pair<const L, size_t>> init)
+      : std::map<L, size_t>{init} {}
+
   void print(const std::string& s = "Assignment: ",
              const std::function<std::string(L)>& labelFormatter =
                  &DefaultFormatter) const {

gtsam/discrete/DiscreteValues.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/discrete/DiscreteValues.h>
 
+#include <boost/range/combine.hpp>
 #include <sstream>
 
 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);

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?
+ * A map from keys to values
- * 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)
  * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
  public:
   using Base = Assignment<Key>;  // base class
 
+  /// @name Standard Constructors
+  /// @{
   using Assignment::Assignment;  // all constructors
 
   // Define the implicit default constructor.
@@ -50,14 +45,49 @@ class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
   // Construct from assignment.
   explicit DiscreteValues(const Base& a) : Base(a) {}
 
+  // Construct from initializer list.
+  DiscreteValues(std::initializer_list<std::pair<const Key, size_t>> init)
+      : Assignment<Key>{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 in base class;
+  std::pair<iterator, bool> insert( const value_type& value ){
+    return Base::insert(value);
+  }
+
+  /** 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<DiscreteValues> CartesianProduct(
       const DiscreteKeys& keys) {
     return Base::CartesianProduct<DiscreteValues>(keys);
   }
 
+  /// @}
   /// @name Wrapper support
   /// @{
 

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -25,10 +25,7 @@
 #include <gtsam/discrete/DecisionTree-inl.h>  // for convert only
 #define DISABLE_TIMING
 
-#include <boost/assign/std/map.hpp>
-#include <boost/assign/std/vector.hpp>
 #include <boost/tokenizer.hpp>
-using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/timing.h>

gtsam/discrete/tests/testDecisionTree.cpp

@@ -27,9 +27,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/assign/std/vector.hpp>
-using namespace boost::assign;
-
 using namespace std;
 using namespace gtsam;
 

gtsam/discrete/tests/testDecisionTreeFactor.cpp

@@ -23,9 +23,6 @@
 #include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/discrete/Signature.h>
 
-#include <boost/assign/std/map.hpp>
-using namespace boost::assign;
-
 using namespace std;
 using namespace gtsam;
 

gtsam/discrete/tests/testDiscreteBayesNet.cpp

@@ -26,11 +26,6 @@
 #include <CppUnitLite/TestHarness.h>
 
 
-#include <boost/assign/list_inserter.hpp>
-#include <boost/assign/std/map.hpp>
-
-using namespace boost::assign;
-
 #include <iostream>
 #include <string>
 #include <vector>
@@ -115,11 +110,11 @@ TEST(DiscreteBayesNet, Asia) {
   EXPECT(assert_equal(expected2, *chordal->back()));
 
   // now sample from it
-  DiscreteValues expectedSample;
+  DiscreteValues expectedSample{{Asia.first, 1},       {Dyspnea.first, 1},
+                                {XRay.first, 1},       {Tuberculosis.first, 0},
+                                {Smoking.first, 1},    {Either.first, 1},
+                                {LungCancer.first, 1}, {Bronchitis.first, 0}};
   SETDEBUG("DiscreteConditional::sample", false);
-  insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)(
-      Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)(
-      LungCancer.first, 1)(Bronchitis.first, 0);
   auto actualSample = chordal2->sample();
   EXPECT(assert_equal(expectedSample, actualSample));
 }

gtsam/discrete/tests/testDiscreteBayesTree.cpp

@@ -21,9 +21,6 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/inference/BayesNet.h>
 
-#include <boost/assign/std/vector.hpp>
-using namespace boost::assign;
-
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>

gtsam/discrete/tests/testDiscreteConditional.cpp

@@ -17,10 +17,7 @@
  * @date    Feb 14, 2011
  */
 
-#include <boost/assign/std/map.hpp>
-#include <boost/assign/std/vector.hpp>
 #include <boost/make_shared.hpp>
-using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>

gtsam/discrete/tests/testDiscreteFactor.cpp

@@ -21,9 +21,6 @@
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 
-#include <boost/assign/std/map.hpp>
-using namespace boost::assign;
-
 using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;

gtsam/discrete/tests/testDiscreteFactorGraph.cpp

@@ -23,9 +23,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/assign/std/map.hpp>
-using namespace boost::assign;
-
 using namespace std;
 using namespace gtsam;
 
@@ -49,9 +46,7 @@ TEST_UNSAFE(DiscreteFactorGraph, debugScheduler) {
 
   // Check MPE.
   auto actualMPE = graph.optimize();
-  DiscreteValues mpe;
+  EXPECT(assert_equal({{0, 2}, {1, 1}, {2, 0}, {3, 0}}, actualMPE));
-  insert(mpe)(0, 2)(1, 1)(2, 0)(3, 0);
-  EXPECT(assert_equal(mpe, actualMPE));
 }
 
 /* ************************************************************************* */
@@ -149,8 +144,7 @@ TEST(DiscreteFactorGraph, test) {
   EXPECT(assert_equal(expectedBayesNet, *actual2));
 
   // Test mpe
-  DiscreteValues mpe;
+  DiscreteValues mpe { {0, 0}, {1, 0}, {2, 0}};
-  insert(mpe)(0, 0)(1, 0)(2, 0);
   auto actualMPE = graph.optimize();
   EXPECT(assert_equal(mpe, actualMPE));
   EXPECT_DOUBLES_EQUAL(9, graph(mpe), 1e-5);  // regression
@@ -182,8 +176,7 @@ TEST_UNSAFE(DiscreteFactorGraph, testMaxProduct) {
   graph.add(C & B, "0.1 0.9 0.4 0.6");
 
   // Created expected MPE
-  DiscreteValues mpe;
+  DiscreteValues mpe{{0, 0}, {1, 1}, {2, 1}};
-  insert(mpe)(0, 0)(1, 1)(2, 1);
 
   // Do max-product with different orderings
   for (Ordering::OrderingType orderingType :
@@ -209,8 +202,7 @@ TEST(DiscreteFactorGraph, marginalIsNotMPE) {
   bayesNet.add(A % "10/9");
 
   // The expected MPE is A=1, B=1
-  DiscreteValues mpe;
+  DiscreteValues mpe { {0, 1}, {1, 1} };
-  insert(mpe)(0, 1)(1, 1);
 
   // Which we verify using max-product:
   DiscreteFactorGraph graph(bayesNet);
@@ -240,8 +232,7 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
   graph.add(T1 & T2 & A, "1 0 0 1 0 1 1 0");
   graph.add(A, "1 0");  // evidence, A = yes (first choice in Darwiche)
 
-  DiscreteValues mpe;
+  DiscreteValues mpe { {0, 1}, {1, 1}, {2, 1}, {3, 1}, {4, 0}};
-  insert(mpe)(4, 0)(2, 1)(3, 1)(0, 1)(1, 1);
   EXPECT_DOUBLES_EQUAL(0.33858, graph(mpe), 1e-5);  // regression
   // You can check visually by printing product:
   // graph.product().print("Darwiche-product");
@@ -267,112 +258,6 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
   EXPECT_LONGS_EQUAL(2, bayesTree->size());
 }
 
-#ifdef OLD
-
-/* ************************************************************************* */
-/**
- * Key type for discrete conditionals
- * Includes name and cardinality
- */
-class Key2 {
-private:
-std::string wff_;
-size_t cardinality_;
-public:
-/** Constructor, defaults to binary */
-Key2(const std::string& name, size_t cardinality = 2) :
-wff_(name), cardinality_(cardinality) {
-}
-const std::string& name() const {
-  return wff_;
-}
-
-/** provide streaming */
-friend std::ostream& operator <<(std::ostream &os, const Key2 &key);
-};
-
-struct Factor2 {
-std::string wff_;
-Factor2() :
-wff_("@") {
-}
-Factor2(const std::string& s) :
-wff_(s) {
-}
-Factor2(const Key2& key) :
-wff_(key.name()) {
-}
-
-friend std::ostream& operator <<(std::ostream &os, const Factor2 &f);
-friend Factor2 operator -(const Key2& key);
-};
-
-std::ostream& operator <<(std::ostream &os, const Factor2 &f) {
-os << f.wff_;
-return os;
-}
-
-/** negation */
-Factor2 operator -(const Key2& key) {
-return Factor2("-" + key.name());
-}
-
-/** OR */
-Factor2 operator ||(const Factor2 &factor1, const Factor2 &factor2) {
-return Factor2(std::string("(") + factor1.wff_ + " || " + factor2.wff_ + ")");
-}
-
-/** AND */
-Factor2 operator &&(const Factor2 &factor1, const Factor2 &factor2) {
-return Factor2(std::string("(") + factor1.wff_ + " && " + factor2.wff_ + ")");
-}
-
-/** implies */
-Factor2 operator >>(const Factor2 &factor1, const Factor2 &factor2) {
-return Factor2(std::string("(") + factor1.wff_ + " >> " + factor2.wff_ + ")");
-}
-
-struct Graph2: public std::list<Factor2> {
-
-/** Add a factor graph*/
-//  void operator +=(const Graph2& graph) {
-//    for(const Factor2& f: graph)
-//        push_back(f);
-//  }
-friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);
-
-};
-
-/** Add a factor */
-//Graph2 operator +=(Graph2& graph, const Factor2& factor) {
-//  graph.push_back(factor);
-//  return graph;
-//}
-std::ostream& operator <<(std::ostream &os, const Graph2& graph) {
-for(const Factor2& f: graph)
-os << f << endl;
-return os;
-}
-
-/* ************************************************************************* */
-TEST(DiscreteFactorGraph, Sugar)
-{
-Key2 M("Mythical"), I("Immortal"), A("Mammal"), H("Horned"), G("Magical");
-
-// Test this desired construction
-Graph2 unicorns;
-unicorns += M >> -A;
-unicorns += (-M) >> (-I && A);
-unicorns += (I || A) >> H;
-unicorns += H >> G;
-
-// should be done by adapting boost::assign:
-// unicorns += (-M) >> (-I && A), (I || A) >> H , H >> G;
-
-cout << unicorns;
-}
-#endif
-
 /* ************************************************************************* */
 TEST(DiscreteFactorGraph, Dot) {
   // Create Factor graph

gtsam/discrete/tests/testDiscreteLookupDAG.cpp

@@ -20,11 +20,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/DiscreteLookupDAG.h>
 
-#include <boost/assign/list_inserter.hpp>
-#include <boost/assign/std/map.hpp>
-
 using namespace gtsam;
-using namespace boost::assign;
 
 /* ************************************************************************* */
 TEST(DiscreteLookupDAG, argmax) {
@@ -43,8 +39,7 @@ TEST(DiscreteLookupDAG, argmax) {
   dag.add(1, DiscreteKeys{A}, adtA);
 
   // The expected MPE is A=1, B=1
-  DiscreteValues mpe;
+  DiscreteValues mpe{{0, 1}, {1, 1}};
-  insert(mpe)(0, 1)(1, 1);
 
   // check:
   auto actualMPE = dag.argmax();

gtsam/discrete/tests/testDiscreteMarginals.cpp

@@ -19,9 +19,6 @@
 
 #include <gtsam/discrete/DiscreteMarginals.h>
 
-#include <boost/assign/std/vector.hpp>
-using namespace boost::assign;
-
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;

gtsam/discrete/tests/testDiscreteValues.cpp

@@ -21,18 +21,28 @@
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/discrete/Signature.h>
 
-#include <boost/assign/std/map.hpp>
-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 +50,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 =
       "<div>\n"
       "<table class='DiscreteValues'>\n"
@@ -64,7 +71,7 @@ TEST(DiscreteValues, htmlWithValueFormatter) {
       "</div>";
   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);
 }
 

gtsam/discrete/tests/testSignature.cpp

@@ -21,12 +21,10 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/Signature.h>
 
-#include <boost/assign/std/vector.hpp>
 #include <vector>
 
 using namespace std;
 using namespace gtsam;
-using namespace boost::assign;
 
 DiscreteKey X(0, 2), Y(1, 3), Z(2, 2);
 
@@ -57,12 +55,8 @@ TEST(testSignature, simple_conditional) {
 
 /* ************************************************************************* */
 TEST(testSignature, simple_conditional_nonparser) {
-  Signature::Table table;
+  Signature::Row row1{1, 1}, row2{2, 3}, row3{1, 4};
-  Signature::Row row1, row2, row3;
+  Signature::Table table{row1, row2, row3};
-  row1 += 1.0, 1.0;
-  row2 += 2.0, 3.0;
-  row3 += 1.0, 4.0;
-  table += row1, row2, row3;
 
   Signature sig(X | Y = table);
   CHECK(sig.key() == X);

gtsam/hybrid/GaussianMixture.cpp

@@ -210,7 +210,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
           DiscreteValues::CartesianProduct(set_diff);
       for (const DiscreteValues &assignment : assignments) {
         DiscreteValues augmented_values(values);
-        augmented_values.insert(assignment.begin(), assignment.end());
+        augmented_values.insert(assignment);
 
         // If any one of the sub-branches are non-zero,
         // we need this conditional.

gtsam/hybrid/HybridBayesNet.cpp

@@ -112,7 +112,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
           DiscreteValues::CartesianProduct(set_diff);
       for (const DiscreteValues &assignment : assignments) {
         DiscreteValues augmented_values(values);
-        augmented_values.insert(assignment.begin(), assignment.end());
+        augmented_values.insert(assignment);
 
         // If any one of the sub-branches are non-zero,
         // we need this probability.

gtsam/hybrid/HybridValues.h

@@ -104,6 +104,28 @@ class GTSAM_EXPORT HybridValues {
    * @param j The index with which the value will be associated. */
   void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
 
+  /** Insert all continuous values from \c values.  Throws an invalid_argument
+   * exception if any keys to be inserted are already used. */
+  HybridValues& insert(const VectorValues& values) {
+    continuous_.insert(values);
+    return *this;
+  }
+
+  /** Insert all discrete values from \c values.  Throws an invalid_argument
+   * exception if any keys to be inserted are already used. */
+  HybridValues& insert(const DiscreteValues& values) {
+    discrete_.insert(values);
+    return *this;
+  }
+
+  /** Insert all values from \c values.  Throws an invalid_argument exception if
+   * any keys to be inserted are already used. */
+  HybridValues& insert(const HybridValues& values) {
+    continuous_.insert(values.continuous());
+    discrete_.insert(values.discrete());
+    return *this;
+  }
+
   // TODO(Shangjie)- insert_or_assign() , similar to Values.h
 
   /**
@@ -118,10 +140,33 @@ class GTSAM_EXPORT HybridValues {
    */
   Vector& at(Key j) { return continuous_.at(j); };
 
-  /** For all key/value pairs in \c values, replace values with corresponding keys in this class
+  /** For all key/value pairs in \c values, replace continuous values with
-  *   with those in \c values.  Throws std::out_of_range if any keys in \c values are not present
+   * corresponding keys in this object with those in \c values.  Throws
-  *   in this class. */
+   * std::out_of_range if any keys in \c values are not present in this object.
-  void update(const VectorValues& values) { continuous_.update(values); }
+   */
+  HybridValues& update(const VectorValues& values) {
+    continuous_.update(values);
+    return *this;
+  }
+
+  /** For all key/value pairs in \c values, replace discrete 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.
+   */
+  HybridValues& update(const DiscreteValues& values) {
+    discrete_.update(values);
+    return *this;
+  }
+
+  /** For all key/value pairs in \c values, replace all 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.
+   */
+  HybridValues& update(const HybridValues& values) {
+    continuous_.update(values.continuous());
+    discrete_.update(values.discrete());
+    return *this;
+  }
 
   /// @}
   /// @name Wrapper support

gtsam/hybrid/hybrid.i

@@ -15,9 +15,18 @@ class HybridValues {
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::HybridValues& other, double tol) const;
+  
   void insert(gtsam::Key j, int value);
   void insert(gtsam::Key j, const gtsam::Vector& value);
+  
+  void insert(const gtsam::VectorValues& values);
+  void insert(const gtsam::DiscreteValues& values);
+  void insert(const gtsam::HybridValues& values);
+  
   void update(const gtsam::VectorValues& values);
+  void update(const gtsam::DiscreteValues& values);
+  void update(const gtsam::HybridValues& values);
+  
   size_t& atDiscrete(gtsam::Key j);
   gtsam::Vector& at(gtsam::Key j);
 };

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -237,8 +237,7 @@ TEST(HybridBayesNet, Error) {
   EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-9));
 
   // Verify error computation and check for specific error value
-  DiscreteValues discrete_values;
+  DiscreteValues discrete_values {{M(0), 1}, {M(1), 1}};
-  boost::assign::insert(discrete_values)(M(0), 1)(M(1), 1);
 
   double total_error = 0;
   for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -40,7 +40,6 @@
 #include <gtsam/linear/JacobianFactor.h>
 
 #include <algorithm>
-#include <boost/assign/std/map.hpp>
 #include <cstddef>
 #include <functional>
 #include <iostream>
@@ -49,8 +48,6 @@
 
 #include "Switching.h"
 
-using namespace boost::assign;
-
 using namespace std;
 using namespace gtsam;
 

gtsam/hybrid/tests/testHybridValues.cpp

@@ -32,22 +32,45 @@
 using namespace std;
 using namespace gtsam;
 
-TEST(HybridValues, basics) {
+static const HybridValues kExample{{{99, Vector2(2, 3)}}, {{100, 3}}};
+
+/* ************************************************************************* */
+TEST(HybridValues, Basics) {
   HybridValues values;
   values.insert(99, Vector2(2, 3));
   values.insert(100, 3);
+  EXPECT(assert_equal(kExample, values));
   EXPECT(assert_equal(values.at(99), Vector2(2, 3)));
   EXPECT(assert_equal(values.atDiscrete(100), int(3)));
 
-  values.print();
-
   HybridValues values2;
   values2.insert(100, 3);
   values2.insert(99, Vector2(2, 3));
-  EXPECT(assert_equal(values2, values));
+  EXPECT(assert_equal(kExample, values2));
+}
 
-  values2.insert(98, Vector2(2, 3));
+/* ************************************************************************* */
-  EXPECT(!assert_equal(values2, values));
+// Check insert
+TEST(HybridValues, Insert) {
+  HybridValues actual;
+  EXPECT(assert_equal({{}, {{100, 3}}},  //
+                      actual.insert(DiscreteValues{{100, 3}})));
+  EXPECT(assert_equal(kExample,  //
+                      actual.insert(VectorValues{{99, Vector2(2, 3)}})));
+  HybridValues actual2;
+  EXPECT(assert_equal(kExample, actual2.insert(kExample)));
+}
+
+/* ************************************************************************* */
+// Check update.
+TEST(HybridValues, Update) {
+  HybridValues actual(kExample);
+  EXPECT(assert_equal({{{99, Vector2(2, 3)}}, {{100, 2}}},
+                      actual.update(DiscreteValues{{100, 2}})));
+  EXPECT(assert_equal({{{99, Vector1(4)}}, {{100, 2}}},
+                      actual.update(VectorValues{{99, Vector1(4)}})));
+  HybridValues actual2(kExample);
+  EXPECT(assert_equal(kExample, actual2.update(kExample)));
 }
 
 /* ************************************************************************* */

gtsam/linear/VectorValues.cpp

@@ -98,30 +98,34 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  void VectorValues::update(const VectorValues& values)
+  VectorValues& VectorValues::update(const VectorValues& values) {
-  {
     iterator hint = begin();
-    for(const KeyValuePair& key_value: values)
+    for (const KeyValuePair& key_value : values) {
-    {
+      // Use this trick to find the value using a hint, since we are inserting
-      // Use this trick to find the value using a hint, since we are inserting from another sorted map
+      // from another sorted map
       size_t oldSize = values_.size();
       hint = values_.insert(hint, key_value);
-      if(values_.size() > oldSize) {
+      if (values_.size() > oldSize) {
         values_.unsafe_erase(hint);
-        throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
+        throw out_of_range(
+            "Requested to update a VectorValues with another VectorValues that "
+            "contains keys not present in the first.");
       } else {
         hint->second = key_value.second;
       }
     }
+    return *this;
   }
 
   /* ************************************************************************ */
-  void VectorValues::insert(const VectorValues& values)
+  VectorValues& VectorValues::insert(const VectorValues& values) {
-  {
     size_t originalSize = size();
     values_.insert(values.begin(), values.end());
-    if(size() != originalSize + values.size())
+    if (size() != originalSize + values.size())
-      throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
+      throw invalid_argument(
+          "Requested to insert a VectorValues into another VectorValues that "
+          "already contains one or more of its keys.");
+    return *this;
   }
 
   /* ************************************************************************ */

gtsam/linear/VectorValues.h

@@ -88,11 +88,16 @@ namespace gtsam {
     /// @name Standard Constructors
     /// @{
 
-    /**
+    /// Default constructor creates an empty VectorValues.
-     * Default constructor creates an empty VectorValues.
-     */
     VectorValues() {}
 
+    /// Construct from initializer list.
+    VectorValues(std::initializer_list<std::pair<Key, Vector>> init) {
+      for (const auto& p : init) {
+        values_.insert(p);  // Insert key-value pair into map
+      }
+    }
+
     /** Merge two VectorValues into one, this is more efficient than inserting
      * elements one by one. */
     VectorValues(const VectorValues& first, const VectorValues& second);
@@ -167,7 +172,7 @@ namespace gtsam {
     /** For all key/value pairs in \c values, replace values with corresponding keys in this class
     *   with those in \c values.  Throws std::out_of_range if any keys in \c values are not present
     *   in this class. */
-    void update(const VectorValues& values);
+    VectorValues& update(const VectorValues& values);
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
@@ -198,7 +203,7 @@ namespace gtsam {
 
     /** Insert all values from \c values.  Throws an invalid_argument exception if any keys to be
      *  inserted are already used. */
-    void insert(const VectorValues& values);
+    VectorValues& insert(const VectorValues& values);
 
     /** insert that mimics the STL map insert - if the value already exists, the map is not modified
      *  and an iterator to the existing value is returned, along with 'false'.  If the value did not

gtsam/linear/linear.i

@@ -237,6 +237,7 @@ class VectorValues {
   void insert(size_t j, Vector value);
   Vector vector() const;
   Vector at(size_t j) const;
+  void insert(const gtsam::VectorValues& values);
   void update(const gtsam::VectorValues& values);
 
   //Advanced Interface

gtsam/linear/tests/testVectorValues.cpp

@@ -73,6 +73,22 @@ TEST(VectorValues, basics)
   CHECK_EXCEPTION(actual.dim(3), out_of_range);
 }
 
+/* ************************************************************************* */
+
+static const VectorValues kExample = {{99, Vector2(2, 3)}};
+
+// Check insert
+TEST(VectorValues, Insert) {
+  VectorValues actual;
+  EXPECT(assert_equal(kExample, actual.insert(kExample)));
+}
+
+// Check update.
+TEST(VectorValues, Update) {
+  VectorValues actual(kExample);
+  EXPECT(assert_equal(kExample, actual.update(kExample)));
+}
+
 /* ************************************************************************* */
 TEST(VectorValues, combine)
 {

gtsam_unstable/discrete/examples/schedulingExample.cpp

@@ -12,14 +12,11 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/assign/std/vector.hpp>
-#include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
 #include <boost/format.hpp>
 
 #include <algorithm>
 
-using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 

gtsam_unstable/discrete/examples/schedulingQuals12.cpp

@@ -12,14 +12,11 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/assign/std/vector.hpp>
-#include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
 #include <boost/format.hpp>
 
 #include <algorithm>
 
-using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 

gtsam_unstable/discrete/examples/schedulingQuals13.cpp

@@ -12,14 +12,11 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/assign/std/vector.hpp>
-#include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
 #include <boost/format.hpp>
 
 #include <algorithm>
 
-using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 

gtsam_unstable/discrete/tests/testCSP.cpp

@@ -8,8 +8,6 @@
 #include <gtsam_unstable/discrete/CSP.h>
 #include <gtsam_unstable/discrete/Domain.h>
 
-#include <boost/assign/std/map.hpp>
-using boost::assign::insert;
 #include <CppUnitLite/TestHarness.h>
 
 #include <fstream>
@@ -133,8 +131,7 @@ TEST(CSP, allInOne) {
 
   // Solve
   auto mpe = csp.optimize();
-  DiscreteValues expected;
+  DiscreteValues expected {{ID.first, 1}, {UT.first, 0}, {AZ.first, 1}};
-  insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
   EXPECT(assert_equal(expected, mpe));
   EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9);
 }
@@ -172,8 +169,8 @@ TEST(CSP, WesternUS) {
   csp.addAllDiff(WY, CO);
   csp.addAllDiff(CO, NM);
 
-  DiscreteValues mpe;
+  DiscreteValues mpe{{0, 2}, {1, 3}, {2, 2}, {3, 1}, {4, 1}, {5, 3},
-  insert(mpe)(0, 2)(1, 3)(2, 2)(3, 1)(4, 1)(5, 3)(6, 3)(7, 2)(8, 0)(9, 1)(10, 0);
+                     {6, 3}, {7, 2}, {8, 0}, {9, 1}, {10, 0}};
 
   // Create ordering according to example in ND-CSP.lyx
   Ordering ordering;
@@ -224,8 +221,7 @@ TEST(CSP, ArcConsistency) {
 
   // Solve
   auto mpe = csp.optimize();
-  DiscreteValues expected;
+  DiscreteValues expected {{ID.first, 1}, {UT.first, 0}, {AZ.first, 2}};
-  insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
   EXPECT(assert_equal(expected, mpe));
   EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9);
 

gtsam_unstable/discrete/tests/testScheduler.cpp

@@ -10,11 +10,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam_unstable/discrete/Scheduler.h>
 
-#include <boost/assign/std/map.hpp>
-#include <boost/assign/std/vector.hpp>
 #include <boost/optional.hpp>
 
-using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 

gtsam_unstable/discrete/tests/testSudoku.cpp

@@ -9,8 +9,6 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/discrete/CSP.h>
 
-#include <boost/assign/std/map.hpp>
-using boost::assign::insert;
 #include <stdarg.h>
 
 #include <iostream>
@@ -128,11 +126,14 @@ TEST(Sudoku, small) {
   // optimize and check
   auto solution = csp.optimize();
   DiscreteValues expected;
-  insert(expected)(csp.key(0, 0), 0)(csp.key(0, 1), 1)(csp.key(0, 2), 2)(
+  expected = {{csp.key(0, 0), 0}, {csp.key(0, 1), 1},
-      csp.key(0, 3), 3)(csp.key(1, 0), 2)(csp.key(1, 1), 3)(csp.key(1, 2), 0)(
+              {csp.key(0, 2), 2}, {csp.key(0, 3), 3},  //
-      csp.key(1, 3), 1)(csp.key(2, 0), 3)(csp.key(2, 1), 2)(csp.key(2, 2), 1)(
+              {csp.key(1, 0), 2}, {csp.key(1, 1), 3},
-      csp.key(2, 3), 0)(csp.key(3, 0), 1)(csp.key(3, 1), 0)(csp.key(3, 2), 3)(
+              {csp.key(1, 2), 0}, {csp.key(1, 3), 1},  //
-      csp.key(3, 3), 2);
+              {csp.key(2, 0), 3}, {csp.key(2, 1), 2},
+              {csp.key(2, 2), 1}, {csp.key(2, 3), 0},  //
+              {csp.key(3, 0), 1}, {csp.key(3, 1), 0},
+              {csp.key(3, 2), 3}, {csp.key(3, 3), 2}};
   EXPECT(assert_equal(expected, solution));
   // csp.printAssignment(solution);