From 64cd43636235bfba3cb42ba5a1c61ee89b20c2b0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 15 Jul 2023 16:53:17 -0400
Subject: [PATCH 01/45] update InconsistentEliminationRequested to show
 leftover keys

---
 .../inference/EliminateableFactorGraph-inst.h | 10 +--
 gtsam/inference/inferenceExceptions.h         | 63 ++++++++++++++-----
 .../linear/tests/testGaussianFactorGraph.cpp  | 52 ++++++++++++---
 3 files changed, 95 insertions(+), 30 deletions(-)

diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h
index 8a524e353..f6b11f785 100644
--- a/gtsam/inference/EliminateableFactorGraph-inst.h
+++ b/gtsam/inference/EliminateableFactorGraph-inst.h
@@ -74,8 +74,9 @@ namespace gtsam {
       EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
       const auto [bayesNet, factorGraph] = etree.eliminate(function);
       // If any factors are remaining, the ordering was incomplete
-      if(!factorGraph->empty())
-        throw InconsistentEliminationRequested();
+      if(!factorGraph->empty()) {
+        throw InconsistentEliminationRequested(factorGraph->keys());
+      }
       // Return the Bayes net
       return bayesNet;
     }
@@ -136,8 +137,9 @@ namespace gtsam {
       JunctionTreeType junctionTree(etree);
       const auto [bayesTree, factorGraph] = junctionTree.eliminate(function);
       // If any factors are remaining, the ordering was incomplete
-      if(!factorGraph->empty())
-        throw InconsistentEliminationRequested();
+      if(!factorGraph->empty()) {
+        throw InconsistentEliminationRequested(factorGraph->keys());
+      }
       // Return the Bayes tree
       return bayesTree;
     }
diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h
index fa3e3cb25..061abeb85 100644
--- a/gtsam/inference/inferenceExceptions.h
+++ b/gtsam/inference/inferenceExceptions.h
@@ -12,30 +12,59 @@
 /**
  * @file    inferenceExceptions.h
  * @brief   Exceptions that may be thrown by inference algorithms
- * @author  Richard Roberts
+ * @author  Richard Roberts, Varun Agrawal
  * @date    Apr 25, 2013
  */
 #pragma once
 
 #include <gtsam/global_includes.h>
+
 #include <exception>
+#include <sstream>
 
 namespace gtsam {
 
-  /** An inference algorithm was called with inconsistent arguments.  The factor graph, ordering, or
-   *  variable index were inconsistent with each other, or a full elimination routine was called
-   *  with an ordering that does not include all of the variables. */
-  class InconsistentEliminationRequested : public std::exception {
-  public:
-    InconsistentEliminationRequested() noexcept {}
-    ~InconsistentEliminationRequested() noexcept override {}
-    const char* what() const noexcept override {
-      return
-        "An inference algorithm was called with inconsistent arguments.  The\n"
-        "factor graph, ordering, or variable index were inconsistent with each\n"
-        "other, or a full elimination routine was called with an ordering that\n"
-        "does not include all of the variables.";
-    }
-  };
+/** An inference algorithm was called with inconsistent arguments.  The factor
+ * graph, ordering, or variable index were inconsistent with each other, or a
+ * full elimination routine was called with an ordering that does not include
+ * all of the variables. */
+class InconsistentEliminationRequested : public std::exception {
+  KeySet keys_;
+  const KeyFormatter& keyFormatter = DefaultKeyFormatter;
 
-}
+ public:
+  InconsistentEliminationRequested() noexcept {}
+
+  InconsistentEliminationRequested(
+      const KeySet& keys,
+      const KeyFormatter& key_formatter = DefaultKeyFormatter)
+      : keys_(keys), keyFormatter(key_formatter) {}
+
+  ~InconsistentEliminationRequested() noexcept override {}
+  const char* what() const noexcept override {
+    // Format keys for printing
+    std::stringstream sstr;
+    for (auto key : keys_) {
+      sstr << keyFormatter(key) << ", ";
+    }
+    std::string keys = sstr.str();
+    // remove final comma and space.
+    keys.pop_back();
+    keys.pop_back();
+
+    static std::string msg =
+        "An inference algorithm was called with inconsistent "
+        "arguments.  "
+        "The\n"
+        "factor graph, ordering, or variable index were "
+        "inconsistent with "
+        "each\n"
+        "other, or a full elimination routine was called with "
+        "an ordering "
+        "that\n"
+        "does not include all of the variables.\n";
+    msg += ("Leftover keys after elimination: " + keys);
+    return msg.c_str();
+  }
+};
+}  // namespace gtsam
diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp
index e9e626296..b140c3d85 100644
--- a/gtsam/linear/tests/testGaussianFactorGraph.cpp
+++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp
@@ -18,16 +18,16 @@
  *  @author Richard Roberts
  **/
 
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/inference/VariableSlots.h>
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/base/debug.h>
-#include <gtsam/base/VerticalBlockMatrix.h>
-
-#include <gtsam/base/TestableAssertions.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/inference/VariableSlots.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 using namespace std;
 using namespace gtsam;
@@ -435,6 +435,40 @@ TEST(GaussianFactorGraph, ProbPrime) {
   EXPECT_DOUBLES_EQUAL(expected, gfg.probPrime(values), 1e-12);
 }
 
+TEST(GaussianFactorGraph, InconsistentEliminationMessage) {
+  // Create empty graph
+  GaussianFactorGraph fg;
+  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
+
+  using gtsam::symbol_shorthand::X;
+  fg.emplace_shared<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2),
+                                    unit2);
+  fg.emplace_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
+                                    Vector2(2.0, -1.0), unit2);
+  fg.emplace_shared<JacobianFactor>(1, -5 * I_2x2, 2, 5 * I_2x2,
+                                    Vector2(-1.0, 1.5), unit2);
+  fg.emplace_shared<JacobianFactor>(2, -5 * I_2x2, X(3), 5 * I_2x2,
+                                    Vector2(-1.0, 1.5), unit2);
+
+  Ordering ordering{0, 1};
+
+  try {
+    fg.eliminateSequential(ordering);
+  } catch (const exception& exc) {
+    std::string expected_exception_message = "An inference algorithm was called with inconsistent "
+        "arguments.  "
+        "The\n"
+        "factor graph, ordering, or variable index were "
+        "inconsistent with "
+        "each\n"
+        "other, or a full elimination routine was called with "
+        "an ordering "
+        "that\n"
+        "does not include all of the variables.\n"
+        "Leftover keys after elimination: 2, x3";
+    EXPECT(expected_exception_message == exc.what());
+  }
+}
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From fbbf673f7bdbede4faca53bd309ac41c9ab12777 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Jun 2023 15:40:02 +0100
Subject: [PATCH 02/45] Improved documentation and tests

(cherry picked from commit febeacd68686ed0b7ced72458eb0b31a196bdab7)
---
 gtsam/discrete/AlgebraicDecisionTree.h    | 64 +++++++++++++++++++---
 gtsam/discrete/DecisionTree-inl.h         |  2 +-
 gtsam/discrete/DecisionTree.h             | 31 +++++++++--
 gtsam/discrete/tests/testDecisionTree.cpp | 66 ++++++++++++++++++-----
 4 files changed, 137 insertions(+), 26 deletions(-)

diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index 425ad15f9..20ab4bd68 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -29,9 +29,9 @@
 namespace gtsam {
 
   /**
-   * Algebraic Decision Trees fix the range to double
-   * Just has some nice constructors and some syntactic sugar
-   * TODO: consider eliminating this class altogether?
+   * An algebraic decision tree fixes the range of a DecisionTree to double.
+   * Just has some nice constructors and some syntactic sugar.
+   * TODO(dellaert): consider eliminating this class altogether?
    *
    * @ingroup discrete
    */
@@ -81,20 +81,62 @@ namespace gtsam {
     AlgebraicDecisionTree(const L& label, double y1, double y2)
         : Base(label, y1, y2) {}
 
-    /** Create a new leaf function splitting on a variable */
+    /**
+     * @brief Create a new leaf function splitting on a variable
+     *
+     * @param labelC: The label with cardinality 2
+     * @param y1: The value for the first key
+     * @param y2: The value for the second key
+     *
+     * Example:
+     * @code{.cpp}
+     * std::pair<string, size_t> A {"a", 2};
+     * AlgebraicDecisionTree<string> a(A, 0.6, 0.4);
+     * @endcode
+     */
     AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1,
                           double y2)
         : Base(labelC, y1, y2) {}
 
-    /** Create from keys and vector table */
+    /** 
+     * @brief Create from keys with cardinalities and a vector table
+     * 
+     * @param labelCs: The keys, with cardinalities, given as pairs
+     * @param ys: The vector table
+     *
+     * Example with three keys, A, B, and C, with cardinalities 2, 3, and 2,
+     * respectively, and a vector table of size 12:
+     * @code{.cpp}
+     * DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+     * const vector<double> cpt{
+     *   1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11,  //
+     *   1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10};
+     * AlgebraicDecisionTree<Key> expected(A & B & C, cpt);
+     * @endcode
+     * The table is given in the following order:
+     *   A=0, B=0, C=0
+     *   A=0, B=0, C=1
+     *   ...
+     *   A=1, B=1, C=1
+     * Hence, the first line in the table is for A==0, and the second for A==1.
+     * In each line, the first two entries are for B==0, the next two for B==1,
+     * and the last two for B==2. Each pair is for a C value of 0 and 1.
+     */
     AlgebraicDecisionTree  //
         (const std::vector<typename Base::LabelC>& labelCs,
-        const std::vector<double>& ys) {
+         const std::vector<double>& ys) {
       this->root_ =
           Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end());
     }
 
-    /** Create from keys and string table */
+    /** 
+     * @brief Create from keys and string table
+     * 
+     * @param labelCs: The keys, with cardinalities, given as pairs
+     * @param table: The string table, given as a string of doubles.
+     * 
+     * @note Table needs to be in same order as the vector table in the other constructor.
+     */
     AlgebraicDecisionTree  //
         (const std::vector<typename Base::LabelC>& labelCs,
         const std::string& table) {
@@ -109,7 +151,13 @@ namespace gtsam {
           Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end());
     }
 
-    /** Create a new function splitting on a variable */
+    /** 
+     * @brief Create a range of decision trees, splitting on a single variable.
+     * 
+     * @param begin: Iterator to beginning of a range of decision trees
+     * @param end: Iterator to end of a range of decision trees
+     * @param label: The label to split on
+     */
     template <typename Iterator>
     AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label)
         : Base(nullptr) {
diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 9d618dea0..c99a51501 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -626,7 +626,7 @@ namespace gtsam {
   // B=1
   //  A=0: 3
   //  A=1: 4
-  // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce
+  // Note, through the magic of "compose", create([A B],[1 3 2 4]) will produce
   // exactly the same tree as above: the highest label is always the root.
   // However, it will be *way* faster if labels are given highest to lowest.
   template<typename L, typename Y>
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index ed1908485..68f3ee73f 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -39,9 +39,23 @@
 namespace gtsam {
 
   /**
-   * Decision Tree
-   * L = label for variables
-   * Y = function range (any algebra), e.g., bool, int, double
+   * @brief a decision tree is a function from assignments to values.
+   * @tparam L label for variables
+   * @tparam Y function range (any algebra), e.g., bool, int, double
+   * 
+   * After creating a decision tree on some variables, the tree can be evaluated
+   * on an assignment to those variables. Example:
+   * 
+   * @code{.cpp}
+   * // Create a decision stump one one variable 'a' with values 10 and 20.
+   * DecisionTree<char, int> tree('a', 10, 20);
+   * 
+   * // Evaluate the tree on an assignment to the variable.
+   * int value0 = tree({{'a', 0}}); // value0 = 10
+   * int value1 = tree({{'a', 1}}); // value1 = 20
+   * @endcode
+   * 
+   * More examples can be found in testDecisionTree.cpp
    *
    * @ingroup discrete
    */
@@ -136,7 +150,8 @@ namespace gtsam {
     NodePtr root_;
 
    protected:
-    /** Internal recursive function to create from keys, cardinalities, 
+    /** 
+     * Internal recursive function to create from keys, cardinalities, 
      * and Y values 
      */
     template<typename It, typename ValueIt>
@@ -167,7 +182,13 @@ namespace gtsam {
     /** Create a constant */
     explicit DecisionTree(const Y& y);
 
-    /// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
+    /**
+     * @brief Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
+     * 
+     * @param label The variable to split on.
+     * @param y1 The value for the first assignment.
+     * @param y2 The value for the second assignment.
+     */
     DecisionTree(const L& label, const Y& y1, const Y& y2);
 
     /** Allow Label+Cardinality for convenience */
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index d2a94ddc3..6ca8d843e 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -75,6 +75,19 @@ struct traits<CrazyDecisionTree> : public Testable<CrazyDecisionTree> {};
 
 GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree)
 
+/* ************************************************************************** */
+// Test char labels and int range
+/* ************************************************************************** */
+
+// Create a decision stump one one variable 'a' with values 10 and 20.
+TEST(DecisionTree, constructor) {
+  DecisionTree<char, int> tree('a', 10, 20);
+
+  // Evaluate the tree on an assignment to the variable.
+  EXPECT_LONGS_EQUAL(10, tree({{'a', 0}}));
+  EXPECT_LONGS_EQUAL(20, tree({{'a', 1}}));
+}
+
 /* ************************************************************************** */
 // Test string labels and int range
 /* ************************************************************************** */
@@ -118,18 +131,47 @@ struct Ring {
   static inline int mul(const int& a, const int& b) { return a * b; }
 };
 
+/* ************************************************************************** */
+// Check that creating decision trees respects key order.
+TEST(DecisionTree, constructor_order) {
+  // Create labels
+  string A("A"), B("B");
+
+  const std::vector<int> ys1 = {1, 2, 3, 4};
+  DT tree1({{B, 2}, {A, 2}}, ys1); // faster version, as B is "higher" than A!
+
+  const std::vector<int> ys2 = {1, 3, 2, 4};
+  DT tree2({{A, 2}, {B, 2}}, ys2); // slower version !
+
+  // Both trees will be the same, tree is order from high to low labels.
+  // Choice(B)
+  //  0 Choice(A)
+  //  0 0 Leaf 1
+  //  0 1 Leaf 2
+  //  1 Choice(A)
+  //  1 0 Leaf 3
+  //  1 1 Leaf 4
+
+  EXPECT(tree2.equals(tree1));
+
+  // Check the values are as expected by calling the () operator:
+  EXPECT_LONGS_EQUAL(1, tree1({{A, 0}, {B, 0}}));
+  EXPECT_LONGS_EQUAL(3, tree1({{A, 0}, {B, 1}}));
+  EXPECT_LONGS_EQUAL(2, tree1({{A, 1}, {B, 0}}));
+  EXPECT_LONGS_EQUAL(4, tree1({{A, 1}, {B, 1}}));
+}
+
 /* ************************************************************************** */
 // test DT
 TEST(DecisionTree, example) {
   // Create labels
   string A("A"), B("B"), C("C");
 
-  // create a value
-  Assignment<string> x00, x01, x10, x11;
-  x00[A] = 0, x00[B] = 0;
-  x01[A] = 0, x01[B] = 1;
-  x10[A] = 1, x10[B] = 0;
-  x11[A] = 1, x11[B] = 1;
+  // Create assignments using brace initialization:
+  Assignment<string> x00{{A, 0}, {B, 0}};
+  Assignment<string> x01{{A, 0}, {B, 1}};
+  Assignment<string> x10{{A, 1}, {B, 0}};
+  Assignment<string> x11{{A, 1}, {B, 1}};
 
   // empty
   DT empty;
@@ -241,8 +283,7 @@ TEST(DecisionTree, ConvertValuesOnly) {
   StringBoolTree f2(f1, bool_of_int);
 
   // Check a value
-  Assignment<string> x00;
-  x00["A"] = 0, x00["B"] = 0;
+  Assignment<string> x00 {{A, 0}, {B, 0}};
   EXPECT(!f2(x00));
 }
 
@@ -266,10 +307,11 @@ TEST(DecisionTree, ConvertBoth) {
 
   // Check some values
   Assignment<Label> x00, x01, x10, x11;
-  x00[X] = 0, x00[Y] = 0;
-  x01[X] = 0, x01[Y] = 1;
-  x10[X] = 1, x10[Y] = 0;
-  x11[X] = 1, x11[Y] = 1;
+  x00 = {{X, 0}, {Y, 0}};
+  x01 = {{X, 0}, {Y, 1}};
+  x10 = {{X, 1}, {Y, 0}};
+  x11 = {{X, 1}, {Y, 1}};
+
   EXPECT(!f2(x00));
   EXPECT(!f2(x01));
   EXPECT(f2(x10));

From 7f169a7a0cf1e77c5d460a39ee1870d502dd0178 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Jun 2023 16:04:18 +0100
Subject: [PATCH 03/45] Better documentation and new test

(cherry picked from commit 6cb0fa7cd7f9d981b843945e8f71183d611412aa)
---
 gtsam/discrete/DecisionTreeFactor.h           | 41 +++++++++++++++++--
 .../discrete/tests/testDecisionTreeFactor.cpp | 25 +++++++----
 2 files changed, 55 insertions(+), 11 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 95054bcdb..92b02afd1 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -63,11 +63,46 @@ namespace gtsam {
     /** Constructor from DiscreteKeys and AlgebraicDecisionTree */
     DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
 
-    /** Constructor from doubles */
+    /**
+     * @brief Constructor from doubles
+     *
+     * @param keys The discrete keys.
+     * @param table The table of values.
+     *
+     * @throw std::invalid_argument if the size of `table` does not match the
+     * number of assignments.
+     *
+     * Example:
+     * @code{.cpp}
+     * DiscreteKey X(0,2), Y(1,3);
+     * const std::vector<double> table {2, 5, 3, 6, 4, 7};
+     * DecisionTreeFactor f1({X, Y}, table);
+     * @endcode
+     *
+     * The values in the table should be laid out so that the first key varies
+     * the slowest. and the last key the fastest.
+     */
     DecisionTreeFactor(const DiscreteKeys& keys,
-                      const std::vector<double>& table);
+                       const std::vector<double>& table);
 
-    /** Constructor from string */
+    /**
+     * @brief Constructor from string
+     *
+     * @param keys The discrete keys.
+     * @param table The table of values.
+     *
+     * @throw std::invalid_argument if the size of `table` does not match the
+     * number of assignments.
+     *
+     * Example:
+     * @code{.cpp}
+     * DiscreteKey X(0,2), Y(1,3);
+     * DecisionTreeFactor factor({X, Y}, "2 5 3 6 4 7");
+     * @endcode
+     *
+     * The values in the table should be laid out so that the first key varies
+     * the slowest. and the last key the fastest.
+     */
     DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
 
     /// Single-key specialization
diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 3dbb3e64f..8e203c56a 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -27,6 +27,18 @@
 using namespace std;
 using namespace gtsam;
 
+/* ************************************************************************* */
+TEST(DecisionTreeFactor, constructors_match) {
+  // Declare two keys
+  DiscreteKey X(0, 2), Y(1, 3);
+
+  // Create with vector and with string
+  const std::vector<double> table {2, 5, 3, 6, 4, 7};
+  DecisionTreeFactor f1({X, Y}, table);
+  DecisionTreeFactor f2({X, Y}, "2 5 3 6 4 7");
+  EXPECT(assert_equal(f1, f2));
+}
+
 /* ************************************************************************* */
 TEST( DecisionTreeFactor, constructors)
 {
@@ -41,16 +53,13 @@ TEST( DecisionTreeFactor, constructors)
   EXPECT_LONGS_EQUAL(2,f2.size());
   EXPECT_LONGS_EQUAL(3,f3.size());
 
-  DiscreteValues values;
-  values[0] = 1; // x
-  values[1] = 2; // y
-  values[2] = 1; // z
-  EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9);
-  EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9);
-  EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9);
+  DiscreteValues x121{{0, 1}, {1, 2}, {2, 1}};
+  EXPECT_DOUBLES_EQUAL(8, f1(x121), 1e-9);
+  EXPECT_DOUBLES_EQUAL(7, f2(x121), 1e-9);
+  EXPECT_DOUBLES_EQUAL(75, f3(x121), 1e-9);
 
   // Assert that error = -log(value)
-  EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9);
+  EXPECT_DOUBLES_EQUAL(-log(f1(x121)), f1.error(x121), 1e-9);
 }
 
 /* ************************************************************************* */

From 968a72344c65a11c4dd7725a7c151d4e0abba0e2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 16 Jul 2023 22:01:38 +0200
Subject: [PATCH 04/45] Fixed regression test

---
 python/gtsam/tests/test_VisualISAMExample.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py
index 39f563a35..dcf1af6f2 100644
--- a/python/gtsam/tests/test_VisualISAMExample.py
+++ b/python/gtsam/tests/test_VisualISAMExample.py
@@ -84,7 +84,7 @@ class TestVisualISAMExample(GtsamTestCase):
 
             values.insert(key, v)
 
-        self.assertAlmostEqual(isam.error(values), 34212421.14731998)
+        self.assertAlmostEqual(isam.error(values), 34212421.14732)
 
     def test_isam2_update(self):
         """

From d5f62dd4ee0e3fdd011b1fa7e3118ab3bfbb5df5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Jun 2023 16:27:28 +0100
Subject: [PATCH 05/45] wrapped List[float] constructor

(cherry picked from commit e1802814ca9c2be20208a9dd4e86093985a7291f)
---
 gtsam/discrete/discrete.i                     | 11 ++++++++---
 python/gtsam/tests/test_DecisionTreeFactor.py |  6 ++++++
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 78efd57e2..a89d71b24 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -44,14 +44,19 @@ class DiscreteFactor {
 #include <gtsam/discrete/DecisionTreeFactor.h>
 virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
   DecisionTreeFactor();
-  
+
   DecisionTreeFactor(const gtsam::DiscreteKey& key,
                      const std::vector<double>& spec);
   DecisionTreeFactor(const gtsam::DiscreteKey& key, string table);
-  
+
+  DecisionTreeFactor(const gtsam::DiscreteKeys& keys,
+                     const std::vector<double>& table);
   DecisionTreeFactor(const gtsam::DiscreteKeys& keys, string table);
+
+  DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys,
+                     const std::vector<double>& table);
   DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table);
-  
+
   DecisionTreeFactor(const gtsam::DiscreteConditional& c);
   
   void print(string s = "DecisionTreeFactor\n",
diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py
index 1b8598953..12308bb3c 100644
--- a/python/gtsam/tests/test_DecisionTreeFactor.py
+++ b/python/gtsam/tests/test_DecisionTreeFactor.py
@@ -26,7 +26,13 @@ class TestDecisionTreeFactor(GtsamTestCase):
         self.B = (5, 2)
         self.factor = DecisionTreeFactor([self.A, self.B], "1 2  3 4  5 6")
 
+    def test_from_floats(self):
+        """Test whether we can construct a factor from floats."""
+        actual = DecisionTreeFactor([self.A, self.B], [1., 2.,  3., 4.,  5., 6.])
+        self.gtsamAssertEquals(actual, self.factor)
+
     def test_enumerate(self):
+        """Test whether we can enumerate the factor."""
         actual = self.factor.enumerate()
         _, values = zip(*actual)
         self.assertEqual(list(values), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0])

From 67faa5c09e5ed488ab6a2899660c59f4fe030606 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Jun 2023 22:38:56 +0100
Subject: [PATCH 06/45] Derive factors from gtsam::Factor

(cherry picked from commit 40e5a1a6abb6cc4e937dc8e876d627835d7b2313)
---
 gtsam/discrete/discrete.i   |  6 +++---
 gtsam/hybrid/hybrid.i       |  5 +----
 gtsam/inference/inference.i | 11 +++++++++++
 gtsam/linear/linear.i       | 10 +---------
 gtsam/nonlinear/nonlinear.i |  5 +----
 gtsam/symbolic/symbolic.i   |  7 ++++---
 6 files changed, 21 insertions(+), 23 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index a89d71b24..b6c6ee2cd 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -31,13 +31,11 @@ string html(const gtsam::DiscreteValues& values,
             std::map<gtsam::Key, std::vector<std::string>> names);
 
 #include <gtsam/discrete/DiscreteFactor.h>
-class DiscreteFactor {
+virtual class DiscreteFactor : gtsam::Factor {
   void print(string s = "DiscreteFactor\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const;
-  bool empty() const;
-  size_t size() const;
   double operator()(const gtsam::DiscreteValues& values) const;
 };
 
@@ -225,6 +223,8 @@ class DiscreteBayesTree {
   bool empty() const;
   const DiscreteBayesTreeClique* operator[](size_t j) const;
 
+  double evaluate(const gtsam::DiscreteValues& values) const;
+
   string dot(const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   void saveGraph(string s,
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 7cc93d4b0..2855dfb5e 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -35,14 +35,11 @@ class HybridValues {
 };
 
 #include <gtsam/hybrid/HybridFactor.h>
-virtual class HybridFactor {
+virtual class HybridFactor : gtsam::Factor {
   void print(string s = "HybridFactor\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const;
-  bool empty() const;
-  size_t size() const;
-  gtsam::KeyVector keys() const;
 
   // Standard interface:
   double error(const gtsam::HybridValues &values) const;
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index f9696258a..e4eeaa480 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -194,4 +194,15 @@ class VariableIndex {
   size_t nEntries() const;
 };
 
+#include <gtsam/inference/Factor.h>
+virtual class Factor {
+  void print(string s = "Factor\n", const gtsam::KeyFormatter& keyFormatter =
+                                        gtsam::DefaultKeyFormatter) const;
+  void printKeys(string s = "") const;
+  bool equals(const gtsam::Factor& other, double tol = 1e-9) const;
+  bool empty() const;
+  size_t size() const;
+  gtsam::KeyVector keys() const;
+};
+
 }  // namespace gtsam
diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index c0230f1c2..cd5000d9f 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -261,8 +261,7 @@ class VectorValues {
 };
 
 #include <gtsam/linear/GaussianFactor.h>
-virtual class GaussianFactor {
-  gtsam::KeyVector keys() const;
+virtual class GaussianFactor : gtsam::Factor {
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::GaussianFactor& lf, double tol) const;
@@ -273,8 +272,6 @@ virtual class GaussianFactor {
   Matrix information() const;
   Matrix augmentedJacobian() const;
   pair<Matrix, Vector> jacobian() const;
-  size_t size() const;
-  bool empty() const;
 };
 
 #include <gtsam/linear/JacobianFactor.h>
@@ -301,10 +298,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   //Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  void printKeys(string s) const;
-  gtsam::KeyVector& keys() const;
   bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  size_t size() const;
   Vector unweighted_error(const gtsam::VectorValues& c) const;
   Vector error_vector(const gtsam::VectorValues& c) const;
   double error(const gtsam::VectorValues& c) const;
@@ -346,10 +340,8 @@ virtual class HessianFactor : gtsam::GaussianFactor {
   HessianFactor(const gtsam::GaussianFactorGraph& factors);
 
   //Testable
-  size_t size() const;
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  void printKeys(string s) const;
   bool equals(const gtsam::GaussianFactor& lf, double tol) const;
   double error(const gtsam::VectorValues& c) const;
 
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 4d0d847c1..06451ab1f 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -109,13 +109,10 @@ class NonlinearFactorGraph {
 };
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-virtual class NonlinearFactor {
+virtual class NonlinearFactor : gtsam::Factor {
   // Factor base class
-  size_t size() const;
-  gtsam::KeyVector keys() const;
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  void printKeys(string s) const;
   // NonlinearFactor
   bool equals(const gtsam::NonlinearFactor& other, double tol) const;
   double error(const gtsam::Values& c) const;
diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i
index 1f1d4b48f..839e75672 100644
--- a/gtsam/symbolic/symbolic.i
+++ b/gtsam/symbolic/symbolic.i
@@ -4,7 +4,7 @@
 namespace gtsam {
 
 #include <gtsam/symbolic/SymbolicFactor.h>
-virtual class SymbolicFactor {
+virtual class SymbolicFactor : gtsam::Factor {
   // Standard Constructors and Named Constructors
   SymbolicFactor(const gtsam::SymbolicFactor& f);
   SymbolicFactor();
@@ -18,12 +18,10 @@ virtual class SymbolicFactor {
   static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js);
 
   // From Factor
-  size_t size() const;
   void print(string s = "SymbolicFactor",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::SymbolicFactor& other, double tol) const;
-  gtsam::KeyVector keys();
 };
 
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
@@ -161,6 +159,9 @@ class SymbolicBayesTree {
   gtsam::SymbolicConditional* marginalFactor(size_t key) const;
   gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
   gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
+
+  string dot(const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
 };
 
 class SymbolicBayesTreeClique {

From ea12d0dac9b4191f6779fb3c4392ea48eb30ae79 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Jun 2023 22:39:14 +0100
Subject: [PATCH 07/45] Test more things in BayesTree

(cherry picked from commit c8fe2fcff292fcd0917d76f916e83efe9c4df7e4)
---
 python/gtsam/tests/test_DiscreteBayesTree.py | 36 +++++++++++++++++---
 1 file changed, 31 insertions(+), 5 deletions(-)

diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py
index b1ed4fe69..e1754ca64 100644
--- a/python/gtsam/tests/test_DiscreteBayesTree.py
+++ b/python/gtsam/tests/test_DiscreteBayesTree.py
@@ -13,10 +13,17 @@ Author: Frank Dellaert
 
 import unittest
 
-from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique,
-                   DiscreteConditional, DiscreteFactorGraph, Ordering)
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import (
+    DiscreteBayesNet,
+    DiscreteBayesTreeClique,
+    DiscreteConditional,
+    DiscreteFactorGraph,
+    DiscreteValues,
+    Ordering,
+)
+
 
 class TestDiscreteBayesNet(GtsamTestCase):
     """Tests for Discrete Bayes Nets."""
@@ -65,15 +72,34 @@ class TestDiscreteBayesNet(GtsamTestCase):
         #     bayesTree[key].printSignature()
         # bayesTree.saveGraph("test_DiscreteBayesTree.dot")
 
-        self.assertFalse(bayesTree.empty())
-        self.assertEqual(12, bayesTree.size())
-
         # The root is P( 8 12 14), we can retrieve it by key:
         root = bayesTree[8]
         self.assertIsInstance(root, DiscreteBayesTreeClique)
         self.assertTrue(root.isRoot())
         self.assertIsInstance(root.conditional(), DiscreteConditional)
 
+        # Test all methods in DiscreteBayesTree
+        self.gtsamAssertEquals(bayesTree, bayesTree)
+
+        # Check value at 0
+        zero_values = DiscreteValues()
+        for j in range(15):
+            zero_values[j] = 0
+        value_at_zeros = bayesTree.evaluate(zero_values)
+        self.assertAlmostEqual(value_at_zeros, 0.0)
+
+        # Check value at max
+        values_star = factorGraph.optimize()
+        max_value = bayesTree.evaluate(values_star)
+        self.assertAlmostEqual(max_value, 0.002548)
+
+        # Check operator sugar
+        max_value = bayesTree(values_star)
+        self.assertAlmostEqual(max_value, 0.002548)
+
+        self.assertFalse(bayesTree.empty())
+        self.assertEqual(12, bayesTree.size())
+
 
 if __name__ == "__main__":
     unittest.main()

From b7e2650a021fdfdaeaac7892e30356c1b9812461 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Jun 2023 23:00:32 +0100
Subject: [PATCH 08/45] wrap operator[] in DiscreteBayesTree

(cherry picked from commit fa7bde7529fefc4dde1a0b9aede2ceba344a3905)
---
 gtsam/symbolic/symbolic.i | 49 +++++++++++++++++++--------------------
 1 file changed, 24 insertions(+), 25 deletions(-)

diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i
index 839e75672..c05f35895 100644
--- a/gtsam/symbolic/symbolic.i
+++ b/gtsam/symbolic/symbolic.i
@@ -138,6 +138,22 @@ class SymbolicBayesNet {
 };
 
 #include <gtsam/symbolic/SymbolicBayesTree.h>
+
+class SymbolicBayesTreeClique {
+  SymbolicBayesTreeClique();
+  SymbolicBayesTreeClique(const gtsam::SymbolicConditional* conditional);
+  bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter);
+  const gtsam::SymbolicConditional* conditional() const;
+  bool isRoot() const;
+  gtsam::SymbolicBayesTreeClique* parent() const;
+  size_t treeSize() const;
+  size_t numCachedSeparatorMarginals() const;
+  void deleteCachedShortcuts();
+};
+
+
 class SymbolicBayesTree {
   // Constructors
   SymbolicBayesTree();
@@ -149,9 +165,14 @@ class SymbolicBayesTree {
   bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
 
   // Standard Interface
-  // size_t findParentClique(const gtsam::IndexVector& parents) const;
-  size_t size();
-  void saveGraph(string s) const;
+  bool empty() const;
+  size_t size() const;
+
+  const gtsam::SymbolicBayesTreeClique* operator[](size_t j) const;
+
+  void saveGraph(string s,
+                const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
   void clear();
   void deleteCachedShortcuts();
   size_t numCachedSeparatorMarginals() const;
@@ -164,26 +185,4 @@ class SymbolicBayesTree {
                  gtsam::DefaultKeyFormatter) const;
 };
 
-class SymbolicBayesTreeClique {
-  SymbolicBayesTreeClique();
-  // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional);
-
-  bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  size_t numCachedSeparatorMarginals() const;
-  // gtsam::sharedConditional* conditional() const;
-  bool isRoot() const;
-  size_t treeSize() const;
-  gtsam::SymbolicBayesTreeClique* parent() const;
-
-  //   // TODO: need wrapped versions graphs, BayesNet
-  //  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function)
-  //  const; FactorGraph<FactorType> marginal(derived_ptr root, Eliminate
-  //  function) const; FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr
-  //  root, Eliminate function) const;
-  //
-  void deleteCachedShortcuts();
-};
-
 }  // namespace gtsam

From 07e8d24cbf7c721f80a8e2c0bd05bff456cb6d0f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 5 Jun 2023 02:11:13 +0100
Subject: [PATCH 09/45] Wrapped elimination and junction trees

(cherry picked from commit 93d9ab6a2e6eb787345114d0e6303ce671f2f82f)
---
 gtsam/discrete/DiscreteJunctionTree.h |  2 ++
 gtsam/discrete/discrete.i             | 45 +++++++++++++++++++++++++--
 gtsam/inference/ClusterTree.h         |  4 +--
 gtsam/inference/inference.i           |  2 +-
 gtsam/symbolic/SymbolicJunctionTree.h |  2 ++
 gtsam/symbolic/symbolic.i             | 37 ++++++++++++++++++++++
 6 files changed, 86 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h
index 385c659e4..f6171c672 100644
--- a/gtsam/discrete/DiscreteJunctionTree.h
+++ b/gtsam/discrete/DiscreteJunctionTree.h
@@ -66,4 +66,6 @@ namespace gtsam {
     DiscreteJunctionTree(const DiscreteEliminationTree& eliminationTree);
   };
 
+  /// typedef for wrapper:
+  using DiscreteCluster = DiscreteJunctionTree::Cluster;
 }
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index b6c6ee2cd..6df443300 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -62,6 +62,8 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const;
 
+  size_t cardinality(gtsam::Key j) const;
+  
   double operator()(const gtsam::DiscreteValues& values) const;
   gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
   size_t cardinality(gtsam::Key j) const;
@@ -247,9 +249,9 @@ class DiscreteBayesTree {
 class DiscreteLookupTable : gtsam::DiscreteConditional{
   DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys,
                       const gtsam::DecisionTreeFactor::ADT& potentials);
-  void print(
-    const std::string& s = "Discrete Lookup Table: ",
-    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+  void print(string s = "Discrete Lookup Table: ",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
   size_t argmax(const gtsam::DiscreteValues& parentsValues) const;
 };
 
@@ -333,4 +335,41 @@ class DiscreteFactorGraph {
               std::map<gtsam::Key, std::vector<std::string>> names) const;
 };
 
+#include <gtsam/discrete/DiscreteEliminationTree.h>
+
+class DiscreteEliminationTree {
+  DiscreteEliminationTree(const gtsam::DiscreteFactorGraph& factorGraph,
+                          const gtsam::VariableIndex& structure,
+                          const gtsam::Ordering& order);
+
+  DiscreteEliminationTree(const gtsam::DiscreteFactorGraph& factorGraph,
+                          const gtsam::Ordering& order);
+
+  void print(
+      string name = "EliminationTree: ",
+      const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::DiscreteEliminationTree& other,
+              double tol = 1e-9) const;
+};
+
+#include <gtsam/discrete/DiscreteJunctionTree.h>
+
+class DiscreteCluster {
+  gtsam::Ordering orderedFrontalKeys;
+  gtsam::DiscreteFactorGraph factors;
+  const gtsam::DiscreteCluster& operator[](size_t i) const;
+  size_t nrChildren() const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+};
+
+class DiscreteJunctionTree {
+  DiscreteJunctionTree(const gtsam::DiscreteEliminationTree& eliminationTree);
+  void print(
+      string name = "JunctionTree: ",
+      const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
+  size_t nrRoots() const;
+  const gtsam::DiscreteCluster& operator[](size_t i) const;
+};
+
 }  // namespace gtsam
diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h
index db80cee3b..43859f3d3 100644
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@@ -49,7 +49,7 @@ class ClusterTree {
     virtual ~Cluster() {}
 
     const Cluster& operator[](size_t i) const {
-      return *(children[i]);
+      return *(children.at(i));
     }
 
     /// Construct from factors associated with a single key
@@ -161,7 +161,7 @@ class ClusterTree {
   }
 
   const Cluster& operator[](size_t i) const {
-    return *(roots_[i]);
+    return *(roots_.at(i));
   }
 
   /// @}
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index e4eeaa480..13d8e032b 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -148,7 +148,7 @@ class Ordering {
 
   // Standard interface
   size_t size() const;
-  size_t at(size_t key) const;
+  size_t at(size_t i) const;
   void push_back(size_t key);
 
   // enabling serialization functionality
diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h
index a6ca49e62..8c274599a 100644
--- a/gtsam/symbolic/SymbolicJunctionTree.h
+++ b/gtsam/symbolic/SymbolicJunctionTree.h
@@ -65,4 +65,6 @@ namespace gtsam {
     SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree);
   };
 
+  /// typedef for wrapper:
+  using SymbolicCluster = SymbolicJunctionTree::Cluster;
 }
diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i
index c05f35895..4da59dfa9 100644
--- a/gtsam/symbolic/symbolic.i
+++ b/gtsam/symbolic/symbolic.i
@@ -137,6 +137,43 @@ class SymbolicBayesNet {
       const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
 };
 
+#include <gtsam/symbolic/SymbolicEliminationTree.h>
+
+class SymbolicEliminationTree {
+  SymbolicEliminationTree(const gtsam::SymbolicFactorGraph& factorGraph,
+                          const gtsam::VariableIndex& structure,
+                          const gtsam::Ordering& order);
+
+  SymbolicEliminationTree(const gtsam::SymbolicFactorGraph& factorGraph,
+                          const gtsam::Ordering& order);
+
+  void print(
+      string name = "EliminationTree: ",
+      const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::SymbolicEliminationTree& other,
+              double tol = 1e-9) const;
+};
+
+#include <gtsam/symbolic/SymbolicJunctionTree.h>
+
+class SymbolicCluster {
+  gtsam::Ordering orderedFrontalKeys;
+  gtsam::SymbolicFactorGraph factors;
+  const gtsam::SymbolicCluster& operator[](size_t i) const;
+  size_t nrChildren() const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+};
+
+class SymbolicJunctionTree {
+  SymbolicJunctionTree(const gtsam::SymbolicEliminationTree& eliminationTree);
+  void print(
+      string name = "JunctionTree: ",
+      const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
+  size_t nrRoots() const;
+  const gtsam::SymbolicCluster& operator[](size_t i) const;
+};
+
 #include <gtsam/symbolic/SymbolicBayesTree.h>
 
 class SymbolicBayesTreeClique {

From 4d99bb1db4cd1aae65675ab5d67568f09b7e45e6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 5 Jun 2023 15:17:05 -0700
Subject: [PATCH 10/45] cartesianProduct

(cherry picked from commit 1f5e924eac4d4beb3e5a380cd423684855f195c7)
---
 gtsam/discrete/DiscreteValues.h | 5 +++++
 gtsam/discrete/discrete.i       | 2 ++
 2 files changed, 7 insertions(+)

diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h
index 8a6d6f930..9ec08302b 100644
--- a/gtsam/discrete/DiscreteValues.h
+++ b/gtsam/discrete/DiscreteValues.h
@@ -120,6 +120,11 @@ class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
   /// @}
 };
 
+/// Free version of CartesianProduct.
+inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
+  return DiscreteValues::CartesianProduct(keys);
+}
+
 /// Free version of markdown.
 std::string markdown(const DiscreteValues& values,
                      const KeyFormatter& keyFormatter = DefaultKeyFormatter,
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 6df443300..158dc9016 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -17,6 +17,8 @@ class DiscreteKeys {
 };
 
 // DiscreteValues is added in specializations/discrete.h as a std::map
+std::vector<gtsam::DiscreteValues> cartesianProduct(
+    const gtsam::DiscreteKeys& keys);
 string markdown(
     const gtsam::DiscreteValues& values,
     const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);

From 340635859c4ca3bd363833d3e2c361b3c0d7ccfe Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 5 Jun 2023 19:10:18 -0700
Subject: [PATCH 11/45] Wrap forgotten max

(cherry picked from commit 3fd5c2501e8d6324ee97a8442c83b1ca65d02b34)
---
 gtsam/discrete/discrete.i | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 158dc9016..040dabf48 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -73,6 +73,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
   gtsam::DecisionTreeFactor* sum(size_t nrFrontals) const;
   gtsam::DecisionTreeFactor* sum(const gtsam::Ordering& keys) const;
   gtsam::DecisionTreeFactor* max(size_t nrFrontals) const;
+  gtsam::DecisionTreeFactor* max(const gtsam::Ordering& keys) const;
 
   string dot(
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,

From 159a78581f0fd35e1c8cd266cc9a399ea6bfaa41 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 10 Jun 2023 09:39:13 -0700
Subject: [PATCH 12/45] refactor into smaller tests

(cherry picked from commit 9857e62a5636d50a7f12660b4dd836c24c31ef29)
---
 .../discrete/tests/testDiscreteBayesTree.cpp  | 173 +++++++++++-------
 1 file changed, 109 insertions(+), 64 deletions(-)

diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index 99038566e..67f00c8a0 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
@@ -16,10 +16,10 @@
  */
 
 #include <gtsam/base/Vector.h>
+#include <gtsam/inference/BayesNet.h>
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteBayesTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/inference/BayesNet.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -32,7 +32,8 @@ static constexpr bool debug = false;
 
 /* ************************************************************************* */
 struct TestFixture {
-  vector<DiscreteKey> keys;
+  DiscreteKeys keys;
+  std::vector<DiscreteValues> assignments;
   DiscreteBayesNet bayesNet;
   std::shared_ptr<DiscreteBayesTree> bayesTree;
 
@@ -47,6 +48,9 @@ struct TestFixture {
       keys.push_back(key_i);
     }
 
+    // Enumerate all assignments.
+    assignments = DiscreteValues::CartesianProduct(keys);
+
     // Create thin-tree Bayesnet.
     bayesNet.add(keys[14] % "1/3");
 
@@ -74,9 +78,9 @@ struct TestFixture {
 };
 
 /* ************************************************************************* */
+// Check that BN and BT give the same answer on all configurations
 TEST(DiscreteBayesTree, ThinTree) {
-  const TestFixture self;
-  const auto& keys = self.keys;
+  TestFixture self;
 
   if (debug) {
     GTSAM_PRINT(self.bayesNet);
@@ -95,47 +99,56 @@ TEST(DiscreteBayesTree, ThinTree) {
     EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals()));
   }
 
-  auto R = self.bayesTree->roots().front();
-
-  // Check whether BN and BT give the same answer on all configurations
-  auto allPosbValues = DiscreteValues::CartesianProduct(
-      keys[0] & keys[1] & keys[2] & keys[3] & keys[4] & keys[5] & keys[6] &
-      keys[7] & keys[8] & keys[9] & keys[10] & keys[11] & keys[12] & keys[13] &
-      keys[14]);
-  for (size_t i = 0; i < allPosbValues.size(); ++i) {
-    DiscreteValues x = allPosbValues[i];
+  for (const auto& x : self.assignments) {
     double expected = self.bayesNet.evaluate(x);
     double actual = self.bayesTree->evaluate(x);
     DOUBLES_EQUAL(expected, actual, 1e-9);
   }
+}
 
-  // Calculate all some marginals for DiscreteValues==all1
-  Vector marginals = Vector::Zero(15);
-  double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0,
-         joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0,
-         joint_4_11 = 0, joint_11_13 = 0, joint_11_13_14 = 0,
-         joint_11_12_13_14 = 0, joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
-  for (size_t i = 0; i < allPosbValues.size(); ++i) {
-    DiscreteValues x = allPosbValues[i];
+/* ************************************************************************* */
+// Check calculation of separator marginals
+TEST(DiscreteBayesTree, separatorMarginal) {
+  TestFixture self;
+
+  // Calculate some marginals for DiscreteValues==all1
+  double marginal_14 = 0, joint_8_12 = 0;
+  for (auto& x : self.assignments) {
     double px = self.bayesTree->evaluate(x);
-    for (size_t i = 0; i < 15; i++)
-      if (x[i]) marginals[i] += px;
-    if (x[12] && x[14]) {
-      joint_12_14 += px;
-      if (x[9]) joint_9_12_14 += px;
-      if (x[8]) joint_8_12_14 += px;
-    }
     if (x[8] && x[12]) joint_8_12 += px;
-    if (x[2]) {
-      if (x[8]) joint82 += px;
-      if (x[1]) joint12 += px;
-    }
-    if (x[4]) {
-      if (x[2]) joint24 += px;
-      if (x[5]) joint45 += px;
-      if (x[6]) joint46 += px;
-      if (x[11]) joint_4_11 += px;
-    }
+    if (x[14]) marginal_14 += px;
+  }
+  DiscreteValues all1 = self.assignments.back();
+
+  // check separator marginal P(S0)
+  auto clique = (*self.bayesTree)[0];
+  DiscreteFactorGraph separatorMarginal0 =
+      clique->separatorMarginal(EliminateDiscrete);
+  DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
+
+  // check separator marginal P(S9), should be P(14)
+  clique = (*self.bayesTree)[9];
+  DiscreteFactorGraph separatorMarginal9 =
+      clique->separatorMarginal(EliminateDiscrete);
+  DOUBLES_EQUAL(marginal_14, separatorMarginal9(all1), 1e-9);
+
+  // check separator marginal of root, should be empty
+  clique = (*self.bayesTree)[11];
+  DiscreteFactorGraph separatorMarginal11 =
+      clique->separatorMarginal(EliminateDiscrete);
+  LONGS_EQUAL(0, separatorMarginal11.size());
+}
+
+/* ************************************************************************* */
+// Check shortcuts in the tree
+TEST(DiscreteBayesTree, shortcut) {
+  TestFixture self;
+
+  // Calculate some marginals for DiscreteValues==all1
+  double joint_11_13 = 0, joint_11_13_14 = 0, joint_11_12_13_14 = 0,
+         joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
+  for (auto& x : self.assignments) {
+    double px = self.bayesTree->evaluate(x);
     if (x[11] && x[13]) {
       joint_11_13 += px;
       if (x[8] && x[12]) joint_8_11_12_13 += px;
@@ -148,32 +161,12 @@ TEST(DiscreteBayesTree, ThinTree) {
       }
     }
   }
-  DiscreteValues all1 = allPosbValues.back();
+  DiscreteValues all1 = self.assignments.back();
 
-  // check separator marginal P(S0)
-  auto clique = (*self.bayesTree)[0];
-  DiscreteFactorGraph separatorMarginal0 =
-      clique->separatorMarginal(EliminateDiscrete);
-  DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
-
-  DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
-  DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
-  DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
-
-  // check separator marginal P(S9), should be P(14)
-  clique = (*self.bayesTree)[9];
-  DiscreteFactorGraph separatorMarginal9 =
-      clique->separatorMarginal(EliminateDiscrete);
-  DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
-
-  // check separator marginal of root, should be empty
-  clique = (*self.bayesTree)[11];
-  DiscreteFactorGraph separatorMarginal11 =
-      clique->separatorMarginal(EliminateDiscrete);
-  LONGS_EQUAL(0, separatorMarginal11.size());
+  auto R = self.bayesTree->roots().front();
 
   // check shortcut P(S9||R) to root
-  clique = (*self.bayesTree)[9];
+  auto clique = (*self.bayesTree)[9];
   DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete);
   LONGS_EQUAL(1, shortcut.size());
   DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
@@ -202,15 +195,67 @@ TEST(DiscreteBayesTree, ThinTree) {
       shortcut.print("shortcut:");
     }
   }
+}
+
+/* ************************************************************************* */
+// Check all marginals
+TEST(DiscreteBayesTree, marginalFactor) {
+  TestFixture self;
+
+  Vector marginals = Vector::Zero(15);
+  for (size_t i = 0; i < self.assignments.size(); ++i) {
+    DiscreteValues& x = self.assignments[i];
+    double px = self.bayesTree->evaluate(x);
+    for (size_t i = 0; i < 15; i++)
+      if (x[i]) marginals[i] += px;
+  }
 
   // Check all marginals
-  DiscreteFactor::shared_ptr marginalFactor;
+  DiscreteValues all1 = self.assignments.back();
   for (size_t i = 0; i < 15; i++) {
-    marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete);
+    auto marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete);
     double actual = (*marginalFactor)(all1);
     DOUBLES_EQUAL(marginals[i], actual, 1e-9);
   }
+}
 
+/* ************************************************************************* */
+// Check a number of joint marginals.
+TEST(DiscreteBayesTree, Joints) {
+  TestFixture self;
+
+  // Calculate some marginals for DiscreteValues==all1
+  Vector marginals = Vector::Zero(15);
+  double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint82 = 0,
+         joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, joint_4_11 = 0;
+  for (size_t i = 0; i < self.assignments.size(); ++i) {
+    DiscreteValues& x = self.assignments[i];
+    double px = self.bayesTree->evaluate(x);
+    for (size_t i = 0; i < 15; i++)
+      if (x[i]) marginals[i] += px;
+    if (x[12] && x[14]) {
+      joint_12_14 += px;
+      if (x[9]) joint_9_12_14 += px;
+      if (x[8]) joint_8_12_14 += px;
+    }
+    if (x[2]) {
+      if (x[8]) joint82 += px;
+      if (x[1]) joint12 += px;
+    }
+    if (x[4]) {
+      if (x[2]) joint24 += px;
+      if (x[5]) joint45 += px;
+      if (x[6]) joint46 += px;
+      if (x[11]) joint_4_11 += px;
+    }
+  }
+
+  // regression tests:
+  DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
+  DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
+  DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
+
+  DiscreteValues all1 = self.assignments.back();
   DiscreteBayesNet::shared_ptr actualJoint;
 
   // Check joint P(8, 2)
@@ -240,7 +285,7 @@ TEST(DiscreteBayesTree, ThinTree) {
 
 /* ************************************************************************* */
 TEST(DiscreteBayesTree, Dot) {
-  const TestFixture self;
+  TestFixture self;
   string actual = self.bayesTree->dot();
   EXPECT(actual ==
          "digraph G{\n"

From bd4230baaefac6fd4e3221282b5a9ae064e9d09a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 10 Jun 2023 12:34:30 -0700
Subject: [PATCH 13/45] multifrontal lookup table tests

(cherry picked from commit 0a24a8ac434f01f31ba47b3034476305efe644aa)
---
 gtsam/discrete/DecisionTreeFactor.h           |  4 +-
 gtsam/discrete/DiscreteBayesTree.h            |  5 ++
 gtsam/discrete/discrete.i                     |  2 +
 .../discrete/tests/testDiscreteBayesTree.cpp  | 65 +++++++++++++++++--
 4 files changed, 69 insertions(+), 7 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 92b02afd1..7988757b5 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -80,7 +80,7 @@ namespace gtsam {
      * @endcode
      *
      * The values in the table should be laid out so that the first key varies
-     * the slowest. and the last key the fastest.
+     * the slowest, and the last key the fastest.
      */
     DecisionTreeFactor(const DiscreteKeys& keys,
                        const std::vector<double>& table);
@@ -101,7 +101,7 @@ namespace gtsam {
      * @endcode
      *
      * The values in the table should be laid out so that the first key varies
-     * the slowest. and the last key the fastest.
+     * the slowest, and the last key the fastest.
      */
     DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
 
diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h
index e9b1325a0..34f8f5d4a 100644
--- a/gtsam/discrete/DiscreteBayesTree.h
+++ b/gtsam/discrete/DiscreteBayesTree.h
@@ -58,6 +58,11 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
 
   //** evaluate conditional probability of subtree for given DiscreteValues */
   double evaluate(const DiscreteValues& values) const;
+
+  //** (Preferred) sugar for the above for given DiscreteValues */
+  double operator()(const DiscreteValues& values) const {
+    return evaluate(values);
+  }
 };
 
 /* ************************************************************************* */
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 040dabf48..32f0439ea 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -215,6 +215,7 @@ class DiscreteBayesTreeClique {
       const string& s = "Clique: ",
       const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
   double evaluate(const gtsam::DiscreteValues& values) const;
+  double operator()(const gtsam::DiscreteValues& values) const;
 };
 
 class DiscreteBayesTree {
@@ -229,6 +230,7 @@ class DiscreteBayesTree {
   const DiscreteBayesTreeClique* operator[](size_t j) const;
 
   double evaluate(const gtsam::DiscreteValues& values) const;
+  double operator()(const gtsam::DiscreteValues& values) const;
 
   string dot(const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index 67f00c8a0..f0a630762 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/base/Vector.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteBayesTree.h>
@@ -26,7 +27,6 @@
 #include <iostream>
 #include <vector>
 
-using namespace std;
 using namespace gtsam;
 static constexpr bool debug = false;
 
@@ -108,7 +108,7 @@ TEST(DiscreteBayesTree, ThinTree) {
 
 /* ************************************************************************* */
 // Check calculation of separator marginals
-TEST(DiscreteBayesTree, separatorMarginal) {
+TEST(DiscreteBayesTree, SeparatorMarginals) {
   TestFixture self;
 
   // Calculate some marginals for DiscreteValues==all1
@@ -141,7 +141,7 @@ TEST(DiscreteBayesTree, separatorMarginal) {
 
 /* ************************************************************************* */
 // Check shortcuts in the tree
-TEST(DiscreteBayesTree, shortcut) {
+TEST(DiscreteBayesTree, Shortcuts) {
   TestFixture self;
 
   // Calculate some marginals for DiscreteValues==all1
@@ -199,7 +199,7 @@ TEST(DiscreteBayesTree, shortcut) {
 
 /* ************************************************************************* */
 // Check all marginals
-TEST(DiscreteBayesTree, marginalFactor) {
+TEST(DiscreteBayesTree, MarginalFactors) {
   TestFixture self;
 
   Vector marginals = Vector::Zero(15);
@@ -286,7 +286,7 @@ TEST(DiscreteBayesTree, Joints) {
 /* ************************************************************************* */
 TEST(DiscreteBayesTree, Dot) {
   TestFixture self;
-  string actual = self.bayesTree->dot();
+  std::string actual = self.bayesTree->dot();
   EXPECT(actual ==
          "digraph G{\n"
          "0[label=\"13, 11, 6, 7\"];\n"
@@ -313,6 +313,61 @@ TEST(DiscreteBayesTree, Dot) {
          "}");
 }
 
+/* ************************************************************************* */
+// Check that we can have a multi-frontal lookup table
+TEST(DiscreteBayesTree, Lookup) {
+  using gtsam::symbol_shorthand::A;
+  using gtsam::symbol_shorthand::X;
+
+  // Make a small planning-like graph: 3 states, 2 actions
+  DiscreteFactorGraph graph;
+  const DiscreteKey x1{X(1), 3}, x2{X(2), 3}, x3{X(3), 3};
+  const DiscreteKey a1{A(1), 2}, a2{A(2), 2};
+  const DiscreteKeys keys{x1, x2, x3, a1, a2};
+  // Constraint on start and goal
+  graph.add(DiscreteKeys{x1}, std::vector<double>{1, 0, 0});
+  graph.add(DiscreteKeys{x3}, std::vector<double>{0, 0, 1});
+  // Should I stay or should I go?
+  // "Reward" (exp(-cost)) for an action is 10, and rewards multiply:
+  const double r = 10;
+  std::vector<double> table{
+      r, 0, 0, 0, r, 0,  // x1 = 0
+      0, r, 0, 0, 0, r,  // x1 = 1
+      0, 0, r, 0, 0, r   // x1 = 2
+  };
+  graph.add(DiscreteKeys{x1, a1, x2}, table);
+  graph.add(DiscreteKeys{x2, a2, x3}, table);
+
+  // eliminate for MPE (maximum probable explanation).
+  Ordering ordering{A(2), X(3), X(1), A(1), X(2)};
+  auto lookup = graph.eliminateMultifrontal(ordering, EliminateForMPE);
+
+  // Check that the lookup table is correct
+  EXPECT_LONGS_EQUAL(2, lookup->size());
+  auto lookup_x1_a1_x2 = (*lookup)[X(1)]->conditional();
+  EXPECT_LONGS_EQUAL(3, lookup_x1_a1_x2->frontals().size());
+  // check that sum is 100
+  DiscreteValues empty;
+  EXPECT_DOUBLES_EQUAL(100, (*lookup_x1_a1_x2->sum(3))(empty), 1e-9);
+  // And that only non-zero reward is for x1 a1 x2 == 0 1 1
+  EXPECT_DOUBLES_EQUAL(100, (*lookup_x1_a1_x2)({{X(1),0},{A(1),1},{X(2),1}}), 1e-9);
+
+  auto lookup_a2_x3 = (*lookup)[X(3)]->conditional();
+  // check that the sum depends on x2 and is non-zero only for x2 \in {1,2}
+  auto sum_x2 = lookup_a2_x3->sum(2);
+  EXPECT_DOUBLES_EQUAL(0, (*sum_x2)({{X(2),0}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(10, (*sum_x2)({{X(2),1}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(20, (*sum_x2)({{X(2),2}}), 1e-9);
+  EXPECT_LONGS_EQUAL(2, lookup_a2_x3->frontals().size());
+  // And that the non-zero rewards are for 
+  // x2 a2 x3 == 1 1 2
+  EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),1},{A(2),1},{X(3),2}}), 1e-9);
+  // x2 a2 x3 == 2 0 2
+  EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),2},{A(2),0},{X(3),2}}), 1e-9);
+  // x2 a2 x3 == 2 1 2
+  EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),2},{A(2),1},{X(3),2}}), 1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 3a9f5578d664bfbd585ff68aed192fff498abc66 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 10 Jun 2023 13:56:14 -0700
Subject: [PATCH 14/45] multifrontal MPE in python

(cherry picked from commit 10f30e1ca93f7c335684f814b64944c3a5b7a51e)
---
 gtsam/discrete/DiscreteFactorGraph.h          | 39 +++++++----
 gtsam/discrete/discrete.i                     | 52 +++++++++++----
 .../discrete/tests/testDiscreteBayesTree.cpp  |  3 +-
 gtsam/inference/EliminateableFactorGraph.h    |  6 +-
 gtsam/inference/inference.i                   |  1 +
 python/gtsam/tests/test_DiscreteBayesTree.py  | 64 ++++++++++++++++---
 6 files changed, 128 insertions(+), 37 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index 650fe7636..d0dc282b4 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -41,16 +41,30 @@ class DiscreteJunctionTree;
 
 /**
  * @brief Main elimination function for DiscreteFactorGraph.
- * 
- * @param factors 
- * @param keys 
- * @return GTSAM_EXPORT
+ *
+ * @param factors The factor graph to eliminate.
+ * @param frontalKeys An ordering for which variables to eliminate.
+ * @return A pair of the resulting conditional and the separator factor.
  * @ingroup discrete
  */
-GTSAM_EXPORT std::pair<std::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
-EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys);
+GTSAM_EXPORT
+std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
+EliminateDiscrete(const DiscreteFactorGraph& factors,
+                  const Ordering& frontalKeys);
+
+/**
+ * @brief Alternate elimination function for that creates non-normalized lookup tables.
+ *
+ * @param factors The factor graph to eliminate.
+ * @param frontalKeys An ordering for which variables to eliminate.
+ * @return A pair of the resulting lookup table and the separator factor.
+ * @ingroup discrete
+ */
+GTSAM_EXPORT
+std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
+EliminateForMPE(const DiscreteFactorGraph& factors,
+                const Ordering& frontalKeys);
 
-/* ************************************************************************* */
 template<> struct EliminationTraits<DiscreteFactorGraph>
 {
   typedef DiscreteFactor FactorType;                   ///< Type of factors in factor graph
@@ -60,12 +74,14 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
   typedef DiscreteEliminationTree EliminationTreeType; ///< Type of elimination tree
   typedef DiscreteBayesTree BayesTreeType;             ///< Type of Bayes tree
   typedef DiscreteJunctionTree JunctionTreeType;       ///< Type of Junction tree
+  
   /// The default dense elimination function
   static std::pair<std::shared_ptr<ConditionalType>,
                    std::shared_ptr<FactorType> >
   DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
     return EliminateDiscrete(factors, keys);
   }
+  
   /// The default ordering generation function
   static Ordering DefaultOrderingFunc(
       const FactorGraphType& graph,
@@ -74,7 +90,6 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
   }
 };
 
-/* ************************************************************************* */
 /**
  * A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
  *   Factor == DiscreteFactor
@@ -108,8 +123,8 @@ class GTSAM_EXPORT DiscreteFactorGraph
 
   /** Implicit copy/downcast constructor to override explicit template container
    * constructor */
-  template <class DERIVEDFACTOR>
-  DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+  template <class DERIVED_FACTOR>
+  DiscreteFactorGraph(const FactorGraph<DERIVED_FACTOR>& graph) : Base(graph) {}
 
   /// @name Testable
   /// @{
@@ -227,10 +242,6 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /// @}
 };  // \ DiscreteFactorGraph
 
-std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
-EliminateForMPE(const DiscreteFactorGraph& factors,
-                const Ordering& frontalKeys);
-
 /// traits
 template <>
 struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 32f0439ea..b66069340 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -275,6 +275,14 @@ class DiscreteLookupDAG {
 };
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
+EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors,
+                  const gtsam::Ordering& frontalKeys);
+
+std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
+EliminateForMPE(const gtsam::DiscreteFactorGraph& factors,
+                const gtsam::Ordering& frontalKeys);
+
 class DiscreteFactorGraph {
   DiscreteFactorGraph();
   DiscreteFactorGraph(const gtsam::DiscreteBayesNet& bayesNet);
@@ -289,6 +297,7 @@ class DiscreteFactorGraph {
   void add(const gtsam::DiscreteKey& j, const std::vector<double>& spec);
   void add(const gtsam::DiscreteKeys& keys, string spec);
   void add(const std::vector<gtsam::DiscreteKey>& keys, string spec);
+  void add(const std::vector<gtsam::DiscreteKey>& keys, const std::vector<double>& spec);
 
   bool empty() const;
   size_t size() const;
@@ -302,25 +311,46 @@ class DiscreteFactorGraph {
   double operator()(const gtsam::DiscreteValues& values) const;
   gtsam::DiscreteValues optimize() const;
 
-  gtsam::DiscreteBayesNet sumProduct();
-  gtsam::DiscreteBayesNet sumProduct(gtsam::Ordering::OrderingType type);
+  gtsam::DiscreteBayesNet sumProduct(
+      gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
   gtsam::DiscreteBayesNet sumProduct(const gtsam::Ordering& ordering);
 
-  gtsam::DiscreteLookupDAG maxProduct();
-  gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type);
+  gtsam::DiscreteLookupDAG maxProduct(
+      gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
   gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
 
-  gtsam::DiscreteBayesNet* eliminateSequential();
-  gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
+  gtsam::DiscreteBayesNet* eliminateSequential(
+      gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
+  gtsam::DiscreteBayesNet* eliminateSequential(
+      gtsam::Ordering::OrderingType type,
+      const gtsam::DiscreteFactorGraph::Eliminate& function);
   gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  gtsam::DiscreteBayesNet* eliminateSequential(
+      const gtsam::Ordering& ordering,
+      const gtsam::DiscreteFactorGraph::Eliminate& function);
   pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
-      eliminatePartialSequential(const gtsam::Ordering& ordering);
+  eliminatePartialSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
+  eliminatePartialSequential(
+      const gtsam::Ordering& ordering,
+      const gtsam::DiscreteFactorGraph::Eliminate& function);
 
-  gtsam::DiscreteBayesTree* eliminateMultifrontal();
-  gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
-  gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);  
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(
+      gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(
+      gtsam::Ordering::OrderingType type,
+      const gtsam::DiscreteFactorGraph::Eliminate& function);
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(
+      const gtsam::Ordering& ordering);
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(
+      const gtsam::Ordering& ordering,
+      const gtsam::DiscreteFactorGraph::Eliminate& function);
   pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
-      eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
+  eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
+  pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
+  eliminatePartialMultifrontal(
+      const gtsam::Ordering& ordering,
+      const gtsam::DiscreteFactorGraph::Eliminate& function);
 
   string dot(
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index f0a630762..0288561cd 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
@@ -323,10 +323,11 @@ TEST(DiscreteBayesTree, Lookup) {
   DiscreteFactorGraph graph;
   const DiscreteKey x1{X(1), 3}, x2{X(2), 3}, x3{X(3), 3};
   const DiscreteKey a1{A(1), 2}, a2{A(2), 2};
-  const DiscreteKeys keys{x1, x2, x3, a1, a2};
+
   // Constraint on start and goal
   graph.add(DiscreteKeys{x1}, std::vector<double>{1, 0, 0});
   graph.add(DiscreteKeys{x3}, std::vector<double>{0, 0, 1});
+
   // Should I stay or should I go?
   // "Reward" (exp(-cost)) for an action is 10, and rewards multiply:
   const double r = 10;
diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h
index ac4b96d28..52e1479a9 100644
--- a/gtsam/inference/EliminateableFactorGraph.h
+++ b/gtsam/inference/EliminateableFactorGraph.h
@@ -51,12 +51,12 @@ namespace gtsam {
    *  algorithms.  Any factor graph holding eliminateable factors can derive from this class to
    *  expose functions for computing marginals, conditional marginals, doing multifrontal and
    *  sequential elimination, etc. */
-  template<class FACTORGRAPH>
+  template<class FACTOR_GRAPH>
   class EliminateableFactorGraph
   {
   private:
-    typedef EliminateableFactorGraph<FACTORGRAPH> This; ///< Typedef to this class.
-    typedef FACTORGRAPH FactorGraphType; ///< Typedef to factor graph type
+    typedef EliminateableFactorGraph<FACTOR_GRAPH> This; ///< Typedef to this class.
+    typedef FACTOR_GRAPH FactorGraphType; ///< Typedef to factor graph type
     // Base factor type stored in this graph (private because derived classes will get this from
     // their FactorGraph base class)
     typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index 13d8e032b..b25e2fd62 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -104,6 +104,7 @@ class Ordering {
   // Standard Constructors and Named Constructors
   Ordering();
   Ordering(const gtsam::Ordering& other);
+  Ordering(const std::vector<size_t>& keys);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py
index e1754ca64..720f884e0 100644
--- a/python/gtsam/tests/test_DiscreteBayesTree.py
+++ b/python/gtsam/tests/test_DiscreteBayesTree.py
@@ -13,16 +13,14 @@ Author: Frank Dellaert
 
 import unittest
 
+import numpy as np
+from gtsam.symbol_shorthand import A, X
 from gtsam.utils.test_case import GtsamTestCase
 
-from gtsam import (
-    DiscreteBayesNet,
-    DiscreteBayesTreeClique,
-    DiscreteConditional,
-    DiscreteFactorGraph,
-    DiscreteValues,
-    Ordering,
-)
+import gtsam
+from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique,
+                   DiscreteConditional, DiscreteFactorGraph,
+                   DiscreteKeys, DiscreteValues, Ordering)
 
 
 class TestDiscreteBayesNet(GtsamTestCase):
@@ -100,6 +98,56 @@ class TestDiscreteBayesNet(GtsamTestCase):
         self.assertFalse(bayesTree.empty())
         self.assertEqual(12, bayesTree.size())
 
+    def test_discrete_bayes_tree_lookup(self):
+        """Check that we can have a multi-frontal lookup table."""
+        # Make a small planning-like graph: 3 states, 2 actions
+        graph = DiscreteFactorGraph()
+        x1, x2, x3 = (X(1), 3), (X(2), 3), (X(3), 3)
+        a1, a2 = (A(1), 2), (A(2), 2)
+
+        # Constraint on start and goal
+        graph.add([x1], np.array([1, 0, 0]))
+        graph.add([x3], np.array([0, 0, 1]))
+
+        # Should I stay or should I go?
+        # "Reward" (exp(-cost)) for an action is 10, and rewards multiply:
+        r = 10
+        table = np.array([
+            r, 0, 0, 0, r, 0,  # x1 = 0
+            0, r, 0, 0, 0, r,  # x1 = 1
+            0, 0, r, 0, 0, r   # x1 = 2
+        ])
+        graph.add([x1, a1, x2], table)
+        graph.add([x2, a2, x3], table)
+
+        # Eliminate for MPE (maximum probable explanation).
+        ordering = Ordering([A(2), X(3), X(1), A(1), X(2)])
+        lookup = graph.eliminateMultifrontal(ordering, gtsam.EliminateForMPE)
+
+        # Check that the lookup table is correct
+        assert len(lookup) == 2
+        lookup_x1_a1_x2 = lookup[X(1)].conditional()
+        assert len(lookup_x1_a1_x2.frontals()) == 3
+        # Check that sum is 100
+        empty = gtsam.DiscreteValues()
+        assert np.isclose(lookup_x1_a1_x2.sum(3)(empty), 100, atol=1e-9)
+        # And that only non-zero reward is for x1 a1 x2 == 0 1 1
+        assert np.isclose(lookup_x1_a1_x2({X(1): 0, A(1): 1, X(2): 1}), 100, atol=1e-9)
+
+        lookup_a2_x3 = lookup[X(3)].conditional()
+        # Check that the sum depends on x2 and is non-zero only for x2 in {1, 2}
+        sum_x2 = lookup_a2_x3.sum(2)
+        assert np.isclose(sum_x2({X(2): 0}), 0, atol=1e-9)
+        assert np.isclose(sum_x2({X(2): 1}), 10, atol=1e-9)
+        assert np.isclose(sum_x2({X(2): 2}), 20, atol=1e-9)
+        assert len(lookup_a2_x3.frontals()) == 2
+        # And that the non-zero rewards are for
+        # x2 a2 x3 == 1 1 2
+        assert np.isclose(lookup_a2_x3({X(2): 1, A(2): 1, X(3): 2}), 10, atol=1e-9)
+        # x2 a2 x3 == 2 0 2
+        assert np.isclose(lookup_a2_x3({X(2): 2, A(2): 0, X(3): 2}), 10, atol=1e-9)
+        # x2 a2 x3 == 2 1 2
+        assert np.isclose(lookup_a2_x3({X(2): 2, A(2): 1, X(3): 2}), 10, atol=1e-9)
 
 if __name__ == "__main__":
     unittest.main()

From a4b9f0c705b8eaf54575807a27d4bd677aaa84a8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 19 Jun 2023 07:38:19 -0700
Subject: [PATCH 15/45] more discrete BT wrapper + test

(cherry picked from commit 3d979763f0797af41f250622a99db630af14139d)
---
 gtsam/discrete/discrete.i                    |  5 +++
 gtsam/inference/BayesTreeCliqueBase.h        |  8 ++++-
 python/gtsam/tests/test_DiscreteBayesTree.py | 37 ++++++++++++--------
 3 files changed, 34 insertions(+), 16 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index b66069340..fe8cbc7f3 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -211,6 +211,11 @@ class DiscreteBayesTreeClique {
   DiscreteBayesTreeClique(const gtsam::DiscreteConditional* conditional);
   const gtsam::DiscreteConditional* conditional() const;
   bool isRoot() const;
+  size_t nrChildren() const;
+  const gtsam::DiscreteBayesTreeClique* operator[](size_t i) const;
+  void print(string s = "DiscreteBayesTreeClique",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
   void printSignature(
       const string& s = "Clique: ",
       const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 976d1a7f2..7f25ad223 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -140,9 +140,15 @@ namespace gtsam {
     /** Access the conditional */
     const sharedConditional& conditional() const { return conditional_; }
 
-    /** is this the root of a Bayes tree ? */
+    /// Is this the root of a Bayes tree? 
     inline bool isRoot() const { return parent_.expired(); }
 
+    /// Return the number of children.
+    size_t nrChildren() const { return children.size(); }
+
+    /// Return the child at index i.
+    const derived_ptr operator[](size_t i) const { return children[i]; }
+
     /** The size of subtree rooted at this clique, i.e., nr of Cliques */
     size_t treeSize() const;
 
diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py
index 720f884e0..09fb5a865 100644
--- a/python/gtsam/tests/test_DiscreteBayesTree.py
+++ b/python/gtsam/tests/test_DiscreteBayesTree.py
@@ -125,29 +125,36 @@ class TestDiscreteBayesNet(GtsamTestCase):
         lookup = graph.eliminateMultifrontal(ordering, gtsam.EliminateForMPE)
 
         # Check that the lookup table is correct
-        assert len(lookup) == 2
+        assert lookup.size() == 2
         lookup_x1_a1_x2 = lookup[X(1)].conditional()
-        assert len(lookup_x1_a1_x2.frontals()) == 3
+        assert lookup_x1_a1_x2.nrFrontals() == 3
         # Check that sum is 100
         empty = gtsam.DiscreteValues()
-        assert np.isclose(lookup_x1_a1_x2.sum(3)(empty), 100, atol=1e-9)
+        self.assertAlmostEqual(lookup_x1_a1_x2.sum(3)(empty), 100)
         # And that only non-zero reward is for x1 a1 x2 == 0 1 1
-        assert np.isclose(lookup_x1_a1_x2({X(1): 0, A(1): 1, X(2): 1}), 100, atol=1e-9)
+        values = DiscreteValues()
+        values[X(1)] = 0
+        values[A(1)] = 1
+        values[X(2)] = 1
+        self.assertAlmostEqual(lookup_x1_a1_x2(values), 100)
 
         lookup_a2_x3 = lookup[X(3)].conditional()
         # Check that the sum depends on x2 and is non-zero only for x2 in {1, 2}
         sum_x2 = lookup_a2_x3.sum(2)
-        assert np.isclose(sum_x2({X(2): 0}), 0, atol=1e-9)
-        assert np.isclose(sum_x2({X(2): 1}), 10, atol=1e-9)
-        assert np.isclose(sum_x2({X(2): 2}), 20, atol=1e-9)
-        assert len(lookup_a2_x3.frontals()) == 2
-        # And that the non-zero rewards are for
-        # x2 a2 x3 == 1 1 2
-        assert np.isclose(lookup_a2_x3({X(2): 1, A(2): 1, X(3): 2}), 10, atol=1e-9)
-        # x2 a2 x3 == 2 0 2
-        assert np.isclose(lookup_a2_x3({X(2): 2, A(2): 0, X(3): 2}), 10, atol=1e-9)
-        # x2 a2 x3 == 2 1 2
-        assert np.isclose(lookup_a2_x3({X(2): 2, A(2): 1, X(3): 2}), 10, atol=1e-9)
+        values = DiscreteValues()
+        values[X(2)] = 0
+        self.assertAlmostEqual(sum_x2(values), 0)
+        values[X(2)] = 1
+        self.assertAlmostEqual(sum_x2(values), 10)
+        values[X(2)] = 2
+        self.assertAlmostEqual(sum_x2(values), 20)
+        assert lookup_a2_x3.nrFrontals() == 2
+        # And that the non-zero rewards are for x2 a2 x3 == 1 1 2
+        values = DiscreteValues()
+        values[X(2)] = 1
+        values[A(2)] = 1
+        values[X(3)] = 2
+        self.assertAlmostEqual(lookup_a2_x3(values), 10)
 
 if __name__ == "__main__":
     unittest.main()

From 433ef9050f16c22b1eb549d1ccd12c252da444a3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 16 Jul 2023 12:37:01 +0200
Subject: [PATCH 16/45] Better test names

(cherry picked from commit 3edde4d69377f6e442ea63104bc8371b2659b55f)
---
 gtsam/discrete/tests/testDecisionTree.cpp       | 6 +++---
 gtsam/discrete/tests/testDecisionTreeFactor.cpp | 2 +-
 gtsam/inference/BayesTreeCliqueBase.h           | 2 +-
 python/gtsam/tests/test_DiscreteBayesTree.py    | 6 +++---
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 6ca8d843e..38833851a 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -80,7 +80,7 @@ GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree)
 /* ************************************************************************** */
 
 // Create a decision stump one one variable 'a' with values 10 and 20.
-TEST(DecisionTree, constructor) {
+TEST(DecisionTree, Constructor) {
   DecisionTree<char, int> tree('a', 10, 20);
 
   // Evaluate the tree on an assignment to the variable.
@@ -133,7 +133,7 @@ struct Ring {
 
 /* ************************************************************************** */
 // Check that creating decision trees respects key order.
-TEST(DecisionTree, constructor_order) {
+TEST(DecisionTree, ConstructorOrder) {
   // Create labels
   string A("A"), B("B");
 
@@ -163,7 +163,7 @@ TEST(DecisionTree, constructor_order) {
 
 /* ************************************************************************** */
 // test DT
-TEST(DecisionTree, example) {
+TEST(DecisionTree, Example) {
   // Create labels
   string A("A"), B("B"), C("C");
 
diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 8e203c56a..4b86fcbe3 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -28,7 +28,7 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST(DecisionTreeFactor, constructors_match) {
+TEST(DecisionTreeFactor, ConstructorsMatch) {
   // Declare two keys
   DiscreteKey X(0, 2), Y(1, 3);
 
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 7f25ad223..171ebdc41 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -140,7 +140,7 @@ namespace gtsam {
     /** Access the conditional */
     const sharedConditional& conditional() const { return conditional_; }
 
-    /// Is this the root of a Bayes tree? 
+    /// Return true if this clique is the root of a Bayes tree. 
     inline bool isRoot() const { return parent_.expired(); }
 
     /// Return the number of children.
diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py
index 09fb5a865..c8a93e63b 100644
--- a/python/gtsam/tests/test_DiscreteBayesTree.py
+++ b/python/gtsam/tests/test_DiscreteBayesTree.py
@@ -20,7 +20,7 @@ from gtsam.utils.test_case import GtsamTestCase
 import gtsam
 from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique,
                    DiscreteConditional, DiscreteFactorGraph,
-                   DiscreteKeys, DiscreteValues, Ordering)
+                   DiscreteValues, Ordering)
 
 
 class TestDiscreteBayesNet(GtsamTestCase):
@@ -32,7 +32,7 @@ class TestDiscreteBayesNet(GtsamTestCase):
         # Define DiscreteKey pairs.
         keys = [(j, 2) for j in range(15)]
 
-        # Create thin-tree Bayesnet.
+        # Create thin-tree Bayes net.
         bayesNet = DiscreteBayesNet()
 
         bayesNet.add(keys[0], [keys[8], keys[12]], "2/3 1/4 3/2 4/1")
@@ -121,7 +121,7 @@ class TestDiscreteBayesNet(GtsamTestCase):
         graph.add([x2, a2, x3], table)
 
         # Eliminate for MPE (maximum probable explanation).
-        ordering = Ordering([A(2), X(3), X(1), A(1), X(2)])
+        ordering = Ordering(keys=[A(2), X(3), X(1), A(1), X(2)])
         lookup = graph.eliminateMultifrontal(ordering, gtsam.EliminateForMPE)
 
         # Check that the lookup table is correct

From 6eeceb679af17ee7c260b8ca46f50600945571e5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 17 Jul 2023 16:54:50 +0200
Subject: [PATCH 17/45] Fix unit tests

---
 gtsam/discrete/tests/testDiscreteBayesTree.cpp | 16 ++++++++--------
 gtsam_unstable/linear/tests/testQPSolver.cpp   |  4 ++--
 python/gtsam/tests/test_DiscreteBayesTree.py   | 12 ++++++------
 3 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index 0288561cd..617eb7c9d 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
@@ -347,26 +347,26 @@ TEST(DiscreteBayesTree, Lookup) {
   EXPECT_LONGS_EQUAL(2, lookup->size());
   auto lookup_x1_a1_x2 = (*lookup)[X(1)]->conditional();
   EXPECT_LONGS_EQUAL(3, lookup_x1_a1_x2->frontals().size());
-  // check that sum is 100
+  // check that sum is 1.0 (not 100, as we now normalize)
   DiscreteValues empty;
-  EXPECT_DOUBLES_EQUAL(100, (*lookup_x1_a1_x2->sum(3))(empty), 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.0, (*lookup_x1_a1_x2->sum(3))(empty), 1e-9);
   // And that only non-zero reward is for x1 a1 x2 == 0 1 1
-  EXPECT_DOUBLES_EQUAL(100, (*lookup_x1_a1_x2)({{X(1),0},{A(1),1},{X(2),1}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.0, (*lookup_x1_a1_x2)({{X(1),0},{A(1),1},{X(2),1}}), 1e-9);
 
   auto lookup_a2_x3 = (*lookup)[X(3)]->conditional();
   // check that the sum depends on x2 and is non-zero only for x2 \in {1,2}
   auto sum_x2 = lookup_a2_x3->sum(2);
   EXPECT_DOUBLES_EQUAL(0, (*sum_x2)({{X(2),0}}), 1e-9);
-  EXPECT_DOUBLES_EQUAL(10, (*sum_x2)({{X(2),1}}), 1e-9);
-  EXPECT_DOUBLES_EQUAL(20, (*sum_x2)({{X(2),2}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.0, (*sum_x2)({{X(2),1}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2.0, (*sum_x2)({{X(2),2}}), 1e-9);
   EXPECT_LONGS_EQUAL(2, lookup_a2_x3->frontals().size());
   // And that the non-zero rewards are for 
   // x2 a2 x3 == 1 1 2
-  EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),1},{A(2),1},{X(3),2}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.0, (*lookup_a2_x3)({{X(2),1},{A(2),1},{X(3),2}}), 1e-9);
   // x2 a2 x3 == 2 0 2
-  EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),2},{A(2),0},{X(3),2}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.0, (*lookup_a2_x3)({{X(2),2},{A(2),0},{X(3),2}}), 1e-9);
   // x2 a2 x3 == 2 1 2
-  EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),2},{A(2),1},{X(3),2}}), 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.0, (*lookup_a2_x3)({{X(2),2},{A(2),1},{X(3),2}}), 1e-9);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp
index 12bd93416..b307c7302 100644
--- a/gtsam_unstable/linear/tests/testQPSolver.cpp
+++ b/gtsam_unstable/linear/tests/testQPSolver.cpp
@@ -181,7 +181,7 @@ TEST(QPSolver, iterate) {
   QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
                         100);
 
-  int it = 0;
+  // int it = 0;
   while (!state.converged) {
     state = solver.iterate(state);
     // These checks will fail because the expected solutions obtained from
@@ -190,7 +190,7 @@ TEST(QPSolver, iterate) {
     // do not recompute dual variables after every step!!!
     //    CHECK(assert_equal(expected[it], state.values, 1e-10));
     //    CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
-    it++;
+    // it++;
   }
 
   CHECK(assert_equal(expected[3], state.values, 1e-10));
diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py
index c8a93e63b..2a9b6ea09 100644
--- a/python/gtsam/tests/test_DiscreteBayesTree.py
+++ b/python/gtsam/tests/test_DiscreteBayesTree.py
@@ -128,15 +128,15 @@ class TestDiscreteBayesNet(GtsamTestCase):
         assert lookup.size() == 2
         lookup_x1_a1_x2 = lookup[X(1)].conditional()
         assert lookup_x1_a1_x2.nrFrontals() == 3
-        # Check that sum is 100
+        # Check that sum is 1.0 (not 100, as we now normalize to prevent underflow)
         empty = gtsam.DiscreteValues()
-        self.assertAlmostEqual(lookup_x1_a1_x2.sum(3)(empty), 100)
+        self.assertAlmostEqual(lookup_x1_a1_x2.sum(3)(empty), 1.0)
         # And that only non-zero reward is for x1 a1 x2 == 0 1 1
         values = DiscreteValues()
         values[X(1)] = 0
         values[A(1)] = 1
         values[X(2)] = 1
-        self.assertAlmostEqual(lookup_x1_a1_x2(values), 100)
+        self.assertAlmostEqual(lookup_x1_a1_x2(values), 1.0)
 
         lookup_a2_x3 = lookup[X(3)].conditional()
         # Check that the sum depends on x2 and is non-zero only for x2 in {1, 2}
@@ -145,16 +145,16 @@ class TestDiscreteBayesNet(GtsamTestCase):
         values[X(2)] = 0
         self.assertAlmostEqual(sum_x2(values), 0)
         values[X(2)] = 1
-        self.assertAlmostEqual(sum_x2(values), 10)
+        self.assertAlmostEqual(sum_x2(values), 1.0)  # not 10, as we normalize
         values[X(2)] = 2
-        self.assertAlmostEqual(sum_x2(values), 20)
+        self.assertAlmostEqual(sum_x2(values), 2.0)  # not 20, as we normalize
         assert lookup_a2_x3.nrFrontals() == 2
         # And that the non-zero rewards are for x2 a2 x3 == 1 1 2
         values = DiscreteValues()
         values[X(2)] = 1
         values[A(2)] = 1
         values[X(3)] = 2
-        self.assertAlmostEqual(lookup_a2_x3(values), 10)
+        self.assertAlmostEqual(lookup_a2_x3(values), 1.0)  # not 10...
 
 if __name__ == "__main__":
     unittest.main()

From cf1292791ef90012d15b642401ac5a123b5d2eea Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 3 Jul 2023 08:50:36 -0400
Subject: [PATCH 18/45] update HybridFactorGraph wrapper

---
 gtsam/hybrid/HybridFactor.h | 1 +
 gtsam/hybrid/hybrid.i       | 1 +
 2 files changed, 2 insertions(+)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 13d5c2cba..afd1c8032 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -20,6 +20,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Factor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 7cc93d4b0..5e0e6dca0 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -179,6 +179,7 @@ class HybridGaussianFactorGraph {
   void push_back(const gtsam::HybridBayesTree& bayesTree);
   void push_back(const gtsam::GaussianMixtureFactor* gmm);
   void push_back(gtsam::DecisionTreeFactor* factor);
+  void push_back(gtsam::TableFactor* factor);
   void push_back(gtsam::JacobianFactor* factor);
 
   bool empty() const;

From 73eb40595df5c1dbe9f40a5eb42458f61a126198 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 17 Jul 2023 17:14:24 +0200
Subject: [PATCH 19/45] use at(i) instead of [i]

---
 gtsam/inference/BayesTreeCliqueBase.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 171ebdc41..a7b1cf06c 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -147,7 +147,7 @@ namespace gtsam {
     size_t nrChildren() const { return children.size(); }
 
     /// Return the child at index i.
-    const derived_ptr operator[](size_t i) const { return children[i]; }
+    const derived_ptr operator[](size_t i) const { return children.at(i); }
 
     /** The size of subtree rooted at this clique, i.e., nr of Cliques */
     size_t treeSize() const;

From aafc33db696491bde6d0b89894e070a2f89b3a99 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Jul 2023 21:12:05 -0400
Subject: [PATCH 20/45] unary apply methods for TableFactor

---
 gtsam/discrete/TableFactor.cpp           | 41 +++++++++++++++++++++++-
 gtsam/discrete/TableFactor.h             | 28 ++++++++++++++--
 gtsam/discrete/tests/testTableFactor.cpp | 36 +++++++++++++++++++--
 3 files changed, 100 insertions(+), 5 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 74eb3ddb3..2be8e077d 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -64,7 +64,7 @@ TableFactor::TableFactor(const DiscreteConditional& c)
 Eigen::SparseVector<double> TableFactor::Convert(
     const std::vector<double>& table) {
   Eigen::SparseVector<double> sparse_table(table.size());
-  // Count number of nonzero elements in table and reserving the space.
+  // Count number of nonzero elements in table and reserve the space.
   const uint64_t nnz = std::count_if(table.begin(), table.end(),
                                      [](uint64_t i) { return i != 0; });
   sparse_table.reserve(nnz);
@@ -218,6 +218,45 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
   cout << "number of nnzs: " << sparse_table_.nonZeros() << endl;
 }
 
+/* ************************************************************************ */
+TableFactor TableFactor::apply(Unary op) const {
+  // Initialize new factor.
+  uint64_t cardi = 1;
+  for (auto [key, c] : cardinalities_) cardi *= c;
+  Eigen::SparseVector<double> sparse_table(cardi);
+  sparse_table.reserve(sparse_table_.nonZeros());
+
+  // Populate
+  for (SparseIt it(sparse_table_); it; ++it) {
+    sparse_table.coeffRef(it.index()) = op(it.value());
+  }
+
+  // Free unused memory and return.
+  sparse_table.pruned();
+  sparse_table.data().squeeze();
+  return TableFactor(discreteKeys(), sparse_table);
+}
+
+/* ************************************************************************ */
+TableFactor TableFactor::apply(UnaryAssignment op) const {
+  // Initialize new factor.
+  uint64_t cardi = 1;
+  for (auto [key, c] : cardinalities_) cardi *= c;
+  Eigen::SparseVector<double> sparse_table(cardi);
+  sparse_table.reserve(sparse_table_.nonZeros());
+
+  // Populate
+  for (SparseIt it(sparse_table_); it; ++it) {
+    DiscreteValues assignment = findAssignments(it.index());
+    sparse_table.coeffRef(it.index()) = op(assignment, it.value());
+  }
+
+  // Free unused memory and return.
+  sparse_table.pruned();
+  sparse_table.data().squeeze();
+  return TableFactor(discreteKeys(), sparse_table);
+}
+
 /* ************************************************************************ */
 TableFactor TableFactor::apply(const TableFactor& f, Binary op) const {
   if (keys_.empty() && sparse_table_.nonZeros() == 0)
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index bd637bb7d..981e1507b 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -93,6 +93,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   typedef std::shared_ptr<TableFactor> shared_ptr;
   typedef Eigen::SparseVector<double>::InnerIterator SparseIt;
   typedef std::vector<std::pair<DiscreteValues, double>> AssignValList;
+  using Unary = std::function<double(const double&)>;
+  using UnaryAssignment =
+      std::function<double(const Assignment<Key>&, const double&)>;
   using Binary = std::function<double(const double, const double)>;
 
  public:
@@ -218,6 +221,18 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   /// @name Advanced Interface
   /// @{
 
+  /**
+   * Apply unary operator `op(*this)` where `op` accepts the discrete value.
+   * @param op a unary operator that operates on TableFactor
+   */
+  TableFactor apply(Unary op) const;
+  /**
+   * Apply unary operator `op(*this)` where `op` accepts the discrete assignment
+   * and the value at that assignment.
+   * @param op a unary operator that operates on TableFactor
+   */
+  TableFactor apply(UnaryAssignment op) const;
+
   /**
    * Apply binary operator (*this) "op" f
    * @param f the second argument for op
@@ -225,10 +240,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
    */
   TableFactor apply(const TableFactor& f, Binary op) const;
 
-  /// Return keys in contract mode.
+  /**
+   * Return keys in contract mode.
+   *
+   * Modes are each of the dimensions of a sparse tensor,
+   * and the contract modes represent which dimensions will
+   * be involved in contraction (aka tensor multiplication).
+   */
   DiscreteKeys contractDkeys(const TableFactor& f) const;
 
-  /// Return keys in free mode.
+  /**
+   * @brief Return keys in free mode which are the dimensions
+   * not involved in the contraction operation.
+   */
   DiscreteKeys freeDkeys(const TableFactor& f) const;
 
   /// Return union of DiscreteKeys in two factors.
diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index b307d78f6..e85e4254c 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -93,8 +93,7 @@ void printTime(map<double, pair<chrono::microseconds, chrono::microseconds>>
   for (auto&& kv : measured_time) {
     cout << "dropout: " << kv.first
          << " | TableFactor time: " << kv.second.first.count()
-         << " | DecisionTreeFactor time: " << kv.second.second.count() <<
-         endl;
+         << " | DecisionTreeFactor time: " << kv.second.second.count() << endl;
   }
 }
 
@@ -361,6 +360,39 @@ TEST(TableFactor, htmlWithValueFormatter) {
   EXPECT(actual == expected);
 }
 
+/* ************************************************************************* */
+TEST(TableFactor, Unary) {
+  // Declare a bunch of keys
+  DiscreteKey X(0, 2), Y(1, 3);
+
+  // Create factors
+  TableFactor f(X & Y, "2 5 3 6 2 7");
+  auto op = [](const double x) { return 2 * x; };
+  auto g = f.apply(op);
+
+  TableFactor expected(X & Y, "4 10 6 12 4 14");
+  EXPECT(assert_equal(g, expected));
+
+  auto sq_op = [](const double x) { return x * x; };
+  auto g_sq = f.apply(sq_op);
+  TableFactor expected_sq(X & Y, "4 25 9 36 4 49");
+  EXPECT(assert_equal(g_sq, expected_sq));
+}
+
+/* ************************************************************************* */
+TEST(TableFactor, UnaryAssignment) {
+  // Declare a bunch of keys
+  DiscreteKey X(0, 2), Y(1, 3);
+
+  // Create factors
+  TableFactor f(X & Y, "2 5 3 6 2 7");
+  auto op = [](const Assignment<Key>& key, const double x) { return 2 * x; };
+  auto g = f.apply(op);
+
+  TableFactor expected(X & Y, "4 10 6 12 4 14");
+  EXPECT(assert_equal(g, expected));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From f6d42d0ee07662bf7baf1055239c73147f4e8705 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 3 Jul 2023 16:40:27 -0400
Subject: [PATCH 21/45] small improvements

---
 gtsam/discrete/DecisionTreeFactor.h |  3 +++
 gtsam/hybrid/HybridBayesNet.cpp     | 10 +++++-----
 gtsam/hybrid/HybridSmoother.cpp     |  3 ++-
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 6cce6e5d4..e90a2f96f 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -147,6 +147,9 @@ namespace gtsam {
     /// @name Advanced Interface
     /// @{
 
+    /// Inherit all the `apply` methods from AlgebraicDecisionTree
+    using ADT::apply;
+
     /**
      * Apply binary operator (*this) "op" f
      * @param f the second argument for op
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 266e02b0d..9df58814b 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -155,18 +155,18 @@ void HybridBayesNet::updateDiscreteConditionals(
       auto discrete = conditional->asDiscrete();
 
       // Convert pointer from conditional to factor
-      auto discreteTree =
-          std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
+      auto discreteFactor =
+          std::dynamic_pointer_cast<DecisionTreeFactor>(discrete);
       // Apply prunerFunc to the underlying AlgebraicDecisionTree
-      DecisionTreeFactor::ADT prunedDiscreteTree =
-          discreteTree->apply(prunerFunc(prunedDiscreteProbs, *conditional));
+      DecisionTreeFactor::ADT prunedDiscreteFactor =
+          discreteFactor->apply(prunerFunc(prunedDiscreteProbs, *conditional));
 
       gttic_(HybridBayesNet_MakeConditional);
       // Create the new (hybrid) conditional
       KeyVector frontals(discrete->frontals().begin(),
                          discrete->frontals().end());
       auto prunedDiscrete = std::make_shared<DiscreteLookupTable>(
-          frontals.size(), conditional->discreteKeys(), prunedDiscreteTree);
+          frontals.size(), conditional->discreteKeys(), prunedDiscreteFactor);
       conditional = std::make_shared<HybridConditional>(prunedDiscrete);
       gttoc_(HybridBayesNet_MakeConditional);
 
diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index 56c62cf19..27d3f70fc 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -72,7 +72,8 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph,
       addConditionals(graph, hybridBayesNet_, ordering);
 
   // Eliminate.
-  auto bayesNetFragment = graph.eliminateSequential(ordering);
+  HybridBayesNet::shared_ptr bayesNetFragment =
+      graph.eliminateSequential(ordering);
 
   /// Prune
   if (maxNrLeaves) {

From 53d00864bbf41d352c8f0bee765691fbcf7002c5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 5 Jul 2023 11:04:56 -0400
Subject: [PATCH 22/45] small cleanup

---
 gtsam/hybrid/HybridBayesNet.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 9df58814b..0db1b0c48 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -146,8 +146,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
 /* ************************************************************************* */
 void HybridBayesNet::updateDiscreteConditionals(
     const DecisionTreeFactor &prunedDiscreteProbs) {
-  KeyVector prunedTreeKeys = prunedDiscreteProbs.keys();
-
+  //TODO(Varun) Should prune the joint conditional, maybe during elimination?
   // Loop with index since we need it later.
   for (size_t i = 0; i < this->size(); i++) {
     HybridConditional::shared_ptr conditional = this->at(i);

From 6d69ca16dacc6269b55a85b55b0d8def40e62195 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 5 Jul 2023 11:09:14 -0400
Subject: [PATCH 23/45] add separate Hybrid ISAM and Smoother tests

---
 gtsam/hybrid/tests/testHybridEstimation.cpp | 55 +++++++++++++++++++++
 1 file changed, 55 insertions(+)

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index b5f5244fa..b8edc39d8 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -140,6 +140,61 @@ TEST(HybridEstimation, IncrementalSmoother) {
   EXPECT(assert_equal(expected_continuous, result));
 }
 
+/****************************************************************************/
+// Test approximate inference with an additional pruning step.
+TEST(HybridEstimation, ISAM) {
+  size_t K = 15;
+  std::vector<double> measurements = {0, 1, 2, 2, 2, 2,  3,  4,  5,  6, 6,
+                                      7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
+  // Ground truth discrete seq
+  std::vector<size_t> discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0,
+                                      1, 1, 1, 0, 0, 1, 1, 0, 0, 0};
+  // Switching example of robot moving in 1D
+  // with given measurements and equal mode priors.
+  Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1");
+  HybridNonlinearISAM isam;
+  HybridNonlinearFactorGraph graph;
+  Values initial;
+
+  // gttic_(Estimation);
+
+  // Add the X(0) prior
+  graph.push_back(switching.nonlinearFactorGraph.at(0));
+  initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
+
+  HybridGaussianFactorGraph linearized;
+
+  for (size_t k = 1; k < K; k++) {
+    // Motion Model
+    graph.push_back(switching.nonlinearFactorGraph.at(k));
+    // Measurement
+    graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1));
+
+    initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
+
+    isam.update(graph, initial, 3);
+    // isam.bayesTree().print("\n\n");
+
+    graph.resize(0);
+    initial.clear();
+  }
+
+  Values result = isam.estimate();
+  DiscreteValues assignment = isam.assignment();
+
+  DiscreteValues expected_discrete;
+  for (size_t k = 0; k < K - 1; k++) {
+    expected_discrete[M(k)] = discrete_seq[k];
+  }
+  EXPECT(assert_equal(expected_discrete, assignment));
+
+  Values expected_continuous;
+  for (size_t k = 0; k < K; k++) {
+    expected_continuous.insert(X(k), measurements[k]);
+  }
+  EXPECT(assert_equal(expected_continuous, result));
+}
+
 /**
  * @brief A function to get a specific 1D robot motion problem as a linearized
  * factor graph. This is the problem P(X|Z, M), i.e. estimating the continuous

From f751a5bfcf085f429cf48ce51f854587d1b28cad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 6 Jul 2023 16:37:22 -0400
Subject: [PATCH 24/45] overload apply method in DecisionTreeFactor

---
 gtsam/discrete/DecisionTreeFactor.cpp | 8 ++++++++
 gtsam/discrete/DecisionTreeFactor.h   | 7 +++++--
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index ff18268b1..56f1659dc 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -101,6 +101,14 @@ namespace gtsam {
     return DecisionTreeFactor(keys, result);
   }
 
+  /* ************************************************************************ */
+  DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
+    // apply operand
+    ADT result = ADT::apply(op);
+    // Make a new factor
+    return DecisionTreeFactor(discreteKeys(), result);
+  }
+
   /* ************************************************************************ */
   DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
       size_t nrFrontals, ADT::Binary op) const {
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index e90a2f96f..e92c82b77 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -147,8 +147,11 @@ namespace gtsam {
     /// @name Advanced Interface
     /// @{
 
-    /// Inherit all the `apply` methods from AlgebraicDecisionTree
-    using ADT::apply;
+    /**
+     * Apply unary operator (*this) "op" f
+     * @param op a unary operator that operates on AlgebraicDecisionTree
+     */
+    DecisionTreeFactor apply(ADT::UnaryAssignment op) const;
 
     /**
      * Apply binary operator (*this) "op" f

From 4e902fc8a71f2124b066d823474eeab63fec019c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 6 Jul 2023 22:22:51 -0400
Subject: [PATCH 25/45] fix continuousKeySet method

---
 gtsam/hybrid/HybridFactorGraph.cpp            |  2 ++
 gtsam/hybrid/tests/testHybridFactorGraph.cpp  | 28 +++++++++++++++++++
 .../tests/testHybridGaussianFactorGraph.cpp   |  2 +-
 3 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index d96a890f4..235ffc87f 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -67,6 +67,8 @@ const KeySet HybridFactorGraph::continuousKeySet() const {
       for (const Key& key : p->continuousKeys()) {
         keys.insert(key);
       }
+    } else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
+      keys.insert(p->keys().begin(), p->keys().end());
     }
   }
   return keys;
diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
index f5b4ec0b1..33c0761eb 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
@@ -18,7 +18,9 @@
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/utilities.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/nonlinear/PriorFactor.h>
 
 using namespace std;
@@ -37,6 +39,32 @@ TEST(HybridFactorGraph, Constructor) {
   HybridFactorGraph fg;
 }
 
+/* ************************************************************************* */
+// Test if methods to get keys work as expected.
+TEST(HybridFactorGraph, Keys) {
+  HybridGaussianFactorGraph hfg;
+
+  // Add prior on x0
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+
+  // Add factor between x0 and x1
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  // Add a gaussian mixture factor ϕ(x1, c1)
+  DiscreteKey m1(M(1), 2);
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+      M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+      std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
+  hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt));
+
+  KeySet expected_continuous{X(0), X(1)};
+  EXPECT(
+      assert_container_equality(expected_continuous, hfg.continuousKeySet()));
+
+  KeySet expected_discrete{M(1)};
+  EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet()));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 8276264ae..1da897103 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -902,7 +902,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
   // Test resulting posterior Bayes net has correct size:
   EXPECT_LONGS_EQUAL(8, posterior->size());
 
-  // TODO(dellaert): this test fails - no idea why.
+  // Ratio test
   EXPECT(ratioTest(bn, measurements, *posterior));
 }
 

From 4e13fb717bcc1a49358f25acd45a987246d011c5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 6 Jul 2023 23:07:38 -0400
Subject: [PATCH 26/45] simplify HybridEliminate

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 86 +++++++++++-----------
 1 file changed, 45 insertions(+), 41 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 2b23ed4db..d2ea3d5ef 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -348,64 +348,68 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
   // When the number of assignments is large we may encounter stack overflows.
   // However this is also the case with iSAM2, so no pressure :)
 
-  // PREPROCESS: Identify the nature of the current elimination
-
-  // TODO(dellaert): just check the factors:
+  // Check the factors:
   // 1. if all factors are discrete, then we can do discrete elimination:
   // 2. if all factors are continuous, then we can do continuous elimination:
   // 3. if not, we do hybrid elimination:
 
-  // First, identify the separator keys, i.e. all keys that are not frontal.
-  KeySet separatorKeys;
+  bool only_discrete = true, only_continuous = true;
   for (auto &&factor : factors) {
-    separatorKeys.insert(factor->begin(), factor->end());
-  }
-  // remove frontals from separator
-  for (auto &k : frontalKeys) {
-    separatorKeys.erase(k);
-  }
-
-  // Build a map from keys to DiscreteKeys
-  auto mapFromKeyToDiscreteKey = factors.discreteKeyMap();
-
-  // Fill in discrete frontals and continuous frontals.
-  std::set<DiscreteKey> discreteFrontals;
-  KeySet continuousFrontals;
-  for (auto &k : frontalKeys) {
-    if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
-      discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k));
-    } else {
-      continuousFrontals.insert(k);
+    if (auto hybrid_factor = std::dynamic_pointer_cast<HybridFactor>(factor)) {
+      if (hybrid_factor->isDiscrete()) {
+        only_continuous = false;
+      } else if (hybrid_factor->isContinuous()) {
+        only_discrete = false;
+      } else if (hybrid_factor->isHybrid()) {
+        only_continuous = false;
+        only_discrete = false;
+      }
+    } else if (auto cont_factor =
+                   std::dynamic_pointer_cast<GaussianFactor>(factor)) {
+      only_discrete = false;
+    } else if (auto discrete_factor =
+                   std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
+      only_continuous = false;
     }
   }
 
-  // Fill in discrete discrete separator keys and continuous separator keys.
-  std::set<DiscreteKey> discreteSeparatorSet;
-  KeyVector continuousSeparator;
-  for (auto &k : separatorKeys) {
-    if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
-      discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
-    } else {
-      continuousSeparator.push_back(k);
-    }
-  }
-
-  // Check if we have any continuous keys:
-  const bool discrete_only =
-      continuousFrontals.empty() && continuousSeparator.empty();
-
   // NOTE: We should really defer the product here because of pruning
 
-  if (discrete_only) {
+  if (only_discrete) {
     // Case 1: we are only dealing with discrete
     return discreteElimination(factors, frontalKeys);
-  } else if (mapFromKeyToDiscreteKey.empty()) {
+  } else if (only_continuous) {
     // Case 2: we are only dealing with continuous
     return continuousElimination(factors, frontalKeys);
   } else {
     // Case 3: We are now in the hybrid land!
+    KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end());
+
+    // Find all the keys in the set of continuous keys
+    // which are not in the frontal keys. This is our continuous separator.
+    KeyVector continuousSeparator;
+    auto continuousKeySet = factors.continuousKeySet();
+    std::set_difference(
+        continuousKeySet.begin(), continuousKeySet.end(),
+        frontalKeysSet.begin(), frontalKeysSet.end(),
+        std::inserter(continuousSeparator, continuousSeparator.begin()));
+
+    // Similarly for the discrete separator.
+    KeySet discreteSeparatorSet;
+    std::set<DiscreteKey> discreteSeparator;
+    auto discreteKeySet = factors.discreteKeySet();
+    std::set_difference(
+        discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
+        frontalKeysSet.end(),
+        std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
+    // Convert from set of keys to set of DiscreteKeys
+    auto discreteKeyMap = factors.discreteKeyMap();
+    for (auto key : discreteSeparatorSet) {
+      discreteSeparator.insert(discreteKeyMap.at(key));
+    }
+
     return hybridElimination(factors, frontalKeys, continuousSeparator,
-                             discreteSeparatorSet);
+                             discreteSeparator);
   }
 }
 

From f6b1872b1335ab22045aef998832b3aff8020558 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 8 Jul 2023 13:09:35 -0400
Subject: [PATCH 27/45] initial changes

---
 gtsam/hybrid/HybridFactorGraph.cpp          |  3 +--
 gtsam/hybrid/HybridGaussianFactorGraph.cpp  | 16 +++++++++-------
 gtsam/hybrid/HybridGaussianFactorGraph.h    |  1 +
 gtsam/hybrid/HybridJunctionTree.cpp         |  4 ++--
 gtsam/hybrid/HybridNonlinearFactorGraph.cpp |  3 ++-
 gtsam/hybrid/tests/testHybridBayesTree.cpp  |  2 +-
 6 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 235ffc87f..f7b96f694 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -17,7 +17,6 @@
  * @date   January, 2023
  */
 
-#include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 
 namespace gtsam {
@@ -26,7 +25,7 @@ namespace gtsam {
 std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
   std::set<DiscreteKey> keys;
   for (auto& factor : factors_) {
-    if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
+    if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
       for (const DiscreteKey& key : p->discreteKeys()) {
         keys.insert(key);
       }
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index d2ea3d5ef..fb4b69aaf 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -48,8 +48,6 @@
 #include <utility>
 #include <vector>
 
-// #define HYBRID_TIMING
-
 namespace gtsam {
 
 /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph:
@@ -120,7 +118,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
         // TODO(dellaert): in C++20, we can use std::visit.
         continue;
       }
-    } else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    } else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
       // Don't do anything for discrete-only factors
       // since we want to eliminate continuous values only.
       continue;
@@ -167,8 +165,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   DiscreteFactorGraph dfg;
 
   for (auto &f : factors) {
-    if (auto dtf = dynamic_pointer_cast<DecisionTreeFactor>(f)) {
-      dfg.push_back(dtf);
+    if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
+      dfg.push_back(df);
     } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
       // Ignore orphaned clique.
       // TODO(dellaert): is this correct? If so explain here.
@@ -262,9 +260,13 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
     };
 
     DecisionTree<Key, double> probabilities(eliminationResults, probability);
+
+    auto dtf =
+        std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities);
+
     return {
         std::make_shared<HybridConditional>(gaussianMixture),
-        std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities)};
+        std::make_shared<TableFactor>(discreteSeparator, dtf->probabilities())};
   } else {
     // Otherwise, we create a resulting GaussianMixtureFactor on the separator,
     // taking care to correct for conditional constant.
@@ -433,7 +435,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
       // Add the gaussian factor error to every leaf of the error tree.
       error_tree = error_tree.apply(
           [error](double leaf_value) { return leaf_value + error; });
-    } else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    } else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
       // If factor at `idx` is discrete-only, we skip.
       continue;
     } else {
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 421e69aa0..b3f159150 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -40,6 +40,7 @@ class HybridEliminationTree;
 class HybridBayesTree;
 class HybridJunctionTree;
 class DecisionTreeFactor;
+class TableFactor;
 class JacobianFactor;
 class HybridValues;
 
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 6f2898bf1..22d3c7dd2 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -66,7 +66,7 @@ struct HybridConstructorTraversalData {
         for (auto& k : hf->discreteKeys()) {
           data.discreteKeys.insert(k.first);
         }
-      } else if (auto hf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      } else if (auto hf = std::dynamic_pointer_cast<DiscreteFactor>(f)) {
         for (auto& k : hf->discreteKeys()) {
           data.discreteKeys.insert(k.first);
         }
@@ -161,7 +161,7 @@ HybridJunctionTree::HybridJunctionTree(
   Data rootData(0);
   rootData.junctionTreeNode =
       std::make_shared<typename Base::Node>();  // Make a dummy node to gather
-                                                  // the junction tree roots
+                                                // the junction tree roots
   treeTraversal::DepthFirstForest(eliminationTree, rootData,
                                   Data::ConstructorTraversalVisitorPre,
                                   Data::ConstructorTraversalVisitorPost);
diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp
index 260f534e3..2459e4ec9 100644
--- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp
@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
@@ -67,7 +68,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
     } else if (auto nlf = dynamic_pointer_cast<NonlinearFactor>(f)) {
       const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues);
       linearFG->push_back(gf);
-    } else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    } else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
       // If discrete-only: doesn't need linearization.
       linearFG->push_back(f);
     } else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp
index 578f5d605..81b257c32 100644
--- a/gtsam/hybrid/tests/testHybridBayesTree.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp
@@ -146,7 +146,7 @@ TEST(HybridBayesTree, Optimize) {
 
   DiscreteFactorGraph dfg;
   for (auto&& f : *remainingFactorGraph) {
-    auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(f);
+    auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(f);
     assert(discreteFactor);
     dfg.push_back(discreteFactor);
   }

From 2940e69a73479b88f20b7c46474e4eae5e0d9845 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 9 Jul 2023 20:24:24 -0400
Subject: [PATCH 28/45] discreteConditionals returns DiscreteConditional

---
 gtsam/hybrid/HybridBayesNet.cpp | 18 +++++++-----------
 gtsam/hybrid/HybridBayesNet.h   |  4 ++--
 2 files changed, 9 insertions(+), 13 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 0db1b0c48..3b5ab5b80 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -38,21 +38,17 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
 }
 
 /* ************************************************************************* */
-DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
-  AlgebraicDecisionTree<Key> discreteProbs;
-
+DiscreteConditional::shared_ptr HybridBayesNet::discreteConditionals() const {
   // The canonical decision tree factor which will get
   // the discrete conditionals added to it.
-  DecisionTreeFactor discreteProbsFactor;
+  DiscreteConditional discreteProbs;
 
   for (auto &&conditional : *this) {
     if (conditional->isDiscrete()) {
-      // Convert to a DecisionTreeFactor and add it to the main factor.
-      DecisionTreeFactor f(*conditional->asDiscrete());
-      discreteProbsFactor = discreteProbsFactor * f;
+      discreteProbs = discreteProbs * (*conditional->asDiscrete());
     }
   }
-  return std::make_shared<DecisionTreeFactor>(discreteProbsFactor);
+  return std::make_shared<DiscreteConditional>(discreteProbs);
 }
 
 /* ************************************************************************* */
@@ -146,8 +142,8 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
 /* ************************************************************************* */
 void HybridBayesNet::updateDiscreteConditionals(
     const DecisionTreeFactor &prunedDiscreteProbs) {
-  //TODO(Varun) Should prune the joint conditional, maybe during elimination?
-  // Loop with index since we need it later.
+  // TODO(Varun) Should prune the joint conditional, maybe during elimination?
+  //  Loop with index since we need it later.
   for (size_t i = 0; i < this->size(); i++) {
     HybridConditional::shared_ptr conditional = this->at(i);
     if (conditional->isDiscrete()) {
@@ -179,7 +175,7 @@ void HybridBayesNet::updateDiscreteConditionals(
 HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
   // Get the decision tree of only the discrete keys
   gttic_(HybridBayesNet_PruneDiscreteConditionals);
-  DecisionTreeFactor::shared_ptr discreteConditionals =
+  DiscreteConditional::shared_ptr discreteConditionals =
       this->discreteConditionals();
   const DecisionTreeFactor prunedDiscreteProbs =
       discreteConditionals->prune(maxNrLeaves);
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 23fc4d5d3..19e88d754 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -139,9 +139,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /**
    * @brief Get all the discrete conditionals as a decision tree factor.
    *
-   * @return DecisionTreeFactor::shared_ptr
+   * @return DiscreteConditional::shared_ptr
    */
-  DecisionTreeFactor::shared_ptr discreteConditionals() const;
+  DiscreteConditional::shared_ptr discreteConditionals() const;
 
   /**
    * @brief Sample from an incomplete BayesNet, given missing variables.

From 2f4133fd49ee3367c7ff7684187f84f4eeb49dc2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Jul 2023 19:39:28 -0400
Subject: [PATCH 29/45] Revert "remove nrAssignments from the DecisionTree"

This reverts commit 647d3c0744171ffa78f585a39a88bfa4be4d2002.
---
 gtsam/discrete/DecisionTree-inl.h         | 13 +++++++-
 gtsam/discrete/DecisionTree.h             | 36 +++++++++++++++++++++++
 gtsam/discrete/tests/testDecisionTree.cpp | 32 ++++++++++++++++++++
 3 files changed, 80 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 9d618dea0..19e3e2887 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -93,7 +93,8 @@ namespace gtsam {
     /// print
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
-      std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
+      std::cout << s << " Leaf [" << nrAssignments() << "]"
+                << valueFormatter(constant_) << std::endl;
     }
 
     /** Write graphviz format to stream `os`. */
@@ -827,6 +828,16 @@ namespace gtsam {
     return total;
   }
 
+  /****************************************************************************/
+  template <typename L, typename Y>
+  size_t DecisionTree<L, Y>::nrAssignments() const {
+    size_t n = 0;
+    this->visitLeaf([&n](const DecisionTree<L, Y>::Leaf& leaf) {
+      n += leaf.nrAssignments();
+    });
+    return n;
+  }
+
   /****************************************************************************/
   // fold is just done with a visit
   template <typename L, typename Y>
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index ed1908485..bee0ce5c7 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -299,6 +299,42 @@ namespace gtsam {
     /// Return the number of leaves in the tree.
     size_t nrLeaves() const;
 
+    /**
+     * @brief This is a convenience function which returns the total number of
+     * leaf assignments in the decision tree.
+     * This function is not used for anymajor operations within the discrete
+     * factor graph framework.
+     *
+     * Leaf assignments represent the cardinality of each leaf node, e.g. in a
+     * binary tree each leaf has 2 assignments. This includes counts removed
+     * from implicit pruning hence, it will always be >= nrLeaves().
+     *
+     * E.g. we have a decision tree as below, where each node has 2 branches:
+     *
+     * Choice(m1)
+     * 0 Choice(m0)
+     * 0 0 Leaf 0.0
+     * 0 1 Leaf 0.0
+     * 1 Choice(m0)
+     * 1 0 Leaf 1.0
+     * 1 1 Leaf 2.0
+     *
+     * In the unpruned form, the tree will have 4 assignments, 2 for each key,
+     * and 4 leaves.
+     *
+     * In the pruned form, the number of assignments is still 4 but the number
+     * of leaves is now 3, as below:
+     *
+     * Choice(m1)
+     * 0 Leaf 0.0
+     * 1 Choice(m0)
+     * 1 0 Leaf 1.0
+     * 1 1 Leaf 2.0
+     *
+     * @return size_t
+     */
+    size_t nrAssignments() const;
+
     /**
      * @brief Fold a binary function over the tree, returning accumulator.
      *
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index d2a94ddc3..395915068 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -531,6 +531,38 @@ TEST(DecisionTree, ApplyWithAssignment) {
   EXPECT_LONGS_EQUAL(5, count);
 }
 
+/* ************************************************************************** */
+// Test number of assignments.
+TEST(DecisionTree, NrAssignments2) {
+  using gtsam::symbol_shorthand::M;
+
+  std::vector<double> probs = {0, 0, 1, 2};
+
+  /* Create the decision tree
+    Choice(m1)
+    0 Leaf 0.000000
+    1 Choice(m0)
+    1 0 Leaf 1.000000
+    1 1 Leaf 2.000000
+  */
+  DiscreteKeys keys{{M(1), 2}, {M(0), 2}};
+  DecisionTree<Key, double> dt1(keys, probs);
+  EXPECT_LONGS_EQUAL(4, dt1.nrAssignments());
+
+  /* Create the DecisionTree
+    Choice(m1)
+    0 Choice(m0)
+    0 0 Leaf 0.000000
+    0 1 Leaf 1.000000
+    1 Choice(m0)
+    1 0 Leaf 0.000000
+    1 1 Leaf 2.000000
+  */
+  DiscreteKeys keys2{{M(0), 2}, {M(1), 2}};
+  DecisionTree<Key, double> dt2(keys2, probs);
+  EXPECT_LONGS_EQUAL(4, dt2.nrAssignments());
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From ddb36c2e7b20606bd31baeac0b190083f5d9c780 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Jul 2023 19:39:36 -0400
Subject: [PATCH 30/45] Revert "enumerate all assignments for computing
 probabilities to prune"

This reverts commit 8c38e45c83c6bbeb2ef5e7c7d73807b2eed6831f.
---
 .../tests/testGaussianMixtureFactor.cpp       |  4 +-
 .../tests/testHybridNonlinearFactorGraph.cpp  | 38 +++++++++----------
 2 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
index 75ba5a059..5207e9372 100644
--- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
@@ -108,7 +108,7 @@ TEST(GaussianMixtureFactor, Printing) {
   std::string expected =
       R"(Hybrid [x1 x2; 1]{
  Choice(1) 
- 0 Leaf :
+ 0 Leaf [1]:
   A[x1] = [
 	0;
 	0
@@ -120,7 +120,7 @@ TEST(GaussianMixtureFactor, Printing) {
   b = [ 0 0 ]
   No noise model
 
- 1 Leaf :
+ 1 Leaf [1]:
   A[x1] = [
 	0;
 	0
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index af3a23b94..7bcaf1762 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -492,7 +492,7 @@ factor 0:
 factor 1: 
 Hybrid [x0 x1; m0]{
  Choice(m0) 
- 0 Leaf :
+ 0 Leaf [1]:
   A[x0] = [
 	-1
 ]
@@ -502,7 +502,7 @@ Hybrid [x0 x1; m0]{
   b = [ -1 ]
   No noise model
 
- 1 Leaf :
+ 1 Leaf [1]:
   A[x0] = [
 	-1
 ]
@@ -516,7 +516,7 @@ Hybrid [x0 x1; m0]{
 factor 2: 
 Hybrid [x1 x2; m1]{
  Choice(m1) 
- 0 Leaf :
+ 0 Leaf [1]:
   A[x1] = [
 	-1
 ]
@@ -526,7 +526,7 @@ Hybrid [x1 x2; m1]{
   b = [ -1 ]
   No noise model
 
- 1 Leaf :
+ 1 Leaf [1]:
   A[x1] = [
 	-1
 ]
@@ -550,16 +550,16 @@ factor 4:
   b = [ -10 ]
   No noise model
 factor 5:  P( m0 ):
- Leaf  0.5
+ Leaf [2] 0.5
 
 factor 6:  P( m1 | m0 ):
  Choice(m1) 
  0 Choice(m0) 
- 0 0 Leaf 0.33333333
- 0 1 Leaf  0.6
+ 0 0 Leaf [1]0.33333333
+ 0 1 Leaf [1] 0.6
  1 Choice(m0) 
- 1 0 Leaf 0.66666667
- 1 1 Leaf  0.4
+ 1 0 Leaf [1]0.66666667
+ 1 1 Leaf [1] 0.4
 
 )";
   EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph));
@@ -570,13 +570,13 @@ size: 3
 conditional 0: Hybrid  P( x0 | x1 m0)
  Discrete Keys = (m0, 2), 
  Choice(m0) 
- 0 Leaf  p(x0 | x1)
+ 0 Leaf [1] p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.85087 ]
   No noise model
 
- 1 Leaf  p(x0 | x1)
+ 1 Leaf [1] p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.95037 ]
@@ -586,26 +586,26 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
  Choice(m1) 
  0 Choice(m0) 
- 0 0 Leaf  p(x1 | x2)
+ 0 0 Leaf [1] p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.99901 ]
   No noise model
 
- 0 1 Leaf  p(x1 | x2)
+ 0 1 Leaf [1] p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.90098 ]
   No noise model
 
  1 Choice(m0) 
- 1 0 Leaf  p(x1 | x2)
+ 1 0 Leaf [1] p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10.098 ]
   No noise model
 
- 1 1 Leaf  p(x1 | x2)
+ 1 1 Leaf [1] p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10 ]
@@ -615,14 +615,14 @@ conditional 2: Hybrid  P( x2 | m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
  Choice(m1) 
  0 Choice(m0) 
- 0 0 Leaf  p(x2)
+ 0 0 Leaf [1] p(x2)
   R = [ 10.0494 ]
   d = [ -10.1489 ]
   mean: 1 elements
   x2: -1.0099
   No noise model
 
- 0 1 Leaf  p(x2)
+ 0 1 Leaf [1] p(x2)
   R = [ 10.0494 ]
   d = [ -10.1479 ]
   mean: 1 elements
@@ -630,14 +630,14 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   No noise model
 
  1 Choice(m0) 
- 1 0 Leaf  p(x2)
+ 1 0 Leaf [1] p(x2)
   R = [ 10.0494 ]
   d = [ -10.0504 ]
   mean: 1 elements
   x2: -1.0001
   No noise model
 
- 1 1 Leaf  p(x2)
+ 1 1 Leaf [1] p(x2)
   R = [ 10.0494 ]
   d = [ -10.0494 ]
   mean: 1 elements

From f4adfac4fa4fff940165fa99283c31b7fd18123f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 12 Jul 2023 22:54:39 -0400
Subject: [PATCH 31/45] Undo TableFactor return in Hybrid GFG so we can group
 the changes together

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index fb4b69aaf..2d4ac83f6 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -261,12 +261,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
     DecisionTree<Key, double> probabilities(eliminationResults, probability);
 
-    auto dtf =
-        std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities);
-
     return {
         std::make_shared<HybridConditional>(gaussianMixture),
-        std::make_shared<TableFactor>(discreteSeparator, dtf->probabilities())};
+        std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities)};
   } else {
     // Otherwise, we create a resulting GaussianMixtureFactor on the separator,
     // taking care to correct for conditional constant.

From 6a26ecf971a30d822b586768a508338068c4a56f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Jul 2023 15:31:06 -0400
Subject: [PATCH 32/45] templetize functions in Switching fixture

---
 gtsam/hybrid/tests/Switching.h | 27 ++++++---------------------
 1 file changed, 6 insertions(+), 21 deletions(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 5842e1f1a..4b2d3f11b 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -202,31 +202,16 @@ struct Switching {
    * @brief Add "mode chain" to HybridNonlinearFactorGraph from M(0) to M(K-2).
    * E.g. if K=4, we want M0, M1 and M2.
    *
-   * @param fg The nonlinear factor graph to which the mode chain is added.
+   * @param fg The factor graph to which the mode chain is added.
    */
-  void addModeChain(HybridNonlinearFactorGraph *fg,
+  template <typename FACTORGRAPH>
+  void addModeChain(FACTORGRAPH *fg,
                     std::string discrete_transition_prob = "1/2 3/2") {
-    fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
+    fg->template emplace_shared<DiscreteDistribution>(modes[0], "1/1");
     for (size_t k = 0; k < K - 2; k++) {
       auto parents = {modes[k]};
-      fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
-                                              discrete_transition_prob);
-    }
-  }
-
-  /**
-   * @brief Add "mode chain" to HybridGaussianFactorGraph from M(0) to M(K-2).
-   * E.g. if K=4, we want M0, M1 and M2.
-   *
-   * @param fg The gaussian factor graph to which the mode chain is added.
-   */
-  void addModeChain(HybridGaussianFactorGraph *fg,
-                    std::string discrete_transition_prob = "1/2 3/2") {
-    fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
-    for (size_t k = 0; k < K - 2; k++) {
-      auto parents = {modes[k]};
-      fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
-                                              discrete_transition_prob);
+      fg->template emplace_shared<DiscreteConditional>(
+          modes[k + 1], parents, discrete_transition_prob);
     }
   }
 };

From f7071298c3b63bda597b7e6c9e33740ea62f1381 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Jul 2023 16:06:16 -0400
Subject: [PATCH 33/45] small improvements to comments and code structure

---
 gtsam/hybrid/HybridBayesNet.cpp | 7 +++----
 gtsam/hybrid/HybridSmoother.cpp | 3 ++-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 3b5ab5b80..ff2752bcb 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -39,8 +39,7 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
 
 /* ************************************************************************* */
 DiscreteConditional::shared_ptr HybridBayesNet::discreteConditionals() const {
-  // The canonical decision tree factor which will get
-  // the discrete conditionals added to it.
+  // The joint discrete probability.
   DiscreteConditional discreteProbs;
 
   for (auto &&conditional : *this) {
@@ -152,7 +151,7 @@ void HybridBayesNet::updateDiscreteConditionals(
       // Convert pointer from conditional to factor
       auto discreteFactor =
           std::dynamic_pointer_cast<DecisionTreeFactor>(discrete);
-      // Apply prunerFunc to the underlying AlgebraicDecisionTree
+      // Apply prunerFunc to the underlying conditional
       DecisionTreeFactor::ADT prunedDiscreteFactor =
           discreteFactor->apply(prunerFunc(prunedDiscreteProbs, *conditional));
 
@@ -173,7 +172,7 @@ void HybridBayesNet::updateDiscreteConditionals(
 
 /* ************************************************************************* */
 HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
-  // Get the decision tree of only the discrete keys
+  // Get the joint distribution of only the discrete keys
   gttic_(HybridBayesNet_PruneDiscreteConditionals);
   DiscreteConditional::shared_ptr discreteConditionals =
       this->discreteConditionals();
diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index 27d3f70fc..afa8340d2 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -97,7 +97,8 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
   HybridGaussianFactorGraph graph(originalGraph);
   HybridBayesNet hybridBayesNet(originalHybridBayesNet);
 
-  // If we are not at the first iteration, means we have conditionals to add.
+  // If hybridBayesNet is not empty,
+  // it means we have conditionals to add to the factor graph.
   if (!hybridBayesNet.empty()) {
     // We add all relevant conditional mixtures on the last continuous variable
     // in the previous `hybridBayesNet` to the graph

From 3fe2682d93d30e6ad011dd099cf428f680fe7bd2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 14 Jul 2023 17:52:22 -0400
Subject: [PATCH 34/45] prune joint discrete probability which is faster

---
 gtsam/hybrid/HybridBayesNet.cpp           | 85 ++++++++++-------------
 gtsam/hybrid/HybridBayesNet.h             | 13 +---
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 26 +++++--
 3 files changed, 58 insertions(+), 66 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index ff2752bcb..b4bf61220 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -37,19 +37,6 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
   return Base::equals(bn, tol);
 }
 
-/* ************************************************************************* */
-DiscreteConditional::shared_ptr HybridBayesNet::discreteConditionals() const {
-  // The joint discrete probability.
-  DiscreteConditional discreteProbs;
-
-  for (auto &&conditional : *this) {
-    if (conditional->isDiscrete()) {
-      discreteProbs = discreteProbs * (*conditional->asDiscrete());
-    }
-  }
-  return std::make_shared<DiscreteConditional>(discreteProbs);
-}
-
 /* ************************************************************************* */
 /**
  * @brief Helper function to get the pruner functional.
@@ -139,52 +126,52 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
 }
 
 /* ************************************************************************* */
-void HybridBayesNet::updateDiscreteConditionals(
-    const DecisionTreeFactor &prunedDiscreteProbs) {
-  // TODO(Varun) Should prune the joint conditional, maybe during elimination?
-  //  Loop with index since we need it later.
+DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
+    size_t maxNrLeaves) {
+  // Get the joint distribution of only the discrete keys
+  gttic_(HybridBayesNet_PruneDiscreteConditionals);
+  // The joint discrete probability.
+  DiscreteConditional discreteProbs;
+
+  std::vector<size_t> discrete_factor_idxs;
+  // Record frontal keys so we can maintain ordering
+  Ordering discrete_frontals;
+
   for (size_t i = 0; i < this->size(); i++) {
-    HybridConditional::shared_ptr conditional = this->at(i);
+    auto conditional = this->at(i);
     if (conditional->isDiscrete()) {
-      auto discrete = conditional->asDiscrete();
+      discreteProbs = discreteProbs * (*conditional->asDiscrete());
 
-      // Convert pointer from conditional to factor
-      auto discreteFactor =
-          std::dynamic_pointer_cast<DecisionTreeFactor>(discrete);
-      // Apply prunerFunc to the underlying conditional
-      DecisionTreeFactor::ADT prunedDiscreteFactor =
-          discreteFactor->apply(prunerFunc(prunedDiscreteProbs, *conditional));
-
-      gttic_(HybridBayesNet_MakeConditional);
-      // Create the new (hybrid) conditional
-      KeyVector frontals(discrete->frontals().begin(),
-                         discrete->frontals().end());
-      auto prunedDiscrete = std::make_shared<DiscreteLookupTable>(
-          frontals.size(), conditional->discreteKeys(), prunedDiscreteFactor);
-      conditional = std::make_shared<HybridConditional>(prunedDiscrete);
-      gttoc_(HybridBayesNet_MakeConditional);
-
-      // Add it back to the BayesNet
-      this->at(i) = conditional;
+      Ordering conditional_keys(conditional->frontals());
+      discrete_frontals += conditional_keys;
+      discrete_factor_idxs.push_back(i);
     }
   }
+  const DecisionTreeFactor prunedDiscreteProbs =
+      discreteProbs.prune(maxNrLeaves);
+  gttoc_(HybridBayesNet_PruneDiscreteConditionals);
+
+  // Eliminate joint probability back into conditionals
+  gttic_(HybridBayesNet_UpdateDiscreteConditionals);
+  DiscreteFactorGraph dfg{prunedDiscreteProbs};
+  DiscreteBayesNet::shared_ptr dbn = dfg.eliminateSequential(discrete_frontals);
+
+  // Assign pruned discrete conditionals back at the correct indices.
+  for (size_t i = 0; i < discrete_factor_idxs.size(); i++) {
+    size_t idx = discrete_factor_idxs.at(i);
+    this->at(idx) = std::make_shared<HybridConditional>(dbn->at(i));
+  }
+  gttoc_(HybridBayesNet_UpdateDiscreteConditionals);
+
+  return prunedDiscreteProbs;
 }
 
 /* ************************************************************************* */
 HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
-  // Get the joint distribution of only the discrete keys
-  gttic_(HybridBayesNet_PruneDiscreteConditionals);
-  DiscreteConditional::shared_ptr discreteConditionals =
-      this->discreteConditionals();
-  const DecisionTreeFactor prunedDiscreteProbs =
-      discreteConditionals->prune(maxNrLeaves);
-  gttoc_(HybridBayesNet_PruneDiscreteConditionals);
+  DecisionTreeFactor prunedDiscreteProbs =
+      this->pruneDiscreteConditionals(maxNrLeaves);
 
-  gttic_(HybridBayesNet_UpdateDiscreteConditionals);
-  this->updateDiscreteConditionals(prunedDiscreteProbs);
-  gttoc_(HybridBayesNet_UpdateDiscreteConditionals);
-
-  /* To Prune, we visitWith every leaf in the GaussianMixture.
+  /* To prune, we visitWith every leaf in the GaussianMixture.
    * For each leaf, using the assignment we can check the discrete decision tree
    * for 0.0 probability, then just set the leaf to a nullptr.
    *
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 19e88d754..e71cfe9b4 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -136,13 +136,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
    */
   VectorValues optimize(const DiscreteValues &assignment) const;
 
-  /**
-   * @brief Get all the discrete conditionals as a decision tree factor.
-   *
-   * @return DiscreteConditional::shared_ptr
-   */
-  DiscreteConditional::shared_ptr discreteConditionals() const;
-
   /**
    * @brief Sample from an incomplete BayesNet, given missing variables.
    *
@@ -222,11 +215,11 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
 
  private:
   /**
-   * @brief Update the discrete conditionals with the pruned versions.
+   * @brief Prune all the discrete conditionals.
    *
-   * @param prunedDiscreteProbs
+   * @param maxNrLeaves
    */
-  void updateDiscreteConditionals(const DecisionTreeFactor &prunedDiscreteProbs);
+  DecisionTreeFactor pruneDiscreteConditionals(size_t maxNrLeaves);
 
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index f25675a55..1dfcbd6b7 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -231,7 +231,7 @@ TEST(HybridBayesNet, Pruning) {
   auto prunedTree = prunedBayesNet.evaluate(delta.continuous());
 
   // Regression test on pruned logProbability tree
-  std::vector<double> pruned_leaves = {0.0, 20.346113, 0.0, 19.738098};
+  std::vector<double> pruned_leaves = {0.0, 32.713418, 0.0, 31.735823};
   AlgebraicDecisionTree<Key> expected_pruned(discrete_keys, pruned_leaves);
   EXPECT(assert_equal(expected_pruned, prunedTree, 1e-6));
 
@@ -248,8 +248,10 @@ TEST(HybridBayesNet, Pruning) {
   logProbability +=
       posterior->at(4)->asDiscrete()->logProbability(hybridValues);
 
+  // Regression
   double density = exp(logProbability);
-  EXPECT_DOUBLES_EQUAL(density, actualTree(discrete_values), 1e-9);
+  EXPECT_DOUBLES_EQUAL(density,
+                       1.6078460548731697 * actualTree(discrete_values), 1e-6);
   EXPECT_DOUBLES_EQUAL(density, prunedTree(discrete_values), 1e-9);
   EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues),
                        1e-9);
@@ -283,20 +285,30 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
   EXPECT_LONGS_EQUAL(7, posterior->size());
 
   size_t maxNrLeaves = 3;
-  auto discreteConditionals = posterior->discreteConditionals();
+  DiscreteConditional discreteConditionals;
+  for (auto&& conditional : *posterior) {
+    if (conditional->isDiscrete()) {
+      discreteConditionals =
+          discreteConditionals * (*conditional->asDiscrete());
+    }
+  }
   const DecisionTreeFactor::shared_ptr prunedDecisionTree =
       std::make_shared<DecisionTreeFactor>(
-          discreteConditionals->prune(maxNrLeaves));
+          discreteConditionals.prune(maxNrLeaves));
 
   EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/,
                      prunedDecisionTree->nrLeaves());
 
-  auto original_discrete_conditionals = *(posterior->at(4)->asDiscrete());
+  // regression
+  DiscreteKeys dkeys{{M(0), 2}, {M(1), 2}, {M(2), 2}};
+  DecisionTreeFactor::ADT potentials(
+      dkeys, std::vector<double>{0, 0, 0, 0.505145423, 0, 1, 0, 0.494854577});
+  DiscreteConditional expected_discrete_conditionals(1, dkeys, potentials);
 
   // Prune!
   posterior->prune(maxNrLeaves);
 
-  // Functor to verify values against the original_discrete_conditionals
+  // Functor to verify values against the expected_discrete_conditionals
   auto checker = [&](const Assignment<Key>& assignment,
                      double probability) -> double {
     // typecast so we can use this to get probability value
@@ -304,7 +316,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
     if (prunedDecisionTree->operator()(choices) == 0) {
       EXPECT_DOUBLES_EQUAL(0.0, probability, 1e-9);
     } else {
-      EXPECT_DOUBLES_EQUAL(original_discrete_conditionals(choices), probability,
+      EXPECT_DOUBLES_EQUAL(expected_discrete_conditionals(choices), probability,
                            1e-9);
     }
     return 0.0;

From 2f3fcff9160a623eb6021c013240a5965e6876ec Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Jul 2023 12:47:19 -0400
Subject: [PATCH 35/45] fix tests

---
 gtsam/discrete/tests/testDecisionTree.cpp | 33 +----------------------
 gtsam/hybrid/tests/testMixtureFactor.cpp  |  4 +--
 2 files changed, 3 insertions(+), 34 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 395915068..efa7a1c44 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -25,6 +25,7 @@
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/discrete/Signature.h>
+#include <gtsam/inference/Symbol.h>
 
 #include <iomanip>
 
@@ -531,38 +532,6 @@ TEST(DecisionTree, ApplyWithAssignment) {
   EXPECT_LONGS_EQUAL(5, count);
 }
 
-/* ************************************************************************** */
-// Test number of assignments.
-TEST(DecisionTree, NrAssignments2) {
-  using gtsam::symbol_shorthand::M;
-
-  std::vector<double> probs = {0, 0, 1, 2};
-
-  /* Create the decision tree
-    Choice(m1)
-    0 Leaf 0.000000
-    1 Choice(m0)
-    1 0 Leaf 1.000000
-    1 1 Leaf 2.000000
-  */
-  DiscreteKeys keys{{M(1), 2}, {M(0), 2}};
-  DecisionTree<Key, double> dt1(keys, probs);
-  EXPECT_LONGS_EQUAL(4, dt1.nrAssignments());
-
-  /* Create the DecisionTree
-    Choice(m1)
-    0 Choice(m0)
-    0 0 Leaf 0.000000
-    0 1 Leaf 1.000000
-    1 Choice(m0)
-    1 0 Leaf 0.000000
-    1 1 Leaf 2.000000
-  */
-  DiscreteKeys keys2{{M(0), 2}, {M(1), 2}};
-  DecisionTree<Key, double> dt2(keys2, probs);
-  EXPECT_LONGS_EQUAL(4, dt2.nrAssignments());
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp
index 67a7fd8ae..79188b909 100644
--- a/gtsam/hybrid/tests/testMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testMixtureFactor.cpp
@@ -63,8 +63,8 @@ TEST(MixtureFactor, Printing) {
       R"(Hybrid [x1 x2; 1]
 MixtureFactor
  Choice(1) 
- 0 Leaf Nonlinear factor on 2 keys
- 1 Leaf Nonlinear factor on 2 keys
+ 0 Leaf [1]Nonlinear factor on 2 keys
+ 1 Leaf [1]Nonlinear factor on 2 keys
 )";
   EXPECT(assert_print_equal(expected, mixtureFactor));
 }

From 103641c51ad89738fd90c414fede5de957c1e7c2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Jul 2023 13:31:19 -0400
Subject: [PATCH 36/45] include fix

---
 gtsam/hybrid/HybridFactor.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 13d5c2cba..afd1c8032 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -20,6 +20,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Factor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>

From 89941683a218f22bc37e6dc827c7c198373da4f0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Jul 2023 17:56:19 -0400
Subject: [PATCH 37/45] improved exception message when there are lots of keys

---
 gtsam/inference/inferenceExceptions.h         | 24 ++++++++++-------
 .../linear/tests/testGaussianFactorGraph.cpp  | 26 ++++++++++++++++++-
 2 files changed, 40 insertions(+), 10 deletions(-)

diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h
index 061abeb85..339accad6 100644
--- a/gtsam/inference/inferenceExceptions.h
+++ b/gtsam/inference/inferenceExceptions.h
@@ -29,7 +29,7 @@ namespace gtsam {
  * full elimination routine was called with an ordering that does not include
  * all of the variables. */
 class InconsistentEliminationRequested : public std::exception {
-  KeySet keys_;
+  KeyVector keys_;
   const KeyFormatter& keyFormatter = DefaultKeyFormatter;
 
  public:
@@ -38,21 +38,26 @@ class InconsistentEliminationRequested : public std::exception {
   InconsistentEliminationRequested(
       const KeySet& keys,
       const KeyFormatter& key_formatter = DefaultKeyFormatter)
-      : keys_(keys), keyFormatter(key_formatter) {}
+      : keys_(keys.begin(), keys.end()), keyFormatter(key_formatter) {}
 
   ~InconsistentEliminationRequested() noexcept override {}
   const char* what() const noexcept override {
     // Format keys for printing
     std::stringstream sstr;
-    for (auto key : keys_) {
-      sstr << keyFormatter(key) << ", ";
+    size_t nrKeysToDisplay = std::min(size_t(4), keys_.size());
+    for (size_t i = 0; i < nrKeysToDisplay; i++) {
+      sstr << keyFormatter(keys_.at(i));
+      if (i < nrKeysToDisplay - 1) {
+        sstr << ", ";
+      }
     }
+    if (keys_.size() > nrKeysToDisplay) {
+      sstr << ", ... (total " << keys_.size() << " keys)";
+    }
+    sstr << ".";
     std::string keys = sstr.str();
-    // remove final comma and space.
-    keys.pop_back();
-    keys.pop_back();
 
-    static std::string msg =
+    std::string msg =
         "An inference algorithm was called with inconsistent "
         "arguments.  "
         "The\n"
@@ -64,7 +69,8 @@ class InconsistentEliminationRequested : public std::exception {
         "that\n"
         "does not include all of the variables.\n";
     msg += ("Leftover keys after elimination: " + keys);
-    return msg.c_str();
+    // `new` to allocate memory on heap instead of stack
+    return (new std::string(msg))->c_str();
   }
 };
 }  // namespace gtsam
diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp
index b140c3d85..65d05b018 100644
--- a/gtsam/linear/tests/testGaussianFactorGraph.cpp
+++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp
@@ -465,7 +465,31 @@ TEST(GaussianFactorGraph, InconsistentEliminationMessage) {
         "an ordering "
         "that\n"
         "does not include all of the variables.\n"
-        "Leftover keys after elimination: 2, x3";
+        "Leftover keys after elimination: 2, x3.";
+    EXPECT(expected_exception_message == exc.what());
+  }
+
+  // Test large number of keys
+  fg = GaussianFactorGraph();
+  for (size_t i = 0; i < 1000; i++) {
+    fg.emplace_shared<JacobianFactor>(i, -I_2x2, i + 1, I_2x2,
+                                      Vector2(2.0, -1.0), unit2);
+  }
+
+  try {
+    fg.eliminateSequential(ordering);
+  } catch (const exception& exc) {
+    std::string expected_exception_message = "An inference algorithm was called with inconsistent "
+        "arguments.  "
+        "The\n"
+        "factor graph, ordering, or variable index were "
+        "inconsistent with "
+        "each\n"
+        "other, or a full elimination routine was called with "
+        "an ordering "
+        "that\n"
+        "does not include all of the variables.\n"
+        "Leftover keys after elimination: 2, 3, 4, 5, ... (total 999 keys).";
     EXPECT(expected_exception_message == exc.what());
   }
 }

From cb084b3c16bc532f82bff256038ddd771697d12b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Jul 2023 10:21:56 -0400
Subject: [PATCH 38/45] Fix spacing in DecisionTree::print

---
 gtsam/discrete/DecisionTree-inl.h              |  2 +-
 .../hybrid/tests/testGaussianMixtureFactor.cpp |  4 ++--
 .../tests/testHybridNonlinearFactorGraph.cpp   | 18 +++++++++---------
 gtsam/hybrid/tests/testMixtureFactor.cpp       |  4 ++--
 gtsam/linear/GaussianConditional.cpp           |  2 +-
 5 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 19e3e2887..f998a6065 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -93,7 +93,7 @@ namespace gtsam {
     /// print
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
-      std::cout << s << " Leaf [" << nrAssignments() << "]"
+      std::cout << s << " Leaf [" << nrAssignments() << "] "
                 << valueFormatter(constant_) << std::endl;
     }
 
diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
index 5207e9372..549223497 100644
--- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
@@ -108,7 +108,7 @@ TEST(GaussianMixtureFactor, Printing) {
   std::string expected =
       R"(Hybrid [x1 x2; 1]{
  Choice(1) 
- 0 Leaf [1]:
+ 0 Leaf [1] :
   A[x1] = [
 	0;
 	0
@@ -120,7 +120,7 @@ TEST(GaussianMixtureFactor, Printing) {
   b = [ 0 0 ]
   No noise model
 
- 1 Leaf [1]:
+ 1 Leaf [1] :
   A[x1] = [
 	0;
 	0
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 7bcaf1762..12506b8af 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -492,7 +492,7 @@ factor 0:
 factor 1: 
 Hybrid [x0 x1; m0]{
  Choice(m0) 
- 0 Leaf [1]:
+ 0 Leaf [1] :
   A[x0] = [
 	-1
 ]
@@ -502,7 +502,7 @@ Hybrid [x0 x1; m0]{
   b = [ -1 ]
   No noise model
 
- 1 Leaf [1]:
+ 1 Leaf [1] :
   A[x0] = [
 	-1
 ]
@@ -516,7 +516,7 @@ Hybrid [x0 x1; m0]{
 factor 2: 
 Hybrid [x1 x2; m1]{
  Choice(m1) 
- 0 Leaf [1]:
+ 0 Leaf [1] :
   A[x1] = [
 	-1
 ]
@@ -526,7 +526,7 @@ Hybrid [x1 x2; m1]{
   b = [ -1 ]
   No noise model
 
- 1 Leaf [1]:
+ 1 Leaf [1] :
   A[x1] = [
 	-1
 ]
@@ -550,16 +550,16 @@ factor 4:
   b = [ -10 ]
   No noise model
 factor 5:  P( m0 ):
- Leaf [2] 0.5
+ Leaf [2]  0.5
 
 factor 6:  P( m1 | m0 ):
  Choice(m1) 
  0 Choice(m0) 
- 0 0 Leaf [1]0.33333333
- 0 1 Leaf [1] 0.6
+ 0 0 Leaf [1] 0.33333333
+ 0 1 Leaf [1]  0.6
  1 Choice(m0) 
- 1 0 Leaf [1]0.66666667
- 1 1 Leaf [1] 0.4
+ 1 0 Leaf [1] 0.66666667
+ 1 1 Leaf [1]  0.4
 
 )";
   EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph));
diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp
index 79188b909..03fdccff2 100644
--- a/gtsam/hybrid/tests/testMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testMixtureFactor.cpp
@@ -63,8 +63,8 @@ TEST(MixtureFactor, Printing) {
       R"(Hybrid [x1 x2; 1]
 MixtureFactor
  Choice(1) 
- 0 Leaf [1]Nonlinear factor on 2 keys
- 1 Leaf [1]Nonlinear factor on 2 keys
+ 0 Leaf [1] Nonlinear factor on 2 keys
+ 1 Leaf [1] Nonlinear factor on 2 keys
 )";
   EXPECT(assert_print_equal(expected, mixtureFactor));
 }
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 188c31abe..0112835aa 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -99,7 +99,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
-    cout << s << " p(";
+    cout << (s.empty() ? "" : s + " ") << "p(";
     for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
       cout << formatter(*it) << (nrFrontals() > 1 ? " " : "");
     }

From e693e1bf8cbced77a3d87cefa68b64259f5a2f88 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Jul 2023 11:05:41 -0400
Subject: [PATCH 39/45] break inferenceExceptions into .h and .cpp files

---
 gtsam/inference/inferenceExceptions.cpp | 60 +++++++++++++++++++++++++
 gtsam/inference/inferenceExceptions.h   | 36 ++-------------
 2 files changed, 63 insertions(+), 33 deletions(-)
 create mode 100644 gtsam/inference/inferenceExceptions.cpp

diff --git a/gtsam/inference/inferenceExceptions.cpp b/gtsam/inference/inferenceExceptions.cpp
new file mode 100644
index 000000000..8c2a95337
--- /dev/null
+++ b/gtsam/inference/inferenceExceptions.cpp
@@ -0,0 +1,60 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    inferenceExceptions.cpp
+ * @brief   Exceptions that may be thrown by inference algorithms
+ * @author  Richard Roberts, Varun Agrawal
+ * @date    Apr 25, 2013
+ */
+
+#include <gtsam/inference/inferenceExceptions.h>
+
+#include <sstream>
+
+namespace gtsam {
+
+InconsistentEliminationRequested::InconsistentEliminationRequested(
+    const KeySet& keys, const KeyFormatter& key_formatter)
+    : keys_(keys.begin(), keys.end()), keyFormatter(key_formatter) {}
+
+const char* InconsistentEliminationRequested::what() const noexcept {
+  // Format keys for printing
+  std::stringstream sstr;
+  size_t nrKeysToDisplay = std::min(size_t(4), keys_.size());
+  for (size_t i = 0; i < nrKeysToDisplay; i++) {
+    sstr << keyFormatter(keys_.at(i));
+    if (i < nrKeysToDisplay - 1) {
+      sstr << ", ";
+    }
+  }
+  if (keys_.size() > nrKeysToDisplay) {
+    sstr << ", ... (total " << keys_.size() << " keys)";
+  }
+  sstr << ".";
+  std::string keys = sstr.str();
+
+  std::string msg =
+      "An inference algorithm was called with inconsistent "
+      "arguments.  "
+      "The\n"
+      "factor graph, ordering, or variable index were "
+      "inconsistent with "
+      "each\n"
+      "other, or a full elimination routine was called with "
+      "an ordering "
+      "that\n"
+      "does not include all of the variables.\n";
+  msg += ("Leftover keys after elimination: " + keys);
+  // `new` to allocate memory on heap instead of stack
+  return (new std::string(msg))->c_str();
+}
+}  // namespace gtsam
diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h
index 339accad6..eb5d79e6f 100644
--- a/gtsam/inference/inferenceExceptions.h
+++ b/gtsam/inference/inferenceExceptions.h
@@ -18,9 +18,9 @@
 #pragma once
 
 #include <gtsam/global_includes.h>
+#include <gtsam/inference/Key.h>
 
 #include <exception>
-#include <sstream>
 
 namespace gtsam {
 
@@ -37,40 +37,10 @@ class InconsistentEliminationRequested : public std::exception {
 
   InconsistentEliminationRequested(
       const KeySet& keys,
-      const KeyFormatter& key_formatter = DefaultKeyFormatter)
-      : keys_(keys.begin(), keys.end()), keyFormatter(key_formatter) {}
+      const KeyFormatter& key_formatter = DefaultKeyFormatter);
 
   ~InconsistentEliminationRequested() noexcept override {}
-  const char* what() const noexcept override {
-    // Format keys for printing
-    std::stringstream sstr;
-    size_t nrKeysToDisplay = std::min(size_t(4), keys_.size());
-    for (size_t i = 0; i < nrKeysToDisplay; i++) {
-      sstr << keyFormatter(keys_.at(i));
-      if (i < nrKeysToDisplay - 1) {
-        sstr << ", ";
-      }
-    }
-    if (keys_.size() > nrKeysToDisplay) {
-      sstr << ", ... (total " << keys_.size() << " keys)";
-    }
-    sstr << ".";
-    std::string keys = sstr.str();
 
-    std::string msg =
-        "An inference algorithm was called with inconsistent "
-        "arguments.  "
-        "The\n"
-        "factor graph, ordering, or variable index were "
-        "inconsistent with "
-        "each\n"
-        "other, or a full elimination routine was called with "
-        "an ordering "
-        "that\n"
-        "does not include all of the variables.\n";
-    msg += ("Leftover keys after elimination: " + keys);
-    // `new` to allocate memory on heap instead of stack
-    return (new std::string(msg))->c_str();
-  }
+  const char* what() const noexcept override;
 };
 }  // namespace gtsam

From 53f89d0454aa1930ec0cd66c25bb2aebfaa31fad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Jul 2023 11:11:42 -0400
Subject: [PATCH 40/45] restore header ordering

---
 .../linear/tests/testGaussianFactorGraph.cpp  | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp
index 65d05b018..e4a3d14e8 100644
--- a/gtsam/linear/tests/testGaussianFactorGraph.cpp
+++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp
@@ -18,16 +18,17 @@
  *  @author Richard Roberts
  **/
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/VerticalBlockMatrix.h>
-#include <gtsam/base/debug.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/inference/VariableSlots.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/VariableSlots.h>
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;

From 57291e132faf3b395e9edbd04c4b3daaa249f8c5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 19 Jul 2023 13:09:13 +0200
Subject: [PATCH 41/45] added examples and a test on pMin

---
 gtsam/sfm/ShonanAveraging.cpp              |  3 +++
 gtsam/sfm/tests/testShonanAveraging.cpp    | 14 ++++++++++++++
 python/gtsam/tests/test_ShonanAveraging.py | 19 +++++++++++++++++--
 3 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 8ace9d98c..7c8b07f37 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -894,6 +894,9 @@ template <size_t d>
 std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
                                                   size_t pMin,
                                                   size_t pMax) const {
+  if (pMin < d) {
+    throw std::runtime_error("pMin is smaller than the base dimension d");
+  }
   Values Qstar;
   Values initialSOp = LiftTo<Rot>(pMin, initialEstimate);  // lift to pMin!
   for (size_t p = pMin; p <= pMax; p++) {
diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp
index d398a2a87..dd4759daa 100644
--- a/gtsam/sfm/tests/testShonanAveraging.cpp
+++ b/gtsam/sfm/tests/testShonanAveraging.cpp
@@ -415,6 +415,20 @@ TEST(ShonanAveraging3, PriorWeights) {
   auto result = shonan.run(initial, 3, 3);
   EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
 }
+
+/* ************************************************************************* */
+// Check a small graph created using binary measurements
+TEST(ShonanAveraging3, BinaryMeasurements) {
+  std::vector<BinaryMeasurement<Rot3>> measurements;
+  auto unit3 = noiseModel::Unit::Create(3);
+  measurements.emplace_back(0, 1, Rot3::Yaw(M_PI_2), unit3);
+  measurements.emplace_back(1, 2, Rot3::Yaw(M_PI_2), unit3);
+  ShonanAveraging3 shonan(measurements);
+  Values initial = shonan.initializeRandomly();
+  auto result = shonan.run(initial, 3, 5);
+  EXPECT_DOUBLES_EQUAL(0.0, shonan.cost(result.first), 1e-4);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
index 845f6cf1c..fc0943772 100644
--- a/python/gtsam/tests/test_ShonanAveraging.py
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -10,14 +10,16 @@ Author: Frank Dellaert
 """
 # pylint: disable=invalid-name, no-name-in-module, no-member
 
+import math
 import unittest
 
 import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2,
-                   ShonanAveraging2, ShonanAveraging3,
+from gtsam import (BetweenFactorPose2, BetweenFactorPose3,
+                   BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2,
+                   Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3,
                    ShonanAveragingParameters2, ShonanAveragingParameters3)
 
 DEFAULT_PARAMS = ShonanAveragingParameters3(
@@ -197,6 +199,19 @@ class TestShonanAveraging(GtsamTestCase):
         expected_thetas_deg = np.array([0.0, 90.0, 0.0])
         np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
 
+    def test_measurements3(self):
+        """Create from Measurements."""
+        measurements = []
+        unit3 = gtsam.noiseModel.Unit.Create(3)
+        m01 = BinaryMeasurementRot3(0, 1, Rot3.Yaw(math.radians(90)), unit3)
+        m12 = BinaryMeasurementRot3(1, 2, Rot3.Yaw(math.radians(90)), unit3)
+        measurements.append(m01)
+        measurements.append(m12)
+        obj = ShonanAveraging3(measurements)
+        self.assertIsInstance(obj, ShonanAveraging3)
+        initial = obj.initializeRandomly()
+        _, cost = obj.run(initial, min_p=3, max_p=5)
+        self.assertAlmostEqual(cost, 0)
 
 if __name__ == "__main__":
     unittest.main()

From 62d59c62d4497b76d07f736e900f897845e8d6b3 Mon Sep 17 00:00:00 2001
From: Tal Regev <tal.regev@gmail.com>
Date: Thu, 20 Jul 2023 08:03:42 +0300
Subject: [PATCH 42/45] Fix windows test

---
 .github/workflows/build-windows.yml           | 78 ++++++++++++++-----
 CMakeLists.txt                                |  8 ++
 gtsam/basis/Basis.h                           |  2 +-
 gtsam/discrete/SignatureParser.h              |  4 +-
 gtsam/geometry/Line3.h                        |  2 +-
 gtsam/geometry/Pose2.h                        | 40 +++++-----
 gtsam/geometry/Pose3.h                        |  2 +-
 gtsam/hybrid/HybridFactorGraph.h              |  2 +-
 gtsam/hybrid/HybridSmoother.h                 |  2 +-
 .../tests/testHybridGaussianFactorGraph.cpp   |  1 +
 gtsam/inference/Ordering.h                    | 21 +++--
 gtsam_unstable/linear/LPInitSolver.h          |  3 +-
 gtsam_unstable/linear/QPSParser.h             |  3 +-
 13 files changed, 108 insertions(+), 60 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index cbe0c10f1..565a02756 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -99,26 +99,64 @@ jobs:
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
 
       - name: Build
+        shell: cmd
         run: |
-          # Since Visual Studio is a multi-generator, we need to use --config
-          # https://stackoverflow.com/a/24470998/1236990
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
+          :: Since Visual Studio is a multi-generator, we need to use --config
+          :: https://stackoverflow.com/a/24470998/1236990
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
 
-          # Run GTSAM tests
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic
+          :: Target doesn't exist
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
 
-          # Run GTSAM_UNSTABLE tests
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
+      - name: Test
+        shell: cmd
+        run: |
+          :: Run GTSAM tests
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear
+          
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation          
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm          
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
+
+          :: Compilation error
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam
+
+
+          :: Run GTSAM_UNSTABLE tests
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
+
+          :: Compile. Fail with exception
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
+
+          :: Compilation error
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
+
+          :: Compilation error
+          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition          
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ebe27443a..8b56ea9df 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,6 +32,14 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
 ###############################################################################
 # Gather information, perform checks, set defaults
 
+if(MSVC)
+  set(MSVC_LINKER_FLAGS "/FORCE:MULTIPLE")
+  set(CMAKE_EXE_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+  set(CMAKE_MODULE_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+  set(CMAKE_SHARED_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+  set(CMAKE_STATIC_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+endif()
+
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 include(GtsamMakeConfigFile)
 include(GNUInstallDirs)
diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h
index 41cdeeaaa..0b2b3606b 100644
--- a/gtsam/basis/Basis.h
+++ b/gtsam/basis/Basis.h
@@ -80,7 +80,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
  *
  * @ingroup basis
  */
-Matrix kroneckerProductIdentity(size_t M, const Weights& w);
+Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w);
 
 /**
  * CRTP Base class for function bases
diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h
index e6b402e44..ddc548fc6 100644
--- a/gtsam/discrete/SignatureParser.h
+++ b/gtsam/discrete/SignatureParser.h
@@ -22,6 +22,8 @@
 #include <string>
 #include <vector>
 
+#include <gtsam/dllexport.h>
+
 namespace gtsam {
 /**
  * @brief A simple parser that replaces the boost spirit parser.
@@ -47,7 +49,7 @@ namespace gtsam {
  *
  * Also fails if the rows are not of the same size.
  */
-struct SignatureParser {
+struct GTSAM_EXPORT SignatureParser {
   using Row = std::vector<double>;
   using Table = std::vector<Row>;
 
diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h
index b1a1e4f6e..def49d268 100644
--- a/gtsam/geometry/Line3.h
+++ b/gtsam/geometry/Line3.h
@@ -146,7 +146,7 @@ class GTSAM_EXPORT Line3 {
    * @param Dline -  OptionalJacobian of transformed line with respect to l
    * @return Transformed line in camera frame
    */
-  friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
+  GTSAM_EXPORT friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                            OptionalJacobian<4, 6> Dpose,
                            OptionalJacobian<4, 4> Dline);
 };
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index f1b38c5a6..aad81493f 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -36,7 +36,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Pose2: public LieGroup<Pose2, 3> {
+class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
 
 public:
 
@@ -112,10 +112,10 @@ public:
   /// @{
 
   /** print with optional string */
-  GTSAM_EXPORT void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const;
 
   /** assert equality up to a tolerance */
-  GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
+  bool equals(const Pose2& pose, double tol = 1e-9) const;
 
   /// @}
   /// @name Group
@@ -125,7 +125,7 @@ public:
   inline static Pose2 Identity() { return Pose2(); }
 
   /// inverse
-  GTSAM_EXPORT Pose2 inverse() const;
+  Pose2 inverse() const;
 
   /// compose syntactic sugar
   inline Pose2 operator*(const Pose2& p2) const {
@@ -137,16 +137,16 @@ public:
   /// @{
 
   ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
-  GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
+  static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
 
   ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
-  GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
+  static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
 
   /**
    * Calculate Adjoint map
    * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
    */
-  GTSAM_EXPORT Matrix3 AdjointMap() const;
+  Matrix3 AdjointMap() const;
 
   /// Apply AdjointMap to twist xi
   inline Vector3 Adjoint(const Vector3& xi) const {
@@ -156,7 +156,7 @@ public:
   /**
    * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
    */
-  GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
+  static Matrix3 adjointMap(const Vector3& v);
 
   /**
    * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
@@ -192,15 +192,15 @@ public:
   }
 
   /// Derivative of Expmap
-  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
+  static Matrix3 ExpmapDerivative(const Vector3& v);
 
   /// Derivative of Logmap
-  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
+  static Matrix3 LogmapDerivative(const Pose2& v);
 
   // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
-  struct ChartAtOrigin {
-	GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
-	GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
+  struct GTSAM_EXPORT ChartAtOrigin {
+    static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
+    static Vector3 Local(const Pose2& r, ChartJacobian H = {});
   };
 
   using LieGroup<Pose2, 3>::inverse; // version with derivative
@@ -210,7 +210,7 @@ public:
   /// @{
 
   /** Return point coordinates in pose coordinate frame */
-  GTSAM_EXPORT Point2 transformTo(const Point2& point,
+  Point2 transformTo(const Point2& point,
       OptionalJacobian<2, 3> Dpose = {},
       OptionalJacobian<2, 2> Dpoint = {}) const;
 
@@ -222,7 +222,7 @@ public:
   Matrix transformTo(const Matrix& points) const;
 
   /** Return point coordinates in global frame */
-  GTSAM_EXPORT Point2 transformFrom(const Point2& point,
+  Point2 transformFrom(const Point2& point,
       OptionalJacobian<2, 3> Dpose = {},
       OptionalJacobian<2, 2> Dpoint = {}) const;
 
@@ -273,14 +273,14 @@ public:
   }
 
   //// return transformation matrix
-  GTSAM_EXPORT Matrix3 matrix() const;
+  Matrix3 matrix() const;
 
   /**
    * Calculate bearing to a landmark
    * @param point 2D location of landmark
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  GTSAM_EXPORT Rot2 bearing(const Point2& point,
+  Rot2 bearing(const Point2& point,
                OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
 
   /**
@@ -288,7 +288,7 @@ public:
    * @param point SO(2) location of other pose
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
+  Rot2 bearing(const Pose2& pose,
                OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
 
   /**
@@ -296,7 +296,7 @@ public:
    * @param point 2D location of landmark
    * @return range (double)
    */
-  GTSAM_EXPORT double range(const Point2& point,
+  double range(const Point2& point,
       OptionalJacobian<1, 3> H1={},
       OptionalJacobian<1, 2> H2={}) const;
 
@@ -305,7 +305,7 @@ public:
    * @param point 2D location of other pose
    * @return range (double)
    */
-  GTSAM_EXPORT double range(const Pose2& point,
+  double range(const Pose2& point,
       OptionalJacobian<1, 3> H1={},
       OptionalJacobian<1, 3> H2={}) const;
 
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 6c91d7468..8a807cc23 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -204,7 +204,7 @@ public:
   static Matrix6 LogmapDerivative(const Pose3& xi);
 
   // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
-  struct ChartAtOrigin {
+  struct GTSAM_EXPORT ChartAtOrigin {
     static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
     static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
   };
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index b8e7ff588..8b59fd4f9 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr<Factor>;
  * Hybrid Factor Graph
  * Factor graph with utilities for hybrid factors.
  */
-class HybridFactorGraph : public FactorGraph<Factor> {
+class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
  public:
   using Base = FactorGraph<Factor>;
   using This = HybridFactorGraph;              ///< this class
diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h
index 0767da12f..5c2e4f546 100644
--- a/gtsam/hybrid/HybridSmoother.h
+++ b/gtsam/hybrid/HybridSmoother.h
@@ -24,7 +24,7 @@
 
 namespace gtsam {
 
-class HybridSmoother {
+class GTSAM_EXPORT HybridSmoother {
  private:
   HybridBayesNet hybridBayesNet_;
   HybridGaussianFactorGraph remainingFactorGraph_;
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 1da897103..df38c171e 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -43,6 +43,7 @@
 #include <iostream>
 #include <iterator>
 #include <vector>
+#include <numeric>
 
 #include "Switching.h"
 #include "TinyHybridExample.h"
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index 86d44e072..c3df1b8a0 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -30,7 +30,7 @@
 
 namespace gtsam {
 
-class Ordering: public KeyVector {
+class GTSAM_EXPORT Ordering: public KeyVector {
 protected:
   typedef KeyVector Base;
 
@@ -44,8 +44,7 @@ public:
   typedef Ordering This; ///< Typedef to this class
   typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
-  /// Create an empty ordering
-  GTSAM_EXPORT
+  /// Create an empty ordering  
   Ordering() {
   }
 
@@ -99,7 +98,7 @@ public:
   }
 
   /// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
-  static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
+  static Ordering Colamd(const VariableIndex& variableIndex);
 
   /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
   /// for note on performance).  This internally builds a VariableIndex so if you already have a
@@ -124,7 +123,7 @@ public:
   /// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
   /// \c constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
-  static GTSAM_EXPORT Ordering ColamdConstrainedLast(
+  static Ordering ColamdConstrainedLast(
       const VariableIndex& variableIndex, const KeyVector& constrainLast,
       bool forceOrder = false);
 
@@ -152,7 +151,7 @@ public:
   /// KeyVector \c constrainFirst.   If \c forceOrder is false, the variables in \c
   /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
   /// reduce fill-in as well.
-  static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
+  static Ordering ColamdConstrainedFirst(
       const VariableIndex& variableIndex,
       const KeyVector& constrainFirst, bool forceOrder = false);
 
@@ -181,7 +180,7 @@ public:
   /// appear in \c groups in arbitrary order.  Any variables not present in \c groups will be
   /// assigned to group 0.  This function simply fills the \c cmember argument to CCOLAMD with the
   /// supplied indices, see the CCOLAMD documentation for more information.
-  static GTSAM_EXPORT Ordering ColamdConstrained(
+  static Ordering ColamdConstrained(
       const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
 
   /// Return a natural Ordering. Typically used by iterative solvers
@@ -195,11 +194,11 @@ public:
 
   /// METIS Formatting function
   template<class FACTOR_GRAPH>
-  static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
+  static void CSRFormat(std::vector<int>& xadj,
       std::vector<int>& adj, const FACTOR_GRAPH& graph);
 
   /// Compute an ordering determined by METIS from a VariableIndex
-  static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
+  static Ordering Metis(const MetisIndex& met);
 
   template<class FACTOR_GRAPH>
   static Ordering Metis(const FACTOR_GRAPH& graph) {
@@ -241,18 +240,16 @@ public:
   /// @name Testable
   /// @{
 
-  GTSAM_EXPORT
   void print(const std::string& str = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
 
-  GTSAM_EXPORT
   bool equals(const Ordering& other, double tol = 1e-9) const;
 
   /// @}
 
 private:
   /// Internal COLAMD function
-  static GTSAM_EXPORT Ordering ColamdConstrained(
+  static Ordering ColamdConstrained(
       const VariableIndex& variableIndex, std::vector<int>& cmember);
 
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h
index 7e326117b..9db2a34f0 100644
--- a/gtsam_unstable/linear/LPInitSolver.h
+++ b/gtsam_unstable/linear/LPInitSolver.h
@@ -19,6 +19,7 @@
 
 #pragma once
 
+#include <gtsam_unstable/dllexport.h>
 #include <gtsam_unstable/linear/LP.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
@@ -49,7 +50,7 @@ namespace gtsam {
  * inequality constraint, we can't conclude that the problem is infeasible.
  * However, whether it is infeasible or unbounded, we don't have a unique solution anyway.
  */
-class LPInitSolver {
+class GTSAM_UNSTABLE_EXPORT LPInitSolver {
 private:
   const LP& lp_;
 
diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h
index bd4254d0f..e751f34d2 100644
--- a/gtsam_unstable/linear/QPSParser.h
+++ b/gtsam_unstable/linear/QPSParser.h
@@ -18,12 +18,13 @@
 
 #pragma once
 
+#include <gtsam_unstable/dllexport.h>
 #include <gtsam_unstable/linear/QP.h>
 #include <fstream>
 
 namespace gtsam {
 
-class QPSParser {
+class GTSAM_UNSTABLE_EXPORT QPSParser {
 
 private:
   std::string fileName_;

From 0fc662399aa1c30b7ac9d2c8875791b65eeadc97 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jul 2023 10:12:40 +0200
Subject: [PATCH 43/45] Add a test to debug issue #1588

---
 gtsam/geometry/tests/testRot3.cpp | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 49b978567..1232348f0 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -597,6 +597,25 @@ TEST(Rot3, quaternion) {
   EXPECT(assert_equal(expected2, actual2));
 }
 
+/* ************************************************************************* */
+TEST(Rot3, ConvertQuaternion) {
+  Eigen::Quaterniond eigenQuaternion;
+  eigenQuaternion.w() = 1.0;
+  eigenQuaternion.x() = 2.0;
+  eigenQuaternion.y() = 3.0;
+  eigenQuaternion.z() = 4.0;
+  EXPECT_DOUBLES_EQUAL(1, eigenQuaternion.w(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2, eigenQuaternion.x(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(3, eigenQuaternion.y(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(4, eigenQuaternion.z(), 1e-9);
+
+  Rot3 R(eigenQuaternion);
+  EXPECT_DOUBLES_EQUAL(1, R.toQuaternion().w(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2, R.toQuaternion().x(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(3, R.toQuaternion().y(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(4, R.toQuaternion().z(), 1e-9);
+}
+
 /* ************************************************************************* */
 Matrix Cayley(const Matrix& A) {
   Matrix::Index n = A.cols();

From 00895911c0451347f004ba8a6175381c181364e9 Mon Sep 17 00:00:00 2001
From: Tal Regev <tal.regev@gmail.com>
Date: Mon, 24 Jul 2023 22:16:56 +0300
Subject: [PATCH 44/45] Tests are not failing without debug assertion

---
 .github/workflows/build-windows.yml | 63 +++++++++++++++--------------
 1 file changed, 32 insertions(+), 31 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index 565a02756..98218a894 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -99,64 +99,65 @@ jobs:
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
 
       - name: Build
-        shell: cmd
         run: |
-          :: Since Visual Studio is a multi-generator, we need to use --config
-          :: https://stackoverflow.com/a/24470998/1236990
+          # Since Visual Studio is a multi-generator, we need to use --config
+          # https://stackoverflow.com/a/24470998/1236990
           cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
           cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
 
-          :: Target doesn't exist
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
+          # Target doesn't exist
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
 
       - name: Test
-        shell: cmd
         run: |
-          :: Run GTSAM tests
+          # Run GTSAM tests
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete
-          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
+
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
+
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear
+          # Compile. Fail with exception
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear
           
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation          
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm          
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
+          # Compile. Fail with exception
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
 
-          :: Compilation error
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam
 
 
-          :: Run GTSAM_UNSTABLE tests
+          # Run GTSAM_UNSTABLE tests
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
 
-          :: Compile. Fail with exception
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
 
-          :: Compilation error
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
 
-          :: Compilation error
-          :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition          
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition          

From 8ac72e8e687f9962f541d2f29bd0018727556b3b Mon Sep 17 00:00:00 2001
From: Tal Regev <tal.regev@gmail.com>
Date: Mon, 24 Jul 2023 23:53:55 +0300
Subject: [PATCH 45/45] Change to bash shell to fail on first error

---
 .github/workflows/build-windows.yml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index 98218a894..f4442bdde 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -99,6 +99,7 @@ jobs:
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
 
       - name: Build
+        shell: bash
         run: |
           # Since Visual Studio is a multi-generator, we need to use --config
           # https://stackoverflow.com/a/24470998/1236990
@@ -109,6 +110,7 @@ jobs:
           # cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
 
       - name: Test
+        shell: bash
         run: |
           # Run GTSAM tests
           cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base