Merge pull request #1594 from borglab/ordering

borglab · Jul 28, 2023 · 621ef2e · 621ef2e
2 parents 19c3939 + b51ff74
commit 621ef2e
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 1 deletion.
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
@@ -77,6 +77,18 @@ class GTSAM_EXPORT DiscreteConditional
                       const Signature::Table& table)
       : DiscreteConditional(Signature(key, parents, table)) {}
 
+  /**
+   * Construct from key, parents, and a vector<double> specifying the
+   * conditional probability table (CPT) in 00 01 10 11 order. For
+   * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
+   *
+   * Example: DiscreteConditional P(D, {B,E}, table);
+   */
+  DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+                      const std::vector<double>& table)
+      : DiscreteConditional(1, DiscreteKeys{key} & parents,
+                            ADT(DiscreteKeys{key} & parents, table)) {}
+
   /**
    * Construct from key, parents, and a string specifying the conditional
    * probability table (CPT) in 00 01 10 11 order. For three-valued, it would

diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h
@@ -74,6 +74,12 @@ namespace gtsam {
       return *this;
     }
 
+    /// Add multiple keys (non-const!)
+    DiscreteKeys& operator&(const DiscreteKeys& keys) {
+      this->insert(this->end(), keys.begin(), keys.end());
+      return *this;
+    }
+
     /// Print the keys and cardinalities.
     void print(const std::string& s = "",
                const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp
@@ -46,6 +46,11 @@ TEST(DiscreteConditional, constructors) {
   DiscreteConditional actual2(1, f2);
   DecisionTreeFactor expected2 = f2 / *f2.sum(1);
   EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
+
+  std::vector<double> probs{0.2, 0.5, 0.3, 0.6, 0.4, 0.7, 0.25, 0.55, 0.35, 0.65, 0.45, 0.75};
+  DiscreteConditional actual3(X, {Y, Z}, probs);
+  DecisionTreeFactor expected3 = f2;
+  EXPECT(assert_equal(expected3, static_cast<DecisionTreeFactor>(actual3)));
 }
 
 /* ************************************************************************* */

diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp
@@ -219,6 +219,11 @@ TEST(Ordering, AppendVector) {
 
   Ordering expected{X(0), X(1), X(2)};
   EXPECT(assert_equal(expected, actual));
+
+  actual = Ordering();
+  Ordering addl{X(0), X(1), X(2)};
+  actual += addl;
+  EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */

diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -86,7 +86,10 @@ class NonlinearFactorGraph {
                 const gtsam::noiseModel::Base* noiseModel);
 
   // NonlinearFactorGraph
-  void printErrors(const gtsam::Values& values) const;
+  void printErrors(const gtsam::Values& values,
+                   const string& str = "NonlinearFactorGraph: ",
+                   const gtsam::KeyFormatter& keyFormatter =
+                       gtsam::DefaultKeyFormatter) const;
   double error(const gtsam::Values& values) const;
   double probPrime(const gtsam::Values& values) const;
   gtsam::Ordering orderingCOLAMD() const;