From 1dd3b180b1781a4be2af3f933e4b180d5cca559d Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Mon, 28 Oct 2024 20:33:06 +0800
Subject: [PATCH 001/362] update testIncrementalFixedLagSmoother.cpp to
 reproduce the bug in marginalization

---
 gtsam/inference/VariableIndex.h               |  6 +-
 .../tests/testIncrementalFixedLagSmoother.cpp | 97 ++++++++++++++++++-
 2 files changed, 100 insertions(+), 3 deletions(-)

diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index 207ded0ce..110c0bba4 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -87,9 +87,11 @@ class GTSAM_EXPORT VariableIndex {
   const FactorIndices& operator[](Key variable) const {
     KeyMap::const_iterator item = index_.find(variable);
     if(item == index_.end())
-      throw std::invalid_argument("Requested non-existent variable from VariableIndex");
+      throw std::invalid_argument("Requested non-existent variable '" +
+                                  DefaultKeyFormatter(variable) +
+                                  "' from VariableIndex");
     else
-    return item->second;
+      return item->second;
   }
 
   /// Return true if no factors associated with a variable
diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
index 3454c352a..cb4dcbf9a 100644
--- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
@@ -49,6 +49,41 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini
   return assert_equal(expected, actual);
 }
 
+/* ************************************************************************* */
+void PrintSymbolicTreeHelper(
+    const ISAM2Clique::shared_ptr& clique, const std::string indent = "") {
+
+  // Print the current clique
+  std::cout << indent << "P( ";
+  for(Key key: clique->conditional()->frontals()) {
+    std::cout << DefaultKeyFormatter(key) << " ";
+  }
+  if (clique->conditional()->nrParents() > 0)
+    std::cout << "| ";
+  for(Key key: clique->conditional()->parents()) {
+    std::cout << DefaultKeyFormatter(key) << " ";
+  }
+  std::cout << ")" << std::endl;
+
+  // Recursively print all of the children
+  for(const ISAM2Clique::shared_ptr& child: clique->children) {
+    PrintSymbolicTreeHelper(child, indent + " ");
+  }
+}
+
+/* ************************************************************************* */
+void PrintSymbolicTree(const ISAM2& isam,
+    const std::string& label) {
+  std::cout << label << std::endl;
+  if (!isam.roots().empty()) {
+    for(const ISAM2::sharedClique& root: isam.roots()) {
+      PrintSymbolicTreeHelper(root);
+    }
+  } else
+    std::cout << "{Empty Tree}" << std::endl;
+}
+
+
 /* ************************************************************************* */
 TEST( IncrementalFixedLagSmoother, Example )
 {
@@ -64,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example )
 
   // Create a Fixed-Lag Smoother
   typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
-  IncrementalFixedLagSmoother smoother(7.0, ISAM2Params());
+  IncrementalFixedLagSmoother smoother(9.0, ISAM2Params());
 
   // Create containers to keep the full graph
   Values fullinit;
@@ -158,6 +193,9 @@ TEST( IncrementalFixedLagSmoother, Example )
     Values newValues;
     Timestamps newTimestamps;
 
+    // Add the odometry factor twice to ensure the removeFactor test below works,
+    // where we need to keep the connectivity of the graph.
+    newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
     newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
     newValues.insert(key2, Point2(double(i)+0.1, -0.1));
     newTimestamps[key2] = double(i);
@@ -210,6 +248,10 @@ TEST( IncrementalFixedLagSmoother, Example )
 
 	  const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
 
+    std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl;
+    std::cout << "smootherFactorsBeforeRemove.size() = "
+              << smootherFactorsBeforeRemove.size() << std::endl;
+
 	  // remove factor
 	  smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
 
@@ -231,6 +273,59 @@ TEST( IncrementalFixedLagSmoother, Example )
 	    }
 	  }
   }
+
+  {
+    PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:");
+
+    i = 17;
+    while(i <= 200) {
+      Key key_0 = MakeKey(i);
+      Key key_1 = MakeKey(i-1);
+      Key key_2 = MakeKey(i-2);
+      Key key_3 = MakeKey(i-3);
+      Key key_4 = MakeKey(i-4);
+      Key key_5 = MakeKey(i-5);
+      Key key_6 = MakeKey(i-6);
+      Key key_7 = MakeKey(i-7);
+      Key key_8 = MakeKey(i-8);
+
+      NonlinearFactorGraph newFactors;
+      Values newValues;
+      Timestamps newTimestamps;
+
+      // To make a complex graph
+      newFactors.push_back(BetweenFactor<Point2>(key_1, key_0, Point2(1.0, 0.0), odometerNoise));
+      if (i % 2 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_2, key_1, Point2(1.0, 0.0), odometerNoise));
+      if (i % 3 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_3, key_2, Point2(1.0, 0.0), odometerNoise));
+      if (i % 4 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_4, key_3, Point2(1.0, 0.0), odometerNoise));
+      if (i % 5 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_5, key_4, Point2(1.0, 0.0), odometerNoise));
+      if (i % 6 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_6, key_5, Point2(1.0, 0.0), odometerNoise));
+      if (i % 7 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_7, key_6, Point2(1.0, 0.0), odometerNoise));
+      if (i % 8 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_8, key_7, Point2(1.0, 0.0), odometerNoise));
+
+      newValues.insert(key_0, Point2(double(i)+0.1, -0.1));
+      newTimestamps[key_0] = double(i);
+
+      fullgraph.push_back(newFactors);
+      fullinit.insert(newValues);
+
+      // Update the smoother
+      smoother.update(newFactors, newValues, newTimestamps);
+
+      // Check
+      CHECK(check_smoother(fullgraph, fullinit, smoother, key_0));
+      PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i));
+
+      ++i;
+    }
+  }
 }
 
 /* ************************************************************************* */

From 896e52ca27baf823112a10c43d5442318dccd271 Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Mon, 28 Oct 2024 23:38:04 +0800
Subject: [PATCH 002/362] Fix marginalization in IncrementalFixedLagSmoother.

Add BayesTreeMarginalizationHelper.h and use the new helper
to gather the additional keys to re-eliminate when marginalizing
variables in IncrementalFixedLagSmoother.
---
 .../BayesTreeMarginalizationHelper.h          | 174 ++++++++++++++++++
 .../nonlinear/IncrementalFixedLagSmoother.cpp |  10 +
 .../tests/testIncrementalFixedLagSmoother.cpp |  15 +-
 3 files changed, 196 insertions(+), 3 deletions(-)
 create mode 100644 gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h

diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
new file mode 100644
index 000000000..fe261d10e
--- /dev/null
+++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
@@ -0,0 +1,174 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    BayesTreeMarginalizationHelper.h
+ * @brief   Helper functions for marginalizing variables from a Bayes Tree.
+ *
+ * @author  Jeffrey (Zhiwei Wang)
+ * @date    Oct 28, 2024
+ */
+
+// \callgraph
+#pragma once
+
+#include <gtsam/inference/BayesTree.h>
+#include <gtsam/inference/BayesTreeCliqueBase.h>
+#include <gtsam/base/debug.h>
+#include "gtsam_unstable/dllexport.h"
+
+namespace gtsam {
+
+/**
+ * This class provides helper functions for marginalizing variables from a Bayes Tree.
+ */
+template <typename BayesTree>
+class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper {
+
+public:
+  using Clique = typename BayesTree::Clique;
+  using sharedClique = typename BayesTree::sharedClique;
+
+  /** Get the additional keys that need to be re-eliminated when marginalizing
+   * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree.
+   * 
+   * @param[in] bayesTree The Bayes tree to be marginalized.
+   * @param[in] marginalizableKeys  The keys to be marginalized.
+   * 
+   * 
+   * When marginalizing a variable @f$ \theta @f$ in a Bayes tree, some nodes
+   * may need to be re-eliminated. The variable to be marginalized should be
+   * eliminated first.
+   * 
+   * 1. If @f$ \theta @f$ is already in a leaf node @f$ L @f$, and all other
+   *    frontal variables within @f$ L @f$ are to be marginalized, then this
+   *    node does not need to be re-eliminated; the entire node can be directly
+   *    marginalized.
+   * 
+   * 2. If @f$ \theta @f$ is in a leaf node @f$ L @f$, but @f$ L @f$ contains
+   *    other frontal variables that do not need to be marginalized:
+   *    a. If all other non-marginalized frontal variables are listed after
+   *       @f$ \theta @f$ (each node contains a frontal list, with variables to
+   *       be eliminated earlier in the list), then node @f$ L @f$ does not
+   *       need to be re-eliminated.
+   *    b. Otherwise, if there are non-marginalized nodes listed before
+   *       @f$ \theta @f$, then node @f$ L @f$ needs to be re-eliminated, and
+   *       correspondingly, all nodes between @f$ L @f$ and the root need to be
+   *       re-eliminated.
+   * 
+   * 3. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node),
+   *    but none of @f$ M @f$'s child nodes depend on variable @f$ \theta @f$
+   *    (they only depend on other variables within @f$ M @f$), then during the
+   *    process of marginalizing @f$ \theta @f$, @f$ M @f$ can be treated as a
+   *    leaf node, and @f$ M @f$ should be processed following the same
+   *    approach as for leaf nodes.
+   * 
+   *    In this case, the original elimination of @f$ \theta @f$ does not
+   *    depend on the elimination results of variables in the child nodes.
+   * 
+   * 4. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node),
+   *    and there exist child nodes that depend on variable @f$ \theta @f$,
+   *    then not only does node @f$ M @f$ need to be re-eliminated, but all
+   *    child nodes dependent on @f$ \theta @f$, including descendant nodes
+   *    recursively dependent on @f$ \theta @f$, also need to be re-eliminated.
+   *
+   *    The frontal variables in child nodes were originally eliminated before
+   *    @f$ \theta @f$ and their elimination results are relied upon by
+   *    @f$ \theta @f$'s elimination. When re-eliminating, they should be
+   *    eliminated after @f$ \theta @f$.
+   */
+  static void gatherAdditionalKeysToReEliminate(
+      const BayesTree& bayesTree,
+      const KeyVector& marginalizableKeys,
+      std::set<Key>& additionalKeys) {
+    const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
+
+    std::set<Key> marginalizableKeySet(marginalizableKeys.begin(), marginalizableKeys.end());
+    std::set<sharedClique> checkedCliques;
+
+    std::set<sharedClique> dependentCliques;
+    for (const Key& key : marginalizableKeySet) {
+      sharedClique clique = bayesTree[key];
+      if (checkedCliques.count(clique)) {
+        continue;
+      }
+      checkedCliques.insert(clique);
+
+      bool is_leaf = clique->children.empty();
+      bool need_reeliminate = false;
+      bool has_non_marginalizable_ahead = false;
+      for (Key i: clique->conditional()->frontals()) {
+        if (marginalizableKeySet.count(i)) {
+          if (has_non_marginalizable_ahead) {
+            need_reeliminate = true;
+            break;
+          } else {
+            // Check whether there're child nodes dependent on this key.
+            for(const sharedClique& child: clique->children) {
+              if (std::find(child->conditional()->beginParents(),
+                  child->conditional()->endParents(), i)
+                  != child->conditional()->endParents()) {
+                need_reeliminate = true;
+                break;
+              }
+            }
+          }
+        } else {
+          has_non_marginalizable_ahead = true;
+        }
+      }
+
+      if (!need_reeliminate) {
+        continue;
+      } else {
+        // need to re-eliminate this clique and all its children that depend on
+        // a marginalizable key
+        for (Key i: clique->conditional()->frontals()) {
+          additionalKeys.insert(i);
+          for (const sharedClique& child: clique->children) {
+            if (!dependentCliques.count(child) &&
+                std::find(child->conditional()->beginParents(),
+                child->conditional()->endParents(), i)
+                != child->conditional()->endParents()) {
+              dependentCliques.insert(child);
+            }
+          }
+        }
+      }
+    }
+
+    // Recursively add the dependent keys
+    while (!dependentCliques.empty()) {
+      auto begin = dependentCliques.begin();
+      sharedClique clique = *begin;
+      dependentCliques.erase(begin);
+
+      for (Key key : clique->conditional()->frontals()) {
+        additionalKeys.insert(key);
+      }
+
+      for (const sharedClique& child: clique->children) {
+        dependentCliques.insert(child);
+      }
+    }
+
+    if (debug) {
+      std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: ";
+      for (const Key& key : additionalKeys) {
+        std::cout << DefaultKeyFormatter(key) << " ";
+      }
+      std::cout << std::endl;
+    }
+  }
+};
+// BayesTreeMarginalizationHelper
+
+}/// namespace gtsam
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index 52e56260d..9d27f5713 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -20,11 +20,15 @@
  */
 
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+#include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h>
 #include <gtsam/base/debug.h>
 
+// #define GTSAM_OLD_MARGINALIZATION
+
 namespace gtsam {
 
 /* ************************************************************************* */
+#ifdef GTSAM_OLD_MARGINALIZATION
 void recursiveMarkAffectedKeys(const Key& key,
     const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
 
@@ -45,6 +49,7 @@ void recursiveMarkAffectedKeys(const Key& key,
   }
   // If the key was not found in the separator/parents, then none of its children can have it either
 }
+#endif
 
 /* ************************************************************************* */
 void IncrementalFixedLagSmoother::print(const std::string& s,
@@ -116,12 +121,17 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
 
   // Mark additional keys between the marginalized keys and the leaves
   std::set<Key> additionalKeys;
+#ifdef GTSAM_OLD_MARGINALIZATION
   for(Key key: marginalizableKeys) {
     ISAM2Clique::shared_ptr clique = isam_[key];
     for(const ISAM2Clique::shared_ptr& child: clique->children) {
       recursiveMarkAffectedKeys(key, child, additionalKeys);
     }
   }
+#else
+  BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
+        isam_, marginalizableKeys, additionalKeys);
+#endif
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
 
   // Update iSAM2
diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
index cb4dcbf9a..cd2ba593b 100644
--- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
@@ -99,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example )
 
   // Create a Fixed-Lag Smoother
   typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
-  IncrementalFixedLagSmoother smoother(9.0, ISAM2Params());
+  IncrementalFixedLagSmoother smoother(12.0, ISAM2Params());
 
   // Create containers to keep the full graph
   Values fullinit;
@@ -226,6 +226,7 @@ TEST( IncrementalFixedLagSmoother, Example )
 	  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
 	  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
 	  newTimestamps[key2] = double(i);
+	  ++i;
 
 	  fullgraph.push_back(newFactors);
 	  fullinit.insert(newValues);
@@ -275,10 +276,12 @@ TEST( IncrementalFixedLagSmoother, Example )
   }
 
   {
+    SETDEBUG("BayesTreeMarginalizationHelper", true);
     PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:");
 
-    i = 17;
-    while(i <= 200) {
+    // Do pressure test on marginalization. Enlarge max_i to enhance the test.
+    const int max_i = 500;
+    while(i <= max_i) {
       Key key_0 = MakeKey(i);
       Key key_1 = MakeKey(i-1);
       Key key_2 = MakeKey(i-2);
@@ -288,6 +291,8 @@ TEST( IncrementalFixedLagSmoother, Example )
       Key key_6 = MakeKey(i-6);
       Key key_7 = MakeKey(i-7);
       Key key_8 = MakeKey(i-8);
+      Key key_9 = MakeKey(i-9);
+      Key key_10 = MakeKey(i-10);
 
       NonlinearFactorGraph newFactors;
       Values newValues;
@@ -309,6 +314,10 @@ TEST( IncrementalFixedLagSmoother, Example )
         newFactors.push_back(BetweenFactor<Point2>(key_7, key_6, Point2(1.0, 0.0), odometerNoise));
       if (i % 8 == 0)
         newFactors.push_back(BetweenFactor<Point2>(key_8, key_7, Point2(1.0, 0.0), odometerNoise));
+      if (i % 9 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_9, key_8, Point2(1.0, 0.0), odometerNoise));
+      if (i % 10 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_10, key_9, Point2(1.0, 0.0), odometerNoise));
 
       newValues.insert(key_0, Point2(double(i)+0.1, -0.1));
       newTimestamps[key_0] = double(i);

From c6ba2b5fd852e29d16eb1d963b216ea98d3d0ff5 Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Tue, 29 Oct 2024 15:13:09 +0800
Subject: [PATCH 003/362] update doc string in BayesTreeMarginalizationHelper.h

---
 .../BayesTreeMarginalizationHelper.h          | 64 +++++++------------
 1 file changed, 23 insertions(+), 41 deletions(-)

diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
index fe261d10e..6f5ff425f 100644
--- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
+++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
@@ -37,53 +37,33 @@ public:
   using Clique = typename BayesTree::Clique;
   using sharedClique = typename BayesTree::sharedClique;
 
-  /** Get the additional keys that need to be re-eliminated when marginalizing
+  /** Get the additional keys that need reelimination when marginalizing
    * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree.
    * 
-   * @param[in] bayesTree The Bayes tree to be marginalized.
+   * @param[in] bayesTree The Bayes tree.
    * @param[in] marginalizableKeys  The keys to be marginalized.
    * 
    * 
-   * When marginalizing a variable @f$ \theta @f$ in a Bayes tree, some nodes
-   * may need to be re-eliminated. The variable to be marginalized should be
-   * eliminated first.
+   * When marginalizing a variable @f$ \theta @f$ from a Bayes tree, some
+   * nodes may need reelimination to ensure the variables to marginalize
+   * be eliminated first.
    * 
-   * 1. If @f$ \theta @f$ is already in a leaf node @f$ L @f$, and all other
-   *    frontal variables within @f$ L @f$ are to be marginalized, then this
-   *    node does not need to be re-eliminated; the entire node can be directly
-   *    marginalized.
+   * We should consider two cases:
    * 
-   * 2. If @f$ \theta @f$ is in a leaf node @f$ L @f$, but @f$ L @f$ contains
-   *    other frontal variables that do not need to be marginalized:
-   *    a. If all other non-marginalized frontal variables are listed after
-   *       @f$ \theta @f$ (each node contains a frontal list, with variables to
-   *       be eliminated earlier in the list), then node @f$ L @f$ does not
-   *       need to be re-eliminated.
-   *    b. Otherwise, if there are non-marginalized nodes listed before
-   *       @f$ \theta @f$, then node @f$ L @f$ needs to be re-eliminated, and
-   *       correspondingly, all nodes between @f$ L @f$ and the root need to be
-   *       re-eliminated.
+   * 1. If a child node relies on @f$ \theta @f$ (i.e., @f$ \theta @f$
+   *    is a parent / separator of the node), then the frontal
+   *    variables of the child node need to be reeliminated. In
+   *    addition, all the descendants of the child node also need to
+   *    be reeliminated.
    * 
-   * 3. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node),
-   *    but none of @f$ M @f$'s child nodes depend on variable @f$ \theta @f$
-   *    (they only depend on other variables within @f$ M @f$), then during the
-   *    process of marginalizing @f$ \theta @f$, @f$ M @f$ can be treated as a
-   *    leaf node, and @f$ M @f$ should be processed following the same
-   *    approach as for leaf nodes.
+   * 2. If other frontal variables in the same node with @f$ \theta @f$
+   *    are in front of @f$ \theta @f$ but not to be marginalized, then
+   *    these variables also need to be reeliminated.
    * 
-   *    In this case, the original elimination of @f$ \theta @f$ does not
-   *    depend on the elimination results of variables in the child nodes.
+   * These variables were eliminated before @f$ \theta @f$ in the original
+   * Bayes tree, and after reelimination they will be eliminated after
+   * @f$ \theta @f$ so that @f$ \theta @f$ can be marginalized safely.
    * 
-   * 4. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node),
-   *    and there exist child nodes that depend on variable @f$ \theta @f$,
-   *    then not only does node @f$ M @f$ need to be re-eliminated, but all
-   *    child nodes dependent on @f$ \theta @f$, including descendant nodes
-   *    recursively dependent on @f$ \theta @f$, also need to be re-eliminated.
-   *
-   *    The frontal variables in child nodes were originally eliminated before
-   *    @f$ \theta @f$ and their elimination results are relied upon by
-   *    @f$ \theta @f$'s elimination. When re-eliminating, they should be
-   *    eliminated after @f$ \theta @f$.
    */
   static void gatherAdditionalKeysToReEliminate(
       const BayesTree& bayesTree,
@@ -102,20 +82,21 @@ public:
       }
       checkedCliques.insert(clique);
 
-      bool is_leaf = clique->children.empty();
       bool need_reeliminate = false;
       bool has_non_marginalizable_ahead = false;
       for (Key i: clique->conditional()->frontals()) {
         if (marginalizableKeySet.count(i)) {
           if (has_non_marginalizable_ahead) {
+            // Case 2 in the docstring
             need_reeliminate = true;
             break;
           } else {
-            // Check whether there're child nodes dependent on this key.
+            // Check whether there's a child node dependent on this key.
             for(const sharedClique& child: clique->children) {
               if (std::find(child->conditional()->beginParents(),
                   child->conditional()->endParents(), i)
                   != child->conditional()->endParents()) {
+                // Case 1 in the docstring
                 need_reeliminate = true;
                 break;
               }
@@ -127,10 +108,11 @@ public:
       }
 
       if (!need_reeliminate) {
+        // No variable needs to be reeliminated
         continue;
       } else {
-        // need to re-eliminate this clique and all its children that depend on
-        // a marginalizable key
+        // Need to reeliminate the current clique and all its children
+        // that rely on a marginalizable key.
         for (Key i: clique->conditional()->frontals()) {
           additionalKeys.insert(i);
           for (const sharedClique& child: clique->children) {

From 67b0b78ea1eacdb2647fedab828d9dcff99aac10 Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Thu, 31 Oct 2024 18:58:49 +0800
Subject: [PATCH 004/362] Update BayesTreeMarginalizationHelper:

1. Refactor code in BayesTreeMarginalizationHelper;
2. And avoid the unnecessary re-elimination of subtrees
   that only contain marginalizable variables;
---
 .../BayesTreeMarginalizationHelper.h          | 350 +++++++++++++-----
 .../nonlinear/IncrementalFixedLagSmoother.cpp |   7 +-
 2 files changed, 265 insertions(+), 92 deletions(-)

diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
index 6f5ff425f..04e4e0cac 100644
--- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
+++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
@@ -20,6 +20,7 @@
 // \callgraph
 #pragma once
 
+#include <unordered_map>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 #include <gtsam/base/debug.h>
@@ -37,109 +38,54 @@ public:
   using Clique = typename BayesTree::Clique;
   using sharedClique = typename BayesTree::sharedClique;
 
-  /** Get the additional keys that need reelimination when marginalizing
-   * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree.
+  /**
+   * This function identifies variables that need to be re-eliminated before
+   * performing marginalization.
    * 
-   * @param[in] bayesTree The Bayes tree.
-   * @param[in] marginalizableKeys  The keys to be marginalized.
+   * Re-elimination is necessary for a clique containing marginalizable
+   * variables if:
    * 
+   * 1. Some non-marginalizable variables appear before marginalizable ones
+   *    in that clique;
+   * 2. Or it has a child node depending on a marginalizable variable AND the
+   *    subtree rooted at that child contains non-marginalizables.
    * 
-   * When marginalizing a variable @f$ \theta @f$ from a Bayes tree, some
-   * nodes may need reelimination to ensure the variables to marginalize
-   * be eliminated first.
-   * 
-   * We should consider two cases:
-   * 
-   * 1. If a child node relies on @f$ \theta @f$ (i.e., @f$ \theta @f$
-   *    is a parent / separator of the node), then the frontal
-   *    variables of the child node need to be reeliminated. In
-   *    addition, all the descendants of the child node also need to
-   *    be reeliminated.
-   * 
-   * 2. If other frontal variables in the same node with @f$ \theta @f$
-   *    are in front of @f$ \theta @f$ but not to be marginalized, then
-   *    these variables also need to be reeliminated.
-   * 
-   * These variables were eliminated before @f$ \theta @f$ in the original
-   * Bayes tree, and after reelimination they will be eliminated after
-   * @f$ \theta @f$ so that @f$ \theta @f$ can be marginalized safely.
+   * In addition, the subtrees under the aforementioned cliques that require
+   * re-elimination, which contain non-marginalizable variables in their root
+   * node, also need to be re-eliminated.
    * 
+   * @param[in] bayesTree The Bayes tree
+   * @param[in] marginalizableKeys Keys to be marginalized
+   * @return Set of additional keys that need to be re-eliminated
    */
-  static void gatherAdditionalKeysToReEliminate(
+  static std::set<Key> gatherAdditionalKeysToReEliminate(
       const BayesTree& bayesTree,
-      const KeyVector& marginalizableKeys,
-      std::set<Key>& additionalKeys) {
+      const KeyVector& marginalizableKeys) {
     const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
 
-    std::set<Key> marginalizableKeySet(marginalizableKeys.begin(), marginalizableKeys.end());
-    std::set<sharedClique> checkedCliques;
+    std::set<Key> additionalKeys;
+    std::set<Key> marginalizableKeySet(
+        marginalizableKeys.begin(), marginalizableKeys.end());
+    std::set<sharedClique> dependentSubtrees;
+    CachedSearch cachedSearch;
 
-    std::set<sharedClique> dependentCliques;
-    for (const Key& key : marginalizableKeySet) {
-      sharedClique clique = bayesTree[key];
-      if (checkedCliques.count(clique)) {
-        continue;
-      }
-      checkedCliques.insert(clique);
+    // Check each clique that contains a marginalizable key
+    for (const sharedClique& clique :
+         getCliquesContainingKeys(bayesTree, marginalizableKeySet)) {
 
-      bool need_reeliminate = false;
-      bool has_non_marginalizable_ahead = false;
-      for (Key i: clique->conditional()->frontals()) {
-        if (marginalizableKeySet.count(i)) {
-          if (has_non_marginalizable_ahead) {
-            // Case 2 in the docstring
-            need_reeliminate = true;
-            break;
-          } else {
-            // Check whether there's a child node dependent on this key.
-            for(const sharedClique& child: clique->children) {
-              if (std::find(child->conditional()->beginParents(),
-                  child->conditional()->endParents(), i)
-                  != child->conditional()->endParents()) {
-                // Case 1 in the docstring
-                need_reeliminate = true;
-                break;
-              }
-            }
-          }
-        } else {
-          has_non_marginalizable_ahead = true;
-        }
-      }
+      if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) {
+        // Add frontal variables from current clique
+        addCliqueToKeySet(clique, &additionalKeys);
 
-      if (!need_reeliminate) {
-        // No variable needs to be reeliminated
-        continue;
-      } else {
-        // Need to reeliminate the current clique and all its children
-        // that rely on a marginalizable key.
-        for (Key i: clique->conditional()->frontals()) {
-          additionalKeys.insert(i);
-          for (const sharedClique& child: clique->children) {
-            if (!dependentCliques.count(child) &&
-                std::find(child->conditional()->beginParents(),
-                child->conditional()->endParents(), i)
-                != child->conditional()->endParents()) {
-              dependentCliques.insert(child);
-            }
-          }
-        }
+        // Then gather dependent subtrees to be added later
+        gatherDependentSubtrees(
+            clique, marginalizableKeySet, &dependentSubtrees, &cachedSearch);
       }
     }
 
-    // Recursively add the dependent keys
-    while (!dependentCliques.empty()) {
-      auto begin = dependentCliques.begin();
-      sharedClique clique = *begin;
-      dependentCliques.erase(begin);
-
-      for (Key key : clique->conditional()->frontals()) {
-        additionalKeys.insert(key);
-      }
-
-      for (const sharedClique& child: clique->children) {
-        dependentCliques.insert(child);
-      }
+    // Add the remaining dependent cliques
+    for (const sharedClique& subtree : dependentSubtrees) {
+      addSubtreeToKeySet(subtree, &additionalKeys);
     }
 
     if (debug) {
@@ -149,6 +95,232 @@ public:
       }
       std::cout << std::endl;
     }
+
+    return additionalKeys;
+  }
+
+ protected:
+
+  /**
+   * Gather the cliques containing any of the given keys.
+   * 
+   * @param[in] bayesTree The Bayes tree
+   * @param[in] keysOfInterest Set of keys of interest
+   * @return Set of cliques that contain any of the given keys
+   */
+  static std::set<sharedClique> getCliquesContainingKeys(
+      const BayesTree& bayesTree,
+      const std::set<Key>& keysOfInterest) {
+    std::set<sharedClique> cliques;
+    for (const Key& key : keysOfInterest) {
+      cliques.insert(bayesTree[key]);
+    }
+    return cliques;
+  }
+
+  /**
+   * A struct to cache the results of the below two functions.
+   */
+  struct CachedSearch {
+    std::unordered_map<Clique*, bool> wholeMarginalizableCliques;
+    std::unordered_map<Clique*, bool> wholeMarginalizableSubtrees;
+  };
+
+  /**
+   * Check if all variables in the clique are marginalizable.
+   * 
+   * Note we use a cache map to avoid repeated searches.
+   */
+  static bool isWholeCliqueMarginalizable(
+      const sharedClique& clique,
+      const std::set<Key>& marginalizableKeys,
+      CachedSearch* cache) {
+    auto it = cache->wholeMarginalizableCliques.find(clique.get());
+    if (it != cache->wholeMarginalizableCliques.end()) {
+      return it->second;
+    } else {
+      bool ret = true;
+      for (Key key : clique->conditional()->frontals()) {
+        if (!marginalizableKeys.count(key)) {
+          ret = false;
+          break;
+        }
+      }
+      cache->wholeMarginalizableCliques.insert({clique.get(), ret});
+      return ret;
+    }
+  }
+
+  /**
+   * Check if all variables in the subtree are marginalizable.
+   * 
+   * Note we use a cache map to avoid repeated searches.
+   */
+  static bool isWholeSubtreeMarginalizable(
+      const sharedClique& subtree,
+      const std::set<Key>& marginalizableKeys,
+      CachedSearch* cache) {
+    auto it = cache->wholeMarginalizableSubtrees.find(subtree.get());
+    if (it != cache->wholeMarginalizableSubtrees.end()) {
+      return it->second;
+    } else {
+      bool ret = true;
+      if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) {
+        for (const sharedClique& child : subtree->children) {
+          if (!isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) {
+            ret = false;
+            break;
+          }
+        }
+      } else {
+        ret = false;
+      }
+      cache->wholeMarginalizableSubtrees.insert({subtree.get(), ret});
+      return ret;
+    }
+  }
+
+  /**
+   * Check if a clique contains variables that need reelimination due to
+   * elimination ordering conflicts.
+   * 
+   * @param[in] clique The clique to check
+   * @param[in] marginalizableKeys Set of keys to be marginalized
+   * @return true if any variables in the clique need re-elimination
+   */
+  static bool needsReelimination(
+      const sharedClique& clique,
+      const std::set<Key>& marginalizableKeys,
+      CachedSearch* cache) {
+    bool hasNonMarginalizableAhead = false;
+
+    // Check each frontal variable in order
+    for (Key key : clique->conditional()->frontals()) {
+      if (marginalizableKeys.count(key)) {
+        // If we've seen non-marginalizable variables before this one,
+        // we need to reeliminate
+        if (hasNonMarginalizableAhead) {
+          return true;
+        }
+
+        // Check if any child depends on this marginalizable key and the
+        // subtree rooted at that child contains non-marginalizables.
+        for (const sharedClique& child : clique->children) {
+          if (hasDependency(child, key) &&
+              !isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) {
+            return true;
+          }
+        }
+      } else {
+        hasNonMarginalizableAhead = true;
+      }
+    }
+    return false;
+  }
+
+  /**
+   * Gather all subtrees that depend on a marginalizable key and contain
+   * non-marginalizable variables in their root.
+   *
+   * @param[in] rootClique The starting clique
+   * @param[in] marginalizableKeys Set of keys to be marginalized
+   * @param[out] dependentSubtrees Pointer to set storing dependent cliques
+   */
+  static void gatherDependentSubtrees(
+      const sharedClique& rootClique,
+      const std::set<Key>& marginalizableKeys,
+      std::set<sharedClique>* dependentSubtrees,
+      CachedSearch* cache) {
+    for (Key key : rootClique->conditional()->frontals()) {
+      if (marginalizableKeys.count(key)) {
+        // Find children that depend on this key
+        for (const sharedClique& child : rootClique->children) {
+          if (!dependentSubtrees->count(child) &&
+              hasDependency(child, key)) {
+            getSubtreesContainingNonMarginalizables(
+                child, marginalizableKeys, cache, dependentSubtrees);
+          }
+        }
+      }
+    }
+  }
+
+  /**
+   * Gather all subtrees that contain non-marginalizable variables in its root.
+   */
+  static void getSubtreesContainingNonMarginalizables(
+      const sharedClique& rootClique,
+      const std::set<Key>& marginalizableKeys,
+      CachedSearch* cache,
+      std::set<sharedClique>* subtreesContainingNonMarginalizables) {
+    // If the root clique itself contains non-marginalizable variables, we
+    // just add it to subtreesContainingNonMarginalizables;    
+    if (!isWholeCliqueMarginalizable(rootClique, marginalizableKeys, cache)) {
+      subtreesContainingNonMarginalizables->insert(rootClique);
+      return;
+    }
+
+    // Otherwise, we need to recursively check the children
+    for (const sharedClique& child : rootClique->children) {
+      getSubtreesContainingNonMarginalizables(
+          child, marginalizableKeys, cache,
+          subtreesContainingNonMarginalizables);
+    }
+  }
+
+  /**
+   * Add all frontal variables from a clique to a key set.
+   * 
+   * @param[in] clique Clique to add keys from
+   * @param[out] additionalKeys Pointer to the output key set
+   */
+  static void addCliqueToKeySet(
+      const sharedClique& clique,
+      std::set<Key>* additionalKeys) {
+    for (Key key : clique->conditional()->frontals()) {
+      additionalKeys->insert(key);
+    }
+  }
+
+  /**
+   * Add all frontal variables from a subtree to a key set.
+   *
+   * @param[in] subRoot Root clique of the subtree
+   * @param[out] additionalKeys Pointer to the output key set
+   */
+  static void addSubtreeToKeySet(
+      const sharedClique& subRoot,
+      std::set<Key>* additionalKeys) {
+    std::set<sharedClique> cliques;
+    cliques.insert(subRoot);
+    while(!cliques.empty()) {
+      auto begin = cliques.begin();
+      sharedClique clique = *begin;
+      cliques.erase(begin);
+      addCliqueToKeySet(clique, additionalKeys);
+      for (const sharedClique& child : clique->children) {
+        cliques.insert(child);
+      }
+    }
+  }
+
+  /**
+   * Check if the clique depends on the given key.
+   * 
+   * @param[in] clique Clique to check
+   * @param[in] key Key to check for dependencies
+   * @return true if clique depends on the key
+   */
+  static bool hasDependency(
+      const sharedClique& clique, Key key) {
+    auto conditional = clique->conditional();
+    if (std::find(conditional->beginParents(),
+        conditional->endParents(), key)
+        != conditional->endParents()) {
+      return true;
+    } else {
+      return false;
+    }
   }
 };
 // BayesTreeMarginalizationHelper
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index 9d27f5713..afe4fb3de 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -120,8 +120,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
   }
 
   // Mark additional keys between the marginalized keys and the leaves
-  std::set<Key> additionalKeys;
 #ifdef GTSAM_OLD_MARGINALIZATION
+  std::set<Key> additionalKeys;
   for(Key key: marginalizableKeys) {
     ISAM2Clique::shared_ptr clique = isam_[key];
     for(const ISAM2Clique::shared_ptr& child: clique->children) {
@@ -129,8 +129,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
     }
   }
 #else
-  BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
-        isam_, marginalizableKeys, additionalKeys);
+  std::set<Key> additionalKeys =
+      BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
+          isam_, marginalizableKeys);
 #endif
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
 

From 14c3467520851a0520b4a4ce8f8eadb806c4e82d Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Thu, 31 Oct 2024 19:05:54 +0800
Subject: [PATCH 005/362] Remove old marginalization code in
 IncrementalFixedLagSmoother.cpp

---
 .../nonlinear/IncrementalFixedLagSmoother.cpp | 37 -------------------
 1 file changed, 37 deletions(-)

diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index afe4fb3de..238ff6b3d 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -23,34 +23,8 @@
 #include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h>
 #include <gtsam/base/debug.h>
 
-// #define GTSAM_OLD_MARGINALIZATION
-
 namespace gtsam {
 
-/* ************************************************************************* */
-#ifdef GTSAM_OLD_MARGINALIZATION
-void recursiveMarkAffectedKeys(const Key& key,
-    const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
-
-  // Check if the separator keys of the current clique contain the specified key
-  if (std::find(clique->conditional()->beginParents(),
-      clique->conditional()->endParents(), key)
-      != clique->conditional()->endParents()) {
-
-    // Mark the frontal keys of the current clique
-    for(Key i: clique->conditional()->frontals()) {
-      additionalKeys.insert(i);
-    }
-
-    // Recursively mark all of the children
-    for(const ISAM2Clique::shared_ptr& child: clique->children) {
-      recursiveMarkAffectedKeys(key, child, additionalKeys);
-    }
-  }
-  // If the key was not found in the separator/parents, then none of its children can have it either
-}
-#endif
-
 /* ************************************************************************* */
 void IncrementalFixedLagSmoother::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
@@ -119,20 +93,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
     std::cout << std::endl;
   }
 
-  // Mark additional keys between the marginalized keys and the leaves
-#ifdef GTSAM_OLD_MARGINALIZATION
-  std::set<Key> additionalKeys;
-  for(Key key: marginalizableKeys) {
-    ISAM2Clique::shared_ptr clique = isam_[key];
-    for(const ISAM2Clique::shared_ptr& child: clique->children) {
-      recursiveMarkAffectedKeys(key, child, additionalKeys);
-    }
-  }
-#else
   std::set<Key> additionalKeys =
       BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
           isam_, marginalizableKeys);
-#endif
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
 
   // Update iSAM2

From 1a5e711f0e49451b51b2e54757f3b894b8e4ee50 Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Thu, 31 Oct 2024 21:52:45 +0800
Subject: [PATCH 006/362] Further optimize the implementation of
 BayesTreeMarginalizationHelper: Now we won't re-emilinate any unnecessary
 nodes (we re-emilinated whole subtrees in the previous commits, which is not
 optimal)

---
 .../BayesTreeMarginalizationHelper.h          | 124 ++++++++----------
 1 file changed, 58 insertions(+), 66 deletions(-)

diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
index 04e4e0cac..724814fd0 100644
--- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
+++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
@@ -21,6 +21,7 @@
 #pragma once
 
 #include <unordered_map>
+#include <deque>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 #include <gtsam/base/debug.h>
@@ -50,9 +51,12 @@ public:
    * 2. Or it has a child node depending on a marginalizable variable AND the
    *    subtree rooted at that child contains non-marginalizables.
    * 
-   * In addition, the subtrees under the aforementioned cliques that require
-   * re-elimination, which contain non-marginalizable variables in their root
-   * node, also need to be re-eliminated.
+   * In addition, for any descendant node depending on a marginalizable
+   * variable, if the subtree rooted at that descendant contains
+   * non-marginalizable variables (i.e., it lies on a path from one of the
+   * aforementioned cliques that require re-elimination to a node containing
+   * non-marginalizable variables at the leaf side), then it also needs to
+   * be re-eliminated.
    * 
    * @param[in] bayesTree The Bayes tree
    * @param[in] marginalizableKeys Keys to be marginalized
@@ -66,7 +70,7 @@ public:
     std::set<Key> additionalKeys;
     std::set<Key> marginalizableKeySet(
         marginalizableKeys.begin(), marginalizableKeys.end());
-    std::set<sharedClique> dependentSubtrees;
+    std::set<sharedClique> dependentCliques;
     CachedSearch cachedSearch;
 
     // Check each clique that contains a marginalizable key
@@ -77,17 +81,14 @@ public:
         // Add frontal variables from current clique
         addCliqueToKeySet(clique, &additionalKeys);
 
-        // Then gather dependent subtrees to be added later
-        gatherDependentSubtrees(
-            clique, marginalizableKeySet, &dependentSubtrees, &cachedSearch);
+        // Then add the dependent cliques
+        for (const sharedClique& dependent :
+             gatherDependentCliques(clique, marginalizableKeySet, &cachedSearch)) {
+          addCliqueToKeySet(dependent, &additionalKeys);
+        }
       }
     }
 
-    // Add the remaining dependent cliques
-    for (const sharedClique& subtree : dependentSubtrees) {
-      addSubtreeToKeySet(subtree, &additionalKeys);
-    }
-
     if (debug) {
       std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: ";
       for (const Key& key : additionalKeys) {
@@ -219,53 +220,53 @@ public:
   }
 
   /**
-   * Gather all subtrees that depend on a marginalizable key and contain
-   * non-marginalizable variables in their root.
+   * Gather all dependent nodes that lie on a path from the root clique
+   * to a clique containing a non-marginalizable variable at the leaf side.
    *
-   * @param[in] rootClique The starting clique
+   * @param[in] rootClique The root clique
    * @param[in] marginalizableKeys Set of keys to be marginalized
-   * @param[out] dependentSubtrees Pointer to set storing dependent cliques
    */
-  static void gatherDependentSubtrees(
+  static std::set<sharedClique> gatherDependentCliques(
       const sharedClique& rootClique,
       const std::set<Key>& marginalizableKeys,
-      std::set<sharedClique>* dependentSubtrees,
       CachedSearch* cache) {
-    for (Key key : rootClique->conditional()->frontals()) {
-      if (marginalizableKeys.count(key)) {
-        // Find children that depend on this key
-        for (const sharedClique& child : rootClique->children) {
-          if (!dependentSubtrees->count(child) &&
-              hasDependency(child, key)) {
-            getSubtreesContainingNonMarginalizables(
-                child, marginalizableKeys, cache, dependentSubtrees);
-          }
-        }
+    std::vector<sharedClique> dependentChildren;
+    dependentChildren.reserve(rootClique->children.size());
+    for (const sharedClique& child : rootClique->children) {
+      if (hasDependency(child, marginalizableKeys)) {
+        dependentChildren.push_back(child);
       }
     }
+    return gatherDependentCliquesFromChildren(dependentChildren, marginalizableKeys, cache);
   }
 
   /**
-   * Gather all subtrees that contain non-marginalizable variables in its root.
+   * A helper function for the above gatherDependentCliques().
    */
-  static void getSubtreesContainingNonMarginalizables(
-      const sharedClique& rootClique,
+  static std::set<sharedClique> gatherDependentCliquesFromChildren(
+      const std::vector<sharedClique>& dependentChildren,
       const std::set<Key>& marginalizableKeys,
-      CachedSearch* cache,
-      std::set<sharedClique>* subtreesContainingNonMarginalizables) {
-    // If the root clique itself contains non-marginalizable variables, we
-    // just add it to subtreesContainingNonMarginalizables;    
-    if (!isWholeCliqueMarginalizable(rootClique, marginalizableKeys, cache)) {
-      subtreesContainingNonMarginalizables->insert(rootClique);
-      return;
-    }
+      CachedSearch* cache) {
+    std::deque<sharedClique> descendants(
+        dependentChildren.begin(), dependentChildren.end());
+    std::set<sharedClique> dependentCliques;
+    while (!descendants.empty()) {
+      sharedClique descendant = descendants.front();
+      descendants.pop_front();
 
-    // Otherwise, we need to recursively check the children
-    for (const sharedClique& child : rootClique->children) {
-      getSubtreesContainingNonMarginalizables(
-          child, marginalizableKeys, cache,
-          subtreesContainingNonMarginalizables);
+      // If the subtree rooted at this descendant contains non-marginalizables,
+      // it must lie on a path from the root clique to a clique containing
+      // non-marginalizables at the leaf side.
+      if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) {
+        dependentCliques.insert(descendant);
+      }
+
+      // Add all children of the current descendant to the set descendants.
+      for (const sharedClique& child : descendant->children) {
+        descendants.push_back(child);
+      }
     }
+    return dependentCliques;
   }
 
   /**
@@ -282,28 +283,6 @@ public:
     }
   }
 
-  /**
-   * Add all frontal variables from a subtree to a key set.
-   *
-   * @param[in] subRoot Root clique of the subtree
-   * @param[out] additionalKeys Pointer to the output key set
-   */
-  static void addSubtreeToKeySet(
-      const sharedClique& subRoot,
-      std::set<Key>* additionalKeys) {
-    std::set<sharedClique> cliques;
-    cliques.insert(subRoot);
-    while(!cliques.empty()) {
-      auto begin = cliques.begin();
-      sharedClique clique = *begin;
-      cliques.erase(begin);
-      addCliqueToKeySet(clique, additionalKeys);
-      for (const sharedClique& child : clique->children) {
-        cliques.insert(child);
-      }
-    }
-  }
-
   /**
    * Check if the clique depends on the given key.
    * 
@@ -322,6 +301,19 @@ public:
       return false;
     }
   }
+
+  /**
+   * Check if the clique depends on any of the given keys.
+   */
+  static bool hasDependency(
+      const sharedClique& clique, const std::set<Key>& keys) {
+    for (Key key : keys) {
+      if (hasDependency(clique, key)) {
+        return true;
+      }
+    }
+    return false;
+  }
 };
 // BayesTreeMarginalizationHelper
 

From 0d9c3a99583032e25e36b6135084aaa75159430c Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Thu, 31 Oct 2024 22:23:22 +0800
Subject: [PATCH 007/362] Remove unused variable

---
 gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
index 724814fd0..53d030624 100644
--- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
+++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
@@ -70,7 +70,6 @@ public:
     std::set<Key> additionalKeys;
     std::set<Key> marginalizableKeySet(
         marginalizableKeys.begin(), marginalizableKeys.end());
-    std::set<sharedClique> dependentCliques;
     CachedSearch cachedSearch;
 
     // Check each clique that contains a marginalizable key

From 06dac43cae843e5a2bc16447af9d3bde13e0f54d Mon Sep 17 00:00:00 2001
From: Jeffrey <jeffrey@newthinker.net>
Date: Sat, 2 Nov 2024 17:14:01 +0800
Subject: [PATCH 008/362] Some refinement in BayesTreeMarginalizationHelper:

1. Skip subtrees that have already been visited when searching for
   dependent cliques;
2. Avoid copying shared_ptrs (which needs extra expensive atomic
   operations) in the searching. Use const Clique* instead of
   sharedClique whenever possible;
3. Use std::unordered_set instead of std::set to improve average
   searching speed.
---
 .../BayesTreeMarginalizationHelper.h          | 167 +++++++++++-------
 .../nonlinear/IncrementalFixedLagSmoother.cpp |   2 +-
 2 files changed, 104 insertions(+), 65 deletions(-)

diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
index 53d030624..1e367e55c 100644
--- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
+++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
@@ -21,6 +21,7 @@
 #pragma once
 
 #include <unordered_map>
+#include <unordered_set>
 #include <deque>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
@@ -62,30 +63,18 @@ public:
    * @param[in] marginalizableKeys Keys to be marginalized
    * @return Set of additional keys that need to be re-eliminated
    */
-  static std::set<Key> gatherAdditionalKeysToReEliminate(
+  static std::unordered_set<Key>
+  gatherAdditionalKeysToReEliminate(
       const BayesTree& bayesTree,
       const KeyVector& marginalizableKeys) {
     const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
 
-    std::set<Key> additionalKeys;
-    std::set<Key> marginalizableKeySet(
-        marginalizableKeys.begin(), marginalizableKeys.end());
-    CachedSearch cachedSearch;
+    std::unordered_set<const Clique*> additionalCliques =
+        gatherAdditionalCliquesToReEliminate(bayesTree, marginalizableKeys);    
 
-    // Check each clique that contains a marginalizable key
-    for (const sharedClique& clique :
-         getCliquesContainingKeys(bayesTree, marginalizableKeySet)) {
-
-      if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) {
-        // Add frontal variables from current clique
-        addCliqueToKeySet(clique, &additionalKeys);
-
-        // Then add the dependent cliques
-        for (const sharedClique& dependent :
-             gatherDependentCliques(clique, marginalizableKeySet, &cachedSearch)) {
-          addCliqueToKeySet(dependent, &additionalKeys);
-        }
-      }
+    std::unordered_set<Key> additionalKeys;
+    for (const Clique* clique : additionalCliques) {
+      addCliqueToKeySet(clique, &additionalKeys);
     }
 
     if (debug) {
@@ -100,6 +89,41 @@ public:
   }
 
  protected:
+  /**
+   * This function identifies cliques that need to be re-eliminated before
+   * performing marginalization.
+   * See the docstring of @ref gatherAdditionalKeysToReEliminate().
+   */
+  static std::unordered_set<const Clique*>
+  gatherAdditionalCliquesToReEliminate(
+      const BayesTree& bayesTree,
+      const KeyVector& marginalizableKeys) {
+    std::unordered_set<const Clique*> additionalCliques;
+    std::unordered_set<Key> marginalizableKeySet(
+        marginalizableKeys.begin(), marginalizableKeys.end());
+    CachedSearch cachedSearch;
+
+    // Check each clique that contains a marginalizable key
+    for (const Clique* clique :
+         getCliquesContainingKeys(bayesTree, marginalizableKeySet)) {
+      if (additionalCliques.count(clique)) {
+        // The clique has already been visited. This can happen when an
+        // ancestor of the current clique also contain some marginalizable
+        // varaibles and it's processed beore the current.
+        continue;
+      }
+
+      if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) {
+        // Add the current clique
+        additionalCliques.insert(clique);
+
+        // Then add the dependent cliques
+        gatherDependentCliques(clique, marginalizableKeySet, &additionalCliques,
+                               &cachedSearch);
+      }
+    }
+    return additionalCliques;
+  }
 
   /**
    * Gather the cliques containing any of the given keys.
@@ -108,12 +132,12 @@ public:
    * @param[in] keysOfInterest Set of keys of interest
    * @return Set of cliques that contain any of the given keys
    */
-  static std::set<sharedClique> getCliquesContainingKeys(
+  static std::unordered_set<const Clique*> getCliquesContainingKeys(
       const BayesTree& bayesTree,
-      const std::set<Key>& keysOfInterest) {
-    std::set<sharedClique> cliques;
+      const std::unordered_set<Key>& keysOfInterest) {
+    std::unordered_set<const Clique*> cliques;
     for (const Key& key : keysOfInterest) {
-      cliques.insert(bayesTree[key]);
+      cliques.insert(bayesTree[key].get());
     }
     return cliques;
   }
@@ -122,8 +146,8 @@ public:
    * A struct to cache the results of the below two functions.
    */
   struct CachedSearch {
-    std::unordered_map<Clique*, bool> wholeMarginalizableCliques;
-    std::unordered_map<Clique*, bool> wholeMarginalizableSubtrees;
+    std::unordered_map<const Clique*, bool> wholeMarginalizableCliques;
+    std::unordered_map<const Clique*, bool> wholeMarginalizableSubtrees;
   };
 
   /**
@@ -132,10 +156,10 @@ public:
    * Note we use a cache map to avoid repeated searches.
    */
   static bool isWholeCliqueMarginalizable(
-      const sharedClique& clique,
-      const std::set<Key>& marginalizableKeys,
+      const Clique* clique,
+      const std::unordered_set<Key>& marginalizableKeys,
       CachedSearch* cache) {
-    auto it = cache->wholeMarginalizableCliques.find(clique.get());
+    auto it = cache->wholeMarginalizableCliques.find(clique);
     if (it != cache->wholeMarginalizableCliques.end()) {
       return it->second;
     } else {
@@ -146,7 +170,7 @@ public:
           break;
         }
       }
-      cache->wholeMarginalizableCliques.insert({clique.get(), ret});
+      cache->wholeMarginalizableCliques.insert({clique, ret});
       return ret;
     }
   }
@@ -157,17 +181,17 @@ public:
    * Note we use a cache map to avoid repeated searches.
    */
   static bool isWholeSubtreeMarginalizable(
-      const sharedClique& subtree,
-      const std::set<Key>& marginalizableKeys,
+      const Clique* subtree,
+      const std::unordered_set<Key>& marginalizableKeys,
       CachedSearch* cache) {
-    auto it = cache->wholeMarginalizableSubtrees.find(subtree.get());
+    auto it = cache->wholeMarginalizableSubtrees.find(subtree);
     if (it != cache->wholeMarginalizableSubtrees.end()) {
       return it->second;
     } else {
       bool ret = true;
       if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) {
         for (const sharedClique& child : subtree->children) {
-          if (!isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) {
+          if (!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) {
             ret = false;
             break;
           }
@@ -175,7 +199,7 @@ public:
       } else {
         ret = false;
       }
-      cache->wholeMarginalizableSubtrees.insert({subtree.get(), ret});
+      cache->wholeMarginalizableSubtrees.insert({subtree, ret});
       return ret;
     }
   }
@@ -189,8 +213,8 @@ public:
    * @return true if any variables in the clique need re-elimination
    */
   static bool needsReelimination(
-      const sharedClique& clique,
-      const std::set<Key>& marginalizableKeys,
+      const Clique* clique,
+      const std::unordered_set<Key>& marginalizableKeys,
       CachedSearch* cache) {
     bool hasNonMarginalizableAhead = false;
 
@@ -206,8 +230,8 @@ public:
         // Check if any child depends on this marginalizable key and the
         // subtree rooted at that child contains non-marginalizables.
         for (const sharedClique& child : clique->children) {
-          if (hasDependency(child, key) &&
-              !isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) {
+          if (hasDependency(child.get(), key) &&
+              !isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) {
             return true;
           }
         }
@@ -225,47 +249,59 @@ public:
    * @param[in] rootClique The root clique
    * @param[in] marginalizableKeys Set of keys to be marginalized
    */
-  static std::set<sharedClique> gatherDependentCliques(
-      const sharedClique& rootClique,
-      const std::set<Key>& marginalizableKeys,
+  static void gatherDependentCliques(
+      const Clique* rootClique,
+      const std::unordered_set<Key>& marginalizableKeys,
+      std::unordered_set<const Clique*>* additionalCliques,
       CachedSearch* cache) {
-    std::vector<sharedClique> dependentChildren;
+    std::vector<const Clique*> dependentChildren;
     dependentChildren.reserve(rootClique->children.size());
     for (const sharedClique& child : rootClique->children) {
-      if (hasDependency(child, marginalizableKeys)) {
-        dependentChildren.push_back(child);
+      if (additionalCliques->count(child.get())) {
+        // This child has already been visited. This can happen if the
+        // child itself contains a marginalizable variable and it's
+        // processed before the current rootClique.
+        continue;
+      }
+      if (hasDependency(child.get(), marginalizableKeys)) {
+        dependentChildren.push_back(child.get());
       }
     }
-    return gatherDependentCliquesFromChildren(dependentChildren, marginalizableKeys, cache);
+    gatherDependentCliquesFromChildren(
+        dependentChildren, marginalizableKeys, additionalCliques, cache);
   }
 
   /**
    * A helper function for the above gatherDependentCliques().
    */
-  static std::set<sharedClique> gatherDependentCliquesFromChildren(
-      const std::vector<sharedClique>& dependentChildren,
-      const std::set<Key>& marginalizableKeys,
+  static void gatherDependentCliquesFromChildren(
+      const std::vector<const Clique*>& dependentChildren,
+      const std::unordered_set<Key>& marginalizableKeys,
+      std::unordered_set<const Clique*>* additionalCliques,
       CachedSearch* cache) {
-    std::deque<sharedClique> descendants(
+    std::deque<const Clique*> descendants(
         dependentChildren.begin(), dependentChildren.end());
-    std::set<sharedClique> dependentCliques;
     while (!descendants.empty()) {
-      sharedClique descendant = descendants.front();
+      const Clique* descendant = descendants.front();
       descendants.pop_front();
 
       // If the subtree rooted at this descendant contains non-marginalizables,
       // it must lie on a path from the root clique to a clique containing
       // non-marginalizables at the leaf side.
       if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) {
-        dependentCliques.insert(descendant);
-      }
+        additionalCliques->insert(descendant);
 
-      // Add all children of the current descendant to the set descendants.
-      for (const sharedClique& child : descendant->children) {
-        descendants.push_back(child);
+        // Add children of the current descendant to the set descendants.
+        for (const sharedClique& child : descendant->children) {
+          if (additionalCliques->count(child.get())) {
+            // This child has already been visited.
+            continue;
+          } else {
+            descendants.push_back(child.get());
+          }
+        }
       }
     }
-    return dependentCliques;
   }
 
   /**
@@ -275,8 +311,8 @@ public:
    * @param[out] additionalKeys Pointer to the output key set
    */
   static void addCliqueToKeySet(
-      const sharedClique& clique,
-      std::set<Key>* additionalKeys) {
+      const Clique* clique,
+      std::unordered_set<Key>* additionalKeys) {
     for (Key key : clique->conditional()->frontals()) {
       additionalKeys->insert(key);
     }
@@ -290,8 +326,8 @@ public:
    * @return true if clique depends on the key
    */
   static bool hasDependency(
-      const sharedClique& clique, Key key) {
-    auto conditional = clique->conditional();
+      const Clique* clique, Key key) {
+    auto& conditional = clique->conditional();
     if (std::find(conditional->beginParents(),
         conditional->endParents(), key)
         != conditional->endParents()) {
@@ -305,12 +341,15 @@ public:
    * Check if the clique depends on any of the given keys.
    */
   static bool hasDependency(
-      const sharedClique& clique, const std::set<Key>& keys) {
-    for (Key key : keys) {
-      if (hasDependency(clique, key)) {
+      const Clique* clique, const std::unordered_set<Key>& keys) {
+    auto& conditional = clique->conditional();
+    for (auto it = conditional->beginParents();
+        it != conditional->endParents(); ++it) {
+      if (keys.count(*it)) {
         return true;
       }
     }
+
     return false;
   }
 };
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index 238ff6b3d..0cd9ecbac 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -93,7 +93,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
     std::cout << std::endl;
   }
 
-  std::set<Key> additionalKeys =
+  std::unordered_set<Key> additionalKeys =
       BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
           isam_, marginalizableKeys);
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());

From eca2bb5d8a916043608866792c13e7cb86cae10a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 10:27:51 -0700
Subject: [PATCH 009/362] epipolarLine

---
 gtsam/geometry/FundamentalMatrix.cpp | 28 ++++++++++++++++++++++++++++
 gtsam/geometry/FundamentalMatrix.h   |  7 +++++++
 2 files changed, 35 insertions(+)

diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp
index 5e8c26e62..837ba7263 100644
--- a/gtsam/geometry/FundamentalMatrix.cpp
+++ b/gtsam/geometry/FundamentalMatrix.cpp
@@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const {
          V_.transpose().matrix();
 }
 
+Vector3 FundamentalMatrix::epipolarLine(const Point2& p,
+                                        OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void FundamentalMatrix::print(const std::string& s) const {
   std::cout << s << matrix() << std::endl;
 }
@@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const {
   return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
 }
 
+Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p,
+                                              OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void SimpleFundamentalMatrix::print(const std::string& s) const {
   std::cout << s << " E:\n"
             << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h
index c114c2b5b..df2c2881a 100644
--- a/gtsam/geometry/FundamentalMatrix.h
+++ b/gtsam/geometry/FundamentalMatrix.h
@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Unit3.h>
@@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix {
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;
 
+  /// Computes the epipolar line in a (left) for a given point in b (right)
+  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
+
   /// @name Testable
   /// @{
   /// Print the FundamentalMatrix
@@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
   /// F = Ka^(-T) * E * Kb^(-1)
   Matrix3 matrix() const;
 
+  /// Computes the epipolar line in a (left) for a given point in b (right)
+  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
+
   /// @name Testable
   /// @{
   /// Print the SimpleFundamentalMatrix

From 45fc039d075868c9b038b5a3d5aaebf4aba40cba Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 14:54:57 -0700
Subject: [PATCH 010/362] EssentialMatrixFactor5

---
 gtsam/slam/EssentialMatrixFactor.h            | 147 +++++++++++++++---
 .../slam/tests/testEssentialMatrixFactor.cpp  |  33 +++-
 2 files changed, 150 insertions(+), 30 deletions(-)

diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index b19bb09ab..c8f94681c 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
   typedef EssentialMatrixFactor This;
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
   }
 
   /// vector of errors returns 1D vector
-  Vector evaluateError(
-      const EssentialMatrix& E, OptionalMatrixType H) const override {
+  Vector evaluateError(const EssentialMatrix& E,
+                       OptionalMatrixType H) const override {
     Vector error(1);
     error << E.error(vA_, vB_, H);
     return error;
@@ -118,7 +117,6 @@ class EssentialMatrixFactor2
   typedef EssentialMatrixFactor2 This;
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -178,9 +176,9 @@ class EssentialMatrixFactor2
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(
-      const EssentialMatrix& E, const double& d,
-      OptionalMatrixType DE, OptionalMatrixType Dd) const override {
+  Vector evaluateError(const EssentialMatrix& E, const double& d,
+                       OptionalMatrixType DE,
+                       OptionalMatrixType Dd) const override {
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
     // We then convert to second camera by P2 = 1R2'*(P1-1T2)
@@ -241,7 +239,6 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
   Rot3 cRb_;  ///< Rotation from body to camera frame
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -292,9 +289,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(
-      const EssentialMatrix& E, const double& d,
-      OptionalMatrixType DE, OptionalMatrixType Dd) const override {
+  Vector evaluateError(const EssentialMatrix& E, const double& d,
+                       OptionalMatrixType DE,
+                       OptionalMatrixType Dd) const override {
     if (!DE) {
       // Convert E from body to camera frame
       EssentialMatrix cameraE = cRb_ * E;
@@ -304,8 +301,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
       // Version with derivatives
       Matrix D_e_cameraE, D_cameraE_E;  // 2*5, 5*5
       EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
-      // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2)
-      // does not have the matrix reference version of evaluateError
+      // Using the pointer version of evaluateError since the Base class
+      // (EssentialMatrixFactor2) does not have the matrix reference version of
+      // evaluateError
       Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
       *DE = D_e_cameraE * D_cameraE_E;  // (2*5) * (5*5)
       return e;
@@ -327,7 +325,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
  * Even with a prior, we can only optimize 2 DoF in the calibration. So the
  * prior should have a noise model with very low sigma in the remaining
  * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it
- * works only with a strong prior (low sigma noisemodel) on all degrees of
+ * works only with a strong prior (low sigma noise model) on all degrees of
  * freedom.
  */
 template <class CALIBRATION>
@@ -343,13 +341,12 @@ class EssentialMatrixFactor4
   typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
 
  public:
-
-   // Provide access to the Matrix& version of evaluateError:
+  // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
   /**
    *  Constructor
-   *  @param keyE Essential Matrix (from camera B to A) variable key
+   *  @param keyE Essential Matrix aEb variable key
    *  @param keyK Calibration variable key (common for both cameras)
    *  @param pA point in first camera, in pixel coordinates
    *  @param pB point in second camera, in pixel coordinates
@@ -385,32 +382,32 @@ class EssentialMatrixFactor4
    * @param H2 optional jacobian of error w.r.t K
    * @return * Vector 1D vector of algebraic error
    */
-  Vector evaluateError(
-      const EssentialMatrix& E, const CALIBRATION& K,
-      OptionalMatrixType H1, OptionalMatrixType H2) const override {
+  Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K,
+                       OptionalMatrixType HE,
+                       OptionalMatrixType HK) const override {
     // converting from pixel coordinates to normalized coordinates cA and cB
     JacobianCalibration cA_H_K;  // dcA/dK
     JacobianCalibration cB_H_K;  // dcB/dK
-    Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone);
-    Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone);
+    Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone);
+    Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone);
 
     // convert to homogeneous coordinates
     Vector3 vA = EssentialMatrix::Homogeneous(cA);
     Vector3 vB = EssentialMatrix::Homogeneous(cB);
 
-    if (H2) {
+    if (HK) {
       // compute the jacobian of error w.r.t K
 
       // error function f = vA.T * E * vB
       // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
       // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
       // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
-      *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
+      *HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
             vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
     }
 
     Vector error(1);
-    error << E.error(vA, vB, H1);
+    error << E.error(vA, vB, HE);
 
     return error;
   }
@@ -420,4 +417,104 @@ class EssentialMatrixFactor4
 };
 // EssentialMatrixFactor4
 
+/**
+ * Binary factor that optimizes for E and two calibrations Ka and Kb using the
+ * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are
+ * assumed different for the two images. Don'tt use this factor with same
+ * calibration unknown, as Jacobians will be incorrect...
+ *
+ * Note: even stronger caveats about having priors on calibration apply.
+ */
+template <class CALIBRATION>
+class EssentialMatrixFactor5
+    : public NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> {
+ private:
+  Point2 pA_, pB_;  ///< points in pixel coordinates
+
+  typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> Base;
+  typedef EssentialMatrixFactor5 This;
+
+  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
+  typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
+
+ public:
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
+  /**
+   *  Constructor
+   *  @param keyE Essential Matrix aEb variable key
+   *  @param keyKa Calibration variable key for camera A
+   *  @param keyKb Calibration variable key for camera B
+   *  @param pA point in first camera, in pixel coordinates
+   *  @param pB point in second camera, in pixel coordinates
+   *  @param model noise model is about dot product in ideal, homogeneous
+   * coordinates
+   */
+  EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
+                         const Point2& pB, const SharedNoiseModel& model)
+      : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// print
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
+    Base::print(s);
+    std::cout << "  EssentialMatrixFactor5 with measurements\n  ("
+              << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
+              << std::endl;
+  }
+
+  /**
+   * @brief Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB.
+   *
+   * @param E essential matrix for key keyE
+   * @param Ka calibration for camera A for key keyKa
+   * @param Kb calibration for camera B for key keyKb
+   * @param H1 optional jacobian of error w.r.t E
+   * @param H2 optional jacobian of error w.r.t Ka
+   * @param H3 optional jacobian of error w.r.t Kb
+   * @return * Vector 1D vector of algebraic error
+   */
+  Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka,
+                       const CALIBRATION& Kb, OptionalMatrixType HE,
+                       OptionalMatrixType HKa,
+                       OptionalMatrixType HKb) const override {
+    // converting from pixel coordinates to normalized coordinates cA and cB
+    JacobianCalibration cA_H_Ka;  // dcA/dKa
+    JacobianCalibration cB_H_Kb;  // dcB/dKb
+    Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone);
+    Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone);
+
+    // convert to homogeneous coordinates
+    Vector3 vA = EssentialMatrix::Homogeneous(cA);
+    Vector3 vB = EssentialMatrix::Homogeneous(cB);
+
+    if (HKa) {
+      // compute the jacobian of error w.r.t Ka
+      *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
+    }
+
+    if (HKb) {
+      // compute the jacobian of error w.r.t Kb
+      *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
+    }
+
+    Vector error(1);
+    error << E.error(vA, vB, HE);
+
+    return error;
+  }
+
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+};
+// EssentialMatrixFactor5
+
 }  // namespace gtsam
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index 0f9c2ef9f..609f38187 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -14,8 +14,8 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/nonlinear/factorTesting.h>
-#include <gtsam/slam/EssentialMatrixFactor.h>
 #include <gtsam/sfm/SfmData.h>
+#include <gtsam/slam/EssentialMatrixFactor.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std::placeholders;
@@ -101,7 +101,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
     std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
-        std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
+        std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i),
+                  std::placeholders::_2);
     Expression<EssentialMatrix> E_(key);  // leaf expression
     Expression<double> expr(f, E_);       // unary expression
 
@@ -127,10 +128,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
     std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
-        std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
+        std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i),
+                  std::placeholders::_2);
     std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
-                                    OptionalJacobian<5, 3>,
-                                    OptionalJacobian<5, 2>)>
+                                  OptionalJacobian<5, 3>,
+                                  OptionalJacobian<5, 2>)>
         g;
     Expression<Rot3> R_(key);
     Expression<Unit3> d_(trueDirection);
@@ -529,6 +531,27 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
         1e-6);
 }
 
+//*************************************************************************
+TEST(EssentialMatrixFactor5, factor) {
+  Key keyE(1), keyKa(2), keyKb(3);
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor5<Cal3_S2> factor(keyE, keyKa, keyKb, pA(i), pB(i),
+                                           model1);
+
+    // Check evaluation
+    Vector1 expected;
+    expected << 0;
+    Vector actual = factor.evaluateError(trueE, trueK, trueK);
+    EXPECT(assert_equal(expected, actual, 1e-7));
+
+    Values truth;
+    truth.insert(keyE, trueE);
+    truth.insert(keyKa, trueK);
+    truth.insert(keyKb, trueK);
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7);
+  }
+}
+
 }  // namespace example1
 
 //*************************************************************************

From f8d00f82f16e16110b5d0884b56550f24cd35c74 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 16:06:56 -0700
Subject: [PATCH 011/362] Allow for global calibration

---
 gtsam/sfm/TransferFactor.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h
index c8b249ddc..b73a71aa4 100644
--- a/gtsam/sfm/TransferFactor.h
+++ b/gtsam/sfm/TransferFactor.h
@@ -236,6 +236,8 @@ class EssentialTransferFactorK
   /**
    * @brief Constructor that accepts a vector of point triplets.
    *
+   * @note Calibrations are assumed all different, keys are derived from edges.
+   *
    * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
    * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
    * @param triplets A vector of triplets containing (pa, pb, pc)

From ce196d962fbd62283cda8295cbc93f7cb6722657 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 16:07:22 -0700
Subject: [PATCH 012/362] Wrap EMFs

---
 gtsam/slam/EssentialMatrixFactor.h |  4 +--
 gtsam/slam/slam.i                  | 50 +++++++++++++++++++++++++++---
 2 files changed, 47 insertions(+), 7 deletions(-)

diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index c8f94681c..7a0b46906 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -354,7 +354,7 @@ class EssentialMatrixFactor4
    * coordinates
    */
   EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
-                         const SharedNoiseModel& model)
+                         const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
@@ -452,7 +452,7 @@ class EssentialMatrixFactor5
    * coordinates
    */
   EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
-                         const Point2& pB, const SharedNoiseModel& model)
+                         const Point2& pB, const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 97dcfcae7..a226f06ec 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -107,7 +107,7 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorPoseCal3Unified;
 
-template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   GeneralSFMFactor2(const gtsam::Point2& measured,
@@ -221,15 +221,55 @@ typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
 
 #include <gtsam/slam/EssentialMatrixFactor.h>
 virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
-  EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
-                        const gtsam::Point2& pB,
-                        const gtsam::noiseModel::Base* noiseModel);
+  EssentialMatrixFactor(size_t key, 
+                        const gtsam::Point2& pA, const gtsam::Point2& pB,
+                        const gtsam::noiseModel::Base* model);
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
   gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const;
 };
 
+virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor2(size_t key1, size_t key2, 
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::noiseModel::Base* model);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const;
+};
+
+virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 {
+  EssentialMatrixFactor3(size_t key1, size_t key2, 
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const;
+};
+
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
+                         gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
+virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor4(size_t keyE, size_t keyK,
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::noiseModel::Base* model = nullptr);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const;
+};
+
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
+                         gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
+virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb,
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::noiseModel::Base* model = nullptr);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, 
+                              const CALIBRATION& Ka, const CALIBRATION& Kb) const;
+};
+
 #include <gtsam/slam/EssentialMatrixConstraint.h>
 virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
   EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE,

From 360dc4138c9d51f2683d8bc8af51eb692b76dc7e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 16:07:40 -0700
Subject: [PATCH 013/362] Compare 3 more cases

---
 python/gtsam/examples/ViewGraphComparison.py | 114 ++++++++++++-------
 1 file changed, 70 insertions(+), 44 deletions(-)

diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py
index 1eb43cec8..37558a46c 100644
--- a/python/gtsam/examples/ViewGraphComparison.py
+++ b/python/gtsam/examples/ViewGraphComparison.py
@@ -32,7 +32,7 @@ from gtsam import (
 K = gtsam.symbol_shorthand.K
 
 # Methods to compare
-methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"]
+methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"]
 
 
 # Formatter function for printing keys
@@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std):
     return measurements
 
 
-# Function to compute ground truth matrices
 def compute_ground_truth(method, poses, cal):
+    """Function to compute ground truth edge variables."""
     E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
     E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
     F1 = FundamentalMatrix(cal.K(), E1, cal.K())
@@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal):
         SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
         SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
         return SF1, SF2
-    elif method == "Essential+Ks" or method == "Calibrated":
-        return E1, E2
     else:
-        raise ValueError(f"Unknown method {method}")
+        return E1, E2
 
 
 def build_factor_graph(method, num_cameras, measurements, cal):
     """build the factor graph"""
     graph = NonlinearFactorGraph()
 
+    # Determine the FactorClass based on the method
     if method == "Fundamental":
         FactorClass = gtsam.TransferFactorFundamentalMatrix
     elif method == "SimpleF":
         FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
-    elif method == "Essential+Ks":
+    elif method in ["Essential+Ks", "Essential+K"]:
         FactorClass = gtsam.EssentialTransferFactorKCal3f
-        # add priors on all calibrations:
+    elif method == "Binary+K":
+        FactorClass = gtsam.EssentialMatrixFactor4Cal3f
+    elif method == "Binary+Ks":
+        FactorClass = gtsam.EssentialMatrixFactor5Cal3f
+    elif method == "Calibrated":
+        FactorClass = gtsam.EssentialTransferFactorCal3f
+    else:
+        raise ValueError(f"Unknown method {method}")
+
+    # Add priors on calibrations if necessary
+    if method in ["Essential+Ks", "Binary+Ks"]:
         for i in range(num_cameras):
             model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
             graph.addPriorCal3f(K(i), cal, model)
-    elif method == "Calibrated":
-        FactorClass = gtsam.EssentialTransferFactorCal3f
-        # No priors on calibration needed
-    else:
-        raise ValueError(f"Unknown method {method}")
+    elif method in ["Essential+K", "Binary+K"]:
+        model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
+        graph.addPriorCal3f(K(0), cal, model)
 
     z = measurements  # shorthand
 
     for a in range(num_cameras):
         b = (a + 1) % num_cameras  # Next camera
         c = (a + 2) % num_cameras  # Camera after next
-
-        # Vectors to collect tuples for each factor
-        tuples1 = []
-        tuples2 = []
-        tuples3 = []
-
-        # Collect data for the three factors
-        for j in range(len(measurements[0])):
-            tuples1.append((z[a][j], z[b][j], z[c][j]))
-            tuples2.append((z[a][j], z[c][j], z[b][j]))
-            tuples3.append((z[c][j], z[b][j], z[a][j]))
-
-        # Add transfer factors between views a, b, and c.
-        if method in ["Calibrated"]:
-            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
-            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
-            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
+        if method in ["Binary+Ks", "Binary+K"]:
+            # Add binary Essential Matrix factors
+            ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key()
+            for j in range(len(measurements[0])):
+                if method == "Binary+Ks":
+                    graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j]))
+                    graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j]))
+                else:  # Binary+K
+                    graph.add(FactorClass(ab, K(0), z[a][j], z[b][j]))
+                    graph.add(FactorClass(ac, K(0), z[a][j], z[c][j]))
         else:
-            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
-            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
-            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
+            # Add transfer factors between views a, b, and c
+
+            # Vectors to collect tuples for each factor
+            tuples1 = []
+            tuples2 = []
+            tuples3 = []
+
+            for j in range(len(measurements[0])):
+                tuples1.append((z[a][j], z[b][j], z[c][j]))
+                tuples2.append((z[a][j], z[c][j], z[b][j]))
+                tuples3.append((z[c][j], z[b][j], z[a][j]))
+
+            # Add transfer factors between views a, b, and c.
+            if method in ["Calibrated"]:
+                graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
+                graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
+                graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
+            elif method == "Essential+K":
+                graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1))
+                graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2))
+                graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3))
+            else:
+                graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+                graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+                graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
 
     return graph
 
@@ -159,22 +181,23 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
             initialEstimate.insert(EdgeKey(a, b).key(), F1)
             initialEstimate.insert(EdgeKey(a, c).key(), F2)
             total_dimension += F1.dim() + F2.dim()
-    elif method in ["Essential+Ks", "Calibrated"]:
+    elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]:
         E1, E2 = ground_truth
         for a in range(num_cameras):
-            b = (a + 1) % num_cameras  # Next camera
-            c = (a + 2) % num_cameras  # Camera after next
+            b = (a + 1) % num_cameras
+            c = (a + 2) % num_cameras
             initialEstimate.insert(EdgeKey(a, b).key(), E1)
             initialEstimate.insert(EdgeKey(a, c).key(), E2)
             total_dimension += E1.dim() + E2.dim()
-    else:
-        raise ValueError(f"Unknown method {method}")
 
-    if method == "Essential+Ks":
-        # Insert initial calibrations
+    # Insert initial calibrations
+    if method in ["Essential+Ks", "Binary+Ks"]:
         for i in range(num_cameras):
             initialEstimate.insert(K(i), cal)
             total_dimension += cal.dim()
+    elif method in ["Essential+K", "Binary+K"]:
+        initialEstimate.insert(K(0), cal)
+        total_dimension += cal.dim()
 
     print(f"Total dimension of the problem: {total_dimension}")
     return initialEstimate
@@ -205,7 +228,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
         key_ab = EdgeKey(a, b).key()
         key_ac = EdgeKey(a, c).key()
 
-        if method in ["Essential+Ks", "Calibrated"]:
+        if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]:
             E_est_ab = result.atEssentialMatrix(key_ab)
             E_est_ac = result.atEssentialMatrix(key_ac)
 
@@ -218,15 +241,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
             SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
             F_est_ab = FundamentalMatrix(SF_est_ab)
             F_est_ac = FundamentalMatrix(SF_est_ac)
-        elif method == "Essential+Ks":
-            # Retrieve calibrations from result:
+        elif method in ["Essential+Ks", "Binary+Ks"]:
+            # Retrieve calibrations from result for each camera
             cal_a = result.atCal3f(K(a))
             cal_b = result.atCal3f(K(b))
             cal_c = result.atCal3f(K(c))
-
-            # Convert estimated EssentialMatrices to FundamentalMatrices
             F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
             F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
+        elif method in ["Essential+K", "Binary+K"]:
+            # Use single shared calibration
+            cal_shared = result.atCal3f(K(0))
+            F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K())
+            F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K())
         elif method == "Calibrated":
             # Use ground truth calibration
             F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())

From 183774556851cb098e58c8ab38b180b863d298ba Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 5 Nov 2024 15:06:25 -0500
Subject: [PATCH 014/362] Trying to optimize well

---
 python/gtsam/examples/ViewGraphComparison.py | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py
index 37558a46c..7a9f77d09 100644
--- a/python/gtsam/examples/ViewGraphComparison.py
+++ b/python/gtsam/examples/ViewGraphComparison.py
@@ -117,10 +117,10 @@ def build_factor_graph(method, num_cameras, measurements, cal):
     # Add priors on calibrations if necessary
     if method in ["Essential+Ks", "Binary+Ks"]:
         for i in range(num_cameras):
-            model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
+            model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0)
             graph.addPriorCal3f(K(i), cal, model)
     elif method in ["Essential+K", "Binary+K"]:
-        model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
+        model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0)
         graph.addPriorCal3f(K(0), cal, model)
 
     z = measurements  # shorthand
@@ -186,6 +186,8 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
         for a in range(num_cameras):
             b = (a + 1) % num_cameras
             c = (a + 2) % num_cameras
+            # initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(0.1 * np.ones((5, 1))))
+            # initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(0.1 * np.ones((5, 1))))
             initialEstimate.insert(EdgeKey(a, b).key(), E1)
             initialEstimate.insert(EdgeKey(a, c).key(), E2)
             total_dimension += E1.dim() + E2.dim()
@@ -206,8 +208,9 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
 def optimize(graph, initialEstimate, method):
     """optimize the graph"""
     params = LevenbergMarquardtParams()
-    params.setlambdaInitial(1e10)  # Initialize lambda to a high value
-    params.setlambdaUpperBound(1e10)
+    if method not in ["Calibrated", "Binary+K", "Binary+Ks"]:
+        params.setlambdaInitial(1e10)  # Initialize lambda to a high value
+        params.setlambdaUpperBound(1e10)
     # params.setAbsoluteErrorTol(0.1)
     params.setVerbosityLM("SUMMARY")
     optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
@@ -373,6 +376,8 @@ def main():
 
             # Compute final error
             final_error = graph.error(result)
+            if method in ["Binary+K", "Binary+Ks"]:
+                final_error *= cal.f() * cal.f()
 
             # Store results
             results[method]["distances"].extend(distances)

From 5f667b44255b0e6181b620833ba8097f596cd0b4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 5 Nov 2024 17:24:13 -0500
Subject: [PATCH 015/362] Rename factor

---
 gtsam/sfm/TransferFactor.h             | 18 ++++++++++++++++++
 gtsam/sfm/sfm.i                        |  7 +++++--
 gtsam/sfm/tests/testTransferFactor.cpp |  2 +-
 3 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h
index b73a71aa4..0338762be 100644
--- a/gtsam/sfm/TransferFactor.h
+++ b/gtsam/sfm/TransferFactor.h
@@ -253,6 +253,24 @@ class EssentialTransferFactorK
         TransferEdges<EM>(edge1, edge2),
         triplets_(triplets) {}
 
+  /**
+   * @brief Constructor that accepts a vector of point triplets.
+   *
+   * @note Calibrations are assumed all same, using given key `keyK`.
+   *
+   * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
+   * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
+   * @param keyK Calibration key for all views.
+   * @param triplets A vector of triplets containing (pa, pb, pc)
+   * @param model An optional SharedNoiseModel
+   */
+  EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK,
+                           const std::vector<Triplet>& triplets,
+                           const SharedNoiseModel& model = nullptr)
+      : Base(model, edge1, edge2, keyK, keyK, keyK),
+        TransferEdges<EM>(edge1, edge2),
+        triplets_(triplets) {}
+
   /// Transfer points pa and pb to view c and evaluate error.
   Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
                         const Matrix3& Ecb, const K& Kb, const Point2& pb,
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index a15b73ea1..24f24f1bb 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor {
 template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
 virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor {
   EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
-                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
-                          const gtsam::noiseModel::Base* model = nullptr);
+                           const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                           const gtsam::noiseModel::Base* model = nullptr);
+  EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK,
+                           const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                           const gtsam::noiseModel::Base* model = nullptr);
 };
 
 #include <gtsam/sfm/ShonanFactor.h>
diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp
index ced3d2ce7..b34bdf832 100644
--- a/gtsam/sfm/tests/testTransferFactor.cpp
+++ b/gtsam/sfm/tests/testTransferFactor.cpp
@@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) {
 }
 
 //*************************************************************************
-// Test for EssentialTransferFactor
+// Test for EssentialTransferFactorK
 TEST(EssentialTransferFactor, Jacobians) {
   using namespace fixture;
 

From baa275bf51ed63dd61f3da2c90167491a2f1806e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 26 Nov 2024 09:53:28 -0800
Subject: [PATCH 016/362] address comments

---
 gtsam/slam/EssentialMatrixFactor.h            | 32 +++++++++-------
 .../slam/tests/testEssentialMatrixFactor.cpp  | 37 +++++++++++++++----
 2 files changed, 47 insertions(+), 22 deletions(-)

diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 7a0b46906..5fdf138cc 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -420,10 +420,13 @@ class EssentialMatrixFactor4
 /**
  * Binary factor that optimizes for E and two calibrations Ka and Kb using the
  * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are
- * assumed different for the two images. Don'tt use this factor with same
- * calibration unknown, as Jacobians will be incorrect...
+ * assumed different for the two images, but if you use the same key for Ka and
+ * Kb, the sum of the two K Jacobians equals that of the K Jacobian for
+ * EssentialMatrix4. If you know there is a single global calibration, use
+ * that factor instead.
  *
- * Note: even stronger caveats about having priors on calibration apply.
+ * Note: see the comment about priors from EssentialMatrixFactor4: even stronger
+ * caveats about having priors on calibration apply here.
  */
 template <class CALIBRATION>
 class EssentialMatrixFactor5
@@ -442,17 +445,18 @@ class EssentialMatrixFactor5
   using Base::evaluateError;
 
   /**
-   *  Constructor
-   *  @param keyE Essential Matrix aEb variable key
-   *  @param keyKa Calibration variable key for camera A
-   *  @param keyKb Calibration variable key for camera B
-   *  @param pA point in first camera, in pixel coordinates
-   *  @param pB point in second camera, in pixel coordinates
-   *  @param model noise model is about dot product in ideal, homogeneous
+   * Constructor
+   * @param keyE Essential Matrix aEb variable key
+   * @param keyKa Calibration variable key for camera A
+   * @param keyKb Calibration variable key for camera B
+   * @param pA point in first camera, in pixel coordinates
+   * @param pB point in second camera, in pixel coordinates
+   * @param model noise model is about dot product in ideal, homogeneous
    * coordinates
    */
   EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
-                         const Point2& pB, const SharedNoiseModel& model = nullptr)
+                         const Point2& pB,
+                         const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
@@ -492,17 +496,17 @@ class EssentialMatrixFactor5
     Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone);
     Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone);
 
-    // convert to homogeneous coordinates
+    // Convert to homogeneous coordinates.
     Vector3 vA = EssentialMatrix::Homogeneous(cA);
     Vector3 vB = EssentialMatrix::Homogeneous(cB);
 
     if (HKa) {
-      // compute the jacobian of error w.r.t Ka
+      // Compute the jacobian of error w.r.t Ka.
       *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
     }
 
     if (HKb) {
-      // compute the jacobian of error w.r.t Kb
+      // Compute the jacobian of error w.r.t Kb.
       *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
     }
 
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index 609f38187..11558e38e 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -117,8 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
     // Check evaluation
     Vector expected(1);
     expected << 0;
-    vector<Matrix> Hactual(1);
-    Vector actual = factor.unwhitenedError(values, Hactual);
+    vector<Matrix> actualH(1);
+    Vector actual = factor.unwhitenedError(values, actualH);
     EXPECT(assert_equal(expected, actual, 1e-7));
   }
 }
@@ -151,8 +151,8 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
     // Check evaluation
     Vector expected(1);
     expected << 0;
-    vector<Matrix> Hactual(1);
-    Vector actual = factor.unwhitenedError(values, Hactual);
+    vector<Matrix> actualH(1);
+    Vector actual = factor.unwhitenedError(values, actualH);
     EXPECT(assert_equal(expected, actual, 1e-7));
   }
 }
@@ -213,9 +213,9 @@ TEST(EssentialMatrixFactor2, factor) {
     const Point2 pi = PinholeBase::Project(P2);
     Point2 expected(pi - pB(i));
 
-    Matrix Hactual1, Hactual2;
+    Matrix actualH1, actualH2;
     double d(baseline / P1.z());
-    Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(trueE, d, actualH1, actualH2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     Values val;
@@ -278,9 +278,9 @@ TEST(EssentialMatrixFactor3, factor) {
     const Point2 pi = camera2.project(P1);
     Point2 expected(pi - pB(i));
 
-    Matrix Hactual1, Hactual2;
+    Matrix actualH1, actualH2;
     double d(baseline / P1.z());
-    Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(bodyE, d, actualH1, actualH2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     Values val;
@@ -552,6 +552,27 @@ TEST(EssentialMatrixFactor5, factor) {
   }
 }
 
+//*************************************************************************
+// Test that if we use the same keys for Ka and Kb the sum of the two K
+// Jacobians equals that of the single K Jacobian for EssentialMatrix4
+TEST(EssentialMatrixFactor5, SameKeys) {
+  Key keyE(1), keyK(2);
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor4<Cal3_S2> factor4(keyE, keyK, pA(i), pB(i), model1);
+    EssentialMatrixFactor5<Cal3_S2> factor5(keyE, keyK, keyK, pA(i), pB(i),
+                                            model1);
+
+    Values truth;
+    truth.insert(keyE, trueE);
+    truth.insert(keyK, trueK);
+
+    // Check Jacobians
+    Matrix H0, H1, H2;
+    factor4.evaluateError(trueE, trueK, nullptr, &H0);
+    factor5.evaluateError(trueE, trueK, trueK, nullptr, &H1, &H2);
+    EXPECT(assert_equal(H0, H1 + H2, 1e-9));
+  }
+}
 }  // namespace example1
 
 //*************************************************************************

From d38b577284182e85c3285eca415ac4c622494fd5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 1 Dec 2024 18:55:55 -0500
Subject: [PATCH 017/362] update CMake for finding gperftools

---
 cmake/FindGooglePerfTools.cmake | 35 ++++++++++++++-------------------
 1 file changed, 15 insertions(+), 20 deletions(-)

diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake
index 01243257b..d85c01389 100644
--- a/cmake/FindGooglePerfTools.cmake
+++ b/cmake/FindGooglePerfTools.cmake
@@ -1,35 +1,30 @@
 # -*- cmake -*-
 
-# - Find Google perftools
-# Find the Google perftools includes and libraries
-# This module defines
-#  GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc.
-#  GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools.
+# - Find GPerfTools (formerly Google perftools)
+# Find the GPerfTools libraries
+# If false, do not try to use Google perftools.
 # also defined for general use are
 #  TCMALLOC_LIBRARY, where to find the tcmalloc library.
 
-FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h
-/usr/local/include
-/usr/include
-)
-
 SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc)
-FIND_LIBRARY(TCMALLOC_LIBRARY
+find_library(TCMALLOC_LIBRARY
   NAMES ${TCMALLOC_NAMES}
   PATHS /usr/lib /usr/local/lib
-  )
+)
+find_library(GPERFTOOLS_PROFILER
+   NAMES profiler
+   PATHS /usr/lib /usr/local/lib
+)
 
-IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+IF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER)
     SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY})
     SET(GOOGLE_PERFTOOLS_FOUND "YES")
-ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+ELSE (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER)
   SET(GOOGLE_PERFTOOLS_FOUND "NO")
-ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+ENDIF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER)
 
 IF (GOOGLE_PERFTOOLS_FOUND)
-   IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}")
-   ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
+   MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}")
 ELSE (GOOGLE_PERFTOOLS_FOUND)
    IF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Google perftools library")
@@ -38,5 +33,5 @@ ENDIF (GOOGLE_PERFTOOLS_FOUND)
 
 MARK_AS_ADVANCED(
   TCMALLOC_LIBRARY
-  GOOGLE_PERFTOOLS_INCLUDE_DIR
-  )
+  GPERFTOOLS_PROFILER
+)

From be4964927c657e5e3daab55ec7b36cbe38166000 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 1 Dec 2024 18:57:40 -0500
Subject: [PATCH 018/362] rename TCMALLOC_LIBRARY to GPERFTOOLS_TCMALLOC

---
 cmake/FindGooglePerfTools.cmake | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake
index d85c01389..f7caa7c1c 100644
--- a/cmake/FindGooglePerfTools.cmake
+++ b/cmake/FindGooglePerfTools.cmake
@@ -3,11 +3,12 @@
 # - Find GPerfTools (formerly Google perftools)
 # Find the GPerfTools libraries
 # If false, do not try to use Google perftools.
-# also defined for general use are
-#  TCMALLOC_LIBRARY, where to find the tcmalloc library.
+# Also defined for general use are
+# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library
+# - GPERFTOOLS_PROFILER: where to find the profiler library
 
 SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc)
-find_library(TCMALLOC_LIBRARY
+find_library(GPERFTOOLS_TCMALLOC
   NAMES ${TCMALLOC_NAMES}
   PATHS /usr/lib /usr/local/lib
 )
@@ -16,12 +17,12 @@ find_library(GPERFTOOLS_PROFILER
    PATHS /usr/lib /usr/local/lib
 )
 
-IF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER)
-    SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY})
+IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
+    SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC})
     SET(GOOGLE_PERFTOOLS_FOUND "YES")
-ELSE (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER)
+ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
   SET(GOOGLE_PERFTOOLS_FOUND "NO")
-ENDIF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER)
+ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
 
 IF (GOOGLE_PERFTOOLS_FOUND)
    MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}")
@@ -32,6 +33,6 @@ ELSE (GOOGLE_PERFTOOLS_FOUND)
 ENDIF (GOOGLE_PERFTOOLS_FOUND)
 
 MARK_AS_ADVANCED(
-  TCMALLOC_LIBRARY
+  GPERFTOOLS_TCMALLOC
   GPERFTOOLS_PROFILER
 )

From 486feeb385345d13698e67a0ff4f20161ad8de57 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 1 Dec 2024 18:58:06 -0500
Subject: [PATCH 019/362] link gperftools libraries

---
 cmake/HandlePerfTools.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake
index 499caf80a..6c455d952 100644
--- a/cmake/HandlePerfTools.cmake
+++ b/cmake/HandlePerfTools.cmake
@@ -2,3 +2,4 @@
 ###############################################################################
 # Find Google perftools
 find_package(GooglePerfTools)
+target_link_libraries(${PROJECT_NAME} ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
\ No newline at end of file

From 8473911926e8605d5733bcd34493022cf5bef497 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 2 Dec 2024 09:52:20 -0500
Subject: [PATCH 020/362] computeThreshold as an individual function

---
 gtsam/discrete/DecisionTreeFactor.cpp | 30 +++++++++++++++++++--------
 gtsam/discrete/DecisionTreeFactor.h   | 11 ++++++++++
 2 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index caedab713..9ec3b0ac5 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -407,11 +407,9 @@ namespace gtsam {
   };
 
   /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
-    const size_t N = maxNrAssignments;
-
+  double DecisionTreeFactor::computeThreshold(const size_t N) const {
     // Set of all keys
-    std::set<Key> allKeys(keys().begin(), keys().end());
+    std::set<Key> allKeys = this->labels();
     MinHeap min_heap;
 
     auto op = [&](const Assignment<Key>& a, double p) {
@@ -433,18 +431,25 @@ namespace gtsam {
         nrAssignments *= cardinalities_.at(k);
       }
 
+      // If min-heap is empty, fill it initially.
+      // This is because there is nothing at the top.
       if (min_heap.empty()) {
         min_heap.push(p, std::min(nrAssignments, N));
 
       } else {
-        // If p is larger than the smallest element,
-        // then we insert into the max heap.
-        if (p > min_heap.top()) {
-          for (size_t i = 0; i < std::min(nrAssignments, N); ++i) {
+        for (size_t i = 0; i < std::min(nrAssignments, N); ++i) {
+          // If p is larger than the smallest element,
+          // then we insert into the min heap.
+          // We check against the top each time because the
+          // heap maintains the smallest element at the top.
+          if (p > min_heap.top()) {
             if (min_heap.size() == N) {
               min_heap.pop();
             }
             min_heap.push(p);
+          } else {
+            // p is <= min value so move to the next one
+            break;
           }
         }
       }
@@ -452,7 +457,14 @@ namespace gtsam {
     };
     this->visitWith(op);
 
-    double threshold = min_heap.top();
+    return min_heap.top();
+  }
+
+  /* ************************************************************************ */
+  DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
+    const size_t N = maxNrAssignments;
+
+    double threshold = computeThreshold(N);
 
     // Now threshold the decision tree
     size_t total = 0;
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 7af729f3e..a8ab2644f 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -224,6 +224,17 @@ namespace gtsam {
     /// Get all the probabilities in order of assignment values
     std::vector<double> probabilities() const;
 
+    /**
+     * @brief Compute the probability value which is the threshold above which
+     * only `N` leaves are present.
+     *
+     * This is used for pruning out the smaller probabilities.
+     *
+     * @param N The number of leaves to keep post pruning.
+     * @return double
+     */
+    double computeThreshold(const size_t N) const;
+
     /**
      * @brief Prune the decision tree of discrete variables.
      *

From 1091b9cd2d03258d0f79144b92c4f432628d128a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 2 Dec 2024 09:53:01 -0500
Subject: [PATCH 021/362] test for computeThreshold

---
 .../discrete/tests/testDecisionTreeFactor.cpp | 45 +++++++++++++++----
 1 file changed, 37 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index a41d06c2b..756a0cebe 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -140,11 +140,46 @@ TEST(DecisionTreeFactor, enumerate) {
   EXPECT(actual == expected);
 }
 
+namespace pruning_fixture {
+
+DiscreteKey A(1, 2), B(2, 2), C(3, 2);
+DecisionTreeFactor f(A& B& C, "1 5 3 7 2 6 4 8");
+
+DiscreteKey D(4, 2);
+DecisionTreeFactor factor(
+    D& C & B & A,
+    "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
+    "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
+
+}  // namespace pruning_fixture
+
+/* ************************************************************************* */
+// Check if computing the correct threshold works.
+TEST(DecisionTreeFactor, ComputeThreshold) {
+  using namespace pruning_fixture;
+
+  // Only keep the leaves with the top 5 values.
+  double threshold = f.computeThreshold(5);
+  EXPECT_DOUBLES_EQUAL(4.0, threshold, 1e-9);
+
+  // Check for more extreme pruning where we only keep the top 2 leaves
+  threshold = f.computeThreshold(2);
+  EXPECT_DOUBLES_EQUAL(7.0, threshold, 1e-9);
+
+  threshold = factor.computeThreshold(5);
+  EXPECT_DOUBLES_EQUAL(0.99995287, threshold, 1e-9);
+
+  threshold = factor.computeThreshold(3);
+  EXPECT_DOUBLES_EQUAL(1.0, threshold, 1e-9);
+
+  threshold = factor.computeThreshold(6);
+  EXPECT_DOUBLES_EQUAL(0.61247742, threshold, 1e-9);
+}
+
 /* ************************************************************************* */
 // Check pruning of the decision tree works as expected.
 TEST(DecisionTreeFactor, Prune) {
-  DiscreteKey A(1, 2), B(2, 2), C(3, 2);
-  DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
+  using namespace pruning_fixture;
 
   // Only keep the leaves with the top 5 values.
   size_t maxNrAssignments = 5;
@@ -160,12 +195,6 @@ TEST(DecisionTreeFactor, Prune) {
   DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8");
   EXPECT(assert_equal(expected2, pruned2));
 
-  DiscreteKey D(4, 2);
-  DecisionTreeFactor factor(
-      D & C & B & A,
-      "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
-      "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
-
   DecisionTreeFactor expected3(D & C & B & A,
                                "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 "
                                "0.999952870000 1.0 1.0 1.0 1.0");

From 3980a439c0fc655ac73a2763f60109b5e6a9270f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 2 Dec 2024 09:53:33 -0500
Subject: [PATCH 022/362] link gperftools in the top CMakeLists.txt

---
 cmake/HandlePerfTools.cmake | 3 +--
 gtsam/CMakeLists.txt        | 4 ++++
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake
index 6c455d952..4b5e5c8b3 100644
--- a/cmake/HandlePerfTools.cmake
+++ b/cmake/HandlePerfTools.cmake
@@ -1,5 +1,4 @@
 
 ###############################################################################
 # Find Google perftools
-find_package(GooglePerfTools)
-target_link_libraries(${PROJECT_NAME} ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
\ No newline at end of file
+find_package(GooglePerfTools)
\ No newline at end of file
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 1fc8e4570..dc9f40ed7 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -147,6 +147,10 @@ if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 endif()
 
+if (GOOGLE_PERFTOOLS_FOUND)
+  target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
+endif()
+
 # Add includes for source directories 'BEFORE' boost and any system include
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.

From de1c2d78fd2f14fdd25004edd31cae21d18f6435 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 2 Dec 2024 09:54:32 -0500
Subject: [PATCH 023/362] rename GOOGLE_PERFTOOLS to GPERFTOOLS

---
 cmake/FindGooglePerfTools.cmake | 12 ++++++------
 cmake/HandleAllocators.cmake    |  2 +-
 gtsam/CMakeLists.txt            |  2 +-
 3 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake
index f7caa7c1c..f73f449ec 100644
--- a/cmake/FindGooglePerfTools.cmake
+++ b/cmake/FindGooglePerfTools.cmake
@@ -19,18 +19,18 @@ find_library(GPERFTOOLS_PROFILER
 
 IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
     SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC})
-    SET(GOOGLE_PERFTOOLS_FOUND "YES")
+    SET(GPERFTOOLS_FOUND "YES")
 ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
-  SET(GOOGLE_PERFTOOLS_FOUND "NO")
+  SET(GPERFTOOLS_FOUND "NO")
 ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
 
-IF (GOOGLE_PERFTOOLS_FOUND)
-   MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}")
-ELSE (GOOGLE_PERFTOOLS_FOUND)
+IF (GPERFTOOLS_FOUND)
+   MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}")
+ELSE (GPERFTOOLS_FOUND)
    IF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Google perftools library")
    ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
-ENDIF (GOOGLE_PERFTOOLS_FOUND)
+ENDIF (GPERFTOOLS_FOUND)
 
 MARK_AS_ADVANCED(
   GPERFTOOLS_TCMALLOC
diff --git a/cmake/HandleAllocators.cmake b/cmake/HandleAllocators.cmake
index 63411b17b..38297bbba 100644
--- a/cmake/HandleAllocators.cmake
+++ b/cmake/HandleAllocators.cmake
@@ -7,7 +7,7 @@ else()
     list(APPEND possible_allocators BoostPool STL)
     set(preferred_allocator STL)
 endif()
-if(GOOGLE_PERFTOOLS_FOUND)
+if(GPERFTOOLS_FOUND)
     list(APPEND possible_allocators tcmalloc)
 endif()
 
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index dc9f40ed7..9a4b6ac3a 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -147,7 +147,7 @@ if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 endif()
 
-if (GOOGLE_PERFTOOLS_FOUND)
+if (GPERFTOOLS_FOUND)
   target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
 endif()
 

From 94e31c99df1f9fb68eb043f5a39830694cb698df Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 2 Dec 2024 09:54:56 -0500
Subject: [PATCH 024/362] expose Rot3::expmap in wrapper

---
 gtsam/geometry/geometry.i | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 42d2fe550..a8af78f2f 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -360,6 +360,7 @@ class Rot3 {
   // Standard Interface
   static gtsam::Rot3 Expmap(gtsam::Vector v);
   static gtsam::Vector Logmap(const gtsam::Rot3& p);
+  gtsam::Rot3 expmap(const gtsam::Vector& v);
   gtsam::Vector logmap(const gtsam::Rot3& p);
   gtsam::Matrix matrix() const;
   gtsam::Matrix transpose() const;

From 557d7d58278735c2d352ffe941d596be7c4b1836 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Fri, 29 Nov 2024 19:25:16 -0800
Subject: [PATCH 025/362] wrap BatchFixedLagSmoother.getFactors()

---
 gtsam/nonlinear/nonlinear.i | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 09c234630..5d15c1163 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -715,6 +715,9 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   void print(string s = "BatchFixedLagSmoother:\n") const;
 
   gtsam::LevenbergMarquardtParams params() const;
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Vector, gtsam::Matrix}>

From a9c75d8ef4f2bfd8f188c9f02caad076331b1930 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 2 Dec 2024 12:07:30 -0500
Subject: [PATCH 026/362] add pruned flag to avoid extra pruning

---
 gtsam/hybrid/HybridBayesTree.cpp           |  8 +++++---
 gtsam/hybrid/HybridGaussianConditional.cpp | 12 +++++++-----
 gtsam/hybrid/HybridGaussianConditional.h   | 11 +++++++++--
 3 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 193635a21..1b633e024 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -210,9 +210,11 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
       if (conditional->isHybrid()) {
         auto hybridGaussianCond = conditional->asHybrid();
 
-        // Imperative
-        clique->conditional() = std::make_shared<HybridConditional>(
-            hybridGaussianCond->prune(parentData.prunedDiscreteProbs));
+        if (!hybridGaussianCond->pruned()) {
+          // Imperative
+          clique->conditional() = std::make_shared<HybridConditional>(
+              hybridGaussianCond->prune(parentData.prunedDiscreteProbs));
+        }
       }
       return parentData;
     }
diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 1bec42810..54346679e 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -120,7 +120,7 @@ struct HybridGaussianConditional::Helper {
 
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKeys &discreteParents, Helper &&helper)
+    const DiscreteKeys &discreteParents, Helper &&helper, bool pruned)
     : BaseFactor(discreteParents,
                  FactorValuePairs(
                      [&](const GaussianFactorValuePair
@@ -130,7 +130,8 @@ HybridGaussianConditional::HybridGaussianConditional(
                      },
                      std::move(helper.pairs))),
       BaseConditional(*helper.nrFrontals),
-      negLogConstant_(helper.minNegLogConstant) {}
+      negLogConstant_(helper.minNegLogConstant),
+      pruned_(pruned) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey &discreteParent,
@@ -166,8 +167,9 @@ HybridGaussianConditional::HybridGaussianConditional(
     : HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKeys &discreteParents, const FactorValuePairs &pairs)
-    : HybridGaussianConditional(discreteParents, Helper(pairs)) {}
+    const DiscreteKeys &discreteParents, const FactorValuePairs &pairs,
+    bool pruned)
+    : HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {}
 
 /* *******************************************************************************/
 const HybridGaussianConditional::Conditionals
@@ -331,7 +333,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
 
   FactorValuePairs prunedConditionals = factors().apply(pruner);
   return std::make_shared<HybridGaussianConditional>(discreteKeys(),
-                                                     prunedConditionals);
+                                                     prunedConditionals, true);
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index c485fafce..0d2b1323c 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -68,6 +68,9 @@ class GTSAM_EXPORT HybridGaussianConditional
   ///< Take advantage of the neg-log space so everything is a minimization
   double negLogConstant_;
 
+  /// Flag to indicate if the conditional has been pruned.
+  bool pruned_ = false;
+
  public:
   /// @name Constructors
   /// @{
@@ -150,9 +153,10 @@ class GTSAM_EXPORT HybridGaussianConditional
    *
    * @param discreteParents the discrete parents. Will be placed last.
    * @param conditionalPairs Decision tree of GaussianFactor/scalar pairs.
+   * @param pruned Flag indicating if conditional has been pruned.
    */
   HybridGaussianConditional(const DiscreteKeys &discreteParents,
-                            const FactorValuePairs &pairs);
+                            const FactorValuePairs &pairs, bool pruned = false);
 
   /// @}
   /// @name Testable
@@ -233,6 +237,9 @@ class GTSAM_EXPORT HybridGaussianConditional
   HybridGaussianConditional::shared_ptr prune(
       const DecisionTreeFactor &discreteProbs) const;
 
+  /// Return true if the conditional has already been pruned.
+  bool pruned() const { return pruned_; }
+
   /// @}
 
  private:
@@ -241,7 +248,7 @@ class GTSAM_EXPORT HybridGaussianConditional
 
   /// Private constructor that uses helper struct above.
   HybridGaussianConditional(const DiscreteKeys &discreteParents,
-                            Helper &&helper);
+                            Helper &&helper, bool pruned = false);
 
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;

From beff3fa26992645bd49c045215823ea49dbf9448 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Mon, 2 Dec 2024 09:48:09 -0800
Subject: [PATCH 027/362] add error(HybridValues) to NonlinearFactor

fixes the python build
---
 gtsam/nonlinear/NonlinearFactor.h | 1 +
 gtsam/nonlinear/nonlinear.i       | 1 +
 2 files changed, 2 insertions(+)

diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 725117748..2d3a13766 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -22,6 +22,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/inference/Factor.h>
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 5d15c1163..b684b1aad 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -127,6 +127,7 @@ virtual class NonlinearFactor : gtsam::Factor {
   // NonlinearFactor
   bool equals(const gtsam::NonlinearFactor& other, double tol) const;
   double error(const gtsam::Values& c) const;
+  double error(const gtsam::HybridValues& c) const;
   size_t dim() const;
   bool active(const gtsam::Values& c) const;
   gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;

From bbdeaa400fe5305d86eb40a12a7b7b17516a6c0d Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Sun, 1 Dec 2024 12:45:26 -0500
Subject: [PATCH 028/362] Remove the asserts as they have already been checked
 in Eigen

---
 gtsam/base/Vector.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index f9a5cf079..4ef374234 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -16,6 +16,7 @@
  * @author  Frank Dellaert
  * @author  Alex Hagiopol
  * @author  Varun Agrawal
+ * @author  Fan Jiang
  */
 
 // \callgraph
@@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
  */
 template<class V1, class V2>
 inline double dot(const V1 &a, const V2& b) {
-  assert (b.size()==a.size());
   return a.dot(b);
 }
 
 /** compatibility version for ublas' inner_prod() */
 template<class V1, class V2>
 inline double inner_prod(const V1 &a, const V2& b) {
-  assert (b.size()==a.size());
   return a.dot(b);
 }
 

From 2aa22005f11e8b50d5dd5332e9e59104c1996b4c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 6 Dec 2024 11:32:12 -0500
Subject: [PATCH 029/362] add CMake switch for GPerfTools

---
 cmake/FindGooglePerfTools.cmake | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake
index f73f449ec..4af268528 100644
--- a/cmake/FindGooglePerfTools.cmake
+++ b/cmake/FindGooglePerfTools.cmake
@@ -36,3 +36,5 @@ MARK_AS_ADVANCED(
   GPERFTOOLS_TCMALLOC
   GPERFTOOLS_PROFILER
 )
+
+option(GTSAM_ENABLE_GPERFTOOLS                   "Enable/Disable Gperftools" OFF)

From c00de92854fce0038bf5e5ac3b49bbf45784a77e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 6 Dec 2024 11:32:34 -0500
Subject: [PATCH 030/362] use the switch

---
 gtsam/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 9a4b6ac3a..87440d19f 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -147,7 +147,7 @@ if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 endif()
 
-if (GPERFTOOLS_FOUND)
+if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND)
   target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
 endif()
 

From ddd95f4a51549d3c19cdca36d488da1323430676 Mon Sep 17 00:00:00 2001
From: Jade Turner <spacey-sooty@proton.me>
Date: Sat, 7 Dec 2024 00:33:21 +0800
Subject: [PATCH 031/362] [build] Allow disabling tests and examples

When you have gtsam as a dependency in CMake these would get built. This
doesn't make sense and increases build times, allowing them to be
disabled fixes this.

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
---
 CMakeLists.txt | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e3b462eec..ffedf56e4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -112,10 +112,17 @@ add_subdirectory(CppUnitLite)
 add_subdirectory(gtsam)
 
 # Build Tests
-add_subdirectory(tests)
+option(BUILD_TESTS "Builds unit tests" ON)
+if (BUILD_TESTS)
+    add_subdirectory(tests)
+endif()
+
 
 # Build examples
-add_subdirectory(examples)
+option(BUILD_EXAMPLES "Builds examples" ON)
+if (BUILD_EXAMPLES)
+    add_subdirectory(examples)
+endif()
 
 # Build timing
 add_subdirectory(timing)

From d1d440ad3420efb6a35bef80f39699b6b075e810 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 10:53:32 -0500
Subject: [PATCH 032/362] add nrValues method

---
 gtsam/discrete/DecisionTreeFactor.h     | 6 ++++++
 gtsam/discrete/DiscreteFactor.h         | 6 ++++++
 gtsam/discrete/TableFactor.h            | 6 ++++++
 gtsam_unstable/discrete/AllDiff.h       | 3 +++
 gtsam_unstable/discrete/BinaryAllDiff.h | 3 +++
 gtsam_unstable/discrete/Domain.h        | 2 +-
 gtsam_unstable/discrete/SingleValue.h   | 3 +++
 7 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index a8ab2644f..f417a38d7 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -255,6 +255,12 @@ namespace gtsam {
      */
     DecisionTreeFactor prune(size_t maxNrAssignments) const;
 
+    /**
+     * Get the number of non-zero values contained in this factor.
+     * It could be much smaller than `prod_{key}(cardinality(key))`.
+     */
+    uint64_t nrValues() const override { return nrLeaves(); }
+
     /// @}
     /// @name Wrapper support
     /// @{
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 19af5bd13..7d5047ec6 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -113,6 +113,12 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
+  /**
+   * Get the number of non-zero values contained in this factor.
+   * It could be much smaller than `prod_{key}(cardinality(key))`.
+   */
+  virtual uint64_t nrValues() const = 0;
+
   /// @}
   /// @name Wrapper support
   /// @{
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index f0ecd66a3..b988eebad 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -324,6 +324,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
    */
   TableFactor prune(size_t maxNrAssignments) const;
 
+  /**
+   * Get the number of non-zero values contained in this factor.
+   * It could be much smaller than `prod_{key}(cardinality(key))`.
+   */
+  uint64_t nrValues() const override { return sparse_table_.nonZeros(); }
+
   /// @}
   /// @name Wrapper support
   /// @{
diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index d7a63eae0..42a255bbf 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -72,6 +72,9 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   /// Partially apply known values, domain version
   Constraint::shared_ptr partiallyApply(
       const Domains&) const override;
+
+  /// Get the number of non-zero values contained in this factor.
+  uint64_t nrValues() const override { return 1; };
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 18b335092..22acfb092 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -96,6 +96,9 @@ class BinaryAllDiff : public Constraint {
   AlgebraicDecisionTree<Key> errorTree() const override {
     throw std::runtime_error("BinaryAllDiff::error not implemented");
   }
+
+  /// Get the number of non-zero values contained in this factor.
+  uint64_t nrValues() const override { return 1; };
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 7f7b717c2..ba3771eca 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -49,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   /// Erase a value, non const :-(
   void erase(size_t value) { values_.erase(value); }
 
-  size_t nrValues() const { return values_.size(); }
+  uint64_t nrValues() const override { return values_.size(); }
 
   bool isSingleton() const { return nrValues() == 1; }
 
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 3f7f22d6a..7f2eb2c2c 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -77,6 +77,9 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   /// Partially apply known values, domain version
   Constraint::shared_ptr partiallyApply(
       const Domains& domains) const override;
+
+  /// Get the number of non-zero values contained in this factor.
+  uint64_t nrValues() const override { return 1; };
 };
 
 }  // namespace gtsam

From a68da21527760daff64161f3feffc6cc1d46d1b1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 11:02:30 -0500
Subject: [PATCH 033/362] operator* version which accepts DiscreteFactor

---
 gtsam/discrete/DecisionTreeFactor.cpp | 11 +++++++++++
 gtsam/discrete/DecisionTreeFactor.h   |  5 ++++-
 gtsam/discrete/DiscreteFactor.h       |  7 ++++---
 gtsam/discrete/TableFactor.cpp        |  9 +++++++--
 gtsam/discrete/TableFactor.h          |  5 +++--
 5 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 9ec3b0ac5..e53f8cb90 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -82,6 +82,17 @@ namespace gtsam {
     ADT::print("", formatter);
   }
 
+  /* ************************************************************************ */
+  DiscreteFactor::shared_ptr DecisionTreeFactor::operator*(
+      const DiscreteFactor::shared_ptr& f) const {
+    if (auto derived = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      return std::make_shared<DecisionTreeFactor>(this->operator*(*derived));
+    } else {
+      throw std::runtime_error(
+          "Cannot convert DiscreteFactor to DecisionTreeFactor");
+    }
+  }
+
   /* ************************************************************************ */
   DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
     // apply operand
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index f417a38d7..7afbab0b0 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -144,10 +144,13 @@ namespace gtsam {
     double error(const DiscreteValues& values) const override;
 
     /// multiply two factors
-    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
       return apply(f, ADT::Ring::mul);
     }
 
+    DiscreteFactor::shared_ptr operator*(
+        const DiscreteFactor::shared_ptr& f) const override;
+
     static double safe_div(const double& a, const double& b);
 
     /// divide by factor f (safely)
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 7d5047ec6..4c486dca8 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -107,9 +107,10 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// Compute error for each assignment and return as a tree
   virtual AlgebraicDecisionTree<Key> errorTree() const;
 
-  /// Multiply in a DecisionTreeFactor and return the result as
-  /// DecisionTreeFactor
-  virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;
+  /// Multiply in a DiscreteFactor and return the result as
+  /// DiscreteFactor
+  virtual DiscreteFactor::shared_ptr operator*(
+      const DiscreteFactor::shared_ptr&) const = 0;
 
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index f4e023a4d..7cf520973 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -169,8 +169,13 @@ double TableFactor::error(const HybridValues& values) const {
 }
 
 /* ************************************************************************ */
-DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
-  return toDecisionTreeFactor() * f;
+DiscreteFactor::shared_ptr TableFactor::operator*(
+    const DiscreteFactor::shared_ptr& f) const {
+  if (auto derived = std::dynamic_pointer_cast<TableFactor>(f)) {
+    return std::make_shared<TableFactor>(this->operator*(*derived));
+  } else {
+    throw std::runtime_error("Cannot convert DiscreteFactor to TableFactor");
+  }
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index b988eebad..29cbd5e9b 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -186,8 +186,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, Ring::mul);
   };
 
-  /// multiply with DecisionTreeFactor
-  DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
+  /// multiply with DiscreteFactor
+  DiscreteFactor::shared_ptr operator*(
+      const DiscreteFactor::shared_ptr& f) const override;
 
   static double safe_div(const double& a, const double& b);
 

From a09b77ef407b7ac91b1604153d7b2ec08301b4b8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 11:07:26 -0500
Subject: [PATCH 034/362] return DiscreteFactor shared_ptr as leftover from
 elimination

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 4 ++--
 gtsam/discrete/DiscreteFactorGraph.h   | 7 ++++---
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 4ededbb8b..f81c6085e 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -112,7 +112,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   // Alternate eliminate function for MPE
-  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+  std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
@@ -201,7 +201,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+  std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index d0dc282b4..a5324811c 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -48,7 +48,7 @@ class DiscreteJunctionTree;
  * @ingroup discrete
  */
 GTSAM_EXPORT
-std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
+std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
 EliminateDiscrete(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys);
 
@@ -61,7 +61,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors,
  * @ingroup discrete
  */
 GTSAM_EXPORT
-std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
+std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
 EliminateForMPE(const DiscreteFactorGraph& factors,
                 const Ordering& frontalKeys);
 
@@ -133,6 +133,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
 
   /// @}
 
+  //TODO(Varun): Make compatible with TableFactor
   /** Add a decision-tree factor */
   template <typename... Args>
   void add(Args&&... args) {
@@ -146,7 +147,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
   DiscreteKeys discreteKeys() const;
 
   /** return product of all factors as a single factor */
-  DecisionTreeFactor product() const;
+  DiscreteFactor::shared_ptr product() const;
 
   /** 
    * Evaluates the factor graph given values, returns the joint probability of

From 27bbce150aa2dac82e57ff84a91c17a9873fab7b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 11:10:24 -0500
Subject: [PATCH 035/362] generalize DiscreteFactorGraph::product to
 DiscreteFactor

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index f81c6085e..b27438130 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -64,10 +64,16 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  DecisionTreeFactor DiscreteFactorGraph::product() const {
-    DecisionTreeFactor result;
-    for(const sharedFactor& factor: *this)
-      if (factor) result = (*factor) * result;
+  DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const {
+    sharedFactor result = this->at(0);
+    for (size_t i = 1; i < this->size(); ++i) {
+      const sharedFactor factor = this->at(i);
+      if (factor) {
+        // Predicated on the fact that all discrete factors are of a single type
+        // so there is no type-conversion happening which can be expensive.
+        result = result->operator*(factor);
+      }
+    }
     return result;
   }
 

From 84e419456af55c0d194645c302934316146b3803 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 11:15:06 -0500
Subject: [PATCH 036/362] make normalization code common

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 50 ++++++++++++++++----------
 1 file changed, 32 insertions(+), 18 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index b27438130..29bd1f9ac 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -116,6 +116,28 @@ namespace gtsam {
 //      }
 //  }
 
+  /**
+   * @brief Helper method to normalize the product factor by
+   * the max value to prevent underflow
+   *
+   * @param product The product discrete factor.
+   * @return DiscreteFactor::shared_ptr
+   */
+  static DiscreteFactor::shared_ptr Normalize(
+      const DiscreteFactor::shared_ptr& product) {
+    // Max over all the potentials by pretending all keys are frontal:
+    gttic(DiscreteFindMax);
+    auto normalization = product->max(product->size());
+    gttoc(DiscreteFindMax);
+
+    gttic(DiscreteNormalization);
+    // Normalize the product factor to prevent underflow.
+    auto normalized_product = product->operator/(normalization);
+    gttoc(DiscreteNormalization);
+
+    return normalized_product;
+  }
+
   /* ************************************************************************ */
   // Alternate eliminate function for MPE
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
@@ -123,27 +145,23 @@ namespace gtsam {
                   const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic(product);
-    DecisionTreeFactor product;
-    for (auto&& factor : factors) product = (*factor) * product;
+    DiscreteFactor::shared_ptr product = factors.product();
     gttoc(product);
 
-    // Max over all the potentials by pretending all keys are frontal:
-    auto normalization = product.max(product.size());
-
-    // Normalize the product factor to prevent underflow.
-    product = product / (*normalization);
+    // Normalize the product
+    product = Normalize(product);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
-    DecisionTreeFactor::shared_ptr max = product.max(frontalKeys);
+    DecisionTreeFactor::shared_ptr max = product->max(frontalKeys);
     gttoc(max);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     DiscreteKeys orderedKeys;
     for (auto&& key : frontalKeys)
-      orderedKeys.emplace_back(key, product.cardinality(key));
+      orderedKeys.emplace_back(key, product->cardinality(key));
     for (auto&& key : max->keys())
-      orderedKeys.emplace_back(key, product.cardinality(key));
+      orderedKeys.emplace_back(key, product->cardinality(key));
 
     // Make lookup with product
     gttic(lookup);
@@ -212,19 +230,15 @@ namespace gtsam {
                     const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic(product);
-    DecisionTreeFactor product;
-    for (auto&& factor : factors) product = (*factor) * product;
+    DiscreteFactor::shared_ptr product = factors.product();
     gttoc(product);
 
-    // Max over all the potentials by pretending all keys are frontal:
-    auto normalization = product.max(product.size());
-
-    // Normalize the product factor to prevent underflow.
-    product = product / (*normalization);
+    // Normalize the product 
+    product = Normalize(product);
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);
-    DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
+    DecisionTreeFactor::shared_ptr sum = product->sum(frontalKeys);
     gttoc(sum);
 
     // Ordering keys for the conditional so that frontalKeys are really in front

From 4dac37ce2b0000fd51f67d361c9fbcbd744c06ab Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 11:16:49 -0500
Subject: [PATCH 037/362] make sum and max DiscreteFactor methods

---
 gtsam/discrete/DecisionTreeFactor.h | 19 +++++++++++++++----
 gtsam/discrete/DiscreteFactor.h     | 17 +++++++++++++++++
 gtsam/discrete/TableFactor.h        | 18 ++++++++++++++----
 3 files changed, 46 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 7afbab0b0..8445c5332 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -158,26 +158,37 @@ namespace gtsam {
       return apply(f, safe_div);
     }
 
+    /// divide by factor f (pointer version)
+    DiscreteFactor::shared_ptr operator/(
+        const DiscreteFactor::shared_ptr& f) const override {
+      if (auto derived = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+        return std::make_shared<DecisionTreeFactor>(apply(*derived, safe_div));
+      } else {
+        throw std::runtime_error(
+            "Cannot convert DiscreteFactor to Table Factor");
+      }
+    }
+
     /// Convert into a decision tree
     DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
 
     /// Create new factor by summing all values with the same separator values
-    shared_ptr sum(size_t nrFrontals) const {
+    DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
       return combine(nrFrontals, ADT::Ring::add);
     }
 
     /// Create new factor by summing all values with the same separator values
-    shared_ptr sum(const Ordering& keys) const {
+    DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
       return combine(keys, ADT::Ring::add);
     }
 
     /// Create new factor by maximizing over all values with the same separator.
-    shared_ptr max(size_t nrFrontals) const {
+    DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
       return combine(nrFrontals, ADT::Ring::max);
     }
 
     /// Create new factor by maximizing over all values with the same separator.
-    shared_ptr max(const Ordering& keys) const {
+    DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
       return combine(keys, ADT::Ring::max);
     }
 
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 4c486dca8..1ada7b7b2 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -22,6 +22,7 @@
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/inference/Factor.h>
+#include <gtsam/inference/Ordering.h>
 
 #include <string>
 namespace gtsam {
@@ -114,6 +115,22 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
+  /// Create new factor by summing all values with the same separator values
+  virtual DiscreteFactor::shared_ptr sum(size_t nrFrontals) const = 0;
+
+  /// Create new factor by summing all values with the same separator values
+  virtual DiscreteFactor::shared_ptr sum(const Ordering& keys) const = 0;
+
+  /// Create new factor by maximizing over all values with the same separator.
+  virtual DiscreteFactor::shared_ptr max(size_t nrFrontals) const = 0;
+
+  /// Create new factor by maximizing over all values with the same separator.
+  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const = 0;
+
+  /// divide by factor f (safely)
+  virtual DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& f) const = 0;
+
   /**
    * Get the number of non-zero values contained in this factor.
    * It could be much smaller than `prod_{key}(cardinality(key))`.
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 29cbd5e9b..e452b5be0 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -197,6 +197,16 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, safe_div);
   }
 
+  /// divide by factor f (pointer version)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& f) const override {
+    if (auto derived = std::dynamic_pointer_cast<TableFactor>(f)) {
+      return std::make_shared<TableFactor>(apply(*derived, safe_div));
+    } else {
+      throw std::runtime_error("Cannot convert DiscreteFactor to Table Factor");
+    }
+  }
+
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
 
@@ -205,22 +215,22 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
                      DiscreteKeys parent_keys) const;
 
   /// Create new factor by summing all values with the same separator values
-  shared_ptr sum(size_t nrFrontals) const {
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
     return combine(nrFrontals, Ring::add);
   }
 
   /// Create new factor by summing all values with the same separator values
-  shared_ptr sum(const Ordering& keys) const {
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
     return combine(keys, Ring::add);
   }
 
   /// Create new factor by maximizing over all values with the same separator.
-  shared_ptr max(size_t nrFrontals) const {
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
     return combine(nrFrontals, Ring::max);
   }
 
   /// Create new factor by maximizing over all values with the same separator.
-  shared_ptr max(const Ordering& keys) const {
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
     return combine(keys, Ring::max);
   }
 

From 6c4546779a2dff7a03a1f1cc8978e3facbf3cd16 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 18:20:50 -0500
Subject: [PATCH 038/362] add timing info

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 24 +++++++++++++++---------
 gtsam/discrete/TableFactor.cpp         |  6 ++++++
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 29bd1f9ac..904108afb 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -22,6 +22,7 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteLookupDAG.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 #include <gtsam/inference/FactorGraph-inst.h>
 
@@ -144,12 +145,15 @@ namespace gtsam {
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
-    gttic(product);
+    gttic_(MPEProduct);
     DiscreteFactor::shared_ptr product = factors.product();
-    gttoc(product);
+    gttoc_(MPEProduct);
+
+    gttic_(Normalize);
 
     // Normalize the product
     product = Normalize(product);
+    gttoc_(Normalize);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -229,17 +233,19 @@ namespace gtsam {
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
-    gttic(product);
+    gttic_(product);
     DiscreteFactor::shared_ptr product = factors.product();
-    gttoc(product);
+    gttoc_(product);
 
-    // Normalize the product 
+    gttic_(Normalize);
+    // Normalize the product
     product = Normalize(product);
+    gttoc_(Normalize);
 
     // sum out frontals, this is the factor on the separator
-    gttic(sum);
+    gttic_(sum);
     DecisionTreeFactor::shared_ptr sum = product->sum(frontalKeys);
-    gttoc(sum);
+    gttoc_(sum);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     Ordering orderedKeys;
@@ -249,10 +255,10 @@ namespace gtsam {
                        sum->keys().end());
 
     // now divide product/sum to get conditional
-    gttic(divide);
+    gttic_(divide);
     auto conditional =
         std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
-    gttoc(divide);
+    gttoc_(divide);
 
     return {conditional, sum};
   }
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 7cf520973..b867fa916 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -72,7 +72,9 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
  */
 std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
                                         const DecisionTreeFactor& dt) {
+  gttic_(ComputeLeafOrdering);
   std::vector<double> probs = dt.probabilities();
+  gttoc_(ComputeLeafOrdering);
   std::vector<double> ordered;
 
   size_t n = dkeys[0].second;
@@ -180,12 +182,16 @@ DiscreteFactor::shared_ptr TableFactor::operator*(
 
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
+  gttic_(toDecisionTreeFactor);
   DiscreteKeys dkeys = discreteKeys();
   std::vector<double> table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
     table.push_back(sparse_table_.coeff(i));
   }
+  gttoc_(toDecisionTreeFactor);
+  gttic_(toDecisionTreeFactor_Constructor);
   DecisionTreeFactor f(dkeys, table);
+  gttoc_(toDecisionTreeFactor_Constructor);
   return f;
 }
 

From b0ad350a20a410aa2658839a7707a910d7129bc6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 18:22:30 -0500
Subject: [PATCH 039/362] add note about toDecisionTreeFactor

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

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index b867fa916..53131616d 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -182,14 +182,13 @@ DiscreteFactor::shared_ptr TableFactor::operator*(
 
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
-  gttic_(toDecisionTreeFactor);
   DiscreteKeys dkeys = discreteKeys();
   std::vector<double> table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
     table.push_back(sparse_table_.coeff(i));
   }
-  gttoc_(toDecisionTreeFactor);
   gttic_(toDecisionTreeFactor_Constructor);
+  // NOTE(Varun): This constructor is really expensive!!
   DecisionTreeFactor f(dkeys, table);
   gttoc_(toDecisionTreeFactor_Constructor);
   return f;

From 306a3bae527af21cd121cff602bfa97fa3a5966f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 18:25:27 -0500
Subject: [PATCH 040/362] kill toDecisionTreeFactor to force rethink

---
 gtsam/discrete/DecisionTreeFactor.h |  3 ---
 gtsam/discrete/DiscreteFactor.h     |  2 --
 gtsam/discrete/TableFactor.cpp      | 14 --------------
 gtsam/discrete/TableFactor.h        |  3 ---
 4 files changed, 22 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 8445c5332..642187ff1 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -169,9 +169,6 @@ namespace gtsam {
       }
     }
 
-    /// Convert into a decision tree
-    DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
-
     /// Create new factor by summing all values with the same separator values
     DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
       return combine(nrFrontals, ADT::Ring::add);
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 1ada7b7b2..29984d795 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -113,8 +113,6 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   virtual DiscreteFactor::shared_ptr operator*(
       const DiscreteFactor::shared_ptr&) const = 0;
 
-  virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
-
   /// Create new factor by summing all values with the same separator values
   virtual DiscreteFactor::shared_ptr sum(size_t nrFrontals) const = 0;
 
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 53131616d..a8adf0918 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -180,20 +180,6 @@ DiscreteFactor::shared_ptr TableFactor::operator*(
   }
 }
 
-/* ************************************************************************ */
-DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
-  DiscreteKeys dkeys = discreteKeys();
-  std::vector<double> table;
-  for (auto i = 0; i < sparse_table_.size(); i++) {
-    table.push_back(sparse_table_.coeff(i));
-  }
-  gttic_(toDecisionTreeFactor_Constructor);
-  // NOTE(Varun): This constructor is really expensive!!
-  DecisionTreeFactor f(dkeys, table);
-  gttoc_(toDecisionTreeFactor_Constructor);
-  return f;
-}
-
 /* ************************************************************************ */
 TableFactor TableFactor::choose(const DiscreteValues parent_assign,
                                 DiscreteKeys parent_keys) const {
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index e452b5be0..12266b2a5 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -207,9 +207,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     }
   }
 
-  /// Convert into a decisiontree
-  DecisionTreeFactor toDecisionTreeFactor() const override;
-
   /// Create a TableFactor that is a subset of this TableFactor
   TableFactor choose(const DiscreteValues assignments,
                      DiscreteKeys parent_keys) const;

From 2cd2ab0a43ea57c33392371d7ec1d285b1f005c3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 18:25:40 -0500
Subject: [PATCH 041/362] DiscreteDistribution from TableFactor

---
 gtsam/discrete/DiscreteDistribution.h | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h
index 4b690da15..abe8f7933 100644
--- a/gtsam/discrete/DiscreteDistribution.h
+++ b/gtsam/discrete/DiscreteDistribution.h
@@ -40,10 +40,14 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
   /// Default constructor needed for serialization.
   DiscreteDistribution() {}
 
-  /// Constructor from factor.
+  /// Constructor from DecisionTreeFactor.
   explicit DiscreteDistribution(const DecisionTreeFactor& f)
       : Base(f.size(), f) {}
 
+  /// Constructor from TableFactor.
+  explicit DiscreteDistribution(const TableFactor& f)
+      : Base(f.size(), f) {}
+
   /**
    * Construct from a Signature.
    *

From 9f88a360dfd54f1e00361344f24d9d2ef085b50f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 18:29:13 -0500
Subject: [PATCH 042/362] make evaluate use the Assignment<Key> base class

---
 gtsam/discrete/DecisionTreeFactor.h | 4 ++--
 gtsam/discrete/DiscreteFactor.h     | 3 +++
 gtsam/discrete/TableFactor.cpp      | 2 +-
 gtsam/discrete/TableFactor.h        | 9 ++++++---
 4 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 642187ff1..bf0e23b50 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -129,9 +129,9 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /// Calculate probability for given values `x`, 
+    /// Calculate probability for given values, 
     /// is just look up in AlgebraicDecisionTree.
-    double evaluate(const Assignment<Key>& values) const  {
+    double evaluate(const Assignment<Key>& values) const override {
       return ADT::operator()(values);
     }
 
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 29984d795..23abf725e 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -129,6 +129,9 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   virtual DiscreteFactor::shared_ptr operator/(
       const DiscreteFactor::shared_ptr& f) const = 0;
 
+  /// Calculate probability for given values
+  virtual double evaluate(const Assignment<Key>& values) const = 0;
+
   /**
    * Get the number of non-zero values contained in this factor.
    * It could be much smaller than `prod_{key}(cardinality(key))`.
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index a8adf0918..727c96ce4 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -135,7 +135,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
 }
 
 /* ************************************************************************ */
-double TableFactor::operator()(const DiscreteValues& values) const {
+double TableFactor::operator()(const Assignment<Key>& values) const {
   // a b c d => D * (C * (B * (a) + b) + c) + d
   uint64_t idx = 0, card = 1;
   for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) {
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 12266b2a5..ea222ca5c 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -169,14 +169,17 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   // /// @name Standard Interface
   // /// @{
 
-  /// Calculate probability for given values `x`,
+  /// Calculate probability for given values,
   /// is just look up in TableFactor.
-  double evaluate(const DiscreteValues& values) const {
+  double evaluate(const Assignment<Key>& values) const override {
     return operator()(values);
   }
 
   /// Evaluate probability distribution, sugar.
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const;
+  double operator()(const DiscreteValues& values) const override {
+    return operator()(Assignment<Key>(values));
+  }
 
   /// Calculate error for DiscreteValues `x`, is -log(probability).
   double error(const DiscreteValues& values) const override;

From 2a3b5e62b785c2c9de4e414f5dcb1551569a297c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 18:59:11 -0500
Subject: [PATCH 043/362] use Assignment<Key> for evaluate since it is the base
 class

---
 gtsam/discrete/DiscreteFactor.h | 2 +-
 gtsam/discrete/TableFactor.h    | 3 ---
 2 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 23abf725e..4470d97a7 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -94,7 +94,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   size_t cardinality(Key j) const { return cardinalities_.at(j); }
 
   /// Find value for given assignment of values to variables
-  virtual double operator()(const DiscreteValues&) const = 0;
+  virtual double operator()(const DiscreteValues& values) const = 0;
 
   /// Error is just -log(value)
   virtual double error(const DiscreteValues& values) const;
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index ea222ca5c..8fb04fcba 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -177,9 +177,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
 
   /// Evaluate probability distribution, sugar.
   double operator()(const Assignment<Key>& values) const;
-  double operator()(const DiscreteValues& values) const override {
-    return operator()(Assignment<Key>(values));
-  }
 
   /// Calculate error for DiscreteValues `x`, is -log(probability).
   double error(const DiscreteValues& values) const override;

From fff8458d6bcc0ec54a8c7ebbf0145c3b8b91aafc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:03:26 -0500
Subject: [PATCH 044/362] remove TableFactor constructor in
 DiscreteDistribution

---
 gtsam/discrete/DiscreteDistribution.h | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h
index abe8f7933..09ea50332 100644
--- a/gtsam/discrete/DiscreteDistribution.h
+++ b/gtsam/discrete/DiscreteDistribution.h
@@ -44,10 +44,6 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
   explicit DiscreteDistribution(const DecisionTreeFactor& f)
       : Base(f.size(), f) {}
 
-  /// Constructor from TableFactor.
-  explicit DiscreteDistribution(const TableFactor& f)
-      : Base(f.size(), f) {}
-
   /**
    * Construct from a Signature.
    *

From 295b965b6894574c11786dbef03edc9022311d82 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:09:45 -0500
Subject: [PATCH 045/362] use Assignment<Key> since it is a base class

---
 gtsam/discrete/DecisionTreeFactor.h  | 2 +-
 gtsam/discrete/DiscreteConditional.h | 2 +-
 gtsam/discrete/DiscreteFactor.h      | 2 +-
 gtsam/discrete/TableFactor.h         | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index bf0e23b50..688cd85a6 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -136,7 +136,7 @@ namespace gtsam {
     }
 
     /// Evaluate probability distribution, sugar.
-    double operator()(const DiscreteValues& values) const override {
+    double operator()(const Assignment<Key>& values) const override {
       return ADT::operator()(values);
     }
 
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index f59e29285..ce4fb96e5 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -169,7 +169,7 @@ class GTSAM_EXPORT DiscreteConditional
   }
 
   /// Evaluate, just look up in AlgebraicDecisionTree
-  double evaluate(const DiscreteValues& values) const {
+  double evaluate(const Assignment<Key>& values) const override {
     return ADT::operator()(values);
   }
 
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 4470d97a7..4c1d0afb1 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -94,7 +94,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   size_t cardinality(Key j) const { return cardinalities_.at(j); }
 
   /// Find value for given assignment of values to variables
-  virtual double operator()(const DiscreteValues& values) const = 0;
+  virtual double operator()(const Assignment<Key>& values) const = 0;
 
   /// Error is just -log(value)
   virtual double error(const DiscreteValues& values) const;
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 8fb04fcba..497c42dc2 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -176,7 +176,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   }
 
   /// Evaluate probability distribution, sugar.
-  double operator()(const Assignment<Key>& values) const;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Calculate error for DiscreteValues `x`, is -log(probability).
   double error(const DiscreteValues& values) const override;

From 261038f93620185ede195605a021c30c5b71f4d8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:09:56 -0500
Subject: [PATCH 046/362] fix DiscreteConditional constructor

---
 gtsam/discrete/DiscreteConditional.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 5ab0c59ec..92086d143 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -43,7 +43,9 @@ template class GTSAM_EXPORT
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
                                          const DecisionTreeFactor& f)
-    : BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {}
+    : BaseFactor(f / (*std::dynamic_pointer_cast<DecisionTreeFactor>(
+                         f.sum(nrFrontals)))),
+      BaseConditional(nrFrontals) {}
 
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(size_t nrFrontals,

From 20d6d09e06e705d8384d685226239c109748e7a2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:12:06 -0500
Subject: [PATCH 047/362] use DiscreteFactor everywhere in
 DiscreteFactorGraph.cpp

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 904108afb..a4f92e267 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -55,7 +55,7 @@ namespace gtsam {
   DiscreteKeys DiscreteFactorGraph::discreteKeys() const {
     DiscreteKeys result;
     for (auto&& factor : *this) {
-      if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
+      if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
         DiscreteKeys factor_keys = p->discreteKeys();
         result.insert(result.end(), factor_keys.begin(), factor_keys.end());
       }
@@ -157,7 +157,7 @@ namespace gtsam {
 
     // max out frontals, this is the factor on the separator
     gttic(max);
-    DecisionTreeFactor::shared_ptr max = product->max(frontalKeys);
+    DiscreteFactor::shared_ptr max = product->max(frontalKeys);
     gttoc(max);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
@@ -244,7 +244,7 @@ namespace gtsam {
 
     // sum out frontals, this is the factor on the separator
     gttic_(sum);
-    DecisionTreeFactor::shared_ptr sum = product->sum(frontalKeys);
+    DiscreteFactor::shared_ptr sum = product->sum(frontalKeys);
     gttoc_(sum);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
@@ -257,8 +257,8 @@ namespace gtsam {
     // now divide product/sum to get conditional
     gttic_(divide);
     auto conditional =
-        std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
-    gttoc_(divide);
+        std::make_shared<DiscreteConditional>(product, sum, orderedKeys);
+    gttoc(divide);
 
     return {conditional, sum};
   }

From 32b6bc0a37da20e49cc8d390dc04b3ff9bb77548 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:18:42 -0500
Subject: [PATCH 048/362] update DiscreteConditional

---
 gtsam/discrete/DiscreteConditional.cpp | 19 ++++++++++---------
 gtsam/discrete/DiscreteConditional.h   | 16 ++++++++--------
 2 files changed, 18 insertions(+), 17 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 92086d143..2f900afbe 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -37,8 +37,7 @@ using std::vector;
 namespace gtsam {
 
 // Instantiate base class
-template class GTSAM_EXPORT
-    Conditional<DecisionTreeFactor, DiscreteConditional>;
+template class GTSAM_EXPORT Conditional<DecisionTreeFactor, DiscreteConditional>;
 
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
@@ -54,15 +53,17 @@ DiscreteConditional::DiscreteConditional(size_t nrFrontals,
     : BaseFactor(keys, potentials), BaseConditional(nrFrontals) {}
 
 /* ************************************************************************** */
-DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
-                                         const DecisionTreeFactor& marginal)
-    : BaseFactor(joint / marginal),
-      BaseConditional(joint.size() - marginal.size()) {}
+DiscreteConditional::DiscreteConditional(
+    const DiscreteFactor::shared_ptr& joint,
+    const DiscreteFactor::shared_ptr& marginal)
+    : BaseFactor(*std::dynamic_pointer_cast<DecisionTreeFactor>(
+          joint->operator/(marginal))),
+      BaseConditional(joint->size() - marginal->size()) {}
 
 /* ************************************************************************** */
-DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
-                                         const DecisionTreeFactor& marginal,
-                                         const Ordering& orderedKeys)
+DiscreteConditional::DiscreteConditional(
+    const DiscreteFactor::shared_ptr& joint,
+    const DiscreteFactor::shared_ptr& marginal, const Ordering& orderedKeys)
     : DiscreteConditional(joint, marginal) {
   keys_.clear();
   keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index ce4fb96e5..ec2c5c38d 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -110,16 +110,16 @@ class GTSAM_EXPORT DiscreteConditional
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    */
-  DiscreteConditional(const DecisionTreeFactor& joint,
-                      const DecisionTreeFactor& marginal);
+  DiscreteConditional(const DiscreteFactor::shared_ptr& joint,
+                      const DiscreteFactor::shared_ptr& marginal);
 
   /**
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    * Makes sure the keys are ordered as given. Does not check orderedKeys.
    */
-  DiscreteConditional(const DecisionTreeFactor& joint,
-                      const DecisionTreeFactor& marginal,
+  DiscreteConditional(const DiscreteFactor::shared_ptr& joint,
+                      const DiscreteFactor::shared_ptr& marginal,
                       const Ordering& orderedKeys);
 
   /**
@@ -173,8 +173,8 @@ class GTSAM_EXPORT DiscreteConditional
     return ADT::operator()(values);
   }
 
-  using DecisionTreeFactor::error;       ///< DiscreteValues version
-  using DecisionTreeFactor::operator();  ///< DiscreteValues version
+  using DiscreteFactor::error;       ///< DiscreteValues version
+  using DiscreteFactor::operator();  ///< DiscreteValues version
 
   /**
    * @brief restrict to given *parent* values.
@@ -192,11 +192,11 @@ class GTSAM_EXPORT DiscreteConditional
   shared_ptr choose(const DiscreteValues& given) const;
 
   /** Convert to a likelihood factor by providing value before bar. */
-  DecisionTreeFactor::shared_ptr likelihood(
+  DiscreteFactor::shared_ptr likelihood(
       const DiscreteValues& frontalValues) const;
 
   /** Single variable version of likelihood. */
-  DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
+  DiscreteFactor::shared_ptr likelihood(size_t frontal) const;
 
   /**
    * sample

From 38563da342f233f81f356d76cc78d034db6b9d4f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:24:04 -0500
Subject: [PATCH 049/362] Revert "kill toDecisionTreeFactor to force rethink"

This reverts commit 306a3bae527af21cd121cff602bfa97fa3a5966f.
---
 gtsam/discrete/DecisionTreeFactor.h |  3 +++
 gtsam/discrete/DiscreteFactor.h     |  2 ++
 gtsam/discrete/TableFactor.cpp      | 14 ++++++++++++++
 gtsam/discrete/TableFactor.h        |  3 +++
 4 files changed, 22 insertions(+)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 688cd85a6..f8f2835e5 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -169,6 +169,9 @@ namespace gtsam {
       }
     }
 
+    /// Convert into a decision tree
+    DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
+
     /// Create new factor by summing all values with the same separator values
     DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
       return combine(nrFrontals, ADT::Ring::add);
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 4c1d0afb1..e2d32e828 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -113,6 +113,8 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   virtual DiscreteFactor::shared_ptr operator*(
       const DiscreteFactor::shared_ptr&) const = 0;
 
+  virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
+
   /// Create new factor by summing all values with the same separator values
   virtual DiscreteFactor::shared_ptr sum(size_t nrFrontals) const = 0;
 
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 727c96ce4..50d15ff5e 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -180,6 +180,20 @@ DiscreteFactor::shared_ptr TableFactor::operator*(
   }
 }
 
+/* ************************************************************************ */
+DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
+  DiscreteKeys dkeys = discreteKeys();
+  std::vector<double> table;
+  for (auto i = 0; i < sparse_table_.size(); i++) {
+    table.push_back(sparse_table_.coeff(i));
+  }
+  gttic_(toDecisionTreeFactor_Constructor);
+  // NOTE(Varun): This constructor is really expensive!!
+  DecisionTreeFactor f(dkeys, table);
+  gttoc_(toDecisionTreeFactor_Constructor);
+  return f;
+}
+
 /* ************************************************************************ */
 TableFactor TableFactor::choose(const DiscreteValues parent_assign,
                                 DiscreteKeys parent_keys) const {
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 497c42dc2..47a7c6bbb 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -207,6 +207,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     }
   }
 
+  /// Convert into a decisiontree
+  DecisionTreeFactor toDecisionTreeFactor() const override;
+
   /// Create a TableFactor that is a subset of this TableFactor
   TableFactor choose(const DiscreteValues assignments,
                      DiscreteKeys parent_keys) const;

From 9633ad1fd8631a3abda75a25e5217e516119e458 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:24:29 -0500
Subject: [PATCH 050/362] make DiscreteConditional::likelihood match the
 declaration

---
 gtsam/discrete/DiscreteConditional.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 2f900afbe..048f35e5b 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -201,7 +201,7 @@ DiscreteConditional::shared_ptr DiscreteConditional::choose(
 }
 
 /* ************************************************************************** */
-DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
+DiscreteFactor::shared_ptr DiscreteConditional::likelihood(
     const DiscreteValues& frontalValues) const {
   // Get the big decision tree with all the levels, and then go down the
   // branches based on the value of the frontal variables.
@@ -226,7 +226,7 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
 }
 
 /* ****************************************************************************/
-DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
+DiscreteFactor::shared_ptr DiscreteConditional::likelihood(
     size_t frontal) const {
   if (nrFrontals() != 1)
     throw std::invalid_argument(

From 0b3477fc5ab428a6bbc65ca2b95e02d84b982593 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:39:44 -0500
Subject: [PATCH 051/362] get different classes to play nicely

---
 gtsam/discrete/DiscreteConditional.h  | 4 ++--
 gtsam/discrete/DiscreteDistribution.h | 3 +--
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index ec2c5c38d..77003f232 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -173,8 +173,8 @@ class GTSAM_EXPORT DiscreteConditional
     return ADT::operator()(values);
   }
 
-  using DiscreteFactor::error;       ///< DiscreteValues version
-  using DiscreteFactor::operator();  ///< DiscreteValues version
+  using DecisionTreeFactor::error;       ///< DiscreteValues version
+  using DecisionTreeFactor::operator();  ///< DiscreteValues version
 
   /**
    * @brief restrict to given *parent* values.
diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h
index 09ea50332..28e509f15 100644
--- a/gtsam/discrete/DiscreteDistribution.h
+++ b/gtsam/discrete/DiscreteDistribution.h
@@ -86,8 +86,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
   double operator()(size_t value) const;
 
   /// We also want to keep the Base version, taking DiscreteValues:
-  // TODO(dellaert): does not play well with wrapper!
-  // using Base::operator();
+  using Base::operator();
 
   /// Return entire probability mass function.
   std::vector<double> pmf() const;

From 1d7918841797e3e970587cbec1c60aeaafa21566 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 19:40:20 -0500
Subject: [PATCH 052/362] compiles

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 6 ++++--
 gtsam/discrete/DiscreteLookupDAG.h     | 7 +++++++
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index a4f92e267..c2a16159b 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -170,8 +170,10 @@ namespace gtsam {
     // Make lookup with product
     gttic(lookup);
     size_t nrFrontals = frontalKeys.size();
-    auto lookup = std::make_shared<DiscreteLookupTable>(nrFrontals,
-                                                          orderedKeys, product);
+    //TODO(Varun): Should accept a DiscreteFactor::shared_ptr
+    auto lookup = std::make_shared<DiscreteLookupTable>(
+        nrFrontals, orderedKeys,
+        *std::dynamic_pointer_cast<TableFactor>(product));
     gttoc(lookup);
 
     return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h
index f077a13d9..c811c4c49 100644
--- a/gtsam/discrete/DiscreteLookupDAG.h
+++ b/gtsam/discrete/DiscreteLookupDAG.h
@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteDistribution.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/FactorGraph.h>
 
@@ -54,6 +55,12 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
                       const ADT& potentials)
       : DiscreteConditional(nFrontals, keys, potentials) {}
 
+  //TODO(Varun): Should accept a DiscreteFactor::shared_ptr
+  DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,
+                      const TableFactor& potentials)
+      : DiscreteConditional(nFrontals, keys,
+                            potentials.toDecisionTreeFactor()) {}
+
   /// GTSAM-style print
   void print(
       const std::string& s = "Discrete Lookup Table: ",

From 77578512f80600d27f25cdfeb0c22526e64ce7b9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Dec 2024 21:45:22 -0500
Subject: [PATCH 053/362] timing

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 11 +++++------
 gtsam/discrete/DiscreteLookupDAG.h     | 10 +++++++++-
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index c2a16159b..fa9d9bdc7 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -22,7 +22,6 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteLookupDAG.h>
-#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 #include <gtsam/inference/FactorGraph-inst.h>
 
@@ -127,14 +126,14 @@ namespace gtsam {
   static DiscreteFactor::shared_ptr Normalize(
       const DiscreteFactor::shared_ptr& product) {
     // Max over all the potentials by pretending all keys are frontal:
-    gttic(DiscreteFindMax);
+    gttic_(DiscreteFindMax);
     auto normalization = product->max(product->size());
-    gttoc(DiscreteFindMax);
+    gttoc_(DiscreteFindMax);
 
-    gttic(DiscreteNormalization);
+    gttic_(DiscreteNormalization);
     // Normalize the product factor to prevent underflow.
     auto normalized_product = product->operator/(normalization);
-    gttoc(DiscreteNormalization);
+    gttoc_(DiscreteNormalization);
 
     return normalized_product;
   }
@@ -260,7 +259,7 @@ namespace gtsam {
     gttic_(divide);
     auto conditional =
         std::make_shared<DiscreteConditional>(product, sum, orderedKeys);
-    gttoc(divide);
+    gttoc_(divide);
 
     return {conditional, sum};
   }
diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h
index c811c4c49..21181d374 100644
--- a/gtsam/discrete/DiscreteLookupDAG.h
+++ b/gtsam/discrete/DiscreteLookupDAG.h
@@ -55,7 +55,15 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
                       const ADT& potentials)
       : DiscreteConditional(nFrontals, keys, potentials) {}
 
-  //TODO(Varun): Should accept a DiscreteFactor::shared_ptr
+  /**
+   * @brief Construct a new Discrete Lookup Table object
+   *
+   * @param nFrontals number of frontal variables
+   * @param keys a sorted list of gtsam::Keys
+   * @param potentials Discrete potentials as a TableFactor.
+   *
+   * //TODO(Varun): Should accept a DiscreteFactor::shared_ptr
+   */
   DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,
                       const TableFactor& potentials)
       : DiscreteConditional(nFrontals, keys,

From 947f5b172d95d2438adfbcdf679a5817b0b93939 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sat, 7 Dec 2024 20:21:46 -0800
Subject: [PATCH 054/362] fix CHECK_EQUAL

---
 CppUnitLite/Test.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h
index 691068148..9ee0d10eb 100644
--- a/CppUnitLite/Test.h
+++ b/CppUnitLite/Test.h
@@ -129,7 +129,7 @@ protected:
     result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
 
 #define CHECK_EQUAL(expected,actual)\
-{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); }
+{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } }
 
 #define LONGS_EQUAL(expected,actual)\
 { long actualTemp = actual; \

From 9844a555d4debcf3e0cb7c6047c7b81f8701d27b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 10:34:02 -0500
Subject: [PATCH 055/362] move evaluate and operator() next to each other

---
 gtsam/discrete/DiscreteFactor.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index e2d32e828..3151afe80 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -93,6 +93,9 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   size_t cardinality(Key j) const { return cardinalities_.at(j); }
 
+  /// Calculate probability for given values
+  virtual double evaluate(const Assignment<Key>& values) const = 0;
+
   /// Find value for given assignment of values to variables
   virtual double operator()(const Assignment<Key>& values) const = 0;
 
@@ -131,9 +134,6 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   virtual DiscreteFactor::shared_ptr operator/(
       const DiscreteFactor::shared_ptr& f) const = 0;
 
-  /// Calculate probability for given values
-  virtual double evaluate(const Assignment<Key>& values) const = 0;
-
   /**
    * Get the number of non-zero values contained in this factor.
    * It could be much smaller than `prod_{key}(cardinality(key))`.

From aa25ccfa6ecbaef12e549b3e0ac6a09d5c2d30de Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 11:15:57 -0500
Subject: [PATCH 056/362] implement evaluate in DiscreteFactor

---
 gtsam/discrete/DecisionTreeFactor.h    |  5 -----
 gtsam/discrete/DiscreteConditional.cpp |  2 +-
 gtsam/discrete/DiscreteConditional.h   |  5 -----
 gtsam/discrete/DiscreteFactor.h        | 14 ++++++++++++--
 gtsam/discrete/TableFactor.h           |  8 +-------
 5 files changed, 14 insertions(+), 20 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index f8f2835e5..85491b909 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -131,11 +131,6 @@ namespace gtsam {
 
     /// Calculate probability for given values, 
     /// is just look up in AlgebraicDecisionTree.
-    double evaluate(const Assignment<Key>& values) const override {
-      return ADT::operator()(values);
-    }
-
-    /// Evaluate probability distribution, sugar.
     double operator()(const Assignment<Key>& values) const override {
       return ADT::operator()(values);
     }
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 048f35e5b..c90f7f9a0 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -476,7 +476,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter,
 
 /* ************************************************************************* */
 double DiscreteConditional::evaluate(const HybridValues& x) const {
-  return this->evaluate(x.discrete());
+  return this->operator()(x.discrete());
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 77003f232..29292f57e 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -168,11 +168,6 @@ class GTSAM_EXPORT DiscreteConditional
     static_cast<const BaseConditional*>(this)->print(s, formatter);
   }
 
-  /// Evaluate, just look up in AlgebraicDecisionTree
-  double evaluate(const Assignment<Key>& values) const override {
-    return ADT::operator()(values);
-  }
-
   using DecisionTreeFactor::error;       ///< DiscreteValues version
   using DecisionTreeFactor::operator();  ///< DiscreteValues version
 
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 3151afe80..5b4665d4d 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -93,8 +93,18 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   size_t cardinality(Key j) const { return cardinalities_.at(j); }
 
-  /// Calculate probability for given values
-  virtual double evaluate(const Assignment<Key>& values) const = 0;
+  /**
+   * @brief Calculate probability for given values.
+   * Calls specialized evaluation under the hood.
+   *
+   * Note: Uses Assignment<Key> as it is the base class of DiscreteValues.
+   *
+   * @param values Discrete assignment.
+   * @return double
+   */
+  double evaluate(const Assignment<Key>& values) const {
+    return operator()(values);
+  }
 
   /// Find value for given assignment of values to variables
   virtual double operator()(const Assignment<Key>& values) const = 0;
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 47a7c6bbb..d8df12821 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -169,13 +169,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   // /// @name Standard Interface
   // /// @{
 
-  /// Calculate probability for given values,
-  /// is just look up in TableFactor.
-  double evaluate(const Assignment<Key>& values) const override {
-    return operator()(values);
-  }
-
-  /// Evaluate probability distribution, sugar.
+  /// Evaluate probability distribution, is just look up in TableFactor.
   double operator()(const Assignment<Key>& values) const override;
 
   /// Calculate error for DiscreteValues `x`, is -log(probability).

From 90d7e21941d324df59ceda3487890d403a1af953 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 11:19:35 -0500
Subject: [PATCH 057/362] change from DiscreteValues to Assignment<Key>

---
 gtsam_unstable/discrete/AllDiff.cpp     | 2 +-
 gtsam_unstable/discrete/AllDiff.h       | 2 +-
 gtsam_unstable/discrete/BinaryAllDiff.h | 2 +-
 gtsam_unstable/discrete/Domain.cpp      | 2 +-
 gtsam_unstable/discrete/Domain.h        | 2 +-
 5 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp
index 2bd9e6dfd..a450605b3 100644
--- a/gtsam_unstable/discrete/AllDiff.cpp
+++ b/gtsam_unstable/discrete/AllDiff.cpp
@@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const {
 }
 
 /* ************************************************************************* */
-double AllDiff::operator()(const DiscreteValues& values) const {
+double AllDiff::operator()(const Assignment<Key>& values) const {
   std::set<size_t> taken;  // record values taken by keys
   for (Key dkey : keys_) {
     size_t value = values.at(dkey);      // get the value for that key
diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 42a255bbf..7f539f4c6 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   }
 
   /// Calculate value = expensive !
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree, can be *very* expensive !
   DecisionTreeFactor toDecisionTreeFactor() const override;
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 22acfb092..0e2fce109 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint {
   }
 
   /// Calculate value
-  double operator()(const DiscreteValues& values) const override {
+  double operator()(const Assignment<Key>& values) const override {
     return (double)(values.at(keys_[0]) != values.at(keys_[1]));
   }
 
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index bbbc87667..752228c18 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -30,7 +30,7 @@ string Domain::base1Str() const {
 }
 
 /* ************************************************************************* */
-double Domain::operator()(const DiscreteValues& values) const {
+double Domain::operator()(const Assignment<Key>& values) const {
   return contains(values.at(key()));
 }
 
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index ba3771eca..cd11fc8d9 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   bool contains(size_t value) const { return values_.count(value) > 0; }
 
   /// Calculate value
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;

From 6665659e9de8a9f89dfda0c468692d93052d0f7a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 11:23:04 -0500
Subject: [PATCH 058/362] use BaseFactor instead of DecisionTreeFactor

---
 gtsam/discrete/DiscreteConditional.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 29292f57e..3dbadb3e4 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -168,8 +168,8 @@ class GTSAM_EXPORT DiscreteConditional
     static_cast<const BaseConditional*>(this)->print(s, formatter);
   }
 
-  using DecisionTreeFactor::error;       ///< DiscreteValues version
-  using DecisionTreeFactor::operator();  ///< DiscreteValues version
+  using BaseFactor::error;       ///< DiscreteValues version
+  using BaseFactor::operator();  ///< DiscreteValues version
 
   /**
    * @brief restrict to given *parent* values.

From 9b93411d69b1cde30c720e80e644f096b6d7d6cb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 11:45:10 -0500
Subject: [PATCH 059/362] put Ring struct in a separate file

---
 gtsam/discrete/AlgebraicDecisionTree.h    | 24 ++-------------
 gtsam/discrete/DiscreteConditional.cpp    |  3 +-
 gtsam/discrete/Ring.h                     | 37 +++++++++++++++++++++++
 gtsam/discrete/TableFactor.h              | 16 +---------
 gtsam/discrete/tests/testDecisionTree.cpp |  9 +-----
 5 files changed, 43 insertions(+), 46 deletions(-)
 create mode 100644 gtsam/discrete/Ring.h

diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index 9948b0be6..383346ab1 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -20,12 +20,12 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/Ring.h>
 
-#include <algorithm>
+#include <iomanip>
 #include <limits>
 #include <map>
 #include <string>
-#include <iomanip>
 #include <vector>
 
 namespace gtsam {
@@ -55,26 +55,6 @@ namespace gtsam {
    public:
     using Base = DecisionTree<L, double>;
 
-    /** The Real ring with addition and multiplication */
-    struct Ring {
-      static inline double zero() { return 0.0; }
-      static inline double one() { return 1.0; }
-      static inline double add(const double& a, const double& b) {
-        return a + b;
-      }
-      static inline double max(const double& a, const double& b) {
-        return std::max(a, b);
-      }
-      static inline double mul(const double& a, const double& b) {
-        return a * b;
-      }
-      static inline double div(const double& a, const double& b) {
-        return a / b;
-      }
-      static inline double id(const double& x) { return x; }
-      static inline double negate(const double& x) { return -x; }
-    };
-
     AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {}
 
     // Explicitly non-explicit constructor
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 5ab0c59ec..c44905d79 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/debug.h>
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/Ring.h>
 #include <gtsam/discrete/Signature.h>
 #include <gtsam/hybrid/HybridValues.h>
 
@@ -104,7 +105,7 @@ DiscreteConditional DiscreteConditional::operator*(
   // Finally, add parents to keys, in order
   for (auto&& dk : parents) discreteKeys.push_back(dk);
 
-  ADT product = ADT::apply(other, ADT::Ring::mul);
+  ADT product = ADT::apply(other, Ring::mul);
   return DiscreteConditional(newFrontals.size(), discreteKeys, product);
 }
 
diff --git a/gtsam/discrete/Ring.h b/gtsam/discrete/Ring.h
new file mode 100644
index 000000000..cf7c6424a
--- /dev/null
+++ b/gtsam/discrete/Ring.h
@@ -0,0 +1,37 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    Ring.h
+ * @brief   Real Ring definition
+ * @author  Varun Agrawal
+ * @date    Dec 8, 2024
+ */
+
+#pragma once
+
+#include <algorithm>
+
+/** The Real ring with addition and multiplication */
+struct Ring {
+  static inline double zero() { return 0.0; }
+  static inline double one() { return 1.0; }
+  static inline double add(const double& a, const double& b) { return a + b; }
+  static inline double max(const double& a, const double& b) {
+    return std::max(a, b);
+  }
+  static inline double mul(const double& a, const double& b) { return a * b; }
+  static inline double div(const double& a, const double& b) {
+    return (a == 0 || b == 0) ? 0 : (a / b);
+  }
+  static inline double id(const double& x) { return x; }
+  static inline double negate(const double& x) { return -x; }
+};
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index f0ecd66a3..7015353e1 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -19,6 +19,7 @@
 
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/Ring.h>
 #include <gtsam/inference/Ordering.h>
 
 #include <Eigen/Sparse>
@@ -99,21 +100,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   using Binary = std::function<double(const double, const double)>;
 
  public:
-  /** The Real ring with addition and multiplication */
-  struct Ring {
-    static inline double zero() { return 0.0; }
-    static inline double one() { return 1.0; }
-    static inline double add(const double& a, const double& b) { return a + b; }
-    static inline double max(const double& a, const double& b) {
-      return std::max(a, b);
-    }
-    static inline double mul(const double& a, const double& b) { return a * b; }
-    static inline double div(const double& a, const double& b) {
-      return (a == 0 || b == 0) ? 0 : (a / b);
-    }
-    static inline double id(const double& x) { return x; }
-  };
-
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 526001b51..c664fe6b5 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -23,6 +23,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/Ring.h>
 #include <gtsam/discrete/Signature.h>
 #include <gtsam/inference/Symbol.h>
 
@@ -124,14 +125,6 @@ struct traits<DT> : public Testable<DT> {};
 
 GTSAM_CONCEPT_TESTABLE_INST(DT)
 
-struct Ring {
-  static inline int zero() { return 0; }
-  static inline int one() { return 1; }
-  static inline int id(const int& a) { return a; }
-  static inline int add(const int& a, const int& b) { return a + b; }
-  static inline int mul(const int& a, const int& b) { return a * b; }
-};
-
 /* ************************************************************************** */
 // Check that creating decision trees respects key order.
 TEST(DecisionTree, ConstructorOrder) {

From 7bf8ee167d0405d5822c021ef5b3d2ccc4942ffa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 11:51:55 -0500
Subject: [PATCH 060/362] update DecisionTreeFactor.h

---
 gtsam/discrete/DecisionTreeFactor.h | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index a8ab2644f..9a3cde96d 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -21,11 +21,12 @@
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/Ring.h>
 #include <gtsam/inference/Ordering.h>
 
 #include <algorithm>
-#include <memory>
 #include <map>
+#include <memory>
 #include <stdexcept>
 #include <string>
 #include <utility>
@@ -145,7 +146,7 @@ namespace gtsam {
 
     /// multiply two factors
     DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
-      return apply(f, ADT::Ring::mul);
+      return apply(f, Ring::mul);
     }
 
     static double safe_div(const double& a, const double& b);
@@ -160,22 +161,22 @@ namespace gtsam {
 
     /// Create new factor by summing all values with the same separator values
     shared_ptr sum(size_t nrFrontals) const {
-      return combine(nrFrontals, ADT::Ring::add);
+      return combine(nrFrontals, Ring::add);
     }
 
     /// Create new factor by summing all values with the same separator values
     shared_ptr sum(const Ordering& keys) const {
-      return combine(keys, ADT::Ring::add);
+      return combine(keys, Ring::add);
     }
 
     /// Create new factor by maximizing over all values with the same separator.
     shared_ptr max(size_t nrFrontals) const {
-      return combine(nrFrontals, ADT::Ring::max);
+      return combine(nrFrontals, Ring::max);
     }
 
     /// Create new factor by maximizing over all values with the same separator.
     shared_ptr max(const Ordering& keys) const {
-      return combine(keys, ADT::Ring::max);
+      return combine(keys, Ring::max);
     }
 
     /// @}

From e6b65285217c84f49a07a641c187719c1bbfbf43 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 12:36:23 -0500
Subject: [PATCH 061/362] common definitions of Unary, UnaryAssignment and
 Binary

---
 gtsam/discrete/DecisionTreeFactor.cpp | 12 ++++++------
 gtsam/discrete/DecisionTreeFactor.h   | 16 ++++++++++------
 gtsam/discrete/DiscreteFactor.h       |  5 +++++
 3 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index e53f8cb90..93a7921aa 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -94,7 +94,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
+  DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const {
     // apply operand
     ADT result = ADT::apply(op);
     // Make a new factor
@@ -102,7 +102,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
+  DecisionTreeFactor DecisionTreeFactor::apply(UnaryAssignment op) const {
     // apply operand
     ADT result = ADT::apply(op);
     // Make a new factor
@@ -111,7 +111,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
-                                              ADT::Binary op) const {
+                                               Binary op) const {
     map<Key, size_t> cs;  // new cardinalities
     // make unique key-cardinality map
     for (Key j : keys()) cs[j] = cardinality(j);
@@ -129,8 +129,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
-      size_t nrFrontals, ADT::Binary op) const {
+  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals,
+                                                             Binary op) const {
     if (nrFrontals > size()) {
       throw invalid_argument(
           "DecisionTreeFactor::combine: invalid number of frontal "
@@ -157,7 +157,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
-      const Ordering& frontalKeys, ADT::Binary op) const {
+      const Ordering& frontalKeys, Binary op) const {
     if (frontalKeys.size() > size()) {
       throw invalid_argument(
           "DecisionTreeFactor::combine: invalid number of frontal "
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 983330f4a..f8679af59 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -51,6 +51,10 @@ namespace gtsam {
     typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
     typedef AlgebraicDecisionTree<Key> ADT;
 
+    using Base::Binary;
+    using Base::Unary;
+    using Base::UnaryAssignment;
+
     /// @name Standard Constructors
     /// @{
 
@@ -140,7 +144,7 @@ namespace gtsam {
     double error(const DiscreteValues& values) const override;
 
     /// multiply two factors
-    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
       return apply(f, Ring::mul);
     }
 
@@ -196,21 +200,21 @@ namespace gtsam {
      * Apply unary operator (*this) "op" f
      * @param op a unary operator that operates on AlgebraicDecisionTree
      */
-    DecisionTreeFactor apply(ADT::Unary op) const;
+    DecisionTreeFactor apply(Unary op) const;
 
     /**
      * Apply unary operator (*this) "op" f
      * @param op a unary operator that operates on AlgebraicDecisionTree. Takes
      * both the assignment and the value.
      */
-    DecisionTreeFactor apply(ADT::UnaryAssignment op) const;
+    DecisionTreeFactor apply(UnaryAssignment op) const;
 
     /**
      * Apply binary operator (*this) "op" f
      * @param f the second argument for op
      * @param op a binary operator that operates on AlgebraicDecisionTree
      */
-    DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const;
+    DecisionTreeFactor apply(const DecisionTreeFactor& f, Binary op) const;
 
     /**
      * Combine frontal variables using binary operator "op"
@@ -218,7 +222,7 @@ namespace gtsam {
      * @param op a binary operator that operates on AlgebraicDecisionTree
      * @return shared pointer to newly created DecisionTreeFactor
      */
-    shared_ptr combine(size_t nrFrontals, ADT::Binary op) const;
+    shared_ptr combine(size_t nrFrontals, Binary op) const;
 
     /**
      * Combine frontal variables in an Ordering using binary operator "op"
@@ -226,7 +230,7 @@ namespace gtsam {
      * @param op a binary operator that operates on AlgebraicDecisionTree
      * @return shared pointer to newly created DecisionTreeFactor
      */
-    shared_ptr combine(const Ordering& keys, ADT::Binary op) const;
+    shared_ptr combine(const Ordering& keys, Binary op) const;
 
     /// Enumerate all values into a map from values to double.
     std::vector<std::pair<DiscreteValues, double>> enumerate() const;
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 5b4665d4d..6e9f69619 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -47,6 +47,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   using Values = DiscreteValues;  ///< backwards compatibility
 
+  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)>;
+
  protected:
   /// Map of Keys and their cardinalities.
   std::map<Key, size_t> cardinalities_;

From f85284afb2f07ab82284628e45ef68949f15fa1a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 12:37:36 -0500
Subject: [PATCH 062/362] some cleanup based on previous commit

---
 gtsam/discrete/DecisionTreeFactor.h | 1 +
 gtsam/discrete/TableFactor.h        | 5 -----
 2 files changed, 1 insertion(+), 5 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index f8679af59..af0df47a2 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -51,6 +51,7 @@ namespace gtsam {
     typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
     typedef AlgebraicDecisionTree<Key> ADT;
 
+    // Needed since we have definitions in both DiscreteFactor and DecisionTree
     using Base::Binary;
     using Base::Unary;
     using Base::UnaryAssignment;
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 317596ba9..345cbc254 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -94,12 +94,7 @@ 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:
   /// @name Standard Constructors
   /// @{
 

From 5e86f7ee5122fc6cf4fac609fdf2a639c1e5079f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 15:31:35 -0500
Subject: [PATCH 063/362] remove previously added code

---
 gtsam/discrete/DecisionTreeFactor.cpp  | 11 -----------
 gtsam/discrete/DecisionTreeFactor.h    | 16 +---------------
 gtsam/discrete/DiscreteConditional.cpp | 13 +++++++------
 gtsam/discrete/DiscreteConditional.h   |  8 ++++----
 gtsam/discrete/DiscreteLookupDAG.cpp   |  2 +-
 5 files changed, 13 insertions(+), 37 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 93a7921aa..776d4bd90 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -82,17 +82,6 @@ namespace gtsam {
     ADT::print("", formatter);
   }
 
-  /* ************************************************************************ */
-  DiscreteFactor::shared_ptr DecisionTreeFactor::operator*(
-      const DiscreteFactor::shared_ptr& f) const {
-    if (auto derived = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
-      return std::make_shared<DecisionTreeFactor>(this->operator*(*derived));
-    } else {
-      throw std::runtime_error(
-          "Cannot convert DiscreteFactor to DecisionTreeFactor");
-    }
-  }
-
   /* ************************************************************************ */
   DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const {
     // apply operand
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index af0df47a2..11793f984 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -145,13 +145,10 @@ namespace gtsam {
     double error(const DiscreteValues& values) const override;
 
     /// multiply two factors
-    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
       return apply(f, Ring::mul);
     }
 
-    DiscreteFactor::shared_ptr operator*(
-        const DiscreteFactor::shared_ptr& f) const override;
-
     static double safe_div(const double& a, const double& b);
 
     /// divide by factor f (safely)
@@ -159,17 +156,6 @@ namespace gtsam {
       return apply(f, safe_div);
     }
 
-    /// divide by factor f (pointer version)
-    DiscreteFactor::shared_ptr operator/(
-        const DiscreteFactor::shared_ptr& f) const override {
-      if (auto derived = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
-        return std::make_shared<DecisionTreeFactor>(apply(*derived, safe_div));
-      } else {
-        throw std::runtime_error(
-            "Cannot convert DiscreteFactor to Table Factor");
-      }
-    }
-
     /// Convert into a decision tree
     DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
 
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 6dff84859..58acb21b0 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -38,7 +38,8 @@ using std::vector;
 namespace gtsam {
 
 // Instantiate base class
-template class GTSAM_EXPORT Conditional<DecisionTreeFactor, DiscreteConditional>;
+template class GTSAM_EXPORT
+    Conditional<DecisionTreeFactor, DiscreteConditional>;
 
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
@@ -152,11 +153,11 @@ void DiscreteConditional::print(const string& s,
 /* ************************************************************************** */
 bool DiscreteConditional::equals(const DiscreteFactor& other,
                                  double tol) const {
-  if (!dynamic_cast<const DecisionTreeFactor*>(&other)) {
+  if (!dynamic_cast<const BaseFactor*>(&other)) {
     return false;
   } else {
-    const DecisionTreeFactor& f(static_cast<const DecisionTreeFactor&>(other));
-    return DecisionTreeFactor::equals(f, tol);
+    const BaseFactor& f(static_cast<const BaseFactor&>(other));
+    return BaseFactor::equals(f, tol);
   }
 }
 
@@ -377,7 +378,7 @@ std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter,
   ss << "*\n" << std::endl;
   if (nrParents() == 0) {
     // We have no parents, call factor method.
-    ss << DecisionTreeFactor::markdown(keyFormatter, names);
+    ss << BaseFactor::markdown(keyFormatter, names);
     return ss.str();
   }
 
@@ -429,7 +430,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter,
   ss << "</i></p>\n";
   if (nrParents() == 0) {
     // We have no parents, call factor method.
-    ss << DecisionTreeFactor::html(keyFormatter, names);
+    ss << BaseFactor::html(keyFormatter, names);
     return ss.str();
   }
 
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 3dbadb3e4..298a7b004 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -110,16 +110,16 @@ class GTSAM_EXPORT DiscreteConditional
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    */
-  DiscreteConditional(const DiscreteFactor::shared_ptr& joint,
-                      const DiscreteFactor::shared_ptr& marginal);
+  DiscreteConditional(const DecisionTreeFactor& joint,
+                      const DecisionTreeFactor& marginal);
 
   /**
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    * Makes sure the keys are ordered as given. Does not check orderedKeys.
    */
-  DiscreteConditional(const DiscreteFactor::shared_ptr& joint,
-                      const DiscreteFactor::shared_ptr& marginal,
+  DiscreteConditional(const DecisionTreeFactor& joint,
+                      const DecisionTreeFactor& marginal,
                       const Ordering& orderedKeys);
 
   /**
diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp
index 4d02e007b..ee381fe44 100644
--- a/gtsam/discrete/DiscreteLookupDAG.cpp
+++ b/gtsam/discrete/DiscreteLookupDAG.cpp
@@ -47,7 +47,7 @@ void DiscreteLookupTable::print(const std::string& s,
     }
   }
   cout << "):\n";
-  ADT::print("", formatter);
+  BaseFactor::print("", formatter);
   cout << endl;
 }
 

From 1c14a56f5d9351355107b181b87565fa0613856b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 15:58:07 -0500
Subject: [PATCH 064/362] revert changes to make code generic

---
 gtsam/discrete/DiscreteConditional.cpp | 16 +++++++---------
 gtsam/discrete/DiscreteFactor.h        | 10 +++-------
 gtsam/discrete/TableFactor.cpp         |  9 ++-------
 gtsam/discrete/TableFactor.h           | 14 ++------------
 4 files changed, 14 insertions(+), 35 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 58acb21b0..bd10e28b4 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -55,17 +55,15 @@ DiscreteConditional::DiscreteConditional(size_t nrFrontals,
     : BaseFactor(keys, potentials), BaseConditional(nrFrontals) {}
 
 /* ************************************************************************** */
-DiscreteConditional::DiscreteConditional(
-    const DiscreteFactor::shared_ptr& joint,
-    const DiscreteFactor::shared_ptr& marginal)
-    : BaseFactor(*std::dynamic_pointer_cast<DecisionTreeFactor>(
-          joint->operator/(marginal))),
-      BaseConditional(joint->size() - marginal->size()) {}
+DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
+                                         const DecisionTreeFactor& marginal)
+    : BaseFactor(joint / marginal),
+      BaseConditional(joint.size() - marginal.size()) {}
 
 /* ************************************************************************** */
-DiscreteConditional::DiscreteConditional(
-    const DiscreteFactor::shared_ptr& joint,
-    const DiscreteFactor::shared_ptr& marginal, const Ordering& orderedKeys)
+DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
+                                         const DecisionTreeFactor& marginal,
+                                         const Ordering& orderedKeys)
     : DiscreteConditional(joint, marginal) {
   keys_.clear();
   keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 6e9f69619..a6356a045 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -126,10 +126,9 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// Compute error for each assignment and return as a tree
   virtual AlgebraicDecisionTree<Key> errorTree() const;
 
-  /// Multiply in a DiscreteFactor and return the result as
-  /// DiscreteFactor
-  virtual DiscreteFactor::shared_ptr operator*(
-      const DiscreteFactor::shared_ptr&) const = 0;
+  /// Multiply in a DecisionTreeFactor and return the result as
+  /// DecisionTreeFactor
+  virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;
 
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
@@ -145,9 +144,6 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// Create new factor by maximizing over all values with the same separator.
   virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const = 0;
 
-  /// divide by factor f (safely)
-  virtual DiscreteFactor::shared_ptr operator/(
-      const DiscreteFactor::shared_ptr& f) const = 0;
 
   /**
    * Get the number of non-zero values contained in this factor.
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 50d15ff5e..a4947012e 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -171,13 +171,8 @@ double TableFactor::error(const HybridValues& values) const {
 }
 
 /* ************************************************************************ */
-DiscreteFactor::shared_ptr TableFactor::operator*(
-    const DiscreteFactor::shared_ptr& f) const {
-  if (auto derived = std::dynamic_pointer_cast<TableFactor>(f)) {
-    return std::make_shared<TableFactor>(this->operator*(*derived));
-  } else {
-    throw std::runtime_error("Cannot convert DiscreteFactor to TableFactor");
-  }
+DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
+  return toDecisionTreeFactor() * f;
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 345cbc254..ba1d05fe9 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -161,9 +161,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, Ring::mul);
   };
 
-  /// multiply with DiscreteFactor
-  DiscreteFactor::shared_ptr operator*(
-      const DiscreteFactor::shared_ptr& f) const override;
+  /// multiply with DecisionTreeFactor
+  DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
   static double safe_div(const double& a, const double& b);
 
@@ -172,15 +171,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, safe_div);
   }
 
-  /// divide by factor f (pointer version)
-  DiscreteFactor::shared_ptr operator/(
-      const DiscreteFactor::shared_ptr& f) const override {
-    if (auto derived = std::dynamic_pointer_cast<TableFactor>(f)) {
-      return std::make_shared<TableFactor>(apply(*derived, safe_div));
-    } else {
-      throw std::runtime_error("Cannot convert DiscreteFactor to Table Factor");
-    }
-  }
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;

From b325150b3751b7f5c43b0268f76be8e60f10fe38 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 16:18:42 -0500
Subject: [PATCH 065/362] revert DiscreteFactorGraph::product

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 13 ++++---------
 gtsam/discrete/DiscreteFactorGraph.h   |  2 +-
 2 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index fa9d9bdc7..9e64b0f6d 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -64,15 +64,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const {
-    sharedFactor result = this->at(0);
-    for (size_t i = 1; i < this->size(); ++i) {
-      const sharedFactor factor = this->at(i);
-      if (factor) {
-        // Predicated on the fact that all discrete factors are of a single type
-        // so there is no type-conversion happening which can be expensive.
-        result = result->operator*(factor);
-      }
+  DecisionTreeFactor DiscreteFactorGraph::product() const {
+    DecisionTreeFactor result;
+    for (const sharedFactor& factor : *this) {
+      if (factor) result = result * (*factor);
     }
     return result;
   }
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index a5324811c..43c48c2d0 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -147,7 +147,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
   DiscreteKeys discreteKeys() const;
 
   /** return product of all factors as a single factor */
-  DiscreteFactor::shared_ptr product() const;
+  DecisionTreeFactor product() const;
 
   /** 
    * Evaluates the factor graph given values, returns the joint probability of

From 0afc1984118d8bec1c2327f542cac8b22d11b96f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 16:26:03 -0500
Subject: [PATCH 066/362] revert some DiscreteFactorGraph changes

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 29 +++++++++++++-------------
 1 file changed, 15 insertions(+), 14 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 9e64b0f6d..04849985f 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -118,16 +118,17 @@ namespace gtsam {
    * @param product The product discrete factor.
    * @return DiscreteFactor::shared_ptr
    */
-  static DiscreteFactor::shared_ptr Normalize(
-      const DiscreteFactor::shared_ptr& product) {
+  static DecisionTreeFactor Normalize(const DecisionTreeFactor& product) {
     // Max over all the potentials by pretending all keys are frontal:
     gttic_(DiscreteFindMax);
-    auto normalization = product->max(product->size());
+    auto normalization = product.max(product.size());
     gttoc_(DiscreteFindMax);
 
     gttic_(DiscreteNormalization);
     // Normalize the product factor to prevent underflow.
-    auto normalized_product = product->operator/(normalization);
+    auto normalized_product =
+        product /
+        (*std::dynamic_pointer_cast<DecisionTreeFactor>(normalization));
     gttoc_(DiscreteNormalization);
 
     return normalized_product;
@@ -140,7 +141,7 @@ namespace gtsam {
                   const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic_(MPEProduct);
-    DiscreteFactor::shared_ptr product = factors.product();
+    DecisionTreeFactor product = factors.product();
     gttoc_(MPEProduct);
 
     gttic_(Normalize);
@@ -151,23 +152,22 @@ namespace gtsam {
 
     // max out frontals, this is the factor on the separator
     gttic(max);
-    DiscreteFactor::shared_ptr max = product->max(frontalKeys);
+    DecisionTreeFactor::shared_ptr max =
+        std::dynamic_pointer_cast<DecisionTreeFactor>(product.max(frontalKeys));
     gttoc(max);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     DiscreteKeys orderedKeys;
     for (auto&& key : frontalKeys)
-      orderedKeys.emplace_back(key, product->cardinality(key));
+      orderedKeys.emplace_back(key, product.cardinality(key));
     for (auto&& key : max->keys())
-      orderedKeys.emplace_back(key, product->cardinality(key));
+      orderedKeys.emplace_back(key, product.cardinality(key));
 
     // Make lookup with product
     gttic(lookup);
     size_t nrFrontals = frontalKeys.size();
-    //TODO(Varun): Should accept a DiscreteFactor::shared_ptr
-    auto lookup = std::make_shared<DiscreteLookupTable>(
-        nrFrontals, orderedKeys,
-        *std::dynamic_pointer_cast<TableFactor>(product));
+    auto lookup =
+        std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
     gttoc(lookup);
 
     return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
@@ -230,7 +230,7 @@ namespace gtsam {
                     const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic_(product);
-    DiscreteFactor::shared_ptr product = factors.product();
+    DecisionTreeFactor product = factors.product();
     gttoc_(product);
 
     gttic_(Normalize);
@@ -240,7 +240,8 @@ namespace gtsam {
 
     // sum out frontals, this is the factor on the separator
     gttic_(sum);
-    DiscreteFactor::shared_ptr sum = product->sum(frontalKeys);
+    DecisionTreeFactor::shared_ptr sum = std::dynamic_pointer_cast<DecisionTreeFactor>(
+        product.sum(frontalKeys));
     gttoc_(sum);
 
     // Ordering keys for the conditional so that frontalKeys are really in front

From 975fe627d91fc3900e0553e4e043ddf60aef7e53 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 16:58:19 -0500
Subject: [PATCH 067/362] add methods in gtsam_unstable

---
 gtsam_unstable/discrete/AllDiff.h       | 16 ++++++++++++++++
 gtsam_unstable/discrete/BinaryAllDiff.h | 16 ++++++++++++++++
 gtsam_unstable/discrete/Domain.h        | 16 ++++++++++++++++
 gtsam_unstable/discrete/SingleValue.h   | 18 +++++++++++++++++-
 4 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 7f539f4c6..fb956146b 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -75,6 +75,22 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
 
   /// Get the number of non-zero values contained in this factor.
   uint64_t nrValues() const override { return 1; };
+
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 0e2fce109..fe04fd807 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -99,6 +99,22 @@ class BinaryAllDiff : public Constraint {
 
   /// Get the number of non-zero values contained in this factor.
   uint64_t nrValues() const override { return 1; };
+
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index cd11fc8d9..1fbd7b110 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -114,6 +114,22 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
 
   /// Partially apply known values, domain version
   Constraint::shared_ptr partiallyApply(const Domains& domains) const override;
+
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 7f2eb2c2c..8dc7114ec 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   }
 
   /// Calculate value
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
@@ -80,6 +80,22 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
 
   /// Get the number of non-zero values contained in this factor.
   uint64_t nrValues() const override { return 1; };
+
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
+    throw std::runtime_error("Not implemented");
+  }
+
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
+    throw std::runtime_error("Not implemented");
+  }
 };
 
 }  // namespace gtsam

From fc2d33f437ab65b394ff563ff9f8872101487189 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 17:00:04 -0500
Subject: [PATCH 068/362] add division with DiscreteFactor::shared_ptr for
 convenience

---
 gtsam/discrete/DecisionTreeFactor.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 11793f984..a5178b66f 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -156,6 +156,11 @@ namespace gtsam {
       return apply(f, safe_div);
     }
 
+    /// divide by DiscreteFactor::shared_ptr f (safely)
+    DecisionTreeFactor operator/(const DiscreteFactor::shared_ptr& f) const {
+      return apply(*std::dynamic_pointer_cast<DecisionTreeFactor>(f), safe_div);
+    }
+
     /// Convert into a decision tree
     DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
 

From 2c02efcae2e2b320834baeff157329c6908c332e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 17:02:47 -0500
Subject: [PATCH 069/362] fix tests

---
 gtsam/discrete/tests/testDecisionTreeFactor.cpp  | 6 +++---
 gtsam/discrete/tests/testDiscreteConditional.cpp | 4 ++--
 gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 6 +++---
 gtsam/discrete/tests/testTableFactor.cpp         | 6 +++---
 4 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 756a0cebe..b4c5acc1b 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -111,15 +111,15 @@ TEST(DecisionTreeFactor, sum_max) {
   DecisionTreeFactor f1(v0 & v1, "1 2  3 4  5 6");
 
   DecisionTreeFactor expected(v1, "9 12");
-  DecisionTreeFactor::shared_ptr actual = f1.sum(1);
+  auto actual = std::dynamic_pointer_cast<DecisionTreeFactor>(f1.sum(1));
   CHECK(assert_equal(expected, *actual, 1e-5));
 
   DecisionTreeFactor expected2(v1, "5 6");
-  DecisionTreeFactor::shared_ptr actual2 = f1.max(1);
+  auto actual2 = std::dynamic_pointer_cast<DecisionTreeFactor>(f1.max(1));
   CHECK(assert_equal(expected2, *actual2));
 
   DecisionTreeFactor f2(v1 & v0, "1 2  3 4  5 6");
-  DecisionTreeFactor::shared_ptr actual22 = f2.sum(1);
+  auto actual22 = std::dynamic_pointer_cast<DecisionTreeFactor>(f2.sum(1));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp
index 2482a86a2..d17c76837 100644
--- a/gtsam/discrete/tests/testDiscreteConditional.cpp
+++ b/gtsam/discrete/tests/testDiscreteConditional.cpp
@@ -46,7 +46,7 @@ TEST(DiscreteConditional, constructors) {
   DecisionTreeFactor f2(
       X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor expected2 = f2 / *f2.sum(1);
+  DecisionTreeFactor expected2 = f2 / f2.sum(1);
   EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
 
   std::vector<double> probs{0.2, 0.5, 0.3, 0.6, 0.4, 0.7, 0.25, 0.55, 0.35, 0.65, 0.45, 0.75};
@@ -70,7 +70,7 @@ TEST(DiscreteConditional, constructors_alt_interface) {
   DecisionTreeFactor f2(
       X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor expected2 = f2 / *f2.sum(1);
+  DecisionTreeFactor expected2 = f2 / f2.sum(1);
   EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
 }
 
diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 341eb63e3..e0d696d91 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -113,12 +113,12 @@ TEST(DiscreteFactorGraph, test) {
   const Ordering frontalKeys{0};
   const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys);
 
-  DecisionTreeFactor newFactor = *newFactorPtr;
+  auto newFactor = *std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
 
   // Normalize newFactor by max for comparison with expected
   auto normalization = newFactor.max(newFactor.size());
 
-  newFactor = newFactor / *normalization;
+  newFactor = newFactor / normalization;
 
   // Check Conditional
   CHECK(conditional);
@@ -132,7 +132,7 @@ TEST(DiscreteFactorGraph, test) {
   // Normalize by max.
   normalization = expectedFactor.max(expectedFactor.size());
   // Ensure normalization is correct.
-  expectedFactor = expectedFactor / *normalization;
+  expectedFactor = expectedFactor / normalization;
   EXPECT(assert_equal(expectedFactor, newFactor));
 
   // Test using elimination tree
diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 0f7d7a615..bd3dac514 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -242,15 +242,15 @@ TEST(TableFactor, sum_max) {
   TableFactor f1(v0 & v1, "1 2  3 4  5 6");
 
   TableFactor expected(v1, "9 12");
-  TableFactor::shared_ptr actual = f1.sum(1);
+  auto actual = std::dynamic_pointer_cast<TableFactor>(f1.sum(1));
   CHECK(assert_equal(expected, *actual, 1e-5));
 
   TableFactor expected2(v1, "5 6");
-  TableFactor::shared_ptr actual2 = f1.max(1);
+  auto actual2 = std::dynamic_pointer_cast<TableFactor>(f1.max(1));
   CHECK(assert_equal(expected2, *actual2));
 
   TableFactor f2(v1 & v0, "1 2  3 4  5 6");
-  TableFactor::shared_ptr actual22 = f2.sum(1);
+  auto actual22 = std::dynamic_pointer_cast<TableFactor>(f2.sum(1));
 }
 
 /* ************************************************************************* */

From 360598d3d596a49f0e0778d5b4c5b49e6f17340f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 17:03:31 -0500
Subject: [PATCH 070/362] undo uncomment

---
 gtsam/discrete/DiscreteDistribution.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h
index 28e509f15..09ea50332 100644
--- a/gtsam/discrete/DiscreteDistribution.h
+++ b/gtsam/discrete/DiscreteDistribution.h
@@ -86,7 +86,8 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
   double operator()(size_t value) const;
 
   /// We also want to keep the Base version, taking DiscreteValues:
-  using Base::operator();
+  // TODO(dellaert): does not play well with wrapper!
+  // using Base::operator();
 
   /// Return entire probability mass function.
   std::vector<double> pmf() const;

From 853241c6d09da2a71375cfc507b95ba647e59d29 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 17:07:40 -0500
Subject: [PATCH 071/362] add evaluate to DiscreteConditional

---
 gtsam/discrete/DiscreteConditional.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 298a7b004..75eaf9154 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -169,6 +169,7 @@ class GTSAM_EXPORT DiscreteConditional
   }
 
   using BaseFactor::error;       ///< DiscreteValues version
+  using BaseFactor::evaluate;  // DiscreteValues version
   using BaseFactor::operator();  ///< DiscreteValues version
 
   /**

From 199c0a0f24da5e040ec128b83ad1f9cb3e2ef8eb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 17:15:22 -0500
Subject: [PATCH 072/362] keep using DecisionTreeFactor for DiscreteConditional

---
 gtsam/discrete/DiscreteConditional.cpp | 4 ++--
 gtsam/discrete/DiscreteConditional.h   | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index bd10e28b4..399126d51 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -201,7 +201,7 @@ DiscreteConditional::shared_ptr DiscreteConditional::choose(
 }
 
 /* ************************************************************************** */
-DiscreteFactor::shared_ptr DiscreteConditional::likelihood(
+DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
     const DiscreteValues& frontalValues) const {
   // Get the big decision tree with all the levels, and then go down the
   // branches based on the value of the frontal variables.
@@ -226,7 +226,7 @@ DiscreteFactor::shared_ptr DiscreteConditional::likelihood(
 }
 
 /* ****************************************************************************/
-DiscreteFactor::shared_ptr DiscreteConditional::likelihood(
+DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
     size_t frontal) const {
   if (nrFrontals() != 1)
     throw std::invalid_argument(
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 75eaf9154..96ecefd7a 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -188,11 +188,11 @@ class GTSAM_EXPORT DiscreteConditional
   shared_ptr choose(const DiscreteValues& given) const;
 
   /** Convert to a likelihood factor by providing value before bar. */
-  DiscreteFactor::shared_ptr likelihood(
+  DecisionTreeFactor::shared_ptr likelihood(
       const DiscreteValues& frontalValues) const;
 
   /** Single variable version of likelihood. */
-  DiscreteFactor::shared_ptr likelihood(size_t frontal) const;
+  DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
 
   /**
    * sample

From 214bf4ec1a2ce6329d930b03b8f8f279c6f71de9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Dec 2024 17:15:40 -0500
Subject: [PATCH 073/362] more fixes

---
 gtsam/discrete/DiscreteFactorGraph.cpp  | 4 ++--
 gtsam_unstable/discrete/SingleValue.cpp | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 04849985f..e31f94eae 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -67,7 +67,7 @@ namespace gtsam {
   DecisionTreeFactor DiscreteFactorGraph::product() const {
     DecisionTreeFactor result;
     for (const sharedFactor& factor : *this) {
-      if (factor) result = result * (*factor);
+      if (factor) result = (*factor) * result;
     }
     return result;
   }
@@ -254,7 +254,7 @@ namespace gtsam {
     // now divide product/sum to get conditional
     gttic_(divide);
     auto conditional =
-        std::make_shared<DiscreteConditional>(product, sum, orderedKeys);
+        std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
     gttoc_(divide);
 
     return {conditional, sum};
diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp
index 6b78f38f5..9762aec0f 100644
--- a/gtsam_unstable/discrete/SingleValue.cpp
+++ b/gtsam_unstable/discrete/SingleValue.cpp
@@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const {
 }
 
 /* ************************************************************************* */
-double SingleValue::operator()(const DiscreteValues& values) const {
+double SingleValue::operator()(const Assignment<Key>& values) const {
   return (double)(values.at(keys_[0]) == value_);
 }
 

From 699757d381758cc32c5da96e4b021b42c4e283c0 Mon Sep 17 00:00:00 2001
From: Jade Turner <spacey-sooty@proton.me>
Date: Mon, 9 Dec 2024 15:17:16 +0800
Subject: [PATCH 074/362] Revert "[build] Allow disabling tests and examples"

This reverts commit ddd95f4a51549d3c19cdca36d488da1323430676.
---
 CMakeLists.txt | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ffedf56e4..e3b462eec 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -112,17 +112,10 @@ add_subdirectory(CppUnitLite)
 add_subdirectory(gtsam)
 
 # Build Tests
-option(BUILD_TESTS "Builds unit tests" ON)
-if (BUILD_TESTS)
-    add_subdirectory(tests)
-endif()
-
+add_subdirectory(tests)
 
 # Build examples
-option(BUILD_EXAMPLES "Builds examples" ON)
-if (BUILD_EXAMPLES)
-    add_subdirectory(examples)
-endif()
+add_subdirectory(examples)
 
 # Build timing
 add_subdirectory(timing)

From f8d9da1b8edeebdba2acdfc2100da85d6b8ed2e0 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Mon, 9 Dec 2024 06:57:35 -0800
Subject: [PATCH 075/362] actually fix CHECK_EQUAL

---
 CppUnitLite/Test.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h
index 9ee0d10eb..c3fa83234 100644
--- a/CppUnitLite/Test.h
+++ b/CppUnitLite/Test.h
@@ -129,7 +129,7 @@ protected:
     result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
 
 #define CHECK_EQUAL(expected,actual)\
-{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } }
+{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); return; } }
 
 #define LONGS_EQUAL(expected,actual)\
 { long actualTemp = actual; \

From e46cd5499351c67abd8c799d6ba317d37d21991e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 15:52:42 -0500
Subject: [PATCH 076/362] TableFactor cleanup

---
 gtsam/discrete/TableFactor.cpp | 2 --
 gtsam/discrete/TableFactor.h   | 1 -
 2 files changed, 3 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index a4947012e..32049fde1 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -72,9 +72,7 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
  */
 std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
                                         const DecisionTreeFactor& dt) {
-  gttic_(ComputeLeafOrdering);
   std::vector<double> probs = dt.probabilities();
-  gttoc_(ComputeLeafOrdering);
   std::vector<double> ordered;
 
   size_t n = dkeys[0].second;
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index ba1d05fe9..002c276ca 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -171,7 +171,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, safe_div);
   }
 
-
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
 

From 52c8034d41e8703ad149ee161c9d93ded4df5f0c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 16:16:18 -0500
Subject: [PATCH 077/362] add division by DiscreteFactor in TableFactor

---
 gtsam/discrete/TableFactor.h | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 002c276ca..64e98c6a1 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -171,6 +171,15 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, safe_div);
   }
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  TableFactor operator/(const DiscreteFactor::shared_ptr& f) const {
+    if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+      return apply(*tf, safe_div);
+    } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      return apply(TableFactor(f->discreteKeys(), *dtf), safe_div);
+    }
+  }
+
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
 

From e0e833c2fc71b5848c405d1d5fa20078d1df0cab Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 16:23:55 -0500
Subject: [PATCH 078/362] cleanup

---
 gtsam/discrete/DiscreteLookupDAG.h | 2 --
 gtsam/discrete/TableFactor.cpp     | 2 --
 2 files changed, 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h
index 21181d374..7f31a3f48 100644
--- a/gtsam/discrete/DiscreteLookupDAG.h
+++ b/gtsam/discrete/DiscreteLookupDAG.h
@@ -61,8 +61,6 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
    * @param nFrontals number of frontal variables
    * @param keys a sorted list of gtsam::Keys
    * @param potentials Discrete potentials as a TableFactor.
-   *
-   * //TODO(Varun): Should accept a DiscreteFactor::shared_ptr
    */
   DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,
                       const TableFactor& potentials)
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 32049fde1..32cba84ed 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -180,10 +180,8 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   for (auto i = 0; i < sparse_table_.size(); i++) {
     table.push_back(sparse_table_.coeff(i));
   }
-  gttic_(toDecisionTreeFactor_Constructor);
   // NOTE(Varun): This constructor is really expensive!!
   DecisionTreeFactor f(dkeys, table);
-  gttoc_(toDecisionTreeFactor_Constructor);
   return f;
 }
 

From 84627c0c579805590e703f487fc1a072e6637da6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 16:30:46 -0500
Subject: [PATCH 079/362] fix error

---
 gtsam/discrete/DiscreteFactor.h | 1 -
 gtsam/discrete/TableFactor.h    | 3 +++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index a6356a045..2fe80a54c 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -144,7 +144,6 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// Create new factor by maximizing over all values with the same separator.
   virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const = 0;
 
-
   /**
    * Get the number of non-zero values contained in this factor.
    * It could be much smaller than `prod_{key}(cardinality(key))`.
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 64e98c6a1..41b6287b8 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/Ring.h>
@@ -177,6 +178,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
       return apply(*tf, safe_div);
     } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
       return apply(TableFactor(f->discreteKeys(), *dtf), safe_div);
+    } else {
+      throw std::runtime_error("Unknown derived type for DiscreteFactor");
     }
   }
 

From a425e6e3d4c80f5553d045e54d4e7d7f60a7d258 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 9 Dec 2024 16:36:43 -0500
Subject: [PATCH 080/362] better docs

---
 gtsam/linear/KalmanFilter.h | 226 +++++++++++++++++++++++-------------
 1 file changed, 146 insertions(+), 80 deletions(-)

diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index 65fac877a..7fde9975d 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -11,10 +11,10 @@
 
 /**
  * @file KalmanFilter.h
- * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
+ * @brief Simple linear Kalman filter implemented using factor graphs, i.e.,
+ * performs Cholesky or QR-based SRIF (Square-Root Information Filter).
  * @date Sep 3, 2011
- * @author Stephen Williams
- * @author Frank Dellaert
+ * @authors Stephen Williams, Frank Dellaert
  */
 
 #pragma once
@@ -32,120 +32,186 @@ namespace gtsam {
 /**
  * Kalman Filter class
  *
- * Knows how to maintain a Gaussian density under linear-Gaussian motion and
- * measurement models. It uses the square-root information form, as usual in GTSAM.
+ * Maintains a Gaussian density under linear-Gaussian motion and
+ * measurement models using the square-root information form.
  *
- * The filter is functional, in that it does not have state: you call init() to create
- * an initial state, then predict() and update() that create new states out of an old state.
+ * The filter is functional; it does not maintain internal state. Instead:
+ * - Use `init()` to create an initial filter state,
+ * - Call `predict()` and `update()` to create new states.
  */
 class GTSAM_EXPORT KalmanFilter {
-
-public:
-
+ public:
   /**
-   *  This Kalman filter is a Square-root Information filter
-   *  The type below allows you to specify the factorization variant.
+   * @enum Factorization
+   * @brief Specifies the factorization variant to use.
    */
-  enum Factorization {
-    QR, CHOLESKY
-  };
+  enum Factorization { QR, CHOLESKY };
 
   /**
-   * The Kalman filter state is simply a GaussianDensity
+   * @typedef State
+   * @brief The Kalman filter state, represented as a shared pointer to a
+   * GaussianDensity.
    */
   typedef GaussianDensity::shared_ptr State;
 
-private:
-
-  const size_t n_; /** dimensionality of state */
-  const Matrix I_; /** identity matrix of size n*n */
-  const GaussianFactorGraph::Eliminate function_; /** algorithm */
-
-  State solve(const GaussianFactorGraph& factorGraph) const;
-  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
-
-public:
-
-  // Constructor
-  KalmanFilter(size_t n, Factorization method =
-      KALMANFILTER_DEFAULT_FACTORIZATION) :
-      n_(n), I_(Matrix::Identity(n_, n_)), function_(
-          method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
-              GaussianFactorGraph::Eliminate(EliminateCholesky)) {
-  }
+ private:
+  const size_t n_;  ///< Dimensionality of the state.
+  const Matrix I_;  ///< Identity matrix of size \f$ n \times n \f$.
+  const GaussianFactorGraph::Eliminate
+      function_;  ///< Elimination algorithm used.
 
   /**
-   * Create initial state, i.e., prior density at time k=0
-   * In Kalman Filter notation, these are x_{0|0} and P_{0|0}
-   * @param x0 estimate at time 0
-   * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
+   * Solve the factor graph.
+   * @param factorGraph The Gaussian factor graph to solve.
+   * @return The resulting Kalman filter state.
+   */
+  State solve(const GaussianFactorGraph& factorGraph) const;
+
+  /**
+   * Fuse two states.
+   * @param p The prior state.
+   * @param newFactor The new factor to incorporate.
+   * @return The resulting fused state.
+   */
+  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
+
+ public:
+  /**
+   * Constructor.
+   * @param n Dimensionality of the state.
+   * @param method Factorization method (default: QR unless compile-flag set).
+   */
+  KalmanFilter(size_t n,
+               Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION)
+      : n_(n),
+        I_(Matrix::Identity(n_, n_)),
+        function_(method == QR
+                      ? GaussianFactorGraph::Eliminate(EliminateQR)
+                      : GaussianFactorGraph::Eliminate(EliminateCholesky)) {}
+
+  /**
+   * Create the initial state (prior density at time \f$ k=0 \f$).
+   *
+   * In Kalman Filter notation:
+   * - \f$ x_{0|0} \f$: Initial state estimate.
+   * - \f$ P_{0|0} \f$: Initial covariance matrix.
+   *
+   * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$).
+   * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian
+   * model.
+   * @return Initial Kalman filter state.
    */
   State init(const Vector& x0, const SharedDiagonal& P0) const;
 
-  /// version of init with a full covariance matrix
+  /**
+   * Create the initial state with a full covariance matrix.
+   * @param x0 Initial state estimate.
+   * @param P0 Full covariance matrix.
+   * @return Initial Kalman filter state.
+   */
   State init(const Vector& x0, const Matrix& P0) const;
 
-  /// print
+  /**
+   * Print the Kalman filter details.
+   * @param s Optional string prefix.
+   */
   void print(const std::string& s = "") const;
 
-  /** Return step index k, starts at 0, incremented at each predict. */
-  static Key step(const State& p) {
-    return p->firstFrontalKey();
-  }
+  /**
+   * Return the step index \f$ k \f$ (starts at 0, incremented at each predict
+   * step).
+   * @param p The current state.
+   * @return Step index.
+   */
+  static Key step(const State& p) { return p->firstFrontalKey(); }
 
   /**
-   * Predict the state P(x_{t+1}|Z^t)
-   *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
-   * Details and parameters:
-   *   In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
-   *   where F is the state transition model/matrix, B is the control input model,
-   *   and w is zero-mean, Gaussian white noise with covariance Q.
+   * Predict the next state \f$ P(x_{k+1}|Z^k) \f$.
+   *
+   * In Kalman Filter notation:
+   * - \f$ x_{k+1|k} \f$: Predicted state.
+   * - \f$ P_{k+1|k} \f$: Predicted covariance.
+   *
+   * Motion model:
+   * \f[
+   * x_{k+1} = F \cdot x_k + B \cdot u_k + w
+   * \f]
+   * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$.
+   *
+   * @param p Previous state (\f$ x_k \f$).
+   * @param F State transition matrix (\f$ F \f$).
+   * @param B Control input matrix (\f$ B \f$).
+   * @param u Control vector (\f$ u_k \f$).
+   * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian).
+   * @return Predicted state (\f$ x_{k+1|k} \f$).
    */
   State predict(const State& p, const Matrix& F, const Matrix& B,
-      const Vector& u, const SharedDiagonal& modelQ) const;
+                const Vector& u, const SharedDiagonal& modelQ) const;
 
-  /*
-   *  Version of predict with full covariance
-   *  Q is normally derived as G*w*G^T where w models uncertainty of some
-   *  physical property, such as velocity or acceleration, and G is derived from physics.
-   *  This version allows more realistic models than a diagonal covariance matrix.
+  /**
+   * Predict the next state with a full covariance matrix.
+   *
+   *@note Q is normally derived as G*w*G^T where w models uncertainty of some
+   * physical property, such as velocity or acceleration, and G is derived from
+   * physics. This version allows more realistic models than a diagonal matrix.
+   *
+   * @param p Previous state.
+   * @param F State transition matrix.
+   * @param B Control input matrix.
+   * @param u Control vector.
+   * @param Q Full covariance matrix (\f$ Q \f$).
+   * @return Predicted state.
    */
   State predictQ(const State& p, const Matrix& F, const Matrix& B,
-      const Vector& u, const Matrix& Q) const;
+                 const Vector& u, const Matrix& Q) const;
 
   /**
-   * Predict the state P(x_{t+1}|Z^t)
-   *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
-   *   After the call, that is the density that can be queried.
-   * Details and parameters:
-   *   This version of predict takes GaussianFactor motion model [A0 A1 b]
-   *   with an optional noise model.
+   * Predict the next state using a GaussianFactor motion model.
+   * @param p Previous state.
+   * @param A0 Factor matrix.
+   * @param A1 Factor matrix.
+   * @param b Constant term vector.
+   * @param model Noise model (optional).
+   * @return Predicted state.
    */
   State predict2(const State& p, const Matrix& A0, const Matrix& A1,
-      const Vector& b, const SharedDiagonal& model) const;
+                 const Vector& b, const SharedDiagonal& model = nullptr) const;
 
   /**
-   * Update Kalman filter with a measurement
-   * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t}
-   * will be of the form h(x_{t}) = H*x_{t} + v
-   * where H is the observation model/matrix, and v is zero-mean,
-   * Gaussian white noise with covariance R.
+   * Update the Kalman filter with a measurement.
+   *
+   * Observation model:
+   * \f[
+   * z_k = H \cdot x_k + v
+   * \f]
+   * where \f$ v \f$ is zero-mean Gaussian noise with covariance R.
    * In this version, R is restricted to diagonal Gaussians (model parameter)
+   *
+   * @param p Previous state.
+   * @param H Observation matrix.
+   * @param z Measurement vector.
+   * @param model Noise model (diagonal Gaussian).
+   * @return Updated state.
    */
   State update(const State& p, const Matrix& H, const Vector& z,
-      const SharedDiagonal& model) const;
+               const SharedDiagonal& model) const;
 
-  /*
-   *  Version of update with full covariance
-   *  Q is normally derived as G*w*G^T where w models uncertainty of some
-   *  physical property, such as velocity or acceleration, and G is derived from physics.
-   *  This version allows more realistic models than a diagonal covariance matrix.
+  /**
+   * Update the Kalman filter with a measurement using a full covariance matrix.
+   * @param p Previous state.
+   * @param H Observation matrix.
+   * @param z Measurement vector.
+   * @param R Full covariance matrix.
+   * @return Updated state.
    */
   State updateQ(const State& p, const Matrix& H, const Vector& z,
-      const Matrix& Q) const;
+                const Matrix& R) const;
+
+  /**
+   * Return the dimensionality of the state.
+   * @return Dimensionality of the state.
+   */
+  size_t dim() const { return n_; }
 };
 
-} // \namespace gtsam
-
-/* ************************************************************************* */
-
+}  // namespace gtsam
\ No newline at end of file

From 1ae3fc2ad80ff27e499ea49ac942eff6ffef0910 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 9 Dec 2024 16:38:15 -0500
Subject: [PATCH 081/362] Use LLT rather than inverse

---
 gtsam/linear/KalmanFilter.cpp | 120 ++++++++++++++++++++--------------
 1 file changed, 70 insertions(+), 50 deletions(-)

diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index ded6bc5e3..12a43093c 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -20,26 +20,35 @@
  * @author Frank Dellaert
  */
 
-#include <gtsam/linear/KalmanFilter.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
-#include <gtsam/base/Testable.h>
-
+#include <gtsam/linear/KalmanFilter.h>
 
 using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-// Auxiliary function to solve factor graph and return pointer to root conditional
-KalmanFilter::State //
-KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
+// In the code below we often need to multiply matrices with R, the Cholesky
+// factor of the information matrix Q^{-1}, when given a covariance matrix Q.
+// We can do this efficiently using Eigen, as we have:
+//   Q = L L^T
+//   => Q^{-1} = L^{-T}L^{-1} = R^T R
+//   => L^{-1} = R
+// Rather than explicitly calculate R and do R*A, we should use L.solve(A)
+// The following macro makes L available in the scope where it is used:
+#define FACTORIZE_Q_INTO_L(Q)       \
+  Eigen::LLT<Matrix> llt(Q); \
+  auto L = llt.matrixL();
 
+/* ************************************************************************* */
+// Auxiliary function to solve factor graph and return pointer to root
+// conditional
+KalmanFilter::State KalmanFilter::solve(
+    const GaussianFactorGraph& factorGraph) const {
   // Eliminate the graph using the provided Eliminate function
   Ordering ordering(factorGraph.keys());
-  const auto bayesNet = //
-      factorGraph.eliminateSequential(ordering, function_);
+  const auto bayesNet = factorGraph.eliminateSequential(ordering, function_);
 
   // As this is a filter, all we need is the posterior P(x_t).
   // This is the last GaussianConditional in the resulting BayesNet
@@ -49,9 +58,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
 
 /* ************************************************************************* */
 // Auxiliary function to create a small graph for predict or update and solve
-KalmanFilter::State //
-KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
-
+KalmanFilter::State KalmanFilter::fuse(
+    const State& p, GaussianFactor::shared_ptr newFactor) const {
   // Create a factor graph
   GaussianFactorGraph factorGraph;
   factorGraph.push_back(p);
@@ -63,20 +71,27 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::init(const Vector& x0,
-    const SharedDiagonal& P0) const {
-
+                                       const SharedDiagonal& P0) const {
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
-  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0, P0); // |x-x0|^2_diagSigma
+  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0,
+                                             P0);  // |x-x0|^2_P0
   return solve(factorGraph);
 }
 
 /* ************************************************************************* */
-KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {
-
+KalmanFilter::State KalmanFilter::init(const Vector& x0,
+                                       const Matrix& P0) const {
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
-  factorGraph.emplace_shared<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
+
+  // Perform Cholesky decomposition of P0
+  FACTORIZE_Q_INTO_L(P0)
+
+  // Premultiply I and x0 with R
+  const Matrix R = L.solve(I_);  // = R
+  const Vector b = L.solve(x0);  // = R*x0
+  factorGraph.emplace_shared<JacobianFactor>(0, R, b);
   return solve(factorGraph);
 }
 
@@ -87,21 +102,23 @@ void KalmanFilter::print(const string& s) const {
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const SharedDiagonal& model) const {
-
+                                          const Matrix& B, const Vector& u,
+                                          const SharedDiagonal& model) const {
   // The factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
+  // f2(x_{k},x_{k+1}) =
+  // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T
   Key k = step(p);
   return fuse(p,
-      std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
+              std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const Matrix& Q) const {
-
+                                           const Matrix& B, const Vector& u,
+                                           const Matrix& Q) const {
 #ifndef NDEBUG
-  DenseIndex n = F.cols();
+  const DenseIndex n = dim();
+  assert(F.cols() == n);
   assert(F.rows() == n);
   assert(B.rows() == n);
   assert(B.cols() == u.size());
@@ -109,50 +126,53 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
   assert(Q.cols() == n);
 #endif
 
-  // The factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
-  // See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
-  // TODO: starts to seem more elaborate than straight-up KF equations?
-  Matrix M = Q.inverse(), Ft = trans(F);
-  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
-  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
-  double f = dot(b, g2);
-  Key k = step(p);
-  return fuse(p,
-      std::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
+  // Perform Cholesky decomposition of Q
+  FACTORIZE_Q_INTO_L(Q)
+
+  // Premultiply A1 = -F and A2 = I, b = B * u with R
+  const Matrix A1 = -L.solve(F);    // = -R*F
+  const Matrix A2 = L.solve(I_);    // = R
+  const Vector b = L.solve(B * u);  // = R * (B * u)
+  return predict2(p, A1, A2, b);
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
-    const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
+                                           const Matrix& A1, const Vector& b,
+                                           const SharedDiagonal& model) const {
   // Nhe factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
+  // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2
   Key k = step(p);
   return fuse(p, std::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
-    const Vector& z, const SharedDiagonal& model) const {
+                                         const Vector& z,
+                                         const SharedDiagonal& model) const {
   // The factor related to the measurements would be defined as
-  // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
-  //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
+  // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T
+  //    = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T
   Key k = step(p);
   return fuse(p, std::make_shared<JacobianFactor>(k, H, z, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
-    const Vector& z, const Matrix& Q) const {
+                                          const Vector& z,
+                                          const Matrix& Q) const {
   Key k = step(p);
-  Matrix M = Q.inverse(), Ht = trans(H);
-  Matrix G = Ht * M * H;
-  Vector g = Ht * M * z;
-  double f = dot(z, M * z);
-  return fuse(p, std::make_shared<HessianFactor>(k, G, g, f));
+
+  // Perform Cholesky decomposition of Q
+  FACTORIZE_Q_INTO_L(Q)
+
+  // Pre-multiply H and z with R, respectively
+  const Matrix RtH = L.solve(H);  // = R * H
+  const Vector b = L.solve(z);    // = R * z
+
+  return fuse(p, std::make_shared<JacobianFactor>(k, RtH, b));
 }
 
 /* ************************************************************************* */
 
-} // \namespace gtsam
-
+}  // namespace gtsam

From 88b36da602c9697ca86db5d42921fa6d058cc1af Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 16:55:15 -0500
Subject: [PATCH 082/362] make evaluate a common base method

---
 gtsam/discrete/DecisionTreeFactor.h     |  9 ++-------
 gtsam/discrete/DiscreteFactor.h         | 15 ++++++++++++++-
 gtsam/discrete/TableFactor.cpp          |  3 ++-
 gtsam/discrete/TableFactor.h            | 10 ++--------
 gtsam_unstable/discrete/AllDiff.cpp     |  2 +-
 gtsam_unstable/discrete/AllDiff.h       |  2 +-
 gtsam_unstable/discrete/BinaryAllDiff.h |  2 +-
 gtsam_unstable/discrete/Domain.cpp      |  2 +-
 gtsam_unstable/discrete/Domain.h        |  2 +-
 gtsam_unstable/discrete/SingleValue.cpp |  2 +-
 gtsam_unstable/discrete/SingleValue.h   |  2 +-
 11 files changed, 27 insertions(+), 24 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 9a3cde96d..8f3dd85a6 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -130,14 +130,9 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /// Calculate probability for given values `x`, 
+    /// Calculate probability for given values, 
     /// is just look up in AlgebraicDecisionTree.
-    double evaluate(const Assignment<Key>& values) const  {
-      return ADT::operator()(values);
-    }
-
-    /// Evaluate probability distribution, sugar.
-    double operator()(const DiscreteValues& values) const override {
+    double operator()(const Assignment<Key>& values) const override {
       return ADT::operator()(values);
     }
 
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 19af5bd13..5bc3c463f 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -92,8 +92,21 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   size_t cardinality(Key j) const { return cardinalities_.at(j); }
 
+  /**
+   * @brief Calculate probability for given values.
+   * Calls specialized evaluation under the hood.
+   *
+   * Note: Uses Assignment<Key> as it is the base class of DiscreteValues.
+   *
+   * @param values Discrete assignment.
+   * @return double
+   */
+  double evaluate(const Assignment<Key>& values) const {
+    return operator()(values);
+  }
+
   /// Find value for given assignment of values to variables
-  virtual double operator()(const DiscreteValues&) const = 0;
+  virtual double operator()(const Assignment<Key>& values) const = 0;
 
   /// Error is just -log(value)
   virtual double error(const DiscreteValues& values) const;
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index f4e023a4d..32cba84ed 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -133,7 +133,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
 }
 
 /* ************************************************************************ */
-double TableFactor::operator()(const DiscreteValues& values) const {
+double TableFactor::operator()(const Assignment<Key>& values) const {
   // a b c d => D * (C * (B * (a) + b) + c) + d
   uint64_t idx = 0, card = 1;
   for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) {
@@ -180,6 +180,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   for (auto i = 0; i < sparse_table_.size(); i++) {
     table.push_back(sparse_table_.coeff(i));
   }
+  // NOTE(Varun): This constructor is really expensive!!
   DecisionTreeFactor f(dkeys, table);
   return f;
 }
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 7015353e1..53495d078 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -155,14 +155,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   // /// @name Standard Interface
   // /// @{
 
-  /// Calculate probability for given values `x`,
-  /// is just look up in TableFactor.
-  double evaluate(const DiscreteValues& values) const {
-    return operator()(values);
-  }
-
-  /// Evaluate probability distribution, sugar.
-  double operator()(const DiscreteValues& values) const override;
+  /// Evaluate probability distribution, is just look up in TableFactor.
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Calculate error for DiscreteValues `x`, is -log(probability).
   double error(const DiscreteValues& values) const override;
diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp
index 2bd9e6dfd..a450605b3 100644
--- a/gtsam_unstable/discrete/AllDiff.cpp
+++ b/gtsam_unstable/discrete/AllDiff.cpp
@@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const {
 }
 
 /* ************************************************************************* */
-double AllDiff::operator()(const DiscreteValues& values) const {
+double AllDiff::operator()(const Assignment<Key>& values) const {
   std::set<size_t> taken;  // record values taken by keys
   for (Key dkey : keys_) {
     size_t value = values.at(dkey);      // get the value for that key
diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index d7a63eae0..6fe43568e 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   }
 
   /// Calculate value = expensive !
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree, can be *very* expensive !
   DecisionTreeFactor toDecisionTreeFactor() const override;
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 18b335092..a55865c77 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint {
   }
 
   /// Calculate value
-  double operator()(const DiscreteValues& values) const override {
+  double operator()(const Assignment<Key>& values) const override {
     return (double)(values.at(keys_[0]) != values.at(keys_[1]));
   }
 
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index bbbc87667..752228c18 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -30,7 +30,7 @@ string Domain::base1Str() const {
 }
 
 /* ************************************************************************* */
-double Domain::operator()(const DiscreteValues& values) const {
+double Domain::operator()(const Assignment<Key>& values) const {
   return contains(values.at(key()));
 }
 
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 7f7b717c2..13249d733 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   bool contains(size_t value) const { return values_.count(value) > 0; }
 
   /// Calculate value
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp
index 6b78f38f5..9762aec0f 100644
--- a/gtsam_unstable/discrete/SingleValue.cpp
+++ b/gtsam_unstable/discrete/SingleValue.cpp
@@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const {
 }
 
 /* ************************************************************************* */
-double SingleValue::operator()(const DiscreteValues& values) const {
+double SingleValue::operator()(const Assignment<Key>& values) const {
   return (double)(values.at(keys_[0]) == value_);
 }
 
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 3f7f22d6a..93fe38aaa 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   }
 
   /// Calculate value
-  double operator()(const DiscreteValues& values) const override;
+  double operator()(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;

From e0fedda712808b557ef229b2ec4cda50f25fb743 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 17:22:51 -0500
Subject: [PATCH 083/362] make the Unary and Binary ops common

---
 gtsam/discrete/DecisionTreeFactor.cpp | 12 ++++++------
 gtsam/discrete/DecisionTreeFactor.h   | 15 ++++++++++-----
 gtsam/discrete/DiscreteFactor.h       |  5 +++++
 gtsam/discrete/TableFactor.h          |  4 ----
 4 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 9ec3b0ac5..776d4bd90 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -83,7 +83,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
+  DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const {
     // apply operand
     ADT result = ADT::apply(op);
     // Make a new factor
@@ -91,7 +91,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
+  DecisionTreeFactor DecisionTreeFactor::apply(UnaryAssignment op) const {
     // apply operand
     ADT result = ADT::apply(op);
     // Make a new factor
@@ -100,7 +100,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
-                                              ADT::Binary op) const {
+                                               Binary op) const {
     map<Key, size_t> cs;  // new cardinalities
     // make unique key-cardinality map
     for (Key j : keys()) cs[j] = cardinality(j);
@@ -118,8 +118,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
-      size_t nrFrontals, ADT::Binary op) const {
+  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals,
+                                                             Binary op) const {
     if (nrFrontals > size()) {
       throw invalid_argument(
           "DecisionTreeFactor::combine: invalid number of frontal "
@@ -146,7 +146,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
-      const Ordering& frontalKeys, ADT::Binary op) const {
+      const Ordering& frontalKeys, Binary op) const {
     if (frontalKeys.size() > size()) {
       throw invalid_argument(
           "DecisionTreeFactor::combine: invalid number of frontal "
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 8f3dd85a6..9172180b1 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -51,6 +51,11 @@ namespace gtsam {
     typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
     typedef AlgebraicDecisionTree<Key> ADT;
 
+    // Needed since we have definitions in both DiscreteFactor and DecisionTree
+    using Base::Binary;
+    using Base::Unary;
+    using Base::UnaryAssignment;
+
     /// @name Standard Constructors
     /// @{
 
@@ -182,21 +187,21 @@ namespace gtsam {
      * Apply unary operator (*this) "op" f
      * @param op a unary operator that operates on AlgebraicDecisionTree
      */
-    DecisionTreeFactor apply(ADT::Unary op) const;
+    DecisionTreeFactor apply(Unary op) const;
 
     /**
      * Apply unary operator (*this) "op" f
      * @param op a unary operator that operates on AlgebraicDecisionTree. Takes
      * both the assignment and the value.
      */
-    DecisionTreeFactor apply(ADT::UnaryAssignment op) const;
+    DecisionTreeFactor apply(UnaryAssignment op) const;
 
     /**
      * Apply binary operator (*this) "op" f
      * @param f the second argument for op
      * @param op a binary operator that operates on AlgebraicDecisionTree
      */
-    DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const;
+    DecisionTreeFactor apply(const DecisionTreeFactor& f, Binary op) const;
 
     /**
      * Combine frontal variables using binary operator "op"
@@ -204,7 +209,7 @@ namespace gtsam {
      * @param op a binary operator that operates on AlgebraicDecisionTree
      * @return shared pointer to newly created DecisionTreeFactor
      */
-    shared_ptr combine(size_t nrFrontals, ADT::Binary op) const;
+    shared_ptr combine(size_t nrFrontals, Binary op) const;
 
     /**
      * Combine frontal variables in an Ordering using binary operator "op"
@@ -212,7 +217,7 @@ namespace gtsam {
      * @param op a binary operator that operates on AlgebraicDecisionTree
      * @return shared pointer to newly created DecisionTreeFactor
      */
-    shared_ptr combine(const Ordering& keys, ADT::Binary op) const;
+    shared_ptr combine(const Ordering& keys, Binary op) const;
 
     /// Enumerate all values into a map from values to double.
     std::vector<std::pair<DiscreteValues, double>> enumerate() const;
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 5bc3c463f..7175f7608 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -46,6 +46,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
 
   using Values = DiscreteValues;  ///< backwards compatibility
 
+  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)>;
+
  protected:
   /// Map of Keys and their cardinalities.
   std::map<Key, size_t> cardinalities_;
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 53495d078..e7ed2294f 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -94,10 +94,6 @@ 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:
   /// @name Standard Constructors

From 1152470800d2f7845343b20ce592e32837564b6a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 17:37:11 -0500
Subject: [PATCH 084/362] fix wrapper

---
 gtsam/discrete/discrete.i | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 55c8f9e22..d82ba9794 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -41,7 +41,7 @@ virtual class DiscreteFactor : gtsam::Factor {
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const;
-  double operator()(const gtsam::DiscreteValues& values) const;
+  double operator()(const gtsam::Assignment<gtsam::Key>& values) const;
 };
 
 #include <gtsam/discrete/DecisionTreeFactor.h>
@@ -69,7 +69,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
 
   size_t cardinality(gtsam::Key j) const;
   
-  double operator()(const gtsam::DiscreteValues& values) const;
+  double operator()(const gtsam::Assignment<gtsam::Key>& values) const;
   gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
   size_t cardinality(gtsam::Key j) const;
   gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const;
@@ -248,7 +248,6 @@ class DiscreteBayesTree {
   void saveGraph(string s,
                 const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
-  double operator()(const gtsam::DiscreteValues& values) const;
 
   string markdown(const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;

From ab943b539ec3f7c186aa40be2579407fa320278d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 18:21:26 -0500
Subject: [PATCH 085/362] Revert "fix wrapper"

This reverts commit 1152470800d2f7845343b20ce592e32837564b6a.
---
 gtsam/discrete/discrete.i | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index d82ba9794..55c8f9e22 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -41,7 +41,7 @@ virtual class DiscreteFactor : gtsam::Factor {
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const;
-  double operator()(const gtsam::Assignment<gtsam::Key>& values) const;
+  double operator()(const gtsam::DiscreteValues& values) const;
 };
 
 #include <gtsam/discrete/DecisionTreeFactor.h>
@@ -69,7 +69,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
 
   size_t cardinality(gtsam::Key j) const;
   
-  double operator()(const gtsam::Assignment<gtsam::Key>& values) const;
+  double operator()(const gtsam::DiscreteValues& values) const;
   gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
   size_t cardinality(gtsam::Key j) const;
   gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const;
@@ -248,6 +248,7 @@ class DiscreteBayesTree {
   void saveGraph(string s,
                 const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
+  double operator()(const gtsam::DiscreteValues& values) const;
 
   string markdown(const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;

From a98ac0fdb232e8bf50b48df7008565010bfd0a4f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 21:09:00 -0500
Subject: [PATCH 086/362] make evaluate the overriden method

---
 gtsam/discrete/DecisionTreeFactor.cpp   | 2 +-
 gtsam/discrete/DecisionTreeFactor.h     | 5 ++++-
 gtsam/discrete/DiscreteConditional.h    | 4 ++--
 gtsam/discrete/DiscreteFactor.h         | 8 ++++----
 gtsam/discrete/TableFactor.cpp          | 2 +-
 gtsam/discrete/TableFactor.h            | 2 +-
 gtsam_unstable/discrete/AllDiff.cpp     | 2 +-
 gtsam_unstable/discrete/AllDiff.h       | 2 +-
 gtsam_unstable/discrete/BinaryAllDiff.h | 2 +-
 gtsam_unstable/discrete/Domain.cpp      | 2 +-
 gtsam_unstable/discrete/Domain.h        | 2 +-
 gtsam_unstable/discrete/SingleValue.cpp | 2 +-
 gtsam_unstable/discrete/SingleValue.h   | 2 +-
 13 files changed, 20 insertions(+), 17 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 9ec3b0ac5..a57915e45 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -195,7 +195,7 @@ namespace gtsam {
     // Construct unordered_map with values
     std::vector<std::pair<DiscreteValues, double>> result;
     for (const auto& assignment : assignments) {
-      result.emplace_back(assignment, operator()(assignment));
+      result.emplace_back(assignment, evaluate(assignment));
     }
     return result;
   }
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 8f3dd85a6..741715e43 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -132,10 +132,13 @@ namespace gtsam {
 
     /// Calculate probability for given values, 
     /// is just look up in AlgebraicDecisionTree.
-    double operator()(const Assignment<Key>& values) const override {
+    virtual double evaluate(const Assignment<Key>& values) const override {
       return ADT::operator()(values);
     }
 
+    /// Disambiguate to use DiscreteFactor version. Mainly for wrapper
+    using DiscreteFactor::operator();
+
     /// Calculate error for DiscreteValues `x`, is -log(probability).
     double error(const DiscreteValues& values) const override;
 
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index f59e29285..858623301 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -169,12 +169,12 @@ class GTSAM_EXPORT DiscreteConditional
   }
 
   /// Evaluate, just look up in AlgebraicDecisionTree
-  double evaluate(const DiscreteValues& values) const {
+  virtual double evaluate(const Assignment<Key>& values) const override {
     return ADT::operator()(values);
   }
 
   using DecisionTreeFactor::error;       ///< DiscreteValues version
-  using DecisionTreeFactor::operator();  ///< DiscreteValues version
+  using DiscreteFactor::operator();      ///< DiscreteValues version
 
   /**
    * @brief restrict to given *parent* values.
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 5bc3c463f..2ba670004 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -101,12 +101,12 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
    * @param values Discrete assignment.
    * @return double
    */
-  double evaluate(const Assignment<Key>& values) const {
-    return operator()(values);
-  }
+  virtual double evaluate(const Assignment<Key>& values) const = 0;
 
   /// Find value for given assignment of values to variables
-  virtual double operator()(const Assignment<Key>& values) const = 0;
+  double operator()(const DiscreteValues& values) const {
+    return evaluate(values);
+  }
 
   /// Error is just -log(value)
   virtual double error(const DiscreteValues& values) const;
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 32cba84ed..ea51a996c 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -133,7 +133,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
 }
 
 /* ************************************************************************ */
-double TableFactor::operator()(const Assignment<Key>& values) const {
+double TableFactor::evaluate(const Assignment<Key>& values) const {
   // a b c d => D * (C * (B * (a) + b) + c) + d
   uint64_t idx = 0, card = 1;
   for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) {
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 53495d078..1aecc1669 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -156,7 +156,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   // /// @{
 
   /// Evaluate probability distribution, is just look up in TableFactor.
-  double operator()(const Assignment<Key>& values) const override;
+  double evaluate(const Assignment<Key>& values) const override;
 
   /// Calculate error for DiscreteValues `x`, is -log(probability).
   double error(const DiscreteValues& values) const override;
diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp
index a450605b3..585ca8103 100644
--- a/gtsam_unstable/discrete/AllDiff.cpp
+++ b/gtsam_unstable/discrete/AllDiff.cpp
@@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const {
 }
 
 /* ************************************************************************* */
-double AllDiff::operator()(const Assignment<Key>& values) const {
+double AllDiff::evaluate(const Assignment<Key>& values) const {
   std::set<size_t> taken;  // record values taken by keys
   for (Key dkey : keys_) {
     size_t value = values.at(dkey);      // get the value for that key
diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 6fe43568e..1180abad4 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   }
 
   /// Calculate value = expensive !
-  double operator()(const Assignment<Key>& values) const override;
+  double evaluate(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree, can be *very* expensive !
   DecisionTreeFactor toDecisionTreeFactor() const override;
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index a55865c77..e96bfdfde 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint {
   }
 
   /// Calculate value
-  double operator()(const Assignment<Key>& values) const override {
+  double evaluate(const Assignment<Key>& values) const override {
     return (double)(values.at(keys_[0]) != values.at(keys_[1]));
   }
 
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index 752228c18..74f621dc7 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -30,7 +30,7 @@ string Domain::base1Str() const {
 }
 
 /* ************************************************************************* */
-double Domain::operator()(const Assignment<Key>& values) const {
+double Domain::evaluate(const Assignment<Key>& values) const {
   return contains(values.at(key()));
 }
 
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 13249d733..23a566d24 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   bool contains(size_t value) const { return values_.count(value) > 0; }
 
   /// Calculate value
-  double operator()(const Assignment<Key>& values) const override;
+  double evaluate(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp
index 9762aec0f..220bc9c06 100644
--- a/gtsam_unstable/discrete/SingleValue.cpp
+++ b/gtsam_unstable/discrete/SingleValue.cpp
@@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const {
 }
 
 /* ************************************************************************* */
-double SingleValue::operator()(const Assignment<Key>& values) const {
+double SingleValue::evaluate(const Assignment<Key>& values) const {
   return (double)(values.at(keys_[0]) == value_);
 }
 
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 93fe38aaa..3df1209b8 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   }
 
   /// Calculate value
-  double operator()(const Assignment<Key>& values) const override;
+  double evaluate(const Assignment<Key>& values) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;

From 8145086e5b57315bc94491def0bcdc67aa48148d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Dec 2024 21:09:37 -0500
Subject: [PATCH 087/362] formatting

---
 gtsam/discrete/discrete.i                      | 4 ++--
 python/gtsam/tests/test_DecisionTreeFactor.py  | 3 ++-
 python/gtsam/tests/test_DiscreteBayesTree.py   | 4 ++--
 python/gtsam/tests/test_DiscreteConditional.py | 3 ++-
 python/gtsam/tests/test_DiscreteFactorGraph.py | 5 ++++-
 5 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 55c8f9e22..b2e2524f8 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
   DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table);
 
   DecisionTreeFactor(const gtsam::DiscreteConditional& c);
-  
+
   void print(string s = "DecisionTreeFactor\n",
              const gtsam::KeyFormatter& keyFormatter =
                  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;
diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py
index 12308bb3c..a78d9c94a 100644
--- a/python/gtsam/tests/test_DecisionTreeFactor.py
+++ b/python/gtsam/tests/test_DecisionTreeFactor.py
@@ -13,9 +13,10 @@ Author: Frank Dellaert
 
 import unittest
 
+from gtsam.utils.test_case import GtsamTestCase
+
 from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues,
                    Ordering)
-from gtsam.utils.test_case import GtsamTestCase
 
 
 class TestDecisionTreeFactor(GtsamTestCase):
diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py
index 2a9b6ea09..e08491fab 100644
--- a/python/gtsam/tests/test_DiscreteBayesTree.py
+++ b/python/gtsam/tests/test_DiscreteBayesTree.py
@@ -19,8 +19,8 @@ from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
 from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique,
-                   DiscreteConditional, DiscreteFactorGraph,
-                   DiscreteValues, Ordering)
+                   DiscreteConditional, DiscreteFactorGraph, DiscreteValues,
+                   Ordering)
 
 
 class TestDiscreteBayesNet(GtsamTestCase):
diff --git a/python/gtsam/tests/test_DiscreteConditional.py b/python/gtsam/tests/test_DiscreteConditional.py
index 241a5f0be..6c9eb9aec 100644
--- a/python/gtsam/tests/test_DiscreteConditional.py
+++ b/python/gtsam/tests/test_DiscreteConditional.py
@@ -13,9 +13,10 @@ Author: Varun Agrawal
 
 import unittest
 
-from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys
+
 # Some DiscreteKeys for binary variables:
 A = 0, 2
 B = 1, 2
diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py
index d725ceac8..3053087b4 100644
--- a/python/gtsam/tests/test_DiscreteFactorGraph.py
+++ b/python/gtsam/tests/test_DiscreteFactorGraph.py
@@ -14,9 +14,12 @@ Author: Frank Dellaert
 import unittest
 
 import numpy as np
-from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import (DecisionTreeFactor, DiscreteConditional,
+                   DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering,
+                   Symbol)
+
 OrderingType = Ordering.OrderingType
 
 

From 80b1fe569a39b77986d38ccd127c99b0fdc5f395 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 10:41:36 -0500
Subject: [PATCH 088/362] use product method since it has a nullptr check

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 4ededbb8b..523b99201 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -66,8 +66,9 @@ namespace gtsam {
   /* ************************************************************************* */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
     DecisionTreeFactor result;
-    for(const sharedFactor& factor: *this)
+    for (const sharedFactor& factor : *this) {
       if (factor) result = (*factor) * result;
+    }
     return result;
   }
 
@@ -75,8 +76,9 @@ namespace gtsam {
   double DiscreteFactorGraph::operator()(
       const DiscreteValues &values) const {
     double product = 1.0;
-    for( const sharedFactor& factor: factors_ )
+    for (const sharedFactor& factor : factors_) {
       product *= (*factor)(values);
+    }
     return product;
   }
 
@@ -117,8 +119,7 @@ namespace gtsam {
                   const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic(product);
-    DecisionTreeFactor product;
-    for (auto&& factor : factors) product = (*factor) * product;
+    DecisionTreeFactor product = factors.product();
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:
@@ -206,8 +207,7 @@ namespace gtsam {
                     const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic(product);
-    DecisionTreeFactor product;
-    for (auto&& factor : factors) product = (*factor) * product;
+    DecisionTreeFactor product = factors.product();
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:

From 92e0a55e7817d7507ebc098df85e1e8d7cbfca5f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 10:41:57 -0500
Subject: [PATCH 089/362] generalize discreteKeys method

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 523b99201..96fdfc338 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -54,7 +54,7 @@ namespace gtsam {
   DiscreteKeys DiscreteFactorGraph::discreteKeys() const {
     DiscreteKeys result;
     for (auto&& factor : *this) {
-      if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
+      if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
         DiscreteKeys factor_keys = p->discreteKeys();
         result.insert(result.end(), factor_keys.begin(), factor_keys.end());
       }

From dea9c7f7655576d7629903e38fc357e81898ec82 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 10:49:08 -0500
Subject: [PATCH 090/362] common function for product and normalization

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 32 ++++++++++++++------------
 1 file changed, 17 insertions(+), 15 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 96fdfc338..436d784f7 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -112,11 +112,14 @@ namespace gtsam {
 //      }
 //  }
 
-  /* ************************************************************************ */
-  // Alternate eliminate function for MPE
-  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
-  EliminateForMPE(const DiscreteFactorGraph& factors,
-                  const Ordering& frontalKeys) {
+  /**
+   * @brief Multiply all the `factors` and normalize the
+   * product to prevent underflow.
+   *
+   * @param factors The factors to multiply as a DiscreteFactorGraph.
+   * @return DecisionTreeFactor
+   */
+  static DecisionTreeFactor ProductAndNormalize(const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
     gttic(product);
     DecisionTreeFactor product = factors.product();
@@ -127,6 +130,14 @@ namespace gtsam {
 
     // Normalize the product factor to prevent underflow.
     product = product / (*normalization);
+  }
+
+  /* ************************************************************************ */
+  // Alternate eliminate function for MPE
+  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+  EliminateForMPE(const DiscreteFactorGraph& factors,
+                  const Ordering& frontalKeys) {
+    DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -205,16 +216,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    // PRODUCT: multiply all factors
-    gttic(product);
-    DecisionTreeFactor product = factors.product();
-    gttoc(product);
-
-    // Max over all the potentials by pretending all keys are frontal:
-    auto normalization = product.max(product.size());
-
-    // Normalize the product factor to prevent underflow.
-    product = product / (*normalization);
+    DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);

From 590293bb92f53291a0a77837dfc51314e34c641a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 10:52:15 -0500
Subject: [PATCH 091/362] return tau factor as DiscreteFactor for discrete
 elimination

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 5 +++--
 gtsam/discrete/DiscreteFactorGraph.h   | 5 +++--
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 436d784f7..508a5f0f8 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -14,6 +14,7 @@
  *  @date Feb 14, 2011
  *  @author Duy-Nguyen Ta
  *  @author Frank Dellaert
+ *  @author Varun Agrawal
  */
 
 #include <gtsam/discrete/DiscreteBayesTree.h>
@@ -134,7 +135,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   // Alternate eliminate function for MPE
-  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+  std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
     DecisionTreeFactor product = ProductAndNormalize(factors);
@@ -213,7 +214,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+  std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
     DecisionTreeFactor product = ProductAndNormalize(factors);
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index d0dc282b4..c57d2258c 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -14,6 +14,7 @@
  * @date Feb 14, 2011
  * @author Duy-Nguyen Ta
  * @author Frank Dellaert
+ * @author Varun Agrawal
  */
 
 #pragma once
@@ -48,7 +49,7 @@ class DiscreteJunctionTree;
  * @ingroup discrete
  */
 GTSAM_EXPORT
-std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
+std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
 EliminateDiscrete(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys);
 
@@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors,
  * @ingroup discrete
  */
 GTSAM_EXPORT
-std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
+std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
 EliminateForMPE(const DiscreteFactorGraph& factors,
                 const Ordering& frontalKeys);
 

From 162f61061cbbe02298250c759ff84387dd065039 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 10:52:36 -0500
Subject: [PATCH 092/362] use BaseFactor methods to reduce code in
 DiscreteConditional

---
 gtsam/discrete/DiscreteConditional.h  | 10 +++-------
 gtsam/discrete/DiscreteDistribution.h |  2 +-
 2 files changed, 4 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 858623301..8cba6dbe7 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional
     static_cast<const BaseConditional*>(this)->print(s, formatter);
   }
 
-  /// Evaluate, just look up in AlgebraicDecisionTree
-  virtual double evaluate(const Assignment<Key>& values) const override {
-    return ADT::operator()(values);
-  }
-
-  using DecisionTreeFactor::error;       ///< DiscreteValues version
-  using DiscreteFactor::operator();      ///< DiscreteValues version
+  using BaseFactor::error;       ///< DiscreteValues version
+  using BaseFactor::evaluate;    ///< DiscreteValues version
+  using BaseFactor::operator();  ///< DiscreteValues version
 
   /**
    * @brief restrict to given *parent* values.
diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h
index 4b690da15..09ea50332 100644
--- a/gtsam/discrete/DiscreteDistribution.h
+++ b/gtsam/discrete/DiscreteDistribution.h
@@ -40,7 +40,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
   /// Default constructor needed for serialization.
   DiscreteDistribution() {}
 
-  /// Constructor from factor.
+  /// Constructor from DecisionTreeFactor.
   explicit DiscreteDistribution(const DecisionTreeFactor& f)
       : Base(f.size(), f) {}
 

From 372b133366626d03d5319843fad08e77909156db Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 11:05:50 -0500
Subject: [PATCH 093/362] formatting

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 27 +++++++++++++-------------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 508a5f0f8..ebba93382 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -36,13 +36,12 @@ namespace gtsam {
   template class FactorGraph<DiscreteFactor>;
   template class EliminateableFactorGraph<DiscreteFactorGraph>;
 
-  /* ************************************************************************* */
-  bool DiscreteFactorGraph::equals(const This& fg, double tol) const
-  {
+  /* ************************************************************************ */
+  bool DiscreteFactorGraph::equals(const This& fg, double tol) const {
     return Base::equals(fg, tol);
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   KeySet DiscreteFactorGraph::keys() const {
     KeySet keys;
     for (const sharedFactor& factor : *this) {
@@ -51,7 +50,7 @@ namespace gtsam {
     return keys;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   DiscreteKeys DiscreteFactorGraph::discreteKeys() const {
     DiscreteKeys result;
     for (auto&& factor : *this) {
@@ -64,7 +63,7 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
     DecisionTreeFactor result;
     for (const sharedFactor& factor : *this) {
@@ -73,9 +72,8 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
-  double DiscreteFactorGraph::operator()(
-      const DiscreteValues &values) const {
+  /* ************************************************************************ */
+  double DiscreteFactorGraph::operator()(const DiscreteValues& values) const {
     double product = 1.0;
     for (const sharedFactor& factor : factors_) {
       product *= (*factor)(values);
@@ -83,9 +81,9 @@ namespace gtsam {
     return product;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void DiscreteFactorGraph::print(const string& s,
-      const KeyFormatter& formatter) const {
+                                  const KeyFormatter& formatter) const {
     std::cout << s << std::endl;
     std::cout << "size: " << size() << std::endl;
     for (size_t i = 0; i < factors_.size(); i++) {
@@ -120,7 +118,8 @@ namespace gtsam {
    * @param factors The factors to multiply as a DiscreteFactorGraph.
    * @return DecisionTreeFactor
    */
-  static DecisionTreeFactor ProductAndNormalize(const DiscreteFactorGraph& factors) {
+  static DecisionTreeFactor ProductAndNormalize(
+      const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
     gttic(product);
     DecisionTreeFactor product = factors.product();
@@ -155,8 +154,8 @@ namespace gtsam {
     // Make lookup with product
     gttic(lookup);
     size_t nrFrontals = frontalKeys.size();
-    auto lookup = std::make_shared<DiscreteLookupTable>(nrFrontals,
-                                                          orderedKeys, product);
+    auto lookup =
+        std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
     gttoc(lookup);
 
     return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};

From 62008fc99543c8c87e6a38bea11ff1592b1aac16 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 11:10:25 -0500
Subject: [PATCH 094/362] add return

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index ebba93382..ec6dac2fc 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -130,6 +130,8 @@ namespace gtsam {
 
     // Normalize the product factor to prevent underflow.
     product = product / (*normalization);
+
+    return product;
   }
 
   /* ************************************************************************ */

From 2fd60a47a231b6cbebc6b916d213e64d379b39fc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 11:54:05 -0500
Subject: [PATCH 095/362] fix test

---
 gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 341eb63e3..0d71c12ba 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -113,7 +113,8 @@ TEST(DiscreteFactorGraph, test) {
   const Ordering frontalKeys{0};
   const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys);
 
-  DecisionTreeFactor newFactor = *newFactorPtr;
+  DecisionTreeFactor newFactor =
+      *std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
 
   // Normalize newFactor by max for comparison with expected
   auto normalization = newFactor.max(newFactor.size());

From 588533751b07d69a1940241ba6a144eb30ae4938 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 10 Dec 2024 14:10:33 -0500
Subject: [PATCH 096/362] add another pointer check

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index ec6dac2fc..d0bf21047 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -76,7 +76,7 @@ namespace gtsam {
   double DiscreteFactorGraph::operator()(const DiscreteValues& values) const {
     double product = 1.0;
     for (const sharedFactor& factor : factors_) {
-      product *= (*factor)(values);
+      if (factor) product *= (*factor)(values);
     }
     return product;
   }

From 22d11d7af49ebdec102e5ec16597a00a175621ce Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 Dec 2024 04:00:56 -0500
Subject: [PATCH 097/362] don't print timing info by default

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 50072f547..ba05417aa 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -224,10 +224,10 @@ namespace gtsam {
     DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // sum out frontals, this is the factor on the separator
-    gttic_(sum);
+    gttic(sum);
     DecisionTreeFactor::shared_ptr sum = std::dynamic_pointer_cast<DecisionTreeFactor>(
         product.sum(frontalKeys));
-    gttoc_(sum);
+    gttoc(sum);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     Ordering orderedKeys;
@@ -237,10 +237,10 @@ namespace gtsam {
                        sum->keys().end());
 
     // now divide product/sum to get conditional
-    gttic_(divide);
+    gttic(divide);
     auto conditional =
         std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
-    gttoc_(divide);
+    gttoc(divide);
 
     return {conditional, sum};
   }

From 8ce55a221001d931a41cccfd013b9adc95dad5d9 Mon Sep 17 00:00:00 2001
From: Jade Turner <spacey-sooty@proton.me>
Date: Thu, 12 Dec 2024 00:16:03 +0800
Subject: [PATCH 098/362] document flags

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
---
 INSTALL.md | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/INSTALL.md b/INSTALL.md
index 10bee196c..51f00fd3b 100644
--- a/INSTALL.md
+++ b/INSTALL.md
@@ -27,7 +27,7 @@ $ make install
        downloaded from https://www.threadingbuildingblocks.org/
      - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
        `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
-       achieved with MKL disabled. We therefore advise you to benchmark your problem 
+       achieved with MKL disabled. We therefore advise you to benchmark your problem
        before using MKL.
 
     Tested compilers:
@@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive)
 
 #### CMAKE_INSTALL_PREFIX
 
-The install folder. The default is typically `/usr/local/`. 
+The install folder. The default is typically `/usr/local/`.
 To configure to install to your home directory, you could execute:
 
 ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..```
 
-#### GTSAM_TOOLBOX_INSTALL_PATH 
+#### GTSAM_TOOLBOX_INSTALL_PATH
 
 The Matlab toolbox will be installed in a subdirectory
 of this folder, called 'gtsam'.
@@ -142,7 +142,7 @@ of this folder, called 'gtsam'.
 
 #### GTSAM_BUILD_CONVENIENCE_LIBRARIES
 
-This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. 
+This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam.
 Set with the command line as follows:
 
 ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..```
@@ -159,6 +159,16 @@ Set with the command line as follows:
   ON:             When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam.  In addition, if tests are enabled, the unit tests will be built as well.  The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`.
   OFF (Default)  If disabled, no `gtsam_unstable` code will be included in build or install.
 
+## Convenience Options:
+
+#### GTSAM_BUILD_EXAMPLES_ALWAYS
+
+Whether or not to force building examples, can be true or false.
+
+#### GTSAM_BUILD_TESTS
+
+Whether or not to build tests, can be true or false.
+
 ## Check
 
 `make check` will build and run all of the tests. Note that the tests will only be
@@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM.
 
 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
 2. Enable TBB. On modern processors with multiple cores, this can easily speed up
-    optimization by 30-50%. Please note that this may not be true for very small 
+    optimization by 30-50%. Please note that this may not be true for very small
     problems where the overhead of dispatching work to multiple threads outweighs
     the benefit. We recommend that you benchmark your problem with/without TBB.
 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of

From af9db89ea9ce9e18dbcbfbb3791501ddaf8e6724 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 Dec 2024 13:10:31 -0500
Subject: [PATCH 099/362] Fixed math, use functor not macro

---
 gtsam/linear/KalmanFilter.cpp | 74 ++++++++++++++++++-----------------
 gtsam/linear/KalmanFilter.h   |  2 +-
 2 files changed, 40 insertions(+), 36 deletions(-)

diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index 12a43093c..7c6e78527 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -25,22 +25,27 @@
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/KalmanFilter.h>
 
-using namespace std;
+#include "gtsam/base/Matrix.h"
+
+// In the code below we often get a covariance matrix Q, and we need to create a
+// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as
+//   Q = L L^T
+// and hence
+//   Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1}
+// We then have
+//   0.5 * |Ax - b|^T Q^{-1} |Ax - b|
+// = 0.5 * |Ax - b|^T M^T M |Ax - b|
+// = 0.5 * |MAx - Mb|^T |MAx - Mb|
+// The functor below efficiently multiplies with M by calling L.solve().
+namespace {
+using Matrix = gtsam::Matrix;
+struct InverseL : public Eigen::LLT<Matrix> {
+  InverseL(const Matrix& Q) : Eigen::LLT<Matrix>(Q) {}
+  Matrix operator*(const Matrix& A) const { return matrixL().solve(A); }
+};
+}  // namespace
 
 namespace gtsam {
-
-// In the code below we often need to multiply matrices with R, the Cholesky
-// factor of the information matrix Q^{-1}, when given a covariance matrix Q.
-// We can do this efficiently using Eigen, as we have:
-//   Q = L L^T
-//   => Q^{-1} = L^{-T}L^{-1} = R^T R
-//   => L^{-1} = R
-// Rather than explicitly calculate R and do R*A, we should use L.solve(A)
-// The following macro makes L available in the scope where it is used:
-#define FACTORIZE_Q_INTO_L(Q)       \
-  Eigen::LLT<Matrix> llt(Q); \
-  auto L = llt.matrixL();
-
 /* ************************************************************************* */
 // Auxiliary function to solve factor graph and return pointer to root
 // conditional
@@ -85,19 +90,19 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0,
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
 
-  // Perform Cholesky decomposition of P0
-  FACTORIZE_Q_INTO_L(P0)
+  // Perform Cholesky decomposition of P0 = LL^T
+  InverseL M(P0);
 
-  // Premultiply I and x0 with R
-  const Matrix R = L.solve(I_);  // = R
-  const Vector b = L.solve(x0);  // = R*x0
-  factorGraph.emplace_shared<JacobianFactor>(0, R, b);
+  // Premultiply I and x0 with M=L^{-1}
+  const Matrix A = M * I_;  // = M
+  const Vector b = M * x0;  // = M*x0
+  factorGraph.emplace_shared<JacobianFactor>(0, A, b);
   return solve(factorGraph);
 }
 
 /* ************************************************************************* */
-void KalmanFilter::print(const string& s) const {
-  cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
+void KalmanFilter::print(const std::string& s) const {
+  std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl;
 }
 
 /* ************************************************************************* */
@@ -126,13 +131,13 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
   assert(Q.cols() == n);
 #endif
 
-  // Perform Cholesky decomposition of Q
-  FACTORIZE_Q_INTO_L(Q)
+  // Perform Cholesky decomposition of Q = LL^T
+  InverseL M(Q);
 
-  // Premultiply A1 = -F and A2 = I, b = B * u with R
-  const Matrix A1 = -L.solve(F);    // = -R*F
-  const Matrix A2 = L.solve(I_);    // = R
-  const Vector b = L.solve(B * u);  // = R * (B * u)
+  // Premultiply -F, I, and B * u with M=L^{-1}
+  const Matrix A1 = -(M * F);
+  const Matrix A2 = M * I_;
+  const Vector b = M * (B * u);
   return predict2(p, A1, A2, b);
 }
 
@@ -163,14 +168,13 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
                                           const Matrix& Q) const {
   Key k = step(p);
 
-  // Perform Cholesky decomposition of Q
-  FACTORIZE_Q_INTO_L(Q)
+  // Perform Cholesky decomposition of Q = LL^T
+  InverseL M(Q);
 
-  // Pre-multiply H and z with R, respectively
-  const Matrix RtH = L.solve(H);  // = R * H
-  const Vector b = L.solve(z);    // = R * z
-
-  return fuse(p, std::make_shared<JacobianFactor>(k, RtH, b));
+  // Pre-multiply H and z with M=L^{-1}, respectively
+  const Matrix A = M * H;
+  const Vector b = M * z;
+  return fuse(p, std::make_shared<JacobianFactor>(k, A, b));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index 7fde9975d..2bf0549fa 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -214,4 +214,4 @@ class GTSAM_EXPORT KalmanFilter {
   size_t dim() const { return n_; }
 };
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam

From 2d0672059cc26ec3dd9ea58ba462f2b087eca25c Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Tue, 10 Dec 2024 13:41:19 -0500
Subject: [PATCH 100/362] Update some header includes

---
 gtsam/basis/Chebyshev2.cpp         |  8 ++++++++
 gtsam/basis/Chebyshev2.h           | 10 +---------
 gtsam/geometry/Pose2.h             | 11 +++++++----
 gtsam/geometry/Rot3.h              |  7 +++++--
 gtsam/geometry/triangulation.h     |  7 +++++--
 gtsam/inference/MetisIndex.h       |  7 +++++--
 gtsam/linear/HessianFactor.h       | 18 ++++++++++++------
 gtsam/nonlinear/ExpressionFactor.h |  4 ++--
 gtsam/slam/EssentialMatrixFactor.h |  5 +++--
 gtsam/slam/SmartFactorBase.h       |  7 +++++--
 10 files changed, 53 insertions(+), 31 deletions(-)

diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp
index 63fca64cc..71c3db7f0 100644
--- a/gtsam/basis/Chebyshev2.cpp
+++ b/gtsam/basis/Chebyshev2.cpp
@@ -20,6 +20,14 @@
 
 namespace gtsam {
 
+double Chebyshev2::Point(size_t N, int j, double a, double b) {
+  assert(j >= 0 && size_t(j) < N);
+  const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
+  // We add -PI so that we get values ordered from -1 to +1
+  // sin(-M_PI_2 + dtheta*j); also works
+  return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
+}
+
 Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
   // Allocate space for weights
   Weights weights(N);
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index 1374faeef..207f4b617 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -36,8 +36,6 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/basis/Basis.h>
 
-#include <cassert>
-
 namespace gtsam {
 
 /**
@@ -63,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
    * @param b Upper bound of interval (default: 1)
    * @return double
    */
-  static double Point(size_t N, int j, double a = -1, double b = 1) {
-    assert(j >= 0 && size_t(j) < N);
-    const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
-    // We add -PI so that we get values ordered from -1 to +1
-    // sin(-M_PI_2 + dtheta*j); also works
-    return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
-  }
+  static double Point(size_t N, int j, double a = -1, double b = 1);
 
   /// All Chebyshev points
   static Vector Points(size_t N) {
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index d9cad37f3..ff9a6399e 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -28,7 +28,6 @@
 #include <gtsam/base/std_optional_serialization.h>
 
 #include <optional>
-#include <cassert>
 
 namespace gtsam {
 
@@ -82,9 +81,13 @@ public:
   Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
 
   /** Constructor from 3*3 matrix */
-  Pose2(const Matrix &T) :
-    r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
-    assert(T.rows() == 3 && T.cols() == 3);
+  Pose2(const Matrix &T)
+      : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
+#ifndef NDEBUG
+    if (T.rows() != 3 || T.cols() != 3) {
+      throw;
+    }
+#endif
   }
 
   /// @}
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 398e8d473..d1e330438 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -30,7 +30,6 @@
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
 #include <random>
-#include <cassert>
 
 // You can override the default coordinate mode using this flag
 #ifndef ROT3_DEFAULT_COORDINATES_MODE
@@ -160,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
     inline static Rot3 RzRyRx(const Vector& xyz,
                               OptionalJacobian<3, 3> H = {}) {
-      assert(xyz.size() == 3);
+#ifndef NDEBUG
+      if (xyz.size() != 3) {
+        throw;
+      }
+#endif
       Rot3 out;
       if (H) {
         Vector3 Hx, Hy, Hz;
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 0fa9d074e..9087f01c5 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -35,7 +35,6 @@
 #include <gtsam/slam/TriangulationFactor.h>
 
 #include <optional>
-#include <cassert>
 
 namespace gtsam {
 
@@ -318,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements) {
   const size_t nrMeasurements = measurements.size();
-  assert(nrMeasurements == cameras.size());
+#ifndef NDEBUG
+  if (nrMeasurements != cameras.size()) {
+    throw;
+  }
+#endif
   typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
   for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index cc058dfbd..e051b9ae9 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -25,7 +25,6 @@
 #include <vector>
 #include <map>
 #include <unordered_map>
-#include <cassert>
 
 namespace gtsam {
 /**
@@ -93,7 +92,11 @@ public:
     return nKeys_;
   }
   Key intToKey(int32_t value) const {
-    assert(value >= 0);
+#ifndef NDEBUG
+    if (value < 0) {
+      throw;
+    }
+#endif
     return intKeyBMap_.right.find(value)->second;
   }
 
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 53b6d0e8b..56b3d6537 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -23,8 +23,6 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
 
-#include <cassert>
-
 namespace gtsam {
 
   // Forward declarations
@@ -242,14 +240,18 @@ namespace gtsam {
      * use, for example, begin() + 2 to get the 3rd variable in this factor.
      * @return The linear term \f$ g \f$ */
     SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
-      assert(!empty());
+#ifndef NDEBUG
+      if(empty()) throw;
+#endif
       return info_.aboveDiagonalBlock(j - begin(), size());
     }
 
     /** Return the complete linear term \f$ g \f$ as described above.
      * @return The linear term \f$ g \f$ */
     SymmetricBlockMatrix::constBlock linearTerm() const {
-      assert(!empty());
+#ifndef NDEBUG
+      if(empty()) throw;
+#endif
       // get the last column (except the bottom right block)
       return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
     }
@@ -257,7 +259,9 @@ namespace gtsam {
     /** Return the complete linear term \f$ g \f$ as described above.
      * @return The linear term \f$ g \f$ */
     SymmetricBlockMatrix::Block linearTerm() {
-      assert(!empty());
+#ifndef NDEBUG
+      if(empty()) throw;
+#endif
       return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
     }
 
@@ -326,7 +330,9 @@ namespace gtsam {
      * @param other the HessianFactor to be updated
      */
     void updateHessian(HessianFactor* other) const {
-      assert(other);
+#ifndef NDEBUG
+      if(!other) throw;
+#endif
       updateHessian(other->keys_, &other->info_);
     }
 
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index b3e34d079..63b2469a9 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -43,7 +43,7 @@ namespace gtsam {
  *
  */
 template <typename T>
-class ExpressionFactor : public NoiseModelFactor {
+class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor {
   GTSAM_CONCEPT_ASSERT(IsTestable<T>);
 
 protected:
@@ -246,7 +246,7 @@ struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
  *
  */
 template <typename T, typename... Args>
-class ExpressionFactorN : public ExpressionFactor<T> {
+class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor<T> {
 public:
   static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
   using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 3c2f105d4..601f8e94e 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -25,7 +25,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <iostream>
-#include <cassert>
 
 namespace gtsam {
 
@@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
                         const SharedNoiseModel& model,
                         std::shared_ptr<CALIBRATION> K)
       : Base(model, key) {
-    assert(K);
+#ifndef NDEBUG
+    if (K->empty()) throw;
+#endif
     vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
     vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
   }
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index b8e789aaa..d3e879c33 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -34,7 +34,6 @@
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
-#include <cassert>
 
 namespace gtsam {
 
@@ -136,7 +135,11 @@ protected:
 
   /// Add a bunch of measurements, together with the camera keys.
   void add(const ZVector& measurements, const KeyVector& cameraKeys) {
-    assert(measurements.size() == cameraKeys.size());
+#ifndef NDEBUG
+    if (measurements.size() != cameraKeys.size()) {
+      throw std::runtime_error("Number of measurements and camera keys do not match");
+    }
+#endif
     for (size_t i = 0; i < measurements.size(); i++) {
       this->add(measurements[i], cameraKeys[i]);
     }

From a1ba3d77c8dcc896223bcbe4f08b7c3f207e3c3c Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Tue, 10 Dec 2024 17:25:12 -0500
Subject: [PATCH 101/362] Fix essential matrix factor

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

diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 601f8e94e..5d38cb234 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -71,7 +71,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
                         std::shared_ptr<CALIBRATION> K)
       : Base(model, key) {
 #ifndef NDEBUG
-    if (K->empty()) throw;
+    if (!K) throw;
 #endif
     vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
     vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));

From b76c6d8250270f7359884f0db98ce937f02ae5a3 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Wed, 11 Dec 2024 12:59:13 -0500
Subject: [PATCH 102/362] Fix ExpressionFactor wrong GTSAM_EXPORT

---
 gtsam/nonlinear/ExpressionFactor.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 63b2469a9..b3e34d079 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -43,7 +43,7 @@ namespace gtsam {
  *
  */
 template <typename T>
-class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor {
+class ExpressionFactor : public NoiseModelFactor {
   GTSAM_CONCEPT_ASSERT(IsTestable<T>);
 
 protected:
@@ -246,7 +246,7 @@ struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
  *
  */
 template <typename T, typename... Args>
-class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor<T> {
+class ExpressionFactorN : public ExpressionFactor<T> {
 public:
   static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
   using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;

From ed098eaec64d185fbf91923b89e9442383b18979 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Wed, 4 Dec 2024 11:43:51 -0500
Subject: [PATCH 103/362] Cleanup some includes and fix cassert

---
 examples/CombinedImuFactorsExample.cpp                  | 1 +
 examples/ImuFactorsExample.cpp                          | 1 +
 examples/elaboratePoint2KalmanFilter.cpp                | 2 ++
 gtsam/base/Matrix.cpp                                   | 1 +
 gtsam/base/Vector.cpp                                   | 1 +
 gtsam/base/VerticalBlockMatrix.h                        | 2 ++
 gtsam/base/cholesky.cpp                                 | 1 +
 gtsam/base/tests/testNumericalDerivative.cpp            | 2 ++
 gtsam/basis/Chebyshev2.h                                | 2 ++
 gtsam/discrete/DiscreteConditional.cpp                  | 1 +
 gtsam/discrete/DiscreteLookupDAG.cpp                    | 1 +
 gtsam/geometry/Cal3f.cpp                                | 1 +
 gtsam/geometry/CameraSet.h                              | 1 +
 gtsam/geometry/Pose2.cpp                                | 1 +
 gtsam/geometry/Pose2.h                                  | 1 +
 gtsam/geometry/Rot3.cpp                                 | 1 +
 gtsam/geometry/Rot3.h                                   | 1 +
 gtsam/geometry/SOn.h                                    | 1 +
 gtsam/geometry/triangulation.h                          | 3 ++-
 gtsam/hybrid/HybridJunctionTree.cpp                     | 1 +
 gtsam/inference/BayesTree-inst.h                        | 2 +-
 gtsam/inference/ClusterTree-inst.h                      | 1 +
 gtsam/inference/EliminationTree-inst.h                  | 1 +
 gtsam/inference/JunctionTree-inst.h                     | 2 ++
 gtsam/inference/MetisIndex.h                            | 1 +
 gtsam/inference/Ordering.cpp                            | 1 +
 gtsam/linear/HessianFactor.cpp                          | 3 ++-
 gtsam/linear/HessianFactor.h                            | 1 +
 gtsam/linear/JacobianFactor.cpp                         | 1 +
 gtsam/linear/KalmanFilter.cpp                           | 3 +++
 gtsam/linear/NoiseModel.cpp                             | 1 +
 gtsam/linear/Sampler.cpp                                | 3 +++
 gtsam/linear/SubgraphPreconditioner.cpp                 | 1 +
 gtsam/navigation/ImuFactor.cpp                          | 1 +
 gtsam/navigation/PreintegrationBase.cpp                 | 4 +++-
 gtsam/nonlinear/DoglegOptimizerImpl.cpp                 | 1 +
 gtsam/nonlinear/DoglegOptimizerImpl.h                   | 1 +
 gtsam/nonlinear/Expression-inl.h                        | 1 +
 gtsam/nonlinear/ExtendedKalmanFilter-inl.h              | 2 ++
 gtsam/nonlinear/ISAM2-impl.cpp                          | 1 +
 gtsam/nonlinear/ISAM2-impl.h                            | 1 +
 gtsam/nonlinear/ISAM2.cpp                               | 1 +
 gtsam/nonlinear/ISAM2Clique.cpp                         | 1 +
 gtsam/nonlinear/NonlinearFactor.cpp                     | 2 ++
 gtsam/nonlinear/Values.cpp                              | 1 +
 gtsam/nonlinear/internal/ExpressionNode.h               | 1 +
 gtsam/sfm/ShonanAveraging.cpp                           | 1 +
 gtsam/slam/EssentialMatrixFactor.h                      | 1 +
 gtsam/slam/SmartFactorBase.h                            | 1 +
 gtsam_unstable/discrete/Scheduler.cpp                   | 1 +
 gtsam_unstable/dynamics/DynamicsPriors.h                | 2 ++
 gtsam_unstable/dynamics/FullIMUFactor.h                 | 2 ++
 gtsam_unstable/dynamics/IMUFactor.h                     | 2 ++
 gtsam_unstable/dynamics/PoseRTV.cpp                     | 2 ++
 gtsam_unstable/dynamics/VelocityConstraint.h            | 2 ++
 gtsam_unstable/geometry/BearingS2.cpp                   | 2 ++
 gtsam_unstable/geometry/Pose3Upright.cpp                | 5 +++--
 gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp      | 2 ++
 gtsam_unstable/nonlinear/LinearizedFactor.cpp           | 1 +
 gtsam_unstable/partition/FindSeparator-inl.h            | 1 +
 gtsam_unstable/slam/PartialPriorFactor.h                | 2 ++
 gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp   | 2 ++
 gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp | 2 ++
 tests/testGaussianISAM2.cpp                             | 1 +
 64 files changed, 91 insertions(+), 6 deletions(-)

diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp
index 6ce4f76fa..436b6e283 100644
--- a/examples/CombinedImuFactorsExample.cpp
+++ b/examples/CombinedImuFactorsExample.cpp
@@ -48,6 +48,7 @@
 #include <cstring>
 #include <fstream>
 #include <iostream>
+#include <cassert>
 
 using namespace gtsam;
 using namespace std;
diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index 342f1f220..7cbc8989f 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -49,6 +49,7 @@
 #include <gtsam/slam/dataset.h>
 
 #include <cstring>
+#include <cassert>
 #include <fstream>
 #include <iostream>
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index a2ad8cf0b..bc4e6e340 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -29,6 +29,8 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Point2.h>
 
+#include <cassert>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 247c53cce..1e04bc58a 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -24,6 +24,7 @@
 #include <Eigen/LU>
 
 #include <cstdarg>
+#include <cassert>
 #include <cstring>
 #include <iomanip>
 #include <list>
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index ce7aeba7b..dbd48dfc8 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -24,6 +24,7 @@
 #include <iostream>
 #include <fstream>
 #include <sstream>
+#include <cassert>
 #include <iomanip>
 #include <cmath>
 #include <cstdio>
diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index 777441300..c8fef76e9 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -21,6 +21,8 @@
 #include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   // Forward declarations
diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp
index 71cef2524..9fee627fe 100644
--- a/gtsam/base/cholesky.cpp
+++ b/gtsam/base/cholesky.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
index e8838a476..b14f699a3 100644
--- a/gtsam/base/tests/testNumericalDerivative.cpp
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -18,6 +18,8 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index 849a51104..1374faeef 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -36,6 +36,8 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/basis/Basis.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index c44905d79..eeb5dca3f 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -30,6 +30,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <cassert>
 
 using namespace std;
 using std::pair;
diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp
index 4d02e007b..d1108e7b7 100644
--- a/gtsam/discrete/DiscreteLookupDAG.cpp
+++ b/gtsam/discrete/DiscreteLookupDAG.cpp
@@ -23,6 +23,7 @@
 #include <iterator>
 #include <string>
 #include <utility>
+#include <cassert>
 
 using std::pair;
 using std::vector;
diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp
index 319155dc9..b8e365eed 100644
--- a/gtsam/geometry/Cal3f.cpp
+++ b/gtsam/geometry/Cal3f.cpp
@@ -28,6 +28,7 @@
 #include <gtsam/geometry/Cal3f.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 26d4952c8..e3c16f066 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -26,6 +26,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 05678dc27..640481406 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/concepts.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <iomanip>
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index aad81493f..d9cad37f3 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -28,6 +28,7 @@
 #include <gtsam/base/std_optional_serialization.h>
 
 #include <optional>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index a936bd02a..10ba5894e 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -23,6 +23,7 @@
 #include <gtsam/geometry/SO3.h>
 
 #include <cmath>
+#include <cassert>
 #include <random>
 
 using namespace std;
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 7e05ee4da..398e8d473 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -30,6 +30,7 @@
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
 #include <random>
+#include <cassert>
 
 // You can override the default coordinate mode using this flag
 #ifndef ROT3_DEFAULT_COORDINATES_MODE
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index 7e1762b3f..75ff872a8 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -33,6 +33,7 @@
 #include <type_traits>
 #include <vector>
 #include <random>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index d82da3459..0fa9d074e 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -20,7 +20,7 @@
 
 #pragma once
 
-#include "gtsam/geometry/Point3.h"
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
@@ -35,6 +35,7 @@
 #include <gtsam/slam/TriangulationFactor.h>
 
 #include <optional>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 22d3c7dd2..b9bdcbed2 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -22,6 +22,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 95d475a02..fdfa94d2b 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -27,7 +27,7 @@
 
 #include <fstream>
 #include <queue>
-
+#include <cassert>
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h
index 318624608..2a5f0b455 100644
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/gtsam/inference/ClusterTree-inst.h
@@ -19,6 +19,7 @@
 #include <mutex>
 #endif
 #include <queue>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index e9acc12e1..16449d0f4 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/gtsam/inference/EliminationTree-inst.h
@@ -19,6 +19,7 @@
 
 #include <stack>
 #include <queue>
+#include <cassert>
 
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h
index 0f79c2a9a..6a8d6918a 100644
--- a/gtsam/inference/JunctionTree-inst.h
+++ b/gtsam/inference/JunctionTree-inst.h
@@ -25,6 +25,8 @@
 #include <gtsam/symbolic/SymbolicConditional.h>
 #include <gtsam/symbolic/SymbolicFactor-inst.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 template<class BAYESTREE, class GRAPH, class ETREE_NODE>
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index abdf11c5f..cc058dfbd 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -25,6 +25,7 @@
 #include <vector>
 #include <map>
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 /**
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index cb2ca752d..95eb17a1c 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -18,6 +18,7 @@
 
 #include <vector>
 #include <limits>
+#include <cassert>
 
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 3c3050f90..4e8ef804c 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -25,13 +25,14 @@
 #include <gtsam/base/cholesky.h>
 #include <gtsam/base/debug.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/base/timing.h>
 
 #include <sstream>
+#include <cassert>
 #include <limits>
-#include "gtsam/base/Vector.h"
 
 using namespace std;
 
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 8cbabcd5d..53b6d0e8b 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -23,6 +23,7 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 3cfb5ce7b..51d513e33 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -32,6 +32,7 @@
 #include <gtsam/base/cholesky.h>
 
 #include <cmath>
+#include <cassert>
 #include <sstream>
 #include <stdexcept>
 
diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index ded6bc5e3..6291c90b6 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -26,6 +26,9 @@
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/base/Testable.h>
 
+#ifndef NDEBUG
+#include <cassert>
+#endif
 
 using namespace std;
 
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 884c87270..39666b46b 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <limits>
 #include <stdexcept>
diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp
index 20d4c955b..68f21d723 100644
--- a/gtsam/linear/Sampler.cpp
+++ b/gtsam/linear/Sampler.cpp
@@ -17,6 +17,9 @@
  */
 
 #include <gtsam/linear/Sampler.h>
+
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 96f4847b5..53ea94d6e 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -25,6 +25,7 @@
 #include <gtsam/base/Vector.h>
 
 #include <stdexcept>
+#include <cassert>
 
 using std::cout;
 using std::endl;
diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 88e45d318..5697d54d4 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -23,6 +23,7 @@
 
 /* External or standard includes */
 #include <ostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 7f998987b..dd86b829c 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -20,9 +20,11 @@
  *  @author Varun Agrawal
  **/
 
-#include "PreintegrationBase.h"
+#include <gtsam/navigation/PreintegrationBase.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <cassert>
+
 using namespace std;
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
index 4f1c6fb54..e82a513ca 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <cmath>
+#include <cassert>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
 using namespace std;
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
index 8ce9b361e..1038dd522 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.h
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -17,6 +17,7 @@
 #pragma once
 
 #include <iomanip>
+#include <cassert>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/Ordering.h>
diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index 71ef1d840..b09bcb3a5 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -26,6 +26,7 @@
 #include <set>
 #include <string>
 #include <vector>
+#include <cassert>
 
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 5bf643c12..072f40b58 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -23,6 +23,8 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 20874e2dc..d0b27f3f3 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -25,6 +25,7 @@
 #include <functional>
 #include <limits>
 #include <string>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h
index e9a9696eb..bb0210237 100644
--- a/gtsam/nonlinear/ISAM2-impl.h
+++ b/gtsam/nonlinear/ISAM2-impl.h
@@ -33,6 +33,7 @@
 #include <string>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 47857a2a2..0eb2deef5 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -29,6 +29,7 @@
 #include <map>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp
index d4b050f84..64ab7c66e 100644
--- a/gtsam/nonlinear/ISAM2Clique.cpp
+++ b/gtsam/nonlinear/ISAM2Clique.cpp
@@ -22,6 +22,7 @@
 
 #include <stack>
 #include <utility>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 7312bf6d9..5eabfac91 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -19,6 +19,8 @@
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 6af9f2b6a..1caaa9db1 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -28,6 +28,7 @@
 #include <list>
 #include <memory>
 #include <sstream>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h
index de0c9721e..25587f511 100644
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@@ -26,6 +26,7 @@
 #include <typeinfo>       // operator typeid
 #include <ostream>
 #include <map>
+#include <cassert>
 
 class ExpressionFactorBinaryTest;
 // Forward declare for testing
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 830f1c719..6c9f91e90 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -37,6 +37,7 @@
 #include <random>
 #include <set>
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 5fdf138cc..3c2f105d4 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -25,6 +25,7 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 5d6ec4445..b8e789aaa 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -34,6 +34,7 @@
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp
index 0de4d32c4..53be25d55 100644
--- a/gtsam_unstable/discrete/Scheduler.cpp
+++ b/gtsam_unstable/discrete/Scheduler.cpp
@@ -14,6 +14,7 @@
 #include <cmath>
 #include <fstream>
 #include <iomanip>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h
index d4ebcba19..2dc0d552b 100644
--- a/gtsam_unstable/dynamics/DynamicsPriors.h
+++ b/gtsam_unstable/dynamics/DynamicsPriors.h
@@ -12,6 +12,8 @@
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 #include <gtsam_unstable/slam/PartialPriorFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 // Indices of relevant variables in the PoseRTV tangent vector:
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 02da899b7..eea019cd2 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index ee09600e5..241b6b497 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp
index cf21c315b..9457c99e8 100644
--- a/gtsam_unstable/dynamics/PoseRTV.cpp
+++ b/gtsam_unstable/dynamics/PoseRTV.cpp
@@ -7,6 +7,8 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Vector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index 2bb70e1b5..dbdd27259 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 namespace dynamics {
diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp
index 9dfed612c..23afd158b 100644
--- a/gtsam_unstable/geometry/BearingS2.cpp
+++ b/gtsam_unstable/geometry/BearingS2.cpp
@@ -9,6 +9,8 @@
 
 #include <gtsam_unstable/geometry/BearingS2.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;
diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp
index 9cd9f78f5..c5ef0d70f 100644
--- a/gtsam_unstable/geometry/Pose3Upright.cpp
+++ b/gtsam_unstable/geometry/Pose3Upright.cpp
@@ -6,8 +6,9 @@
  */
 
 #include <iostream>
-#include "gtsam/base/OptionalJacobian.h"
-#include "gtsam/base/Vector.h"
+#include <cassert>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/base/Vector.h>
 
 #include <gtsam_unstable/geometry/Pose3Upright.h>
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 36cb6165f..8dfb15aa2 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/debug.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
index 2d404005d..375c49341 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam_unstable/nonlinear/LinearizedFactor.h>
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index f4718f7a9..21ed91604 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -14,6 +14,7 @@
 #include <iostream>
 #include <vector>
 #include <optional>
+#include <cassert>
 #include <boost/shared_array.hpp>
 
 #include <gtsam/base/timing.h>
diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h
index 7722e5d82..0a735a2e1 100644
--- a/gtsam_unstable/slam/PartialPriorFactor.h
+++ b/gtsam_unstable/slam/PartialPriorFactor.h
@@ -20,6 +20,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Lie.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /**
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
index c4a473154..6d78bdb7a 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
@@ -18,6 +18,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
index 57913385a..52129daa8 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
@@ -21,6 +21,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 721ed51c0..3b507d806 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -25,6 +25,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
 
 using namespace std;
 using namespace gtsam;

From 425c3ac42c10cef214839416f1427362a2f5e32c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 Dec 2024 15:00:53 -0500
Subject: [PATCH 104/362] check if input size matches the total cardinality of
 the keys

---
 gtsam/discrete/TableFactor.cpp | 20 ++++++++++++++++----
 gtsam/discrete/TableFactor.h   | 16 ++++++++++------
 2 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index ea51a996c..ea51fa550 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -98,7 +98,17 @@ TableFactor::TableFactor(const DiscreteConditional& c)
 
 /* ************************************************************************ */
 Eigen::SparseVector<double> TableFactor::Convert(
-    const std::vector<double>& table) {
+    const DiscreteKeys& keys, const std::vector<double>& table) {
+  size_t max_size = 1;
+  for (auto&& [_, cardinality] : keys.cardinalities()) {
+    max_size *= cardinality;
+  }
+  if (table.size() != max_size) {
+    throw std::runtime_error(
+        "The cardinalities of the keys don't match the number of values in the "
+        "input.");
+  }
+
   Eigen::SparseVector<double> sparse_table(table.size());
   // Count number of nonzero elements in table and reserve the space.
   const uint64_t nnz = std::count_if(table.begin(), table.end(),
@@ -113,13 +123,14 @@ Eigen::SparseVector<double> TableFactor::Convert(
 }
 
 /* ************************************************************************ */
-Eigen::SparseVector<double> TableFactor::Convert(const std::string& table) {
+Eigen::SparseVector<double> TableFactor::Convert(const DiscreteKeys& keys,
+                                                 const std::string& table) {
   // Convert string to doubles.
   std::vector<double> ys;
   std::istringstream iss(table);
   std::copy(std::istream_iterator<double>(iss), std::istream_iterator<double>(),
             std::back_inserter(ys));
-  return Convert(ys);
+  return Convert(keys, ys);
 }
 
 /* ************************************************************************ */
@@ -250,7 +261,8 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
     for (auto&& kv : assignment) {
       cout << "(" << formatter(kv.first) << ", " << kv.second << ")";
     }
-    cout << " | " << it.value() << " | " << it.index() << endl;
+    cout << " | " << std::setw(10) << std::left << it.value() << " | "
+         << it.index() << endl;
   }
   cout << "number of nnzs: " << sparse_table_.nonZeros() << endl;
 }
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 07564892f..ee6f456db 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -80,12 +80,16 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return DiscreteKey(keys_[i], cardinalities_.at(keys_[i]));
   }
 
-  /// Convert probability table given as doubles to SparseVector.
-  /// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
-  static Eigen::SparseVector<double> Convert(const std::vector<double>& table);
+  /**
+   * Convert probability table given as doubles to SparseVector.
+   * Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
+   */
+  static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
+                                             const std::vector<double>& table);
 
   /// Convert probability table given as string to SparseVector.
-  static Eigen::SparseVector<double> Convert(const std::string& table);
+  static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
+                                             const std::string& table);
 
  public:
   // typedefs needed to play nice with gtsam
@@ -111,11 +115,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
 
   /** Constructor from doubles */
   TableFactor(const DiscreteKeys& keys, const std::vector<double>& table)
-      : TableFactor(keys, Convert(table)) {}
+      : TableFactor(keys, Convert(keys, table)) {}
 
   /** Constructor from string */
   TableFactor(const DiscreteKeys& keys, const std::string& table)
-      : TableFactor(keys, Convert(table)) {}
+      : TableFactor(keys, Convert(keys, table)) {}
 
   /// Single-key specialization
   template <class SOURCE>

From b5cd82d0b4d81ec33586f55a944be3428cdc3da3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 Dec 2024 15:01:30 -0500
Subject: [PATCH 105/362] helper constructor that doesn't take separate keys

---
 gtsam/discrete/TableFactor.cpp | 5 +++++
 gtsam/discrete/TableFactor.h   | 1 +
 2 files changed, 6 insertions(+)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index ea51fa550..486641e3f 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -92,6 +92,11 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
                          const DecisionTreeFactor& dtf)
     : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
 
+/* ************************************************************************ */
+TableFactor::TableFactor(const DecisionTreeFactor& dtf)
+    : TableFactor(dtf.discreteKeys(),
+                  ComputeLeafOrdering(dtf.discreteKeys(), dtf)) {}
+
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteConditional& c)
     : TableFactor(c.discreteKeys(), c) {}
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index ee6f456db..d27c4740c 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -132,6 +132,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
 
   /// Constructor from DecisionTreeFactor
   TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf);
+  TableFactor(const DecisionTreeFactor& dtf);
 
   /// Constructor from DecisionTree<Key, double>/AlgebraicDecisionTree
   TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& dtree);

From ab3f48bbe9b2da13ebc048350bc8273219d4ac88 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 Dec 2024 15:36:25 -0500
Subject: [PATCH 106/362] fix equality to check for matching keys

---
 gtsam/discrete/DecisionTreeFactor.cpp    |  2 +-
 gtsam/discrete/DiscreteFactor.cpp        |  5 +++++
 gtsam/discrete/DiscreteFactor.h          |  2 +-
 gtsam/discrete/TableFactor.cpp           |  3 ++-
 gtsam/discrete/tests/testTableFactor.cpp | 13 ++++++++-----
 5 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index f68d2ae00..1ac782b88 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -48,7 +48,7 @@ namespace gtsam {
       return false;
     } else {
       const auto& f(static_cast<const DecisionTreeFactor&>(other));
-      return ADT::equals(f, tol);
+      return Base::equals(other, tol) && ADT::equals(f, tol);
     }
   }
 
diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp
index 2b11046f4..68cc1df7d 100644
--- a/gtsam/discrete/DiscreteFactor.cpp
+++ b/gtsam/discrete/DiscreteFactor.cpp
@@ -28,6 +28,11 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const {
+  return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_;
+}
+
 /* ************************************************************************ */
 DiscreteKeys DiscreteFactor::discreteKeys() const {
   DiscreteKeys result;
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index fa1179c39..29981e94b 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -77,7 +77,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// @{
 
   /// equals
-  virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
+  virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const;
 
   /// print
   void print(
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 486641e3f..8e185eb3b 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -144,7 +144,8 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
     return false;
   } else {
     const auto& f(static_cast<const TableFactor&>(other));
-    return sparse_table_.isApprox(f.sparse_table_, tol);
+    return Base::equals(other, tol) &&
+           sparse_table_.isApprox(f.sparse_table_, tol);
   }
 }
 
diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 0f7d7a615..35dcaf093 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -134,14 +134,17 @@ TEST(TableFactor, constructors) {
   EXPECT(assert_equal(expected, f4));
 
   // Test for 9=3x3 values.
-  DiscreteKey V(0, 3), W(1, 3);
+  DiscreteKey V(0, 3), W(1, 3), O(100, 3);
   DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
   TableFactor f5(conditional5);
-  // GTSAM_PRINT(f5);
-  TableFactor expected_f5(
-      X & Y,
-      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667");
+
+  std::string expected_values =
+      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667";
+  TableFactor expected_f5(V & W, expected_values);
   EXPECT(assert_equal(expected_f5, f5, 1e-6));
+
+  TableFactor f5_with_wrong_keys(V & O, expected_values);
+  EXPECT(!assert_equal(f5_with_wrong_keys, f5, 1e-9));
 }
 
 /* ************************************************************************* */

From ed742d3cb76dd10d8b2c3e700f86dde165f37221 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 Dec 2024 16:26:31 -0500
Subject: [PATCH 107/362] fix assertion

---
 gtsam/discrete/tests/testTableFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 35dcaf093..212067cb3 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -144,7 +144,7 @@ TEST(TableFactor, constructors) {
   EXPECT(assert_equal(expected_f5, f5, 1e-6));
 
   TableFactor f5_with_wrong_keys(V & O, expected_values);
-  EXPECT(!assert_equal(f5_with_wrong_keys, f5, 1e-9));
+  EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9));
 }
 
 /* ************************************************************************* */

From d8e412555885925b32d6f3f3f189b53c5f3a1547 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Sun, 1 Dec 2024 12:47:56 -0500
Subject: [PATCH 108/362] Change the deprecated C++ enum math to constexprs

---
 gtsam/base/ProductLieGroup.h  | 4 ++--
 gtsam/geometry/BearingRange.h | 6 +++---
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h
index beb376c19..085da712a 100644
--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair<G, H> {
   typedef std::pair<G, H> Base;
 
 protected:
-  enum {dimension1 = traits<G>::dimension};
-  enum {dimension2 = traits<H>::dimension};
+  constexpr static const size_t dimension1 = traits<G>::dimension;
+  constexpr static const size_t dimension2 = traits<H>::dimension;
 
 public:
   /// Default constructor yields identity
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 19194bb7a..9b8fe1f87 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -55,9 +55,9 @@ private:
   R range_;
 
 public:
-  enum { dimB = traits<B>::dimension };
-  enum { dimR = traits<R>::dimension };
-  enum { dimension = dimB + dimR };
+  constexpr static const size_t dimB = traits<B>::dimension;
+  constexpr static const size_t dimR = traits<R>::dimension;
+  constexpr static const size_t dimension = dimB + dimR;
 
   /// @name Standard Constructors
   /// @{

From 456df093f110af74b6ee6e85ba6e44ced009a680 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Mon, 2 Dec 2024 14:11:51 -0500
Subject: [PATCH 109/362] Change all occurances of unnamed enum (deprecated in
 C++20) to `constexpr`

---
 GTSAM-Concepts.md                        |  2 +-
 gtsam/base/GenericValue.h                |  3 +--
 gtsam/base/Lie.h                         |  4 ++--
 gtsam/base/Manifold.h                    |  4 ++--
 gtsam/base/Matrix.h                      |  2 +-
 gtsam/base/ProductLieGroup.h             |  6 +++---
 gtsam/base/VectorSpace.h                 | 10 +++++-----
 gtsam/basis/Basis.h                      |  2 +-
 gtsam/geometry/BearingRange.h            |  4 +---
 gtsam/geometry/Cal3.h                    |  2 +-
 gtsam/geometry/Cal3Bundler.h             |  2 +-
 gtsam/geometry/Cal3DS2.h                 |  2 +-
 gtsam/geometry/Cal3DS2_Base.h            |  2 +-
 gtsam/geometry/Cal3Fisheye.h             |  2 +-
 gtsam/geometry/Cal3Unified.h             |  2 +-
 gtsam/geometry/Cal3_S2.h                 |  2 +-
 gtsam/geometry/Cal3_S2Stereo.h           |  2 +-
 gtsam/geometry/Cal3f.h                   |  2 +-
 gtsam/geometry/CalibratedCamera.h        |  4 +---
 gtsam/geometry/EssentialMatrix.h         |  2 +-
 gtsam/geometry/FundamentalMatrix.h       |  4 ++--
 gtsam/geometry/Line3.h                   |  2 +-
 gtsam/geometry/OrientedPlane3.h          |  4 +---
 gtsam/geometry/PinholeCamera.h           |  4 +---
 gtsam/geometry/PinholePose.h             |  4 +---
 gtsam/geometry/Quaternion.h              |  4 +---
 gtsam/geometry/SOn.h                     |  2 +-
 gtsam/geometry/SphericalCamera.h         |  4 ++--
 gtsam/geometry/StereoCamera.h            |  4 +---
 gtsam/geometry/StereoPoint2.h            |  2 +-
 gtsam/geometry/Unit3.h                   |  4 +---
 gtsam/navigation/NavState.h              |  4 +---
 gtsam/nonlinear/ExpressionFactor.h       |  6 +++---
 gtsam/nonlinear/NonlinearFactor.h        |  2 +-
 gtsam/nonlinear/PriorFactor.h            |  2 +-
 gtsam/nonlinear/tests/testExpression.cpp |  2 +-
 gtsam/nonlinear/tests/testValues.cpp     |  2 +-
 gtsam/slam/BetweenFactor.h               |  8 ++++----
 gtsam/slam/FrobeniusFactor.h             |  6 +++---
 gtsam/slam/KarcherMeanFactor.h           |  2 +-
 gtsam_unstable/geometry/Event.h          |  2 +-
 41 files changed, 58 insertions(+), 77 deletions(-)

diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md
index 9911b3764..733672fbe 100644
--- a/GTSAM-Concepts.md
+++ b/GTSAM-Concepts.md
@@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr
 In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
 
 * values:
-   * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
+   * `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
 * types:
     * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
     * `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index bb92b6b2e..87ce7a73d 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -184,9 +184,8 @@ public:
 	}
 #endif
 
-
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  constexpr static const bool NeedsToAlign = (sizeof(T) % 16) == 0;
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index ce021e10e..862ae6f4d 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -36,7 +36,7 @@ namespace gtsam {
 template <class Class, int N>
 struct LieGroup {
 
-  enum { dimension = N };
+  inline constexpr static auto dimension = N;
   typedef OptionalJacobian<N, N> ChartJacobian;
   typedef Eigen::Matrix<double, N, N> Jacobian;
   typedef Eigen::Matrix<double, N, 1> TangentVector;
@@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
   /// @name Manifold
   /// @{
   typedef Class ManifoldType;
-  enum { dimension = Class::dimension };
+  inline constexpr static auto dimension = Class::dimension;
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index cb30fa9c0..922b8dd2f 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -55,7 +55,7 @@ namespace internal {
 template<class Class>
 struct HasManifoldPrereqs {
 
-  enum { dim = Class::dimension };
+  inline constexpr static auto dim = Class::dimension;
 
   Class p, q;
   Eigen::Matrix<double, dim, 1> v;
@@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
   GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
 
   // Dimension of the manifold
-  enum { dimension = Class::dimension };
+  inline constexpr static auto dimension = Class::dimension;
 
   // Typedefs required by all manifold types.
   typedef Class ManifoldType;
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 6dba9cb05..c8dc46ed5 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -479,7 +479,7 @@ struct MultiplyWithInverse {
  */
 template <typename T, int N>
 struct MultiplyWithInverseFunction {
-  enum { M = traits<T>::dimension };
+  inline constexpr static auto M = traits<T>::dimension;
   typedef Eigen::Matrix<double, N, 1> VectorN;
   typedef Eigen::Matrix<double, N, N> MatrixN;
 
diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h
index 085da712a..bb7ba72b8 100644
--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@@ -67,9 +67,9 @@ public:
 
   /// @name Manifold
   /// @{
-  enum {dimension = dimension1 + dimension2};
-  inline static size_t Dim() {return dimension;}
-  inline size_t dim() const {return dimension;}
+  inline constexpr static auto dimension = dimension1 + dimension2;
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
 
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h
index a9e9ca106..c5c4ad622 100644
--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@@ -163,7 +163,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
 template<class Class>
 struct HasVectorSpacePrereqs {
 
-  enum { dim = Class::dimension };
+  inline constexpr static auto dim = Class::dimension;
 
   Class p, q;
   Vector v;
@@ -197,7 +197,7 @@ GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
 
   /// @name Manifold
   /// @{
-  enum { dimension = Class::dimension};
+  inline constexpr static auto dimension = Class::dimension;
   typedef Class ManifoldType;
   /// @}
 };
@@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
   /// @name Manifold
   /// @{
   typedef Scalar ManifoldType;
-  enum { dimension = 1 };
+  inline constexpr static auto dimension = 1;
   typedef Eigen::Matrix<double, 1, 1> TangentVector;
   typedef OptionalJacobian<1, 1> ChartJacobian;
 
@@ -305,7 +305,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
 
   /// @name Manifold
   /// @{
-  enum { dimension = M*N};
+  inline constexpr static auto dimension = M * N;
   typedef Fixed ManifoldType;
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
@@ -377,7 +377,7 @@ struct DynamicTraits {
 
   /// @name Manifold
   /// @{
-  enum { dimension = Eigen::Dynamic };
+  inline constexpr static auto dimension = Eigen::Dynamic;
   typedef Eigen::VectorXd TangentVector;
   typedef Eigen::MatrixXd Jacobian;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h
index 1b1c44acc..960440daf 100644
--- a/gtsam/basis/Basis.h
+++ b/gtsam/basis/Basis.h
@@ -291,7 +291,7 @@ class Basis {
    */
   template <class T>
   class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
-    enum { M = traits<T>::dimension };
+    inline constexpr static auto M = traits<T>::dimension;
     using Base = VectorEvaluationFunctor;
 
    public:
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 9b8fe1f87..07229dfca 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -162,9 +162,7 @@ private:
   /// @}
 
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-  enum {
-    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
-  };
+  inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0;
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 1b66eb336..5eecee845 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 {
   double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
 
  public:
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
   ///< shared pointer to calibration object
   using shared_ptr = std::shared_ptr<Cal3>;
 
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 081688d48..01eb9437a 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f {
   // Note: u0 and v0 are constants and not optimized.
 
  public:
-  enum { dimension = 3 };
+  inline constexpr static auto dimension = 3;
   using shared_ptr = std::shared_ptr<Cal3Bundler>;
 
   /// @name Constructors
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index a7ca08afc..0ecfc24ae 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
   using Base = Cal3DS2_Base;
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3DS2>;
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index b0ef0368b..6eff04c10 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3DS2_Base>;
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 3f1cac148..4c542c5be 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
   ///< shared pointer to fisheye calibration object
   using shared_ptr = std::shared_ptr<Cal3Fisheye>;
 
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 309b6dca1..7eb4d2edb 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   double xi_ = 0.0f;  ///< mirror parameter
 
  public:
-  enum { dimension = 10 };
+  inline constexpr static auto dimension = 10;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3Unified>;
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 885be614f..568cde41e 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -33,7 +33,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3_S2 : public Cal3 {
  public:
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
 
   ///< shared pointer to calibration object
   using shared_ptr = std::shared_ptr<Cal3_S2>;
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 4fee1a022..292519cfd 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   double b_ = 1.0f;  ///< Stereo baseline.
 
  public:
-  enum { dimension = 6 };
+  inline constexpr static auto dimension = 6;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3_S2Stereo>;
diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h
index f053f3d11..6c3c214b8 100644
--- a/gtsam/geometry/Cal3f.h
+++ b/gtsam/geometry/Cal3f.h
@@ -34,7 +34,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3f : public Cal3 {
  public:
-  enum { dimension = 1 };
+  inline constexpr static auto dimension = 1;
   using shared_ptr = std::shared_ptr<Cal3f>;
 
   /// @name Constructors
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index dca15feb2..d3d1e1b9a 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -252,9 +252,7 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
 
 public:
 
-  enum {
-    dimension = 6
-  };
+  inline constexpr static auto dimension = 6;
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index d514c4f19..3986f3063 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -82,7 +82,7 @@ class EssentialMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
   inline static size_t Dim() { return dimension;}
   inline size_t dim() const { return dimension;}
 
diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h
index df2c2881a..0752ced7d 100644
--- a/gtsam/geometry/FundamentalMatrix.h
+++ b/gtsam/geometry/FundamentalMatrix.h
@@ -102,7 +102,7 @@ class GTSAM_EXPORT FundamentalMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 7 };  // 3 for U, 1 for s, 3 for V
+  inline constexpr static auto dimension = 7;  // 3 for U, 1 for s, 3 for V
   inline static size_t Dim() { return dimension; }
   inline size_t dim() const { return dimension; }
 
@@ -179,7 +179,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 7 };  // 5 for E, 1 for fa, 1 for fb
+  inline constexpr static auto dimension = 7;  // 5 for E, 1 for fa, 1 for fb
   inline static size_t Dim() { return dimension; }
   inline size_t dim() const { return dimension; }
 
diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h
index def49d268..5ae50e138 100644
--- a/gtsam/geometry/Line3.h
+++ b/gtsam/geometry/Line3.h
@@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 {
   double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
   // Also the closest point on line to origin
  public:
-  enum { dimension = 4 };
+  inline constexpr static auto dimension = 4;
 
   /** Default constructor is the Z axis **/
   Line3() :
diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h
index 07c8445fe..3d84086ac 100644
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@@ -39,9 +39,7 @@ private:
   double d_;    ///< The perpendicular distance to this plane
 
 public:
-  enum {
-    dimension = 3
-  };
+  inline constexpr static auto dimension = 3;
 
   /// @name Constructors
   /// @{
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 8329664fd..0439b2fde 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -51,9 +51,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 6 + DimK
-  }; ///< Dimension depends on calibration
+  inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index df6ec5c08..f1191dbcc 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -245,9 +245,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 6
-  }; ///< There are 6 DOF to optimize for
+  inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h
index f9ed2a383..36f891505 100644
--- a/gtsam/geometry/Quaternion.h
+++ b/gtsam/geometry/Quaternion.h
@@ -45,9 +45,7 @@ struct traits<QUATERNION_TYPE> {
   /// @}
   /// @name Basic manifold traits
   /// @{
-  enum {
-    dimension = 3
-  };
+  inline constexpr static auto dimension = 3;
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
 
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index 75ff872a8..b0da3b213 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -54,7 +54,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
 template <int N>
 class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
  public:
-  enum { dimension = internal::DimensionSO(N) };
+  inline constexpr static auto dimension = internal::DimensionSO(N);
   using MatrixNN = Eigen::Matrix<double, N, N>;
   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
   using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h
index ef20aa7fe..1ff87cec3 100644
--- a/gtsam/geometry/SphericalCamera.h
+++ b/gtsam/geometry/SphericalCamera.h
@@ -41,7 +41,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT EmptyCal {
  public:
-  enum { dimension = 0 };
+  inline constexpr static auto dimension = 0;
   EmptyCal() {}
   virtual ~EmptyCal() = default;
   using shared_ptr = std::shared_ptr<EmptyCal>;
@@ -73,7 +73,7 @@ class GTSAM_EXPORT EmptyCal {
  */
 class GTSAM_EXPORT SphericalCamera {
  public:
-  enum { dimension = 6 };
+  inline constexpr static auto dimension = 6;
 
   using Measurement = Unit3;
   using MeasurementVector = std::vector<Unit3>;
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index da71dd070..026125572 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -61,9 +61,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 6
-  };
+  inline constexpr static auto dimension = 6;
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 4383e212e..7a1910e42 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -37,7 +37,7 @@ private:
   double uL_, uR_, v_;
 
 public:
-  enum { dimension = 3 };
+  inline constexpr static auto dimension = 3;
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 18bc5d9f0..a7304280f 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -53,9 +53,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 2
-  };
+  inline constexpr static auto dimension = 2;
 
   /// @name Constructors
   /// @{
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index f778a7123..35ce435a3 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -42,9 +42,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 9
-  };
+  inline constexpr static auto dimension = 9;
 
   typedef std::pair<Point3, Velocity3> PositionAndVelocity;
 
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index b3e34d079..d9af1933c 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -224,9 +224,9 @@ private:
 #endif
 
  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
- enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
-  public:
-	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+ inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0;
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 // ExpressionFactor
 
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 2d3a13766..101743b78 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -434,7 +434,7 @@ class NoiseModelFactorN
       public detail::NoiseModelFactorAliases<ValueTypes...> {
  public:
   /// N is the number of variables (N-way factor)
-  enum { N = sizeof...(ValueTypes) };
+  inline constexpr static auto N = sizeof...(ValueTypes);
 
   using NoiseModelFactor::unwhitenedError;
 
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index ab2a66a77..159373459 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -118,7 +118,7 @@ namespace gtsam {
 #endif
 
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0;
   public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 6f5fc53f5..67815e262 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -115,7 +115,7 @@ TEST(Expression, Unary3) {
 // Simple test class that implements the `VectorSpace` protocol.
 class Class : public Point3 {
  public:
-  enum {dimension = 3};
+  inline constexpr static auto dimension = 3;
   using Point3::Point3;
   const Vector3& vector() const { return *this; }
   inline static Class Identity() { return Class(0,0,0); }
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index a2b265a56..06eb46d4c 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -54,7 +54,7 @@ int TestValueData::DestructorCount = 0;
 class TestValue {
   TestValueData data_;
 public:
-  enum {dimension = 0};
+  inline constexpr static auto dimension = 0;
   void print(const std::string& str = "") const {}
   bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
   size_t dim() const { return 0; }
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 6eaa5c01b..3e2478e73 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -147,10 +147,10 @@ namespace gtsam {
     }
 #endif
 
-	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
-    public:
-      GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+    // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+    inline constexpr static auto NeedsToAlign = (sizeof(VALUE) % 16) == 0;
+  public:
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits
diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h
index a000a1514..37efa5f24 100644
--- a/gtsam/slam/FrobeniusFactor.h
+++ b/gtsam/slam/FrobeniusFactor.h
@@ -49,7 +49,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
  */
 template <class Rot>
 class FrobeniusPrior : public NoiseModelFactorN<Rot> {
-  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
   using MatrixNN = typename Rot::MatrixNN;
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 
@@ -79,7 +79,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
  */
 template <class Rot>
 class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
-  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
 
  public:
 
@@ -111,7 +111,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
   Rot R12_;  ///< measured rotation between R1 and R2
   Eigen::Matrix<double, Rot::dimension, Rot::dimension>
       R2hat_H_R1_;  ///< fixed derivative of R2hat wrpt R1
-  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
 
  public:
 
diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h
index 4e82ca1d9..6b25dd261 100644
--- a/gtsam/slam/KarcherMeanFactor.h
+++ b/gtsam/slam/KarcherMeanFactor.h
@@ -45,7 +45,7 @@ template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
  * */
 template <class T> class KarcherMeanFactor : public NonlinearFactor {
   // Compile time dimension: can be -1
-  enum { D = traits<T>::dimension };
+  inline constexpr static auto D = traits<T>::dimension;
 
   // Runtime dimension: always >=0
   size_t d_;
diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h
index a3c907646..471ada35b 100644
--- a/gtsam_unstable/geometry/Event.h
+++ b/gtsam_unstable/geometry/Event.h
@@ -39,7 +39,7 @@ class GTSAM_UNSTABLE_EXPORT Event {
   Point3 location_;  ///< Location at time event was generated
 
  public:
-  enum { dimension = 4 };
+  inline constexpr static auto dimension = 4;
 
   /// Default Constructor
   Event() : time_(0), location_(0, 0, 0) {}

From b91c470b6929bffca39bfbd36c39c3795183c79e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 13 Dec 2024 08:07:10 -0500
Subject: [PATCH 110/362] test exposing incorrect conversion

---
 gtsam/discrete/tests/testTableFactor.cpp | 30 ++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 212067cb3..7df6da83e 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -147,6 +147,36 @@ TEST(TableFactor, constructors) {
   EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9));
 }
 
+/* ************************************************************************* */
+// Check conversion from DecisionTreeFactor.
+TEST(TableFactor, Conversion) {
+  /* This is the DecisionTree we are using
+  Choice(m2)
+  0 Choice(m1)
+  0 0 Leaf    0
+  0 1 Choice(m0)
+  0 1 0 Leaf    0
+  0 1 1 Leaf 0.14649446  // 3
+  1 Choice(m1)
+  1 0 Choice(m0)
+  1 0 0 Leaf    0
+  1 0 1 Leaf 0.14648756  // 5
+  1 1 Choice(m0)
+  1 1 0 Leaf 0.14649446  // 6
+  1 1 1 Leaf 0.23918345  // 7
+  */
+  DiscreteKeys dkeys = {{0, 2}, {1, 2}, {2, 2}};
+  DecisionTreeFactor dtf(
+      dkeys, std::vector<double>{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446,
+                                 0.23918345});
+
+  // dtf.print();
+  TableFactor tf(dtf.discreteKeys(), dtf);
+  // tf.print();
+  // tf.toDecisionTreeFactor().print();
+  EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor()));
+}
+
 /* ************************************************************************* */
 // Check multiplication between two TableFactors.
 TEST(TableFactor, multiplication) {

From a8e24efdeca680f391098979ad2402dd3918153f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 13 Dec 2024 09:34:01 -0500
Subject: [PATCH 111/362] update ComputeLeafOrdering to give a correct vector
 of values

---
 gtsam/discrete/TableFactor.cpp | 72 +++++++++++++++++++++++++++-------
 1 file changed, 57 insertions(+), 15 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 8e185eb3b..a2d68853e 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -64,27 +64,69 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
 /**
  * @brief Compute the correct ordering of the leaves in the decision tree.
  *
- * This is done by first taking all the values which have modulo 0 value with
- * the cardinality of the innermost key `n`, and we go up to modulo n.
- *
  * @param dt The DecisionTree
- * @return std::vector<double>
+ * @return Eigen::SparseVector<double>
  */
-std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
-                                        const DecisionTreeFactor& dt) {
-  std::vector<double> probs = dt.probabilities();
-  std::vector<double> ordered;
+static Eigen::SparseVector<double> ComputeLeafOrdering(
+    const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) {
+  // SparseVector needs to know the maximum possible index,
+  // so we compute the product of cardinalities.
+  size_t prod_cardinality = 1;
+  for (auto&& [_, c] : dt.cardinalities()) {
+    prod_cardinality *= c;
+  }
+  Eigen::SparseVector<double> sparse_table(prod_cardinality);
+  size_t nrValues = 0;
+  dt.visit([&nrValues](double x) {
+    if (x > 0) nrValues += 1;
+  });
+  sparse_table.reserve(nrValues);
 
-  size_t n = dkeys[0].second;
+  std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
 
-  for (size_t k = 0; k < n; ++k) {
-    for (size_t idx = 0; idx < probs.size(); ++idx) {
-      if (idx % n == k) {
-        ordered.push_back(probs[idx]);
+  auto op = [&](const Assignment<Key>& assignment, double p) {
+    if (p > 0) {
+      // Get all the keys involved in this assignment
+      std::set<Key> assignment_keys;
+      for (auto&& [k, _] : assignment) {
+        assignment_keys.insert(k);
+      }
+
+      // Find the keys missing in the assignment
+      std::vector<Key> diff;
+      std::set_difference(allKeys.begin(), allKeys.end(),
+                          assignment_keys.begin(), assignment_keys.end(),
+                          std::back_inserter(diff));
+
+      // Generate all assignments using the missing keys
+      DiscreteKeys extras;
+      for (auto&& key : diff) {
+        extras.push_back({key, dt.cardinality(key)});
+      }
+      auto&& extra_assignments = DiscreteValues::CartesianProduct(extras);
+
+      for (auto&& extra : extra_assignments) {
+        // Create new assignment using the extra assignment
+        DiscreteValues updated_assignment(assignment);
+        updated_assignment.insert(extra);
+
+        // Generate index and add to the sparse vector.
+        Eigen::Index idx = 0;
+        size_t prev_cardinality = 1;
+        // We go in reverse since a DecisionTree has the highest label first
+        for (auto&& it = updated_assignment.rbegin();
+             it != updated_assignment.rend(); it++) {
+          idx += prev_cardinality * it->second;
+          prev_cardinality *= dt.cardinality(it->first);
+        }
+        sparse_table.coeffRef(idx) = p;
       }
     }
-  }
-  return ordered;
+  };
+
+  dt.visitWith(op);
+
+  return sparse_table;
 }
 
 /* ************************************************************************ */

From 7d389a53007af3811a0c8195d368e8fcd3825c3f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 13 Dec 2024 09:46:55 -0500
Subject: [PATCH 112/362] clean up test

---
 gtsam/discrete/tests/testTableFactor.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 7df6da83e..a455faaaa 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -170,10 +170,8 @@ TEST(TableFactor, Conversion) {
       dkeys, std::vector<double>{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446,
                                  0.23918345});
 
-  // dtf.print();
   TableFactor tf(dtf.discreteKeys(), dtf);
-  // tf.print();
-  // tf.toDecisionTreeFactor().print();
+
   EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor()));
 }
 

From 9830981351fd5e5b714c6a5dc58b6e67ba8ec7cc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 13 Dec 2024 13:58:19 -0500
Subject: [PATCH 113/362] address review comments

---
 gtsam/discrete/TableFactor.cpp | 61 ++++++++++++++++++++++------------
 1 file changed, 39 insertions(+), 22 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index a2d68853e..de1e1f867 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -62,40 +62,55 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
     : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
 
 /**
- * @brief Compute the correct ordering of the leaves in the decision tree.
+ * @brief Compute the indexing of the leaves in the decision tree based on the
+ * assignment and add the (index, leaf) pair to a SparseVector.
+ *
+ * We visit each leaf in the tree, and using the cardinalities of the keys,
+ * compute the correct index to add the leaf to a SparseVector which
+ *  is then used to create the TableFactor.
  *
  * @param dt The DecisionTree
  * @return Eigen::SparseVector<double>
  */
-static Eigen::SparseVector<double> ComputeLeafOrdering(
+static Eigen::SparseVector<double> ComputeSparseTable(
     const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) {
   // SparseVector needs to know the maximum possible index,
   // so we compute the product of cardinalities.
-  size_t prod_cardinality = 1;
+  size_t cardinalityProduct = 1;
   for (auto&& [_, c] : dt.cardinalities()) {
-    prod_cardinality *= c;
+    cardinalityProduct *= c;
   }
-  Eigen::SparseVector<double> sparse_table(prod_cardinality);
+  Eigen::SparseVector<double> sparseTable(cardinalityProduct);
   size_t nrValues = 0;
   dt.visit([&nrValues](double x) {
     if (x > 0) nrValues += 1;
   });
-  sparse_table.reserve(nrValues);
+  sparseTable.reserve(nrValues);
 
   std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
 
+  /**
+   * @brief Functor which is called by the DecisionTree for each leaf.
+   * For each leaf value, we use the corresponding assignment to compute a
+   * corresponding index into a SparseVector. We then populate sparseTable with
+   * the value at the computed index.
+   *
+   * Takes advantage of the sparsity of the DecisionTree to be efficient. When
+   * merged branches are encountered, we enumerate over the missing keys.
+   *
+   */
   auto op = [&](const Assignment<Key>& assignment, double p) {
     if (p > 0) {
       // Get all the keys involved in this assignment
-      std::set<Key> assignment_keys;
+      std::set<Key> assignmentKeys;
       for (auto&& [k, _] : assignment) {
-        assignment_keys.insert(k);
+        assignmentKeys.insert(k);
       }
 
       // Find the keys missing in the assignment
       std::vector<Key> diff;
       std::set_difference(allKeys.begin(), allKeys.end(),
-                          assignment_keys.begin(), assignment_keys.end(),
+                          assignmentKeys.begin(), assignmentKeys.end(),
                           std::back_inserter(diff));
 
       // Generate all assignments using the missing keys
@@ -103,41 +118,43 @@ static Eigen::SparseVector<double> ComputeLeafOrdering(
       for (auto&& key : diff) {
         extras.push_back({key, dt.cardinality(key)});
       }
-      auto&& extra_assignments = DiscreteValues::CartesianProduct(extras);
+      auto&& extraAssignments = DiscreteValues::CartesianProduct(extras);
 
-      for (auto&& extra : extra_assignments) {
+      for (auto&& extra : extraAssignments) {
         // Create new assignment using the extra assignment
-        DiscreteValues updated_assignment(assignment);
-        updated_assignment.insert(extra);
+        DiscreteValues updatedAssignment(assignment);
+        updatedAssignment.insert(extra);
 
         // Generate index and add to the sparse vector.
         Eigen::Index idx = 0;
-        size_t prev_cardinality = 1;
+        size_t previousCardinality = 1;
         // We go in reverse since a DecisionTree has the highest label first
-        for (auto&& it = updated_assignment.rbegin();
-             it != updated_assignment.rend(); it++) {
-          idx += prev_cardinality * it->second;
-          prev_cardinality *= dt.cardinality(it->first);
+        for (auto&& it = updatedAssignment.rbegin();
+             it != updatedAssignment.rend(); it++) {
+          idx += previousCardinality * it->second;
+          previousCardinality *= dt.cardinality(it->first);
         }
-        sparse_table.coeffRef(idx) = p;
+        sparseTable.coeffRef(idx) = p;
       }
     }
   };
 
+  // Visit each leaf in `dt` to get the Assignment and leaf value
+  // to populate the sparseTable.
   dt.visitWith(op);
 
-  return sparse_table;
+  return sparseTable;
 }
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteKeys& dkeys,
                          const DecisionTreeFactor& dtf)
-    : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
+    : TableFactor(dkeys, ComputeSparseTable(dkeys, dtf)) {}
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DecisionTreeFactor& dtf)
     : TableFactor(dtf.discreteKeys(),
-                  ComputeLeafOrdering(dtf.discreteKeys(), dtf)) {}
+                  ComputeSparseTable(dtf.discreteKeys(), dtf)) {}
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteConditional& c)

From 923afba4d81c083a750be9b9f355310012590b1a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 13 Dec 2024 23:36:27 -0500
Subject: [PATCH 114/362] Endow NavState with group operations

---
 gtsam/navigation/NavState.cpp           | 255 +++++++++++++-
 gtsam/navigation/NavState.h             | 126 ++++++-
 gtsam/navigation/tests/testNavState.cpp | 443 +++++++++++++++++++++++-
 3 files changed, 779 insertions(+), 45 deletions(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 3e2817752..9f91ae415 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
 }
 
 //------------------------------------------------------------------------------
-Matrix7 NavState::matrix() const {
+Matrix5 NavState::matrix() const {
   Matrix3 R = this->R();
-  Matrix7 T;
-  T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
+
+  Matrix5 T = Matrix5::Identity();
+  T.block<3, 3>(0, 0) = R;
+  T.block<3, 1>(0, 3) = t_;
+  T.block<3, 1>(0, 4) = v_;
   return T;
 }
 
@@ -103,9 +106,215 @@ bool NavState::equals(const NavState& other, double tol) const {
       && equal_with_abs_tol(v_, other.v_, tol);
 }
 
+//------------------------------------------------------------------------------
+NavState NavState::inverse() const {
+  Rot3 Rt = R_.inverse();
+  return NavState(Rt, Rt * (-t_), Rt * -(v_));
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
+  if (Hxi) *Hxi = ExpmapDerivative(xi);
+
+  // Order is rotation, position, velocity, represented by ω,ρ,ν
+  Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)),
+      nu(xi(6), xi(7), xi(8));
+
+  Rot3 R = Rot3::Expmap(omega);
+
+  double omega_norm = omega.norm();
+
+  if (omega_norm < 1e-8) {
+    return NavState(Rot3(), Point3(rho), Point3(nu));
+
+  } else {
+    Matrix W = skewSymmetric(omega);
+    double omega_norm2 = omega_norm * omega_norm;
+    double omega_norm3 = omega_norm2 * omega_norm;
+    Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W +
+               ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W);
+
+    return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu));
+  }
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) {
+  if (Hstate) *Hstate = LogmapDerivative(state);
+
+  const Vector3 phi = Rot3::Logmap(state.rotation());
+  const Vector3& p = state.position();
+  const Vector3& v = state.velocity();
+  const double t = phi.norm();
+  if (t < 1e-8) {
+    Vector9 log;
+    log << phi, p, v;
+    return log;
+
+  } else {
+    const Matrix3 W = skewSymmetric(phi / t);
+
+    const double Tan = tan(0.5 * t);
+    const Vector3 Wp = W * p;
+    const Vector3 Wv = W * v;
+    const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp);
+    const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv);
+    Vector9 log;
+    // Order is ω, p, v
+    log << phi, rho, nu;
+    return log;
+  }
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::AdjointMap() const {
+  const Matrix3 R = R_.matrix();
+  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
+  Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R;
+  // Eqn 2 in Barrau20icra
+  Matrix9 adj;
+  adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
+                          OptionalJacobian<9, 9> H_xib) const {
+  const Matrix9 Ad = AdjointMap();
+
+  // Jacobians
+  if (H_state) *H_state = -Ad * adjointMap(xi_b);
+  if (H_xib) *H_xib = Ad;
+
+  return Ad * xi_b;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::adjointMap(const Vector9& xi) {
+  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
+  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
+  Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8));
+  Matrix9 adj;
+  adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
+                          OptionalJacobian<9, 9> Hxi,
+                          OptionalJacobian<9, 9> H_y) {
+  if (Hxi) {
+    Hxi->setZero();
+    for (int i = 0; i < 9; ++i) {
+      Vector9 dxi;
+      dxi.setZero();
+      dxi(i) = 1.0;
+      Matrix9 Gi = adjointMap(dxi);
+      Hxi->col(i) = Gi * y;
+    }
+  }
+
+  const Matrix9& ad_xi = adjointMap(xi);
+  if (H_y) *H_y = ad_xi;
+
+  return ad_xi * y;
+}
+
+/* ************************************************************************* */
+Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi,
+                                               double nearZeroThreshold) {
+  const auto omega = xi.head<3>();
+  const auto nu = xi.segment<3>(3);
+  const auto rho = xi.tail<3>();
+  const Matrix3 V = skewSymmetric(nu);
+  const Matrix3 P = skewSymmetric(rho);
+  const Matrix3 W = skewSymmetric(omega);
+
+  Matrix3 Qv, Qp;
+  Matrix63 Q;
+
+  // The closed-form formula in Barfoot14tro eq. (102)
+  double phi = omega.norm();
+  if (std::abs(phi) > 1e-5) {
+    const double sinPhi = sin(phi), cosPhi = cos(phi);
+    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
+                 phi5 = phi4 * phi;
+    // Invert the sign of odd-order terms to have the right Jacobian
+    Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) +
+         (1 - phi2 / 2 - cosPhi) / phi4 *
+             (W * W * V + V * W * W - 3 * W * V * W) -
+         0.5 *
+             ((1 - phi2 / 2 - cosPhi) / phi4 -
+              3 * (phi - sinPhi - phi3 / 6.) / phi5) *
+             (W * V * W * W + W * W * V * W);
+    Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) +
+         (1 - phi2 / 2 - cosPhi) / phi4 *
+             (W * W * P + P * W * W - 3 * W * P * W) -
+         0.5 *
+             ((1 - phi2 / 2 - cosPhi) / phi4 -
+              3 * (phi - sinPhi - phi3 / 6.) / phi5) *
+             (W * P * W * W + W * W * P * W);
+  } else {
+    Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) +
+         1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) -
+         0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W);
+    Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) +
+         1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) -
+         0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W);
+  }
+
+  Q << Qv, Qp;
+  return Q;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
+  const Vector3 w = xi.head<3>();
+  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
+  const Matrix63 Q = ComputeQforExpmapDerivative(xi);
+  const Matrix3 Qv = Q.topRows<3>();
+  const Matrix3 Qp = Q.bottomRows<3>();
+
+  Matrix9 J;
+  J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw;
+
+  return J;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::LogmapDerivative(const NavState& state) {
+  const Vector9 xi = Logmap(state);
+  const Vector3 w = xi.head<3>();
+  const Matrix3 Jw = Rot3::LogmapDerivative(w);
+  const Matrix63 Q = ComputeQforExpmapDerivative(xi);
+  const Matrix3 Qv = Q.topRows<3>();
+  const Matrix3 Qp = Q.bottomRows<3>();
+  const Matrix3 Qv2 = -Jw * Qv * Jw;
+  const Matrix3 Qp2 = -Jw * Qp * Jw;
+
+  Matrix9 J;
+  J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw;
+  return J;
+}
+
+
+//------------------------------------------------------------------------------
+NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
+                                          ChartJacobian Hxi) {
+  return Expmap(xi, Hxi);
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::ChartAtOrigin::Local(const NavState& state,
+                                       ChartJacobian Hstate) {
+  return Logmap(state, Hstate);
+}
+
 //------------------------------------------------------------------------------
 NavState NavState::retract(const Vector9& xi, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
+  // return LieGroup<NavState, 9>::retract(xi, H1, H2);
+
   Rot3 nRb = R_;
   Point3 n_t = t_, n_v = v_;
   Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
@@ -133,6 +342,30 @@ NavState NavState::retract(const Vector9& xi, //
 //------------------------------------------------------------------------------
 Vector9 NavState::localCoordinates(const NavState& g, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
+  // return LieGroup<NavState, 9>::localCoordinates(g, H1, H2);
+
+  //TODO(Varun) Fix so that test on L680 passes
+
+  // Matrix3 D_dR_R, D_dt_R, D_dv_R;
+  // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
+  // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
+  // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
+
+  // Vector9 xi;
+  // Matrix3 D_xi_R;
+  // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
+  // if (H1) {
+  //   *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
+  //   D_dt_R, -I_3x3, Z_3x3, //
+  //   D_dv_R, Z_3x3, -I_3x3;
+  // }
+  // if (H2) {
+  //   *H2 << D_xi_R, Z_3x3, Z_3x3, //
+  //   Z_3x3, dR.matrix(), Z_3x3, //
+  //   Z_3x3, Z_3x3, dR.matrix();
+  // }
+  // return xi;
+
   Matrix3 D_dR_R, D_dt_R, D_dv_R;
   const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
   const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
@@ -142,15 +375,16 @@ Vector9 NavState::localCoordinates(const NavState& g, //
   Matrix3 D_xi_R;
   xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
   if (H1) {
-    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
-    D_dt_R, -I_3x3, Z_3x3, //
-    D_dv_R, Z_3x3, -I_3x3;
+    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3,  //
+        D_dt_R, -I_3x3, Z_3x3,             //
+        D_dv_R, Z_3x3, -I_3x3;
   }
   if (H2) {
-    *H2 << D_xi_R, Z_3x3, Z_3x3, //
-    Z_3x3, dR.matrix(), Z_3x3, //
-    Z_3x3, Z_3x3, dR.matrix();
+    *H2 << D_xi_R, Z_3x3, Z_3x3,    //
+        Z_3x3, dR.matrix(), Z_3x3,  //
+        Z_3x3, Z_3x3, dR.matrix();
   }
+
   return xi;
 }
 
@@ -213,7 +447,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
 //------------------------------------------------------------------------------
 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
     OptionalJacobian<9, 9> H) const {
-  auto [nRb, n_t, n_v] = (*this);
+  Rot3 nRb = R_;
+  Point3 n_t = t_, n_v = v_;
 
   const double dt2 = dt * dt;
   const Vector3 omega_cross_vel = omega.cross(n_v);
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index 35ce435a3..2f037a795 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -12,7 +12,7 @@
 /**
  * @file    NavState.h
  * @brief   Navigation state composing of attitude, position, and velocity
- * @author  Frank Dellaert
+ * @authors Frank Dellaert, Varun Agrawal, Fan Jiang
  * @date    July 2015
  **/
 
@@ -25,14 +25,18 @@
 namespace gtsam {
 
 /// Velocity is currently typedef'd to Vector3
-typedef Vector3 Velocity3;
+using Velocity3 = Vector3;
 
 /**
  * Navigation state: Pose (rotation, translation) + velocity
- * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
+ * Following Barrau20icra, this class belongs to the Lie group SE_2(3).
+ * This group is also called "double direct isometries”.
+ *
+ * NOTE: While Barrau20icra follow a R,v,t order,
+ * we use a R,t,v order to maintain backwards compatibility.
  */
-class GTSAM_EXPORT NavState {
-private:
+class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
+ private:
 
   // TODO(frank):
   // - should we rename t_ to p_? if not, we should rename dP do dT
@@ -44,7 +48,6 @@ public:
 
   inline constexpr static auto dimension = 9;
 
-  typedef std::pair<Point3, Velocity3> PositionAndVelocity;
 
   /// @name Constructors
   /// @{
@@ -67,11 +70,14 @@ public:
   }
   /// Named constructor with derivatives
   static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
-      OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
-      OptionalJacobian<9, 3> H3);
+                         OptionalJacobian<9, 3> H1 = {},
+                         OptionalJacobian<9, 3> H2 = {},
+                         OptionalJacobian<9, 3> H3 = {});
+
   /// Named constructor with derivatives
   static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
-      OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
+                                   OptionalJacobian<9, 6> H1 = {},
+                                   OptionalJacobian<9, 3> H2 = {});
 
   /// @}
   /// @name Component Access
@@ -109,9 +115,8 @@ public:
   Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;
 
   /// Return matrix group representation, in MATLAB notation:
-  /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
-  /// With this embedding in GL(3), matrix product agrees with compose
-  Matrix7 matrix() const;
+  /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1]
+  Matrix5 matrix() const;
 
   /// @}
   /// @name Testable
@@ -128,7 +133,29 @@ public:
   bool equals(const NavState& other, double tol = 1e-8) const;
 
   /// @}
-  /// @name Manifold
+  /// @name Group
+  /// @{
+
+  /// identity for group operation
+  static NavState Identity() {
+    return NavState();
+  }
+
+  /// inverse transformation with derivatives
+  NavState inverse() const;
+
+  using LieGroup<NavState, 9>::inverse;  // version with derivative
+
+  /// compose syntactic sugar
+  NavState operator*(const NavState& T) const {
+    return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_);
+  }
+
+  /// Syntactic sugar
+  const Rot3& rotation() const { return attitude(); };
+
+  /// @}
+  /// @name Lie Group
   /// @{
 
   // Tangent space sugar.
@@ -162,6 +189,66 @@ public:
       OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
           {}) const;
 
+  /**
+   * Exponential map at identity - create a NavState from canonical coordinates
+   * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$
+   */
+  static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {});
+
+  /**
+   * Log map at identity - return the canonical coordinates \f$
+   * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState
+   */
+  static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {});
+
+  /**
+   * Calculate Adjoint map, transforming a twist in this pose's (i.e, body)
+   * frame to the world spatial frame.
+   */
+  Matrix9 AdjointMap() const;
+
+  /**
+   * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
+   * body-fixed velocity, transforming it to the spatial frame
+   * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
+   * Note that H_xib = AdjointMap()
+   */
+  Vector9 Adjoint(const Vector9& xi_b,
+                  OptionalJacobian<9, 9> H_this = {},
+                  OptionalJacobian<9, 9> H_xib = {}) const;
+  
+  /**
+   * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
+   * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^]
+   */
+  static Matrix9 adjointMap(const Vector9& xi);
+
+  /**
+   * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
+   */
+  static Vector9 adjoint(const Vector9& xi, const Vector9& y,
+                         OptionalJacobian<9, 9> Hxi = {},
+                         OptionalJacobian<9, 9> H_y = {});
+
+  /// Derivative of Expmap
+  static Matrix9 ExpmapDerivative(const Vector9& xi);
+
+  /// Derivative of Logmap
+  static Matrix9 LogmapDerivative(const NavState& xi);
+
+  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
+  struct GTSAM_EXPORT ChartAtOrigin {
+    static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {});
+    static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
+  };
+
+  /**
+   * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative
+   * matrix
+   */
+  static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi,
+                                              double nearZeroThreshold = 1e-5);
+
   /// @}
   /// @name Dynamics
   /// @{
@@ -169,8 +256,9 @@ public:
   /// Integrate forward in time given angular velocity and acceleration in body frame
   /// Uses second order integration for position, returns derivatives except dt.
   NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
-      const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
-      OptionalJacobian<9, 3> G2) const;
+                  const double dt, OptionalJacobian<9, 9> F = {},
+                  OptionalJacobian<9, 3> G1 = {},
+                  OptionalJacobian<9, 3> G2 = {}) const;
 
   /// Compute tangent space contribution due to Coriolis forces
   Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
@@ -201,8 +289,10 @@ private:
 };
 
 // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
-template<>
-struct traits<NavState> : internal::Manifold<NavState> {
-};
+template <>
+struct traits<NavState> : public internal::LieGroup<NavState> {};
+
+template <>
+struct traits<const NavState> : public internal::LieGroup<NavState> {};
 
 } // namespace gtsam
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index b8f081518..8934129ee 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -12,13 +12,16 @@
 /**
  * @file    testNavState.cpp
  * @brief   Unit tests for NavState
- * @author  Frank Dellaert
+ * @authors Frank Dellaert, Varun Agrawal, Fan Jiang
  * @date    July 2015
  */
 
 #include <gtsam/navigation/NavState.h>
-#include <gtsam/base/TestableAssertions.h>
+
+#include <gtsam/base/lieProxies.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/testLie.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -26,6 +29,9 @@ using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
+GTSAM_CONCEPT_TESTABLE_INST(NavState)
+GTSAM_CONCEPT_LIE_INST(NavState)
+
 static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
 static const Point3 kPosition(1.0, 2.0, 3.0);
 static const Pose3 kPose(kAttitude, kPosition);
@@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
 static const Vector3 kGravity(0, 0, 9.81);
 static const Vector9 kZeroXi = Vector9::Zero();
 
+static const Point3 V(3, 0.4, -2.2);
+static const Point3 P(0.2, 0.7, -2);
+static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
+static const Point3 V2(-6.5, 3.5, 6.2);
+static const Point3 P2(3.5, -8.2, 4.2);
+static const NavState T(R, P2, V2);
+static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2);
+static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7),
+                         Point3(1, 2, 3));
+
 /* ************************************************************************* */
 TEST(NavState, Constructor) {
   std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
@@ -46,14 +62,14 @@ TEST(NavState, Constructor) {
       assert_equal(kState1,
           NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
   EXPECT(
-      assert_equal(
-          numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
+assert_equal(
+      numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
   EXPECT(
-      assert_equal(
-          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+assert_equal(
+      numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
   EXPECT(
-      assert_equal(
-          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+assert_equal(
+      numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
 }
 
 /* ************************************************************************* */
@@ -64,7 +80,7 @@ TEST(NavState, Constructor2) {
   Matrix aH1, aH2;
   EXPECT(
       assert_equal(kState1,
-          NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
+                      NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
   EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
   EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
 }
@@ -127,8 +143,8 @@ TEST( NavState, Manifold ) {
   Point3 dt = Point3(xi.segment<3>(3));
   Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
   NavState state2 = NavState(kState1.attitude() * drot,
-      kState1.position() + kState1.attitude() * dt,
-      kState1.velocity() + kState1.attitude() * dvel);
+                             kState1.position() + kState1.attitude() * dt,
+                             kState1.velocity() + kState1.attitude() * dvel);
   EXPECT(assert_equal(state2, kState1.retract(xi)));
   EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
 
@@ -169,6 +185,143 @@ TEST( NavState, Manifold ) {
   EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
 }
 
+/* ************************************************************************* */
+TEST(NavState, Equals) {
+  NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3));
+  NavState pose2 = T3;
+  EXPECT(T3.equals(pose2));
+  NavState origin;
+  EXPECT(!T3.equals(origin));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Compose) {
+  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
+  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
+  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
+                       {3.0, -1.0, 1.0});
+
+  auto ab_c = (nav_state_a * nav_state_b) * nav_state_c;
+  auto a_bc = nav_state_a * (nav_state_b * nav_state_c);
+  CHECK(assert_equal(ab_c, a_bc));
+
+  Matrix actual = (T2 * T2).matrix();
+
+  Matrix expected = T2.matrix() * T2.matrix();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix actualDcompose1, actualDcompose2;
+  T2.compose(T2, actualDcompose1, actualDcompose2);
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::compose<NavState>, T2, T2);
+
+  EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
+  EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::compose<NavState>, T2, T2);
+  EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4));
+}
+
+/* ************************************************************************* */
+// Check compose and its push-forward, another case
+TEST(NavState, Compose2) {
+  const NavState& T1 = T;
+  Matrix actual = (T1 * T2).matrix();
+  Matrix expected = T1.matrix() * T2.matrix();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix actualDcompose1, actualDcompose2;
+  T1.compose(T2, actualDcompose1, actualDcompose2);
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::compose<NavState>, T1, T2);
+  EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
+  EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::compose<NavState>, T1, T2);
+  EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Inverse) {
+  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
+  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
+  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
+                       {3.0, -1.0, 1.0});
+
+  auto a_inv = nav_state_a.inverse();
+  auto a_a_inv = nav_state_a * a_inv;
+  CHECK(assert_equal(a_a_inv, NavState()));
+
+  auto b_inv = nav_state_b.inverse();
+  auto b_b_inv = nav_state_b * b_inv;
+  CHECK(assert_equal(b_b_inv, NavState()));
+
+  Matrix actualDinverse;
+  Matrix actual = T.inverse(actualDinverse).matrix();
+  Matrix expected = T.matrix().inverse();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
+  EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
+  EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
+}
+
+/* ************************************************************************* */
+TEST(NavState, InverseDerivatives) {
+  Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5);
+  Vector3 v(3.5, -8.2, 4.2);
+  Point3 p(3.5, -8.2, 4.2);
+  NavState T(R, p, v);
+
+  Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
+  Matrix actualDinverse;
+  T.inverse(actualDinverse);
+  EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
+  EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Compose_Inverse) {
+  NavState actual = T * T.inverse();
+  NavState expected;
+  EXPECT(assert_equal(actual, expected, 1e-8));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Between) {
+  NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0));
+
+  NavState actual = s1.compose(s2);
+  EXPECT(assert_equal(s2, actual));
+
+  NavState between = s2.between(s1);
+  NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0));
+  EXPECT(assert_equal(expected_between, between));
+
+  NavState expected = T2.inverse() * T3;
+  Matrix actualDBetween1, actualDBetween2;
+  actual = T2.between(T3, actualDBetween1, actualDBetween2);
+  EXPECT(assert_equal(expected, actual));
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::between<NavState>, T2, T3);
+  EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::between<NavState>, T2, T3);
+  EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, interpolate) {
+  EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0)));
+  EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0)));
+}
+
 /* ************************************************************************* */
 static const double dt = 2.0;
 std::function<Vector9(const NavState&, const bool&)> coriolis =
@@ -189,7 +342,7 @@ TEST(NavState, Coriolis) {
 TEST(NavState, Coriolis2) {
   Matrix9 aH;
   NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
-      Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
+                  Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
 
   // first-order
   state2.coriolis(dt, kOmegaCoriolis, false, aH);
@@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) {
 }
 
 TEST(NavState, Coriolis3) {
-  /** Consider a massless planet with an attached nav frame at 
-   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with 
+  /** Consider a massless planet with an attached nav frame at
+   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with
    *  velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
-   *  aligned with the nav frame (i.e., nRb != I_3x3). Test that first and 
+   *  aligned with the nav frame (i.e., nRb != I_3x3). Test that first and
    *  second order Coriolis corrections are as expected.
    */
 
@@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) {
        bRn = nRb.inverse();
 
   // Get expected first and second order corrections in the nav frame
-  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, 
+  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
           n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
-          n_dV1e = dt * n_aCorr1, 
+          n_dV1e = dt * n_aCorr1,
           n_dV2e = dt * (n_aCorr1 + n_aCorr2);
 
   // Get expected first and second order corrections in the body frame
@@ -271,6 +424,262 @@ TEST(NavState, Stream)
   EXPECT(os.str() == expected);
 }
 
+/* ************************************************************************* */
+TEST(NavState, Print) {
+  NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3));
+
+  // Generate the expected output
+  std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
+  std::string p = "p: 1 2 3\n";
+  std::string v = "v: 1 2 3\n";
+  std::string expected = R + p + v;
+
+  EXPECT(assert_print_equal(expected, state));
+}
+
+/* ************************************************************************* */
+#ifndef GTSAM_POSE3_EXPMAP
+TEST(NavState, Retract_first_order) {
+  NavState id;
+  Vector v = Z_9x1;
+  v(0) = 0.3;
+  EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)),
+                      id.retract(v), 1e-2));
+  v(3) = 0.2;
+  v(4) = 0.7;
+  v(5) = -2;
+  v(6) = 3;
+  v(7) = 0.4;
+  v(8) = -2.2;
+  EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2));
+}
+#endif
+
+/* ************************************************************************* */
+TEST(NavState, RetractExpmap) {
+  Vector xi = Z_9x1;
+  xi(0) = 0.3;
+  NavState pose = NavState::Expmap(xi),
+           expected(R, Point3(0, 0, 0), Point3(0, 0, 0));
+  EXPECT(assert_equal(expected, pose, 1e-2));
+  EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Expmap_A_Full) {
+  NavState id;
+  Vector xi = Z_9x1;
+  xi(0) = 0.3;
+  EXPECT(assert_equal(expmap_default<NavState>(id, xi),
+                      NavState(R, Point3(0, 0, 0), Point3(0, 0, 0))));
+  xi(3) = -0.2;
+  xi(4) = -0.394742;
+  xi(5) = 2.08998;
+  xi(6) = 0.2;
+  xi(7) = 0.394742;
+  xi(8) = -2.08998;
+
+  NavState expected(R, -P, P);
+  EXPECT(assert_equal(expected, expmap_default<NavState>(id, xi), 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Expmap_b) {
+  NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0));
+  NavState p2 = p1.retract(
+      (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
+  NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0),
+                    Point3(100.0, 0.0, 0.0));
+  EXPECT(assert_equal(expected, p2, 1e-2));
+}
+
+/* ************************************************************************* */
+// test case for screw motion in the plane
+namespace screwNavState {
+double a = 0.3, c = cos(a), s = sin(a), w = 0.3;
+Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished();
+Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
+Point3 expectedV(0.29552, 0.0446635, 1);
+Point3 expectedP(0.29552, 0.0446635, 1);
+NavState expected(expectedR, expectedV, expectedP);
+}  // namespace screwNavState
+
+/* ************************************************************************* */
+// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
+TEST(NavState, Adjoint_full) {
+  NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse();
+  Vector xiPrime = T.Adjoint(screwNavState::xi);
+  EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6));
+
+  NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse();
+  Vector xiPrime2 = T2.Adjoint(screwNavState::xi);
+  EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6));
+
+  NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse();
+  Vector xiPrime3 = T3.Adjoint(screwNavState::xi);
+  EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Adjoint_compose_full) {
+  // To debug derivatives of compose, assert that
+  // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
+  const NavState& T1 = T;
+  Vector x =
+      (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished();
+  NavState expected = T1 * NavState::Expmap(x) * T2;
+  Vector y = T2.inverse().Adjoint(x);
+  NavState actual = T1 * T2 * NavState::Expmap(y);
+  EXPECT(assert_equal(expected, actual, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Retract_LocalCoordinates) {
+  Vector9 d;
+  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  d /= 10;
+  const Rot3 R = Rot3::Retract(d.head<3>());
+  NavState t = NavState::Retract(d);
+  EXPECT(assert_equal(d, NavState::LocalCoordinates(t)));
+}
+/* ************************************************************************* */
+TEST(NavState, retract_localCoordinates) {
+  Vector9 d12;
+  d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  d12 /= 10;
+  NavState t1 = T, t2 = t1.retract(d12);
+  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
+}
+/* ************************************************************************* */
+TEST(NavState, expmap_logmap) {
+  Vector d12 = Vector9::Constant(0.1);
+  NavState t1 = T, t2 = t1.expmap(d12);
+  EXPECT(assert_equal(d12, t1.logmap(t2)));
+}
+
+/* ************************************************************************* */
+TEST(NavState, retract_localCoordinates2) {
+  NavState t1 = T;
+  NavState t2 = T3;
+  NavState origin;
+  Vector d12 = t1.localCoordinates(t2);
+  EXPECT(assert_equal(t2, t1.retract(d12)));
+  Vector d21 = t2.localCoordinates(t1);
+  EXPECT(assert_equal(t1, t2.retract(d21)));
+  // EXPECT(assert_equal(d12, -d21));
+}
+/* ************************************************************************* */
+TEST(NavState, manifold_expmap) {
+  NavState t1 = T;
+  NavState t2 = T3;
+  NavState origin;
+  Vector d12 = t1.logmap(t2);
+  EXPECT(assert_equal(t2, t1.expmap(d12)));
+  Vector d21 = t2.logmap(t1);
+  EXPECT(assert_equal(t1, t2.expmap(d21)));
+
+  // Check that log(t1,t2)=-log(t2,t1)
+  EXPECT(assert_equal(d12, -d21));
+}
+
+/* ************************************************************************* */
+TEST(NavState, subgroups) {
+  // Frank - Below only works for correct "Agrawal06iros style expmap
+  // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
+  Vector d =
+      (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
+  // exp(-d)=inverse(exp(d))
+  EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse()));
+  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
+  NavState T2 = NavState::Expmap(2 * d);
+  NavState T3 = NavState::Expmap(3 * d);
+  NavState T5 = NavState::Expmap(5 * d);
+  EXPECT(assert_equal(T5, T2 * T3));
+  EXPECT(assert_equal(T5, T3 * T2));
+}
+
+/* ************************************************************************* */
+TEST(NavState, adjointMap) {
+  Matrix res = NavState::adjointMap(screwNavState::xi);
+  Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1),
+                            screwNavState::xi(2));
+  Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4),
+                            screwNavState::xi(5));
+  Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7),
+                            screwNavState::xi(8));
+  Matrix9 expected;
+  expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh;
+  EXPECT(assert_equal(expected, res, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, ExpmapDerivative1) {
+  Matrix9 actualH;
+  Vector9 w;
+  w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  NavState::Expmap(w, actualH);
+
+  std::function<NavState(const Vector9&)> f = [](const Vector9& w) {
+    return NavState::Expmap(w);
+  };
+  Matrix expectedH =
+      numericalDerivative21<NavState, Vector9, OptionalJacobian<9, 9> >(
+          &NavState::Expmap, w, {});
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+/* ************************************************************************* */
+TEST(NavState, LogmapDerivative) {
+  Matrix9 actualH;
+  Vector9 w;
+  w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  NavState p = NavState::Expmap(w);
+  EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5));
+
+  std::function<Vector9(const NavState&)> f = [](const NavState& p) {
+    return NavState::Logmap(p);
+  };
+  Matrix expectedH =
+      numericalDerivative21<Vector9, NavState, OptionalJacobian<9, 9> >(
+          &NavState::Logmap, p, {});
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+//******************************************************************************
+TEST(NavState, Invariants) {
+  NavState id;
+
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, T3));
+  EXPECT(check_group_invariants(T2, id));
+  EXPECT(check_group_invariants(T2, T3));
+
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, T3));
+  EXPECT(check_manifold_invariants(T2, id));
+  EXPECT(check_manifold_invariants(T2, T3));
+}
+
+//******************************************************************************
+TEST(NavState, LieGroupDerivatives) {
+  NavState id;
+
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
+}
+
+//******************************************************************************
+TEST(NavState, ChartDerivatives) {
+  NavState id;
+  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
+    CHECK_CHART_DERIVATIVES(id, id);
+      //  CHECK_CHART_DERIVATIVES(id,T2);
+      //  CHECK_CHART_DERIVATIVES(T2,id);
+      //  CHECK_CHART_DERIVATIVES(T2,T3);
+  }
+}
 
 /* ************************************************************************* */
 int main() {

From d0c1854e63999f0b8e394e8ae20ad4af271f22a5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 01:04:05 -0500
Subject: [PATCH 115/362] ExpmapTranslation

---
 gtsam/geometry/Pose3.cpp | 120 +++++++++++++++++++++------------------
 gtsam/geometry/Pose3.h   |  15 +++++
 2 files changed, 80 insertions(+), 55 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 894314491..15eddf7a7 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -26,8 +26,6 @@
 
 namespace gtsam {
 
-using std::vector;
-
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3)
 
@@ -167,21 +165,23 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
-  if (Hxi) *Hxi = ExpmapDerivative(xi);
+  // Get angular velocity omega and translational velocity v from twist xi
+  const Vector3 w = xi.head<3>(), v = xi.tail<3>();
 
-  // get angular velocity omega and translational velocity v from twist xi
-  Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
+  // Compute rotation using Expmap
+  Rot3 R = Rot3::Expmap(w);
 
-  Rot3 R = Rot3::Expmap(omega);
-  double theta2 = omega.dot(omega);
-  if (theta2 > std::numeric_limits<double>::epsilon()) {
-    Vector3 t_parallel = omega * omega.dot(v);  // translation parallel to axis
-    Vector3 omega_cross_v = omega.cross(v);     // points towards axis
-    Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
-    return Pose3(R, t);
-  } else {
-    return Pose3(R, v);
+  // Compute translation and optionally its Jacobian in w
+  Matrix3 Q;
+  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
+
+  if (Hxi) {
+    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
+    *Hxi << Jw, Z_3x3,
+             Q, Jw;
   }
+
+  return Pose3(R, t);
 }
 
 /* ************************************************************************* */
@@ -240,55 +240,65 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 }
 
 /* ************************************************************************* */
-Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
+Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
+                                           double nearZeroThreshold) {
+  Matrix3 Q;
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  const Matrix3 V = skewSymmetric(v);
-  const Matrix3 W = skewSymmetric(w);
-
-  Matrix3 Q;
-
-#ifdef NUMERICAL_EXPMAP_DERIV
-  Matrix3 Qj = Z_3x3;
-  double invFac = 1.0;
-  Q = Z_3x3;
-  Matrix3 Wj = I_3x3;
-  for (size_t j=1; j<10; ++j) {
-    Qj = Qj*W + Wj*V;
-    invFac = -invFac/(j+1);
-    Q = Q + invFac*Qj;
-    Wj = Wj*W;
-  }
-#else
-  // The closed-form formula in Barfoot14tro eq. (102)
-  double phi = w.norm();
-  const Matrix3 WVW = W * V * W;
-  if (std::abs(phi) > nearZeroThreshold) {
-    const double s = sin(phi), c = cos(phi);
-    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
-                 phi5 = phi4 * phi;
-    // Invert the sign of odd-order terms to have the right Jacobian
-    Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
-        (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
-        0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
-            (WVW * W + W * WVW);
-  } else {
-    Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
-        1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
-        1. / 120. * (WVW * W + W * WVW);
-  }
-#endif
-
+  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
   return Q;
 }
 
+/* ************************************************************************* */
+Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
+                                 OptionalJacobian<3, 3> Q,
+                                 const std::optional<Rot3>& R,
+                                 double nearZeroThreshold) {
+  double phi2 = w.dot(w);
+
+  if (Q) {
+    const Matrix3 V = skewSymmetric(v);
+    const Matrix3 W = skewSymmetric(w);
+    const Matrix3 WVW = W * V * W;
+    const double phi = w.norm();
+
+    if (std::abs(phi) > nearZeroThreshold) {
+      const double s = sin(phi), c = cos(phi);
+      const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
+
+      // Invert the sign of odd-order terms to have the right Jacobian
+      *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
+           (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
+           0.5 *
+               ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
+               (WVW * W + W * WVW);
+    } else {
+      constexpr double one_sixth = 1. / 6.;
+      constexpr double one_twenty_fourth = 1. / 24.;
+      constexpr double one_one_hundred_twentieth = 1. / 120.;
+
+      *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
+           one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
+           one_one_hundred_twentieth * (WVW * W + W * WVW);
+    }
+  }
+
+  // TODO(Frank): this threshold is *different*. Why?
+  if (phi2 > std::numeric_limits<double>::epsilon()) {
+    Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
+    Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
+    Rot3 rotation = R.value_or(Rot3::Expmap(w));
+    Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / phi2;
+    return t;
+  } else {
+    return v;
+  }
+}
+
 /* ************************************************************************* */
 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
-  const Vector3 w = xi.head<3>();
-  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   Matrix6 J;
-  J << Jw, Z_3x3, Q, Jw;
+  Expmap(xi, J);
   return J;
 }
 
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 8a807cc23..d7c280c80 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -217,10 +217,25 @@ public:
   *  (see Chirikjian11book2, pg 44, eq 10.95.
   *  The closed-form formula is identical to formula 102 in Barfoot14tro where
   *  Q_l of the SE3 Expmap left derivative matrix is given.
+  *  This is the Jacobian of ExpmapTranslation and computed there.
   */
   static Matrix3 ComputeQforExpmapDerivative(
       const Vector6& xi, double nearZeroThreshold = 1e-5);
 
+  /**
+   * Compute the translation part of the exponential map, with derivative.
+   * @param w 3D angular velocity
+   * @param v 3D velocity
+   * @param Q Optionally, compute 3x3 Jacobian wrpt w
+   * @param R Optionally, precomputed as Rot3::Expmap(w)
+   * @param nearZeroThreshold threshold for small values
+   * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix
+   */
+  static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
+                                   OptionalJacobian<3, 3> Q = {},
+                                   const std::optional<Rot3>& R = {},
+                                   double nearZeroThreshold = 1e-5);
+
   using LieGroup<Pose3, 6>::inverse; // version with derivative
 
   /**

From 693eafb0747b6fbadeaf9be4c3a5b48ccfbc1dae Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 01:42:35 -0500
Subject: [PATCH 116/362] Use ExpmapTranslation

---
 gtsam/navigation/NavState.cpp | 108 ++++++++++------------------------
 gtsam/navigation/NavState.h   |   7 ---
 2 files changed, 30 insertions(+), 85 deletions(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 9f91ae415..f7e0d48be 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/navigation/NavState.h>
 
 #include <string>
+#include "gtsam/geometry/Rot3.h"
 
 namespace gtsam {
 
@@ -114,28 +115,27 @@ NavState NavState::inverse() const {
 
 //------------------------------------------------------------------------------
 NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
-  if (Hxi) *Hxi = ExpmapDerivative(xi);
+  // Get angular velocity w, translational velocity v, and acceleration a
+  Vector3 w = xi.head<3>();
+  Vector3 rho = xi.segment<3>(3);
+  Vector3 nu = xi.tail<3>();
 
-  // Order is rotation, position, velocity, represented by ω,ρ,ν
-  Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)),
-      nu(xi(6), xi(7), xi(8));
+  // Compute rotation using Expmap
+  Rot3 R = Rot3::Expmap(w);
 
-  Rot3 R = Rot3::Expmap(omega);
+  // Compute translations and optionally their Jacobians
+  Matrix3 Qt, Qv;
+  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R);
+  Vector3 v = Pose3::ExpmapTranslation(w,  nu, Hxi ? &Qv : nullptr, R);
 
-  double omega_norm = omega.norm();
-
-  if (omega_norm < 1e-8) {
-    return NavState(Rot3(), Point3(rho), Point3(nu));
-
-  } else {
-    Matrix W = skewSymmetric(omega);
-    double omega_norm2 = omega_norm * omega_norm;
-    double omega_norm3 = omega_norm2 * omega_norm;
-    Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W +
-               ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W);
-
-    return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu));
+  if (Hxi) {
+    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
+    *Hxi << Jw, Z_3x3, Z_3x3,
+            Qt,    Jw, Z_3x3,
+            Qv, Z_3x3,    Jw;
   }
+
+  return NavState(R, t, v);
 }
 
 //------------------------------------------------------------------------------
@@ -220,64 +220,10 @@ Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
   return ad_xi * y;
 }
 
-/* ************************************************************************* */
-Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi,
-                                               double nearZeroThreshold) {
-  const auto omega = xi.head<3>();
-  const auto nu = xi.segment<3>(3);
-  const auto rho = xi.tail<3>();
-  const Matrix3 V = skewSymmetric(nu);
-  const Matrix3 P = skewSymmetric(rho);
-  const Matrix3 W = skewSymmetric(omega);
-
-  Matrix3 Qv, Qp;
-  Matrix63 Q;
-
-  // The closed-form formula in Barfoot14tro eq. (102)
-  double phi = omega.norm();
-  if (std::abs(phi) > 1e-5) {
-    const double sinPhi = sin(phi), cosPhi = cos(phi);
-    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
-                 phi5 = phi4 * phi;
-    // Invert the sign of odd-order terms to have the right Jacobian
-    Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) +
-         (1 - phi2 / 2 - cosPhi) / phi4 *
-             (W * W * V + V * W * W - 3 * W * V * W) -
-         0.5 *
-             ((1 - phi2 / 2 - cosPhi) / phi4 -
-              3 * (phi - sinPhi - phi3 / 6.) / phi5) *
-             (W * V * W * W + W * W * V * W);
-    Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) +
-         (1 - phi2 / 2 - cosPhi) / phi4 *
-             (W * W * P + P * W * W - 3 * W * P * W) -
-         0.5 *
-             ((1 - phi2 / 2 - cosPhi) / phi4 -
-              3 * (phi - sinPhi - phi3 / 6.) / phi5) *
-             (W * P * W * W + W * W * P * W);
-  } else {
-    Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) +
-         1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) -
-         0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W);
-    Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) +
-         1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) -
-         0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W);
-  }
-
-  Q << Qv, Qp;
-  return Q;
-}
-
 //------------------------------------------------------------------------------
 Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
-  const Vector3 w = xi.head<3>();
-  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  const Matrix63 Q = ComputeQforExpmapDerivative(xi);
-  const Matrix3 Qv = Q.topRows<3>();
-  const Matrix3 Qp = Q.bottomRows<3>();
-
   Matrix9 J;
-  J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw;
-
+  Expmap(xi, J);
   return J;
 }
 
@@ -285,15 +231,21 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
 Matrix9 NavState::LogmapDerivative(const NavState& state) {
   const Vector9 xi = Logmap(state);
   const Vector3 w = xi.head<3>();
+  Vector3 rho = xi.segment<3>(3);
+  Vector3 nu = xi.tail<3>();
+  
+  Matrix3 Qt, Qv;
+  const Rot3 R = Rot3::Expmap(w);
+  Pose3::ExpmapTranslation(w, rho, Qt, R);
+  Pose3::ExpmapTranslation(w,  nu, Qv, R);
   const Matrix3 Jw = Rot3::LogmapDerivative(w);
-  const Matrix63 Q = ComputeQforExpmapDerivative(xi);
-  const Matrix3 Qv = Q.topRows<3>();
-  const Matrix3 Qp = Q.bottomRows<3>();
+  const Matrix3 Qt2 = -Jw * Qt * Jw;
   const Matrix3 Qv2 = -Jw * Qv * Jw;
-  const Matrix3 Qp2 = -Jw * Qp * Jw;
 
   Matrix9 J;
-  J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw;
+  J <<  Jw, Z_3x3, Z_3x3, 
+       Qt2,    Jw, Z_3x3,
+       Qv2, Z_3x3,    Jw;
   return J;
 }
 
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index 2f037a795..95ee71fe9 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -242,13 +242,6 @@ public:
     static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
   };
 
-  /**
-   * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative
-   * matrix
-   */
-  static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi,
-                                              double nearZeroThreshold = 1e-5);
-
   /// @}
   /// @name Dynamics
   /// @{

From 3eba54c05cf703f3a105d11214345afe4f269dbb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 01:54:29 -0500
Subject: [PATCH 117/362] Remove TODO

---
 gtsam/navigation/NavState.cpp | 24 ------------------------
 1 file changed, 24 deletions(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index f7e0d48be..6234b9c27 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -294,30 +294,6 @@ NavState NavState::retract(const Vector9& xi, //
 //------------------------------------------------------------------------------
 Vector9 NavState::localCoordinates(const NavState& g, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
-  // return LieGroup<NavState, 9>::localCoordinates(g, H1, H2);
-
-  //TODO(Varun) Fix so that test on L680 passes
-
-  // Matrix3 D_dR_R, D_dt_R, D_dv_R;
-  // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
-  // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
-  // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
-
-  // Vector9 xi;
-  // Matrix3 D_xi_R;
-  // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
-  // if (H1) {
-  //   *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
-  //   D_dt_R, -I_3x3, Z_3x3, //
-  //   D_dv_R, Z_3x3, -I_3x3;
-  // }
-  // if (H2) {
-  //   *H2 << D_xi_R, Z_3x3, Z_3x3, //
-  //   Z_3x3, dR.matrix(), Z_3x3, //
-  //   Z_3x3, Z_3x3, dR.matrix();
-  // }
-  // return xi;
-
   Matrix3 D_dR_R, D_dt_R, D_dv_R;
   const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
   const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);

From dddb6acde08a31d8df02577110c67625f88df384 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 01:55:16 -0500
Subject: [PATCH 118/362] Remove extra include

---
 gtsam/navigation/NavState.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 6234b9c27..7c7aa373b 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/navigation/NavState.h>
 
 #include <string>
-#include "gtsam/geometry/Rot3.h"
 
 namespace gtsam {
 

From 8193bdbec31158c4c18566aa64e96ac5c5d7b5c7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 12:11:48 -0500
Subject: [PATCH 119/362] Fix nearZero of SE(3) and SE_2(3)

---
 gtsam/geometry/Pose3.cpp | 44 +++++++++++++++++++++-------------------
 1 file changed, 23 insertions(+), 21 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 15eddf7a7..bb1483432 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -21,7 +21,6 @@
 
 #include <cmath>
 #include <iostream>
-#include <limits>
 #include <string>
 
 namespace gtsam {
@@ -254,44 +253,47 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
                                  OptionalJacobian<3, 3> Q,
                                  const std::optional<Rot3>& R,
                                  double nearZeroThreshold) {
-  double phi2 = w.dot(w);
+  const double theta2 = w.dot(w);
+  bool nearZero = (theta2 <= nearZeroThreshold);
 
   if (Q) {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 W = skewSymmetric(w);
     const Matrix3 WVW = W * V * W;
-    const double phi = w.norm();
+    const double theta = w.norm();
 
-    if (std::abs(phi) > nearZeroThreshold) {
-      const double s = sin(phi), c = cos(phi);
-      const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
-
-      // Invert the sign of odd-order terms to have the right Jacobian
-      *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
-           (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
-           0.5 *
-               ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
-               (WVW * W + W * WVW);
-    } else {
-      constexpr double one_sixth = 1. / 6.;
-      constexpr double one_twenty_fourth = 1. / 24.;
-      constexpr double one_one_hundred_twentieth = 1. / 120.;
+    if (nearZero) {
+      static constexpr double one_sixth = 1. / 6.;
+      static constexpr double one_twenty_fourth = 1. / 24.;
+      static constexpr double one_one_hundred_twentieth = 1. / 120.;
 
       *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
            one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
            one_one_hundred_twentieth * (WVW * W + W * WVW);
+    } else {
+      const double s = sin(theta), c = cos(theta);
+      const double theta3 = theta2 * theta, theta4 = theta3 * theta,
+                   theta5 = theta4 * theta;
+
+      // Invert the sign of odd-order terms to have the right Jacobian
+      *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
+           (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
+           0.5 *
+               ((1 - theta2 / 2 - c) / theta4 -
+                3 * (theta - s - theta3 / 6.) / theta5) *
+               (WVW * W + W * WVW);
     }
   }
 
   // TODO(Frank): this threshold is *different*. Why?
-  if (phi2 > std::numeric_limits<double>::epsilon()) {
+  if (nearZero) {
+    return v + 0.5 * w.cross(v);
+  } else {
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
-    Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / phi2;
+    Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2;
     return t;
-  } else {
-    return v;
   }
 }
 

From f7a678e54e551b66b8aec8325f5c5085e903d69a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 12:11:56 -0500
Subject: [PATCH 120/362] Address comments

---
 gtsam/navigation/NavState.cpp           |  2 --
 gtsam/navigation/tests/testNavState.cpp | 10 +++++-----
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 7c7aa373b..b9d48a3cb 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -264,8 +264,6 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& state,
 //------------------------------------------------------------------------------
 NavState NavState::retract(const Vector9& xi, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
-  // return LieGroup<NavState, 9>::retract(xi, H1, H2);
-
   Rot3 nRb = R_;
   Point3 n_t = t_, n_v = v_;
   Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index 8934129ee..427531415 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -490,7 +490,7 @@ TEST(NavState, Expmap_b) {
       (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
   NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0),
                     Point3(100.0, 0.0, 0.0));
-  EXPECT(assert_equal(expected, p2, 1e-2));
+  EXPECT(assert_equal(expected, p2));
 }
 
 /* ************************************************************************* */
@@ -566,7 +566,7 @@ TEST(NavState, retract_localCoordinates2) {
   EXPECT(assert_equal(t2, t1.retract(d12)));
   Vector d21 = t2.localCoordinates(t1);
   EXPECT(assert_equal(t1, t2.retract(d21)));
-  // EXPECT(assert_equal(d12, -d21));
+  // NOTE(FRANK): d12 !== -d21 for arbitrary retractions.
 }
 /* ************************************************************************* */
 TEST(NavState, manifold_expmap) {
@@ -675,9 +675,9 @@ TEST(NavState, ChartDerivatives) {
   NavState id;
   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
     CHECK_CHART_DERIVATIVES(id, id);
-      //  CHECK_CHART_DERIVATIVES(id,T2);
-      //  CHECK_CHART_DERIVATIVES(T2,id);
-      //  CHECK_CHART_DERIVATIVES(T2,T3);
+    CHECK_CHART_DERIVATIVES(id,T2);
+    CHECK_CHART_DERIVATIVES(T2,id);
+    CHECK_CHART_DERIVATIVES(T2,T3);
   }
 }
 

From 440c3ea64bbcd750c692bca7d8cd348db43bb7f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 14:28:44 -0500
Subject: [PATCH 121/362] Simplify functor according to Eade

---
 gtsam/geometry/SO3.cpp | 47 +++++++++++++++++++++++-------------------
 gtsam/geometry/SO3.h   | 27 ++++++++++++++++--------
 2 files changed, 44 insertions(+), 30 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 2585c37be..d4c30119e 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9,
 }
 
 void ExpmapFunctor::init(bool nearZeroApprox) {
+  WW = W * W;
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
     sin_theta = std::sin(theta);
     const double s2 = std::sin(theta / 2.0);
     one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    A = sin_theta / theta;
+    B = one_minus_cos / theta2;
   }
 }
 
@@ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
   const double wx = omega.x(), wy = omega.y(), wz = omega.z();
   W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
   init(nearZeroApprox);
-  if (!nearZero) {
-    K = W / theta;
-    KK = K * K;
-  }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
     : theta2(angle * angle), theta(angle) {
   const double ax = axis.x(), ay = axis.y(), az = axis.z();
-  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
-  W = K * angle;
+  W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
+  W *= angle;
   init(nearZeroApprox);
-  if (!nearZero) {
-    KK = K * K;
-  }
 }
 
 SO3 ExpmapFunctor::expmap() const {
   if (nearZero)
-    return SO3(I_3x3 + W);
+    return SO3(I_3x3 + W + 0.5 * WW);
   else
-    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
+    return SO3(I_3x3 + A * W + B * WW);
+}
+
+Matrix3 DexpFunctor::leftJacobian() const {
+  if (nearZero) {
+    return I_3x3 + 0.5 * W + one_sixth * WW;
+  } else {
+    return I_3x3 + B * W + C * WW;
+  }
 }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
   if (nearZero) {
-    dexp_ = I_3x3 - 0.5 * W;
+    rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW;
   } else {
-    a = one_minus_cos / theta;
-    b = 1.0 - sin_theta / theta;
-    dexp_ = I_3x3 - a * K + b * KK;
+    C = (1 - A) / theta2;
+    rightJacobian_ = I_3x3 - B * W + C * WW;
   }
 }
 
@@ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
       *H1 = 0.5 * skewSymmetric(v);
     } else {
       // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      const Vector3 Kv = K * v;
+      double a = B * theta;
+      double b = C * theta2;
+      const Vector3 Kv = W * v / theta;
       const double Da = (sin_theta - 2.0 * a) / theta2;
       const double Db = (one_minus_cos - 3.0 * b) / theta2;
-      *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
+      *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() -
             skewSymmetric(Kv * b / theta) +
-            (a * I_3x3 - b * K) * skewSymmetric(v / theta);
+            (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta);
     }
   }
-  if (H2) *H2 = dexp_;
-  return dexp_ * v;
+  if (H2) *H2 = rightJacobian_;
+  return rightJacobian_ * v;
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = dexp_.inverse();
+  const Matrix3 invDexp = rightJacobian_.inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
     Matrix3 D_dexpv_omega;
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index cd67bfb8c..8d4401c31 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 class GTSAM_EXPORT ExpmapFunctor {
  protected:
   const double theta2;
-  Matrix3 W, K, KK;
+  Matrix3 W, WW;
+  double A, B; // Ethan Eade's constants
   bool nearZero;
   double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
 
@@ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor {
 
 /// Functor that implements Exponential map *and* its derivatives
 class DexpFunctor : public ExpmapFunctor {
+ protected:
+  static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
-  double a, b;
-  Matrix3 dexp_;
+  double C;  // Ethan Eade's C constant
+  Matrix3 rightJacobian_;
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
+  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
+                                    bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@@ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor {
   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
   // This maps a perturbation v in the tangent space to
   // a perturbation on the manifold Expmap(dexp * v) */
-  const Matrix3& dexp() const { return dexp_; }
+  const Matrix3& dexp() const { return rightJacobian_; }
 
   /// Multiplies with dexp(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
-                    OptionalJacobian<3, 3> H2 = {}) const;
+  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
+                                 OptionalJacobian<3, 3> H1 = {},
+                                 OptionalJacobian<3, 3> H2 = {}) const;
 
   /// Multiplies with dexp().inverse(), with optional derivatives
   GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
-                       OptionalJacobian<3, 3> H1 = {},
-                       OptionalJacobian<3, 3> H2 = {}) const;
+                                    OptionalJacobian<3, 3> H1 = {},
+                                    OptionalJacobian<3, 3> H2 = {}) const;
+
+  // Compute the left Jacobian for Exponential map in SO(3)
+  // Note precomputed, as not used as much
+  Matrix3 leftJacobian() const;
 };
 }  //  namespace so3
 

From 7f1e101c6b1f934e23cbd3914a40396b6d282efb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 14:28:56 -0500
Subject: [PATCH 122/362] Use functor as in SO3

---
 gtsam/geometry/Pose3.cpp           | 73 +++++++++++++++++-------------
 gtsam/geometry/tests/testPose3.cpp | 23 +++++++++-
 2 files changed, 63 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index bb1483432..f867b3088 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -238,14 +238,50 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 #endif
 }
 
+/* ************************************************************************* */
+namespace pose3 {
+class ExpmapFunctor : public so3::DexpFunctor {
+ private:
+  static constexpr double one_twenty_fourth = 1.0 / 24.0;
+  static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
+
+ public:
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
+      : so3::DexpFunctor(omega, nearZeroApprox) {}
+
+  // Compute the Jacobian Q with respect to w
+  Matrix3 computeQ(const Vector3& v) const {
+    const Matrix3 V = skewSymmetric(v);
+    const Matrix3 WVW = W * V * W;
+
+    if (!nearZero) {
+      const double theta3 = theta2 * theta;
+      const double theta4 = theta2 * theta2;
+      const double theta5 = theta4 * theta;
+      const double s = sin_theta;
+      const double a = one_minus_cos - theta2 / 2;
+
+      // The closed-form formula in Barfoot14tro eq. (102)
+      return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
+             a / theta4 * (WW * V + V * WW - 3 * WVW) -
+             0.5 *
+                 (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) *
+                 (WVW * W + W * WVW);
+    } else {
+      return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
+             one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
+             one_one_hundred_twentieth * (WVW * W + W * WVW);
+    }
+  }
+};
+}  // namespace pose3
+
 /* ************************************************************************* */
 Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
                                            double nearZeroThreshold) {
-  Matrix3 Q;
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
-  return Q;
+  return pose3::ExpmapFunctor(w).computeQ(v);
 }
 
 /* ************************************************************************* */
@@ -256,39 +292,12 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   const double theta2 = w.dot(w);
   bool nearZero = (theta2 <= nearZeroThreshold);
 
-  if (Q) {
-    const Matrix3 V = skewSymmetric(v);
-    const Matrix3 W = skewSymmetric(w);
-    const Matrix3 WVW = W * V * W;
-    const double theta = w.norm();
+  if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
 
-    if (nearZero) {
-      static constexpr double one_sixth = 1. / 6.;
-      static constexpr double one_twenty_fourth = 1. / 24.;
-      static constexpr double one_one_hundred_twentieth = 1. / 120.;
-
-      *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
-           one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
-           one_one_hundred_twentieth * (WVW * W + W * WVW);
-    } else {
-      const double s = sin(theta), c = cos(theta);
-      const double theta3 = theta2 * theta, theta4 = theta3 * theta,
-                   theta5 = theta4 * theta;
-
-      // Invert the sign of odd-order terms to have the right Jacobian
-      *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
-           (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
-           0.5 *
-               ((1 - theta2 / 2 - c) / theta4 -
-                3 * (theta - s - theta3 / 6.) / theta5) *
-               (WVW * W + W * WVW);
-    }
-  }
-
-  // TODO(Frank): this threshold is *different*. Why?
   if (nearZero) {
     return v + 0.5 * w.cross(v);
   } else {
+    // Geometric intuition and faster than using functor.
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index c9851f761..4ff4f9a85 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -845,7 +845,28 @@ TEST( Pose3, ExpmapDerivative1) {
 }
 
 /* ************************************************************************* */
-TEST(Pose3, ExpmapDerivative2) {
+TEST( Pose3, ExpmapDerivative2) {
+  Matrix6 actualH;
+  Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0;
+  Pose3::Expmap(w,actualH);
+  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
+      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+/* ************************************************************************* */
+TEST( Pose3, ExpmapDerivative3) {
+  Matrix6 actualH;
+  Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0;
+  Pose3::Expmap(w,actualH);
+  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
+      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
+  // Small angle approximation is not as precise as numerical derivative?
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Pose3, ExpmapDerivative4) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)

From d1634c033520920496c117548013339e2c5cddbe Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 15:48:55 -0500
Subject: [PATCH 123/362] Simplified Jacobians with ABC

---
 gtsam/geometry/Pose3.cpp | 37 +++++++++++++++++++------------------
 gtsam/geometry/SO3.cpp   | 19 ++++++++-----------
 2 files changed, 27 insertions(+), 29 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f867b3088..16ea55665 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -246,33 +246,34 @@ class ExpmapFunctor : public so3::DexpFunctor {
   static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
 
  public:
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
-      : so3::DexpFunctor(omega, nearZeroApprox) {}
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false,
+                bool includeHigherOrder = false)
+      : so3::DexpFunctor(omega, nearZeroApprox),
+        includeHigherOrder(includeHigherOrder) {}
 
-  // Compute the Jacobian Q with respect to w
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
 
     if (!nearZero) {
-      const double theta3 = theta2 * theta;
-      const double theta4 = theta2 * theta2;
-      const double theta5 = theta4 * theta;
-      const double s = sin_theta;
-      const double a = one_minus_cos - theta2 / 2;
-
-      // The closed-form formula in Barfoot14tro eq. (102)
-      return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
-             a / theta4 * (WW * V + V * WW - 3 * WVW) -
-             0.5 *
-                 (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) *
-                 (WVW * W + W * WVW);
+      // Simplified from closed-form formula in Barfoot14tro eq. (102)
+      // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v
+      return -0.5 * V + C * (W * V + V * W - WVW) +
+             (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) -
+             0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW);
     } else {
-      return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
-             one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
-             one_one_hundred_twentieth * (WVW * W + W * WVW);
+      Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW);
+      Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW);
+
+      if (includeHigherOrder) {
+        Q += one_one_hundred_twentieth * (WVW * W + W * WVW);
+      }
+      return Q;
     }
   }
+
+ private:
+  bool includeHigherOrder;
 };
 }  // namespace pose3
 
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index d4c30119e..626599abf 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -85,7 +85,7 @@ SO3 ExpmapFunctor::expmap() const {
 
 Matrix3 DexpFunctor::leftJacobian() const {
   if (nearZero) {
-    return I_3x3 + 0.5 * W + one_sixth * WW;
+    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
   } else {
     return I_3x3 + B * W + C * WW;
   }
@@ -94,7 +94,7 @@ Matrix3 DexpFunctor::leftJacobian() const {
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
   if (nearZero) {
-    rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW;
+    rightJacobian_ = I_3x3 - 0.5 * W;  // + one_sixth * WW;
   } else {
     C = (1 - A) / theta2;
     rightJacobian_ = I_3x3 - B * W + C * WW;
@@ -104,18 +104,15 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
   if (H1) {
+    const Matrix3 V = skewSymmetric(v);
     if (nearZero) {
-      *H1 = 0.5 * skewSymmetric(v);
+      *H1 = 0.5 * V;
     } else {
       // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      double a = B * theta;
-      double b = C * theta2;
-      const Vector3 Kv = W * v / theta;
-      const double Da = (sin_theta - 2.0 * a) / theta2;
-      const double Db = (one_minus_cos - 3.0 * b) / theta2;
-      *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() -
-            skewSymmetric(Kv * b / theta) +
-            (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta);
+      const double Da = (A - 2.0 * B) / theta2;
+      const double Db = (B - 3.0 * C) / theta2;
+      *H1 = (Db * WW - Da * W) * v * omega.transpose() -
+            C * skewSymmetric(W * v) + B * V - C * W * V;
     }
   }
   if (H2) *H2 = rightJacobian_;

From 8040a0a31e188d99583a747de4be2dd6887a37f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 17:22:09 -0500
Subject: [PATCH 124/362] doubleCross, tested

---
 gtsam/geometry/Point3.cpp           | 10 ++++++++++
 gtsam/geometry/Point3.h             |  7 ++++++-
 gtsam/geometry/tests/testPoint3.cpp | 13 +++++++++++++
 3 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index ef91108eb..79fd6b9bf 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
                 p.x() * q.y() - p.y() * q.x());
 }
 
+Point3 doubleCross(const Point3 &p, const Point3 &q,  //
+                   OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
+  if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
+  if (H2) {
+    const Matrix3 W = skewSymmetric(p);
+    *H2 = W * W;
+  }
+  return gtsam::cross(p, gtsam::cross(p, q));
+}
+
 double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
            OptionalJacobian<1, 3> H2) {
   if (H1) *H1 << q.x(), q.y(), q.z();
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 8da83817d..0fa53b1b2 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
 /// normalize, with optional Jacobian
 GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
 
-/// cross product @return this x q
+/// cross product @return p x q
 GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                           OptionalJacobian<3, 3> H_p = {},
                           OptionalJacobian<3, 3> H_q = {});
 
+/// double cross product @return p x (p x q)
+Point3 doubleCross(const Point3& p, const Point3& q,
+                   OptionalJacobian<3, 3> H1 = {},
+                   OptionalJacobian<3, 3> H2 = {});
+
 /// dot product
 GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                         OptionalJacobian<1, 3> H_p = {},
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index dcfb99105..bb49486a8 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -18,6 +18,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Point3.h>
+#include "gtsam/base/Matrix.h"
+#include "gtsam/base/OptionalJacobian.h"
 
 using namespace std::placeholders;
 using namespace gtsam;
@@ -154,6 +156,17 @@ TEST( Point3, cross2) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Point3, doubleCross) {
+  Matrix aH1, aH2;
+  std::function<Point3(const Point3&, const Point3&)> f =
+      [](const Point3& p, const Point3& q) { return doubleCross(p, q); };
+  const Point3 omega(1, 2, 3), theta(4, 5, 6);
+  doubleCross(omega, theta, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
+}
+
 //*************************************************************************
 TEST (Point3, normalize) {
   Matrix actualH;

From 78f17aabc466b81554b513ff22d7d3df42b643ed Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 23:53:51 -0500
Subject: [PATCH 125/362] Simplified applyDexp

---
 gtsam/geometry/SO3.cpp | 82 ++++++++++++++++++++++++++++--------------
 gtsam/geometry/SO3.h   | 17 +++++----
 2 files changed, 66 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 626599abf..bc2e1e6ec 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -18,13 +18,14 @@
  * @date    December 2014
  */
 
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/SO3.h>
 
 #include <Eigen/SVD>
-
 #include <cmath>
-#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
   return H;
 }
 
-GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
+GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
+                             OptionalJacobian<9, 9> H) {
   Matrix3 MR = M * R.matrix();
   if (H) *H = Dcompose(R);
   return MR;
@@ -83,45 +85,54 @@ SO3 ExpmapFunctor::expmap() const {
     return SO3(I_3x3 + A * W + B * WW);
 }
 
-Matrix3 DexpFunctor::leftJacobian() const {
-  if (nearZero) {
-    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
-  } else {
-    return I_3x3 + B * W + C * WW;
-  }
-}
-
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
+  C = (1 - A) / theta2;
+}
+
+Matrix3 DexpFunctor::rightJacobian() const {
   if (nearZero) {
-    rightJacobian_ = I_3x3 - 0.5 * W;  // + one_sixth * WW;
+    return I_3x3 - 0.5 * W;  // + one_sixth * WW;
   } else {
-    C = (1 - A) / theta2;
-    rightJacobian_ = I_3x3 - B * W + C * WW;
+    return I_3x3 - B * W + C * WW;
   }
 }
 
+// Derivative of w x w x v in w:
+static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) {
+  return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose();
+}
+
+// Multiplies v with left Jacobian through vector operations only.
+// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once
+// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  if (H1) {
-    const Matrix3 V = skewSymmetric(v);
-    if (nearZero) {
-      *H1 = 0.5 * V;
-    } else {
-      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      const double Da = (A - 2.0 * B) / theta2;
-      const double Db = (B - 3.0 * C) / theta2;
-      *H1 = (Db * WW - Da * W) * v * omega.transpose() -
-            C * skewSymmetric(W * v) + B * V - C * W * V;
+  // Wv = omega x * v, with Jacobian -V in w
+  const Vector3 Wv = gtsam::cross(omega, v);
+
+  if (nearZero) {
+    if (H1) *H1 = 0.5 * skewSymmetric(v);
+    if (H2) *H2 = I_3x3 - 0.5 * W;
+    return v - 0.5 * Wv;
+  } else {
+    // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
+    const Vector3 WWv = gtsam::cross(omega, Wv);
+    if (H1) {
+      // 1x3 Jacobians of B and C with respect to theta
+      const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
+      const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
+      *H1 = -Wv * HB + B * skewSymmetric(v) +
+            C * doubleCrossJacobian(omega, v) + WWv * HC;
     }
+    if (H2) *H2 = rightJacobian();
+    return v - B * Wv + C * WWv;
   }
-  if (H2) *H2 = rightJacobian_;
-  return rightJacobian_ * v;
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = rightJacobian_.inverse();
+  const Matrix3 invDexp = rightJacobian().inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
     Matrix3 D_dexpv_omega;
@@ -132,6 +143,23 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   return c;
 }
 
+Matrix3 DexpFunctor::leftJacobian() const {
+  if (nearZero) {
+    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
+  } else {
+    return I_3x3 + B * W + C * WW;
+  }
+}
+
+Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
+                                       OptionalJacobian<3, 3> H1,
+                                       OptionalJacobian<3, 3> H2) const {
+  const Matrix3 Jw = leftJacobian();
+  if (H1) H1->setZero();
+  if (H2) *H2 = Jw;
+  return Jw * v;
+}
+
 }  // namespace so3
 
 //******************************************************************************
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 8d4401c31..bd54f0cc4 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -26,7 +26,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
-#include <cmath>
 #include <vector>
 
 namespace gtsam {
@@ -160,7 +159,6 @@ class DexpFunctor : public ExpmapFunctor {
   static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
   double C;  // Ethan Eade's C constant
-  Matrix3 rightJacobian_;
 
  public:
   /// Constructor with element of Lie algebra so(3)
@@ -172,8 +170,11 @@ class DexpFunctor : public ExpmapFunctor {
   // Information Theory, and Lie Groups", Volume 2, 2008.
   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
   // This maps a perturbation v in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v) */
-  const Matrix3& dexp() const { return rightJacobian_; }
+  // a perturbation on the manifold Expmap(dexp * v)
+  GTSAM_EXPORT Matrix3 rightJacobian() const;
+
+  /// differential of expmap == right Jacobian
+  GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
 
   /// Multiplies with dexp(), with optional derivatives
   GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
@@ -186,8 +187,12 @@ class DexpFunctor : public ExpmapFunctor {
                                     OptionalJacobian<3, 3> H2 = {}) const;
 
   // Compute the left Jacobian for Exponential map in SO(3)
-  // Note precomputed, as not used as much
-  Matrix3 leftJacobian() const;
+  GTSAM_EXPORT Matrix3 leftJacobian() const;
+
+  /// Multiplies with leftJacobian(), with optional derivatives
+  GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
+                                         OptionalJacobian<3, 3> H1 = {},
+                                         OptionalJacobian<3, 3> H2 = {}) const;
 };
 }  //  namespace so3
 

From c7f651d98db1f0a7d3bea7b1a047cd1e4f93c7e1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 00:50:38 -0500
Subject: [PATCH 126/362] applyLeftJacobian

---
 gtsam/geometry/SO3.cpp           | 67 +++++++++++++++++++++-----------
 gtsam/geometry/SO3.h             |  6 +++
 gtsam/geometry/tests/testSO3.cpp | 24 ++++++++++++
 3 files changed, 74 insertions(+), 23 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index bc2e1e6ec..13600f884 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -26,6 +26,7 @@
 
 #include <Eigen/SVD>
 #include <cmath>
+#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -98,35 +99,47 @@ Matrix3 DexpFunctor::rightJacobian() const {
   }
 }
 
-// Derivative of w x w x v in w:
-static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) {
-  return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose();
+Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
+  // Wv = omega x * v
+  const Vector3 Wv = gtsam::cross(omega, v);
+  if (H) {
+    // 1x3 Jacobian of B with respect to omega
+    const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
+    // Apply product rule:
+    *H = Wv * HB - B * skewSymmetric(v);
+  }
+  return B * Wv;
+}
+
+Vector3 DexpFunctor::doubleCross(const Vector3& v,
+                                 OptionalJacobian<3, 3> H) const {
+  // WWv = omega x (omega x * v)
+  Matrix3 doubleCrossJacobian;
+  const Vector3 WWv =
+      gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
+  if (H) {
+    // 1x3 Jacobian of C with respect to omega
+    const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
+    // Apply product rule:
+    *H = WWv * HC + C * doubleCrossJacobian;
+  }
+  return C * WWv;
 }
 
 // Multiplies v with left Jacobian through vector operations only.
-// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once
-// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  // Wv = omega x * v, with Jacobian -V in w
-  const Vector3 Wv = gtsam::cross(omega, v);
-
   if (nearZero) {
     if (H1) *H1 = 0.5 * skewSymmetric(v);
     if (H2) *H2 = I_3x3 - 0.5 * W;
-    return v - 0.5 * Wv;
+    return v - 0.5 * gtsam::cross(omega, v);
   } else {
-    // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
-    const Vector3 WWv = gtsam::cross(omega, Wv);
-    if (H1) {
-      // 1x3 Jacobians of B and C with respect to theta
-      const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
-      const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
-      *H1 = -Wv * HB + B * skewSymmetric(v) +
-            C * doubleCrossJacobian(omega, v) + WWv * HC;
-    }
+    Matrix3 D_BWv_omega, D_CWWv_omega;
+    const Vector3 BWv = cross(v, D_BWv_omega);
+    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
+    if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
     if (H2) *H2 = rightJacobian();
-    return v - B * Wv + C * WWv;
+    return v - BWv + CWWv;
   }
 }
 
@@ -154,10 +167,18 @@ Matrix3 DexpFunctor::leftJacobian() const {
 Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
                                        OptionalJacobian<3, 3> H1,
                                        OptionalJacobian<3, 3> H2) const {
-  const Matrix3 Jw = leftJacobian();
-  if (H1) H1->setZero();
-  if (H2) *H2 = Jw;
-  return Jw * v;
+  if (nearZero) {
+    if (H1) *H1 = - 0.5 * skewSymmetric(v);
+    if (H2) *H2 = I_3x3 + 0.5 * W;
+    return v + 0.5 * gtsam::cross(omega, v);
+  } else {
+    Matrix3 D_BWv_omega, D_CWWv_omega;
+    const Vector3 BWv = cross(v, D_BWv_omega);
+    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
+    if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
+    if (H2) *H2 = leftJacobian();
+    return v + BWv + CWWv;
+  }
 }
 
 }  // namespace so3
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index bd54f0cc4..8766fecbe 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -160,6 +160,12 @@ class DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
   double C;  // Ethan Eade's C constant
 
+  /// Computes B * (omega x v).
+  Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Computes C * (omega x (omega x v)).
+  Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
  public:
   /// Constructor with element of Lie algebra so(3)
   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 27c143d31..8773e287a 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
+#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;
@@ -326,6 +327,29 @@ TEST(SO3, ApplyDexp) {
   }
 }
 
+//******************************************************************************
+TEST(SO3, ApplyLeftJacobian) {
+  Matrix aH1, aH2;
+  for (bool nearZeroApprox : {false, true}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
+        };
+    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZeroApprox);
+      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+                        Vector3(0.4, 0.3, 0.2)}) {
+        CHECK(assert_equal(Vector3(local.leftJacobian() * v),
+                            local.applyLeftJacobian(v, aH1, aH2)));
+        CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        CHECK(assert_equal(local.leftJacobian(), aH2));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;

From 82e13806035327385d66b13bbbe36bb1d91b87dd Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 01:53:02 -0500
Subject: [PATCH 127/362] Cleanup

---
 gtsam/geometry/Pose3.cpp            | 30 ++++++++++++-----------------
 gtsam/geometry/tests/testPoint3.cpp |  2 --
 gtsam/geometry/tests/testSO3.cpp    |  1 -
 3 files changed, 12 insertions(+), 21 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 16ea55665..d87e3164c 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
   const Vector3 w = xi.head<3>(), v = xi.tail<3>();
 
   // Compute rotation using Expmap
-  Rot3 R = Rot3::Expmap(w);
+  Matrix3 Jw;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr);
 
   // Compute translation and optionally its Jacobian in w
   Matrix3 Q;
   const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
 
   if (Hxi) {
-    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
     *Hxi << Jw, Z_3x3,
              Q, Jw;
   }
@@ -246,34 +246,27 @@ class ExpmapFunctor : public so3::DexpFunctor {
   static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
 
  public:
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false,
-                bool includeHigherOrder = false)
-      : so3::DexpFunctor(omega, nearZeroApprox),
-        includeHigherOrder(includeHigherOrder) {}
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
+      : so3::DexpFunctor(omega, nearZeroApprox) {}
 
+  // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
+  // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
+  // how to compute mess below from its derivatives in w and v.
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
 
     if (!nearZero) {
       // Simplified from closed-form formula in Barfoot14tro eq. (102)
-      // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v
       return -0.5 * V + C * (W * V + V * W - WVW) +
              (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) -
              0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW);
     } else {
-      Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW);
-      Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW);
-
-      if (includeHigherOrder) {
-        Q += one_one_hundred_twentieth * (WVW * W + W * WVW);
-      }
-      return Q;
+      return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
+             one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
+             one_one_hundred_twentieth * (WVW * W + W * WVW);
     }
   }
-
- private:
-  bool includeHigherOrder;
 };
 }  // namespace pose3
 
@@ -298,7 +291,8 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   if (nearZero) {
     return v + 0.5 * w.cross(v);
   } else {
-    // Geometric intuition and faster than using functor.
+    // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but
+    // formulas below convey geometric insight and creating functor is not free.
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index bb49486a8..b3b751ea1 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -18,8 +18,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Point3.h>
-#include "gtsam/base/Matrix.h"
-#include "gtsam/base/OptionalJacobian.h"
 
 using namespace std::placeholders;
 using namespace gtsam;
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 8773e287a..1e105ceca 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
-#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;

From 039c9b15426287e3669c5e1ffaaf50a2ce505e29 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 15 Dec 2024 12:29:31 -0500
Subject: [PATCH 128/362] add getter for sparse_table_

---
 gtsam/discrete/TableFactor.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index d27c4740c..5ddb4ab43 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -99,7 +99,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   typedef Eigen::SparseVector<double>::InnerIterator SparseIt;
   typedef std::vector<std::pair<DiscreteValues, double>> AssignValList;
 
- public:
   /// @name Standard Constructors
   /// @{
 
@@ -156,6 +155,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   // /// @name Standard Interface
   // /// @{
 
+  /// Getter for the underlying sparse vector
+  Eigen::SparseVector<double> sparseTable() const { return sparse_table_; }
+
   /// Evaluate probability distribution, is just look up in TableFactor.
   double evaluate(const Assignment<Key>& values) const override;
 

From 293c29ebf8f2f706a1dd6857fc5b30593eed5e36 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 15 Dec 2024 12:30:11 -0500
Subject: [PATCH 129/362] update toDecisionTreeFactor to use reverse key format
 so it's faster

---
 gtsam/discrete/TableFactor.cpp | 29 +++++++++++++++++++++++++----
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index de1e1f867..521ca4d47 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -252,12 +252,33 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
-  std::vector<double> table;
+
+  DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend());
+  std::vector<std::pair<uint64_t, double>> pair_table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
-    table.push_back(sparse_table_.coeff(i));
+    std::stringstream ss;
+    for (auto&& [key, _] : rdkeys) {
+      ss << keyValueForIndex(key, i);
+    }
+    // k will be in reverse key order already
+    uint64_t k = std::strtoull(ss.str().c_str(), NULL, 10);
+    pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
-  // NOTE(Varun): This constructor is really expensive!!
-  DecisionTreeFactor f(dkeys, table);
+
+  // Sort based on key so we get values in reverse key order.
+  std::sort(
+      pair_table.begin(), pair_table.end(),
+      [](const std::pair<uint64_t, double>& a,
+         const std::pair<uint64_t, double>& b) { return a.first <= b.first; });
+
+  // Create the table vector
+  std::vector<double> table;
+  std::for_each(pair_table.begin(), pair_table.end(),
+                [&table](const std::pair<uint64_t, double>& pair) {
+                  table.push_back(pair.second);
+                });
+
+  DecisionTreeFactor f(rdkeys, table);
   return f;
 }
 

From 8fefbbf06a751bdd9e637f72571229145be77b13 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 15 Dec 2024 16:03:10 -0500
Subject: [PATCH 130/362] fix toDecisionTreeFactor so the keys are ordered
 correctly

---
 gtsam/discrete/TableFactor.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 521ca4d47..63e9e5d6b 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -153,8 +153,7 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DecisionTreeFactor& dtf)
-    : TableFactor(dtf.discreteKeys(),
-                  ComputeSparseTable(dtf.discreteKeys(), dtf)) {}
+    : TableFactor(dtf.discreteKeys(), dtf) {}
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteConditional& c)
@@ -278,7 +277,8 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
                   table.push_back(pair.second);
                 });
 
-  DecisionTreeFactor f(rdkeys, table);
+  AlgebraicDecisionTree<Key> tree(rdkeys, table);
+  DecisionTreeFactor f(dkeys, tree);
   return f;
 }
 

From fa1249bf14dc815402061bc826b7adbfc2db620f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 17:53:42 -0500
Subject: [PATCH 131/362] Add export

---
 gtsam/geometry/Point3.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 0fa53b1b2..d5c32816e 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -61,9 +61,9 @@ GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                           OptionalJacobian<3, 3> H_q = {});
 
 /// double cross product @return p x (p x q)
-Point3 doubleCross(const Point3& p, const Point3& q,
-                   OptionalJacobian<3, 3> H1 = {},
-                   OptionalJacobian<3, 3> H2 = {});
+GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
+                                OptionalJacobian<3, 3> H1 = {},
+                                OptionalJacobian<3, 3> H2 = {});
 
 /// dot product
 GTSAM_EXPORT double dot(const Point3& p, const Point3& q,

From e96d8487e493d9cce4f60a09ab7ec20c2412210e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 17:54:30 -0500
Subject: [PATCH 132/362] New constants for cross and doubleCross

---
 gtsam/geometry/SO3.cpp           | 65 ++++++++++++------------
 gtsam/geometry/SO3.h             | 22 ++++++---
 gtsam/geometry/tests/testSO3.cpp | 84 +++++++++++++++++++++++---------
 3 files changed, 108 insertions(+), 63 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 13600f884..e341911b9 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
 }
 
 void ExpmapFunctor::init(bool nearZeroApprox) {
-  WW = W * W;
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
-    sin_theta = std::sin(theta);
-    const double s2 = std::sin(theta / 2.0);
-    one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    const double sin_theta = std::sin(theta);
     A = sin_theta / theta;
+    const double s2 = std::sin(theta / 2.0);
+    const double one_minus_cos =
+        2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
     B = one_minus_cos / theta2;
+  } else {
+    // Limits as theta -> 0:
+    A = 1.0;
+    B = 0.5;
   }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
-    : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
-  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
-  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+    : theta2(omega.dot(omega)),
+      theta(std::sqrt(theta2)),
+      W(skewSymmetric(omega)),
+      WW(W * W) {
   init(nearZeroApprox);
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
-    : theta2(angle * angle), theta(angle) {
-  const double ax = axis.x(), ay = axis.y(), az = axis.z();
-  W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
-  W *= angle;
+    : theta2(angle * angle),
+      theta(angle),
+      W(skewSymmetric(axis * angle)),
+      WW(W * W) {
   init(nearZeroApprox);
 }
 
-SO3 ExpmapFunctor::expmap() const {
-  if (nearZero)
-    return SO3(I_3x3 + W + 0.5 * WW);
-  else
-    return SO3(I_3x3 + A * W + B * WW);
-}
+SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  C = (1 - A) / theta2;
+  C = nearZero ? one_sixth : (1 - A) / theta2;
+  D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
+  E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
 }
 
 Matrix3 DexpFunctor::rightJacobian() const {
   if (nearZero) {
-    return I_3x3 - 0.5 * W;  // + one_sixth * WW;
+    return I_3x3 - B * W;  // + C * WW;
   } else {
     return I_3x3 - B * W + C * WW;
   }
@@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
   // Wv = omega x * v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
-    // 1x3 Jacobian of B with respect to omega
-    const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
     // Apply product rule:
-    *H = Wv * HB - B * skewSymmetric(v);
+    // D * omega.transpose() is 1x3 Jacobian of B with respect to omega
+    // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v)
+    *H = Wv * D * omega.transpose() - B * skewSymmetric(v);
   }
   return B * Wv;
 }
@@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
   const Vector3 WWv =
       gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
   if (H) {
-    // 1x3 Jacobian of C with respect to omega
-    const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
     // Apply product rule:
-    *H = WWv * HC + C * doubleCrossJacobian;
+    // E * omega.transpose() is 1x3 Jacobian of C with respect to omega
+    // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v)
+    *H = WWv * E * omega.transpose() + C * doubleCrossJacobian;
   }
   return C * WWv;
 }
@@ -134,12 +136,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
     if (H2) *H2 = I_3x3 - 0.5 * W;
     return v - 0.5 * gtsam::cross(omega, v);
   } else {
-    Matrix3 D_BWv_omega, D_CWWv_omega;
+  Matrix3 D_BWv_omega, D_CWWv_omega;
     const Vector3 BWv = cross(v, D_BWv_omega);
     const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
     if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
-    if (H2) *H2 = rightJacobian();
-    return v - BWv + CWWv;
+  if (H2) *H2 = rightJacobian();
+  return v - BWv + CWWv;
   }
 }
 
@@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
 template <>
 GTSAM_EXPORT
 Matrix3 SO3::Hat(const Vector3& xi) {
-  // skew symmetric matrix X = xi^
-  Matrix3 Y = Z_3x3;
-  Y(0, 1) = -xi(2);
-  Y(0, 2) = +xi(1);
-  Y(1, 2) = -xi(0);
-  return Y - Y.transpose();
+  return skewSymmetric(xi);
 }
 
 //******************************************************************************
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 8766fecbe..6403c3ae0 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 /// Functor implementing Exponential map
 class GTSAM_EXPORT ExpmapFunctor {
  protected:
-  const double theta2;
-  Matrix3 W, WW;
-  double A, B; // Ethan Eade's constants
+  const double theta2, theta;
+  const Matrix3 W, WW;
   bool nearZero;
-  double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
+  // Ethan Eade's constants:
+  double A; // A = sin(theta) / theta or 1 for theta->0
+  double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
 
   void init(bool nearZeroApprox = false);
 
@@ -156,17 +157,19 @@ class GTSAM_EXPORT ExpmapFunctor {
 /// Functor that implements Exponential map *and* its derivatives
 class DexpFunctor : public ExpmapFunctor {
  protected:
-  static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
-  double C;  // Ethan Eade's C constant
+  double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
+  // Constants used in cross and doubleCross
+  double D;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
+  double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
+ public:
   /// Computes B * (omega x v).
   Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
   /// Computes C * (omega x (omega x v)).
   Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
- public:
   /// Constructor with element of Lie algebra so(3)
   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
                                     bool nearZeroApprox = false);
@@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor {
   GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
                                          OptionalJacobian<3, 3> H1 = {},
                                          OptionalJacobian<3, 3> H2 = {}) const;
+
+ protected:
+  static constexpr double one_sixth = 1.0 / 6.0;
+  static constexpr double _one_twelfth = -1.0 / 12.0;
+  static constexpr double _one_sixtieth = -1.0 / 60.0;
 };
 }  //  namespace so3
 
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 1e105ceca..e8de2e93d 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
+namespace test_cases {
+std::vector<Vector3> nearZeros{
+    {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
+std::vector<Vector3> omegas{
+    {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
+std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
+}  // namespace test_cases
+
+//******************************************************************************
+TEST(SO3, Cross) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).cross(v);
+        };
+    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
+    for (Vector3 omega : omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
+        local.cross(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, DoubleCross) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).doubleCross(v);
+        };
+    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
+    for (Vector3 omega : omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
+        local.doubleCross(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+    for (Vector3 omega : test_cases::omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -329,18 +373,16 @@ TEST(SO3, ApplyDexp) {
 //******************************************************************************
 TEST(SO3, ApplyLeftJacobian) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {false, true}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+    for (Vector3 omega : test_cases::omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
         CHECK(assert_equal(Vector3(local.leftJacobian() * v),
-                            local.applyLeftJacobian(v, aH1, aH2)));
+                           local.applyLeftJacobian(v, aH1, aH2)));
         CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
         CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
         CHECK(assert_equal(local.leftJacobian(), aH2));
@@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) {
 //******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
+    for (Vector3 omega : test_cases::omegas) {
+      so3::DexpFunctor local(omega, nearZero);
       Matrix invDexp = local.dexp().inverse();
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+      for (Vector3 v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(invDexp * v),
                             local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));

From 9b0f3c3b38f9eaaa2cc2e9be24a0fa94a3e5c04b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 17:55:22 -0500
Subject: [PATCH 133/362] Incredible simplification using E and F

---
 gtsam/geometry/Pose3.cpp | 21 ++++++++-------------
 1 file changed, 8 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index d87e3164c..7faf433bb 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -243,11 +243,15 @@ namespace pose3 {
 class ExpmapFunctor : public so3::DexpFunctor {
  private:
   static constexpr double one_twenty_fourth = 1.0 / 24.0;
-  static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
+
+  // Constant used in computeQ
+  double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
 
  public:
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
-      : so3::DexpFunctor(omega, nearZeroApprox) {}
+      : so3::DexpFunctor(omega, nearZeroApprox) {
+        F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2;
+      }
 
   // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
   // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
@@ -255,17 +259,8 @@ class ExpmapFunctor : public so3::DexpFunctor {
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
-
-    if (!nearZero) {
-      // Simplified from closed-form formula in Barfoot14tro eq. (102)
-      return -0.5 * V + C * (W * V + V * W - WVW) +
-             (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) -
-             0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW);
-    } else {
-      return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
-             one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
-             one_one_hundred_twentieth * (WVW * W + W * WVW);
-    }
+    return -0.5 * V + C * (W * V + V * W - WVW) +
+           F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
   }
 };
 }  // namespace pose3

From e3906162e7604cb52f2fced9ed4bd2f57f17b942 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sun, 15 Dec 2024 15:44:45 -0800
Subject: [PATCH 134/362] projection and SFM for 2d poses

---
 gtsam/slam/PlanarProjectionFactor.h           | 174 +++++++++++++
 gtsam/slam/PlanarSFMFactor.h                  | 195 ++++++++++++++
 gtsam/slam/slam.i                             |  24 ++
 .../slam/tests/testPlanarProjectionFactor.cpp | 156 +++++++++++
 gtsam/slam/tests/testPlanarSFMFactor.cpp      | 243 ++++++++++++++++++
 5 files changed, 792 insertions(+)
 create mode 100644 gtsam/slam/PlanarProjectionFactor.h
 create mode 100644 gtsam/slam/PlanarSFMFactor.h
 create mode 100644 gtsam/slam/tests/testPlanarProjectionFactor.cpp
 create mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp

diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
new file mode 100644
index 000000000..9589c36d5
--- /dev/null
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -0,0 +1,174 @@
+/**
+ * Similar to GenericProjectionFactor, but:
+ *
+ * * 2d pose variable (robot on the floor)
+ * * constant landmarks
+ * * batched input
+ * * numeric differentiation
+ *
+ * This factor is useful for high-school robotics competitions,
+ * which run robots on the floor, with use fixed maps and fiducial
+ * markers.  The camera offset and calibration are fixed, perhaps
+ * found using PlanarSFMFactor.
+ *
+ * The python version of this factor uses batches, to save on calls
+ * across the C++/python boundary, but here the only extra cost
+ * is instantiating the camera, so there's no batch.
+ *
+ * @see https://www.firstinspires.org/
+ * @see PlanarSFMFactor.h
+ *
+ * @file PlanarSFMFactor.h
+ * @brief for planar smoothing
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+    /**
+     * @class PlanarProjectionFactor
+     * @brief Camera projection for robot on the floor.
+    */
+    class PlanarProjectionFactor : public NoiseModelFactorN<Pose2> {
+        typedef NoiseModelFactorN<Pose2> Base;   
+        static const Pose3 CAM_COORD;
+
+    protected:
+
+        Point3 landmark_; // landmark
+        Point2 measured_; // pixel measurement
+        Pose3 offset_; // camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+
+    public:
+        // Provide access to the Matrix& version of evaluateError:
+        using Base::evaluateError;
+
+        PlanarProjectionFactor() {}
+
+        /**
+         * @param landmarks point in the world
+         * @param measured corresponding point in the camera frame
+         * @param offset constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose in the z=0 plane
+         */
+        PlanarProjectionFactor(
+            const Point3& landmark,
+            const Point2& measured,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model,
+            Key poseKey)
+            : NoiseModelFactorN(model, poseKey),
+            landmark_(landmark),
+            measured_(measured),
+            offset_(offset),
+            calib_(calib)
+        {
+            assert(2 == model->dim());
+        }
+
+        ~PlanarProjectionFactor() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this)));
+        }
+
+        Point2 h2(const Pose2& pose, OptionalMatrixType H1) const {
+            // this is x-forward z-up
+            gtsam::Matrix H0;
+            Pose3 offset_pose = Pose3(pose).compose(offset_, H0);
+            // std::cout << "\nH0\n" << H0 << "\n";
+            // this is z-forward y-down
+            gtsam::Matrix H00;
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+            // std::cout << "\nH00\n" << H00 << "\n";
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
+            if (H1) {
+                // Dpose is 2x6 (R,t)
+                // H1 is 2x3 (x, y, theta)
+                gtsam::Matrix Dpose;
+                Point2 result = camera.project(landmark_, Dpose);
+                // std::cout << "\nDpose 1\n" << Dpose << "\n";
+                Dpose = Dpose * H00 * H0;
+                // std::cout << "\nDpose 2\n" << Dpose << "\n";
+                *H1 = Matrix::Zero(2,3);
+                (*H1)(0,0) = Dpose(0,3); // du/dx
+                (*H1)(1,0) = Dpose(1,3); // dv/dx
+                (*H1)(0,1) = Dpose(0,4); // du/dy
+                (*H1)(1,1) = Dpose(1,4); // dv/dy
+                (*H1)(0,2) = Dpose(0,2); // du/dyaw
+                (*H1)(1,2) = Dpose(1,2); // dv/dyaw
+                return result;
+            } else {
+                return camera.project(landmark_, {}, {}, {});
+            }
+        }
+
+        Point2 h(const Pose2& pose) const {
+            // this is x-forward z-up
+            Pose3 offset_pose = Pose3(pose).compose(offset_);
+            // this is z-forward y-down
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
+            return camera.project2(landmark_);
+        }
+
+        Vector evaluateError(
+            const Pose2& pose,
+            OptionalMatrixType H1 = OptionalNone) const override {
+            try {
+                return h2(pose, H1) - measured_;
+                
+                // Point2 result = h(pose) - measured_;
+                // if (H1) *H1 = numericalDerivative11<Point2, Pose2>(
+                //     [&](const Pose2& p) {return h(p);},
+                //     pose);
+                // return result;
+            }
+            catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                std::cout << "landmark " << landmark_ << "\n";
+                std::cout << "pose " << pose << "\n";
+                std::cout << "offset " << offset_ << "\n";
+                std::cout << "calib " << calib_ << "\n";
+                // TODO: check the size here
+                if (H1) *H1 = Matrix::Zero(2, 3);
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib_.fx());
+            }
+        }
+    };
+
+    // camera "zero" is facing +z; this turns it to face +x
+    const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    template<>
+    struct traits<PlanarProjectionFactor> :
+        public Testable<PlanarProjectionFactor > {
+    };
+
+} // namespace gtsam
diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h
new file mode 100644
index 000000000..c39b40fef
--- /dev/null
+++ b/gtsam/slam/PlanarSFMFactor.h
@@ -0,0 +1,195 @@
+/**
+ * Similar to GeneralSFMFactor, but:
+ *
+ * * 2d pose variable (robot on the floor)
+ * * camera offset variable
+ * * constant landmarks
+ * * batched input
+ * * numeric differentiation
+ *
+ * This factor is useful to find camera calibration and placement, in
+ * a sort of "autocalibrate" mode.  Once a satisfactory solution is
+ * found, the PlanarProjectionFactor should be used for localization.
+ *
+ * The python version of this factor uses batches, to save on calls
+ * across the C++/python boundary, but here the only extra cost
+ * is instantiating the camera, so there's no batch.
+ *
+ * @see https://www.firstinspires.org/
+ * @see PlanarProjectionFactor.h
+ *
+ * @file PlanarSFMFactor.h
+ * @brief for planar smoothing with unknown calibration
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+    /**
+     * @class PlanarSFMFactor
+     * @brief Camera calibration for robot on the floor.
+     */
+    class PlanarSFMFactor : public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
+    private:
+        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;   
+        static const Pose3 CAM_COORD;
+
+    protected:
+
+        Point3 landmark_; // landmark
+        Point2 measured_; // pixel measurement
+
+    public:
+        // Provide access to the Matrix& version of evaluateError:
+        using Base::evaluateError;
+
+        PlanarSFMFactor() {}
+        /**
+         * @param landmarks point in the world
+         * @param measured corresponding point in the camera frame
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param offsetKey index of the 3d camera offset from the robot pose
+         * @param calibKey index of the camera calibration
+         */
+        PlanarSFMFactor(
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model,
+            Key poseKey,
+            Key offsetKey,
+            Key calibKey)
+            : NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
+            landmark_(landmark), measured_(measured)
+        {
+            assert(2 == model->dim());
+        }
+
+        ~PlanarSFMFactor() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this)));
+        }
+
+        Point2 h2(const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1,
+            OptionalMatrixType H2,
+            OptionalMatrixType H3
+            ) const {
+            // std::cout << "POSE: " << pose << "\n";
+            // std::cout << "OFFSET: " << offset << "\n";
+            // std::cout << "CALIB: " << calib << "\n";
+            // this is x-forward z-up
+            gtsam::Matrix H0;
+            Pose3 offset_pose = Pose3(pose).compose(offset, H0);
+            // this is z-forward y-down
+            gtsam::Matrix H00;
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
+            if (H1 || H2) {
+                gtsam::Matrix Dpose;
+                Point2 result = camera.project(landmark_, Dpose, {}, H3);
+                gtsam::Matrix DposeOffset = Dpose * H00;
+                if (H2)
+                    *H2 = DposeOffset; // a deep copy
+                if (H1) {
+                    gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
+                    *H1 = Matrix::Zero(2,3);
+                    (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx
+                    (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx
+                    (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy
+                    (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy
+                    (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw
+                    (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw
+                }
+                return result;
+            } else {
+                return camera.project(landmark_, {}, {}, {});
+            }    
+        }
+
+        Point2 h(const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib) const {
+            // std::cout << "POSE: " << pose << "\n";
+            // std::cout << "OFFSET: " << offset << "\n";
+            // std::cout << "CALIB: " << calib << "\n";
+            // this is x-forward z-up
+            Pose3 offset_pose = Pose3(pose).compose(offset);
+            // this is z-forward y-down
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
+            return camera.project2(landmark_);
+        }
+
+        Vector evaluateError(
+            const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType H2 = OptionalNone,
+            OptionalMatrixType H3 = OptionalNone
+        ) const override {
+            try {
+                return h2(pose,offset,calib,H1,H2,H3) - measured_;
+
+                // Point2 result = h(pose, offset, calib) - measured_;
+                // if (H1) *H1 = numericalDerivative31<Point2, Pose2, Pose3, Cal3DS2>(
+                //     [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);},
+                //     pose, offset, calib);
+                // if (H2) *H2 = numericalDerivative32<Point2, Pose2, Pose3, Cal3DS2>(
+                //     [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);},
+                //     pose, offset, calib);
+                // if (H3) *H3 = numericalDerivative33<Point2, Pose2, Pose3, Cal3DS2>(
+                //     [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);},
+                //     pose, offset, calib);
+                // return result;
+            }
+            catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                std::cout << "landmark " << landmark_ << "\n";
+                std::cout << "pose " << pose << "\n";
+                std::cout << "offset " << offset << "\n";
+                std::cout << "calib " << calib << "\n";
+                // TODO: what should these sizes be?
+                if (H1) *H1 = Matrix::Zero(2, 3);
+                if (H2) *H2 = Matrix::Zero(2, 6);
+                if (H3) *H3 = Matrix::Zero(2, 9);
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib.fx());
+            }
+        }
+    };
+
+    // camera "zero" is facing +z; this turns it to face +x
+    const Pose3 PlanarSFMFactor::CAM_COORD = Pose3(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    template<>
+    struct traits<PlanarSFMFactor> :
+        public Testable<PlanarSFMFactor > {
+    };
+
+} // namespace gtsam
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index a226f06ec..d69b9890e 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/PlanarProjectionFactor.h>
+virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::Pose3& offset,
+            const gtsam::Cal3DS2& calib,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey);
+  void serialize() const;
+};
+
 #include <gtsam/slam/ProjectionFactor.h>
 template <POSE, LANDMARK, CALIBRATION>
 virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
@@ -67,6 +79,18 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                        gtsam::Cal3Unified>
     GenericProjectionFactorCal3Unified;
 
+#include <gtsam/slam/PlanarSFMFactor.h>
+virtual class PlanarSFMFactor  : gtsam::NoiseModelFactor {
+  PlanarSFMFactor(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey,
+            size_t offsetKey,
+            size_t calibKey);
+  void serialize() const;
+};
+
 #include <gtsam/slam/GeneralSFMFactor.h>
 template <CAMERA, LANDMARK>
 virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
new file mode 100644
index 000000000..eafe2041f
--- /dev/null
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -0,0 +1,156 @@
+/**
+ * @file testPlanarProjectionFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarProjectionFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarProjectionFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+
+TEST(PlanarProjectionFactor, error1) {
+    // landmark is on the camera bore (which faces +x)
+    Point3 landmark(1, 0, 0);
+    // so pixel measurement is (cx, cy)
+    Point2 measured(200, 200);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    Values values;
+    Pose2 pose(0, 0, 0);
+    values.insert(X(0), pose);
+
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(1);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    const Matrix23 H1Expected = (Matrix23() << //
+        0, 200, 200,//
+        0, 0, 0).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, error2) {
+    // landmark is in the upper left corner
+    Point3 landmark(1, 1, 1);
+    // upper left corner in pixels
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(1);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    Matrix23 H1Expected = (Matrix23() << //
+        -200, 200, 400, //
+        -200, 0, 200).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, error3) {
+    // landmark is in the upper left corner
+    Point3 landmark(1, 1, 1);
+    // upper left corner in pixels
+    Point2 measured(0, 0);
+    Pose3 offset;
+    // distortion
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(1);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    Matrix23 H1Expected = (Matrix23() << //
+        -360, 280, 640, //
+        -360, 80, 440).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+        PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1;
+        factor.evaluateError(pose, H1);
+
+        Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
+            [&factor](const Pose2& p) {
+                return factor.evaluateError(p);},
+                pose);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+    }
+
+
+}
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+
diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp
new file mode 100644
index 000000000..22ec67561
--- /dev/null
+++ b/gtsam/slam/tests/testPlanarSFMFactor.cpp
@@ -0,0 +1,243 @@
+/**
+ * @file testPlanarSFMFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarSFMFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarSFMFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+using symbol_shorthand::C;
+using symbol_shorthand::K;
+
+TEST(PlanarSFMFactor, error1) {
+    // landmark is on the camera bore (facing +x)
+    Point3 landmark(1, 0, 0);
+    // so px is (cx, cy)
+    Point2 measured(200, 200);
+    // offset is identity
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    Values values;
+    Pose2 pose(0, 0, 0);
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    // du/dx etc for the pose2d
+    Matrix23 H1Expected = (Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        0, 0, 200, 0, 200, 0,//
+        0, -200, 0, 0, 0, 200).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    // du wrt calibration
+    // on-bore, f doesn't matter
+    // but c does
+    Matrix29 H3Expected = (Matrix29() <<//
+        0, 0, 0, 1, 0, 0, 0, 0, 0,//
+        0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarSFMFactor, error2) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    Matrix23 H1Expected = (Matrix23() <<//
+        -200, 200, 400,//
+        -200, 0, 200).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        -200, -200, 400, -200, 200, 0,//
+        200, -400, 200, -200, 0, 200).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    Matrix29 H3Expected = (Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarSFMFactor, error3) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    Matrix23 H1Expected = (Matrix23() <<//
+        -360, 280, 640,//
+        -360, 80, 440).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        -200, -440, 640, -360, 280, 80,//
+        200, -640, 440, -360, 80, 280).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    Matrix29 H3Expected = (Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+
+
+TEST(PlanarSFMFactor, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);    
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+        PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1, H2, H3;
+        factor.evaluateError(pose, offset, calib, H1, H2, H3);
+
+        Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+        CHECK(assert_equal(expectedH2, H2, 1e-6));
+        CHECK(assert_equal(expectedH3, H3, 1e-6));
+    }
+}
+
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

From 06d45c416e2934e9c97ccf598df6820ffea7248a Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sun, 15 Dec 2024 15:44:45 -0800
Subject: [PATCH 135/362] projection and SFM for 2d poses

---
 gtsam/slam/PlanarProjectionFactor.h           | 154 +++++++++++
 gtsam/slam/PlanarSFMFactor.h                  | 165 ++++++++++++
 gtsam/slam/slam.i                             |  24 ++
 .../slam/tests/testPlanarProjectionFactor.cpp | 156 +++++++++++
 gtsam/slam/tests/testPlanarSFMFactor.cpp      | 243 ++++++++++++++++++
 5 files changed, 742 insertions(+)
 create mode 100644 gtsam/slam/PlanarProjectionFactor.h
 create mode 100644 gtsam/slam/PlanarSFMFactor.h
 create mode 100644 gtsam/slam/tests/testPlanarProjectionFactor.cpp
 create mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp

diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
new file mode 100644
index 000000000..49d1f7cac
--- /dev/null
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -0,0 +1,154 @@
+/**
+ * Similar to GenericProjectionFactor, but:
+ *
+ * * 2d pose variable (robot on the floor)
+ * * constant landmarks
+ * * batched input
+ * * numeric differentiation
+ *
+ * This factor is useful for high-school robotics competitions,
+ * which run robots on the floor, with use fixed maps and fiducial
+ * markers.  The camera offset and calibration are fixed, perhaps
+ * found using PlanarSFMFactor.
+ *
+ * The python version of this factor uses batches, to save on calls
+ * across the C++/python boundary, but here the only extra cost
+ * is instantiating the camera, so there's no batch.
+ *
+ * @see https://www.firstinspires.org/
+ * @see PlanarSFMFactor.h
+ *
+ * @file PlanarSFMFactor.h
+ * @brief for planar smoothing
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+    /**
+     * @class PlanarProjectionFactor
+     * @brief Camera projection for robot on the floor.
+     */
+    class PlanarProjectionFactor : public NoiseModelFactorN<Pose2> {
+        typedef NoiseModelFactorN<Pose2> Base;
+        static const Pose3 CAM_COORD;
+
+    protected:
+
+        Point3 landmark_; // landmark
+        Point2 measured_; // pixel measurement
+        Pose3 offset_; // camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+
+    public:
+        // Provide access to the Matrix& version of evaluateError:
+        using Base::evaluateError;
+
+        PlanarProjectionFactor() {}
+
+        /**
+         * @param landmarks point in the world
+         * @param measured corresponding point in the camera frame
+         * @param offset constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose in the z=0 plane
+         */
+        PlanarProjectionFactor(
+            const Point3& landmark,
+            const Point2& measured,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model,
+            Key poseKey)
+            : NoiseModelFactorN(model, poseKey),
+            landmark_(landmark),
+            measured_(measured),
+            offset_(offset),
+            calib_(calib)
+        {
+            assert(2 == model->dim());
+        }
+
+        ~PlanarProjectionFactor() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this)));
+        }
+
+        Point2 h2(const Pose2& pose, OptionalMatrixType H1) const {
+            // this is x-forward z-up
+            gtsam::Matrix H0;
+            Pose3 offset_pose = Pose3(pose).compose(offset_, H0);
+            // this is z-forward y-down
+            gtsam::Matrix H00;
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
+            if (H1) {
+                // Dpose is 2x6 (R,t)
+                // H1 is 2x3 (x, y, theta)
+                gtsam::Matrix Dpose;
+                Point2 result = camera.project(landmark_, Dpose);
+                Dpose = Dpose * H00 * H0;
+                *H1 = Matrix::Zero(2, 3);
+                (*H1)(0, 0) = Dpose(0, 3); // du/dx
+                (*H1)(1, 0) = Dpose(1, 3); // dv/dx
+                (*H1)(0, 1) = Dpose(0, 4); // du/dy
+                (*H1)(1, 1) = Dpose(1, 4); // dv/dy
+                (*H1)(0, 2) = Dpose(0, 2); // du/dyaw
+                (*H1)(1, 2) = Dpose(1, 2); // dv/dyaw
+                return result;
+            }
+            else {
+                return camera.project(landmark_, {}, {}, {});
+            }
+        }
+
+        Vector evaluateError(
+            const Pose2& pose,
+            OptionalMatrixType H1 = OptionalNone) const override {
+            try {
+                return h2(pose, H1) - measured_;
+            } catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                std::cout << "landmark " << landmark_ << "\n";
+                std::cout << "pose " << pose << "\n";
+                std::cout << "offset " << offset_ << "\n";
+                std::cout << "calib " << calib_ << "\n";
+                if (H1) *H1 = Matrix::Zero(2, 3);
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib_.fx());
+            }
+        }
+    };
+
+    // camera "zero" is facing +z; this turns it to face +x
+    const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    template<>
+    struct traits<PlanarProjectionFactor> :
+        public Testable<PlanarProjectionFactor > {
+    };
+
+} // namespace gtsam
diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h
new file mode 100644
index 000000000..bdcbb6ee2
--- /dev/null
+++ b/gtsam/slam/PlanarSFMFactor.h
@@ -0,0 +1,165 @@
+/**
+ * Similar to GeneralSFMFactor, but:
+ *
+ * * 2d pose variable (robot on the floor)
+ * * camera offset variable
+ * * constant landmarks
+ * * batched input
+ * * numeric differentiation
+ *
+ * This factor is useful to find camera calibration and placement, in
+ * a sort of "autocalibrate" mode.  Once a satisfactory solution is
+ * found, the PlanarProjectionFactor should be used for localization.
+ *
+ * The python version of this factor uses batches, to save on calls
+ * across the C++/python boundary, but here the only extra cost
+ * is instantiating the camera, so there's no batch.
+ *
+ * @see https://www.firstinspires.org/
+ * @see PlanarProjectionFactor.h
+ *
+ * @file PlanarSFMFactor.h
+ * @brief for planar smoothing with unknown calibration
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+    /**
+     * @class PlanarSFMFactor
+     * @brief Camera calibration for robot on the floor.
+     */
+    class PlanarSFMFactor : public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
+    private:
+        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;   
+        static const Pose3 CAM_COORD;
+
+    protected:
+
+        Point3 landmark_; // landmark
+        Point2 measured_; // pixel measurement
+
+    public:
+        // Provide access to the Matrix& version of evaluateError:
+        using Base::evaluateError;
+
+        PlanarSFMFactor() {}
+        /**
+         * @param landmarks point in the world
+         * @param measured corresponding point in the camera frame
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param offsetKey index of the 3d camera offset from the robot pose
+         * @param calibKey index of the camera calibration
+         */
+        PlanarSFMFactor(
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model,
+            Key poseKey,
+            Key offsetKey,
+            Key calibKey)
+            : NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
+            landmark_(landmark), measured_(measured)
+        {
+            assert(2 == model->dim());
+        }
+
+        ~PlanarSFMFactor() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this)));
+        }
+
+        Point2 h2(const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1,
+            OptionalMatrixType H2,
+            OptionalMatrixType H3
+            ) const {
+            // this is x-forward z-up
+            gtsam::Matrix H0;
+            Pose3 offset_pose = Pose3(pose).compose(offset, H0);
+            // this is z-forward y-down
+            gtsam::Matrix H00;
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
+            if (H1 || H2) {
+                gtsam::Matrix Dpose;
+                Point2 result = camera.project(landmark_, Dpose, {}, H3);
+                gtsam::Matrix DposeOffset = Dpose * H00;
+                if (H2)
+                    *H2 = DposeOffset; // a deep copy
+                if (H1) {
+                    gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
+                    *H1 = Matrix::Zero(2,3);
+                    (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx
+                    (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx
+                    (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy
+                    (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy
+                    (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw
+                    (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw
+                }
+                return result;
+            } else {
+                return camera.project(landmark_, {}, {}, {});
+            }    
+        }
+
+        Vector evaluateError(
+            const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType H2 = OptionalNone,
+            OptionalMatrixType H3 = OptionalNone
+        ) const override {
+            try {
+                return h2(pose,offset,calib,H1,H2,H3) - measured_;
+            }
+            catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                std::cout << "landmark " << landmark_ << "\n";
+                std::cout << "pose " << pose << "\n";
+                std::cout << "offset " << offset << "\n";
+                std::cout << "calib " << calib << "\n";
+                if (H1) *H1 = Matrix::Zero(2, 3);
+                if (H2) *H2 = Matrix::Zero(2, 6);
+                if (H3) *H3 = Matrix::Zero(2, 9);
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib.fx());
+            }
+        }
+    };
+
+    // camera "zero" is facing +z; this turns it to face +x
+    const Pose3 PlanarSFMFactor::CAM_COORD = Pose3(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    template<>
+    struct traits<PlanarSFMFactor> :
+        public Testable<PlanarSFMFactor > {
+    };
+
+} // namespace gtsam
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index a226f06ec..d69b9890e 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/PlanarProjectionFactor.h>
+virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::Pose3& offset,
+            const gtsam::Cal3DS2& calib,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey);
+  void serialize() const;
+};
+
 #include <gtsam/slam/ProjectionFactor.h>
 template <POSE, LANDMARK, CALIBRATION>
 virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
@@ -67,6 +79,18 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                        gtsam::Cal3Unified>
     GenericProjectionFactorCal3Unified;
 
+#include <gtsam/slam/PlanarSFMFactor.h>
+virtual class PlanarSFMFactor  : gtsam::NoiseModelFactor {
+  PlanarSFMFactor(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey,
+            size_t offsetKey,
+            size_t calibKey);
+  void serialize() const;
+};
+
 #include <gtsam/slam/GeneralSFMFactor.h>
 template <CAMERA, LANDMARK>
 virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
new file mode 100644
index 000000000..eafe2041f
--- /dev/null
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -0,0 +1,156 @@
+/**
+ * @file testPlanarProjectionFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarProjectionFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarProjectionFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+
+TEST(PlanarProjectionFactor, error1) {
+    // landmark is on the camera bore (which faces +x)
+    Point3 landmark(1, 0, 0);
+    // so pixel measurement is (cx, cy)
+    Point2 measured(200, 200);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    Values values;
+    Pose2 pose(0, 0, 0);
+    values.insert(X(0), pose);
+
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(1);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    const Matrix23 H1Expected = (Matrix23() << //
+        0, 200, 200,//
+        0, 0, 0).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, error2) {
+    // landmark is in the upper left corner
+    Point3 landmark(1, 1, 1);
+    // upper left corner in pixels
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(1);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    Matrix23 H1Expected = (Matrix23() << //
+        -200, 200, 400, //
+        -200, 0, 200).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, error3) {
+    // landmark is in the upper left corner
+    Point3 landmark(1, 1, 1);
+    // upper left corner in pixels
+    Point2 measured(0, 0);
+    Pose3 offset;
+    // distortion
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(1);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    Matrix23 H1Expected = (Matrix23() << //
+        -360, 280, 640, //
+        -360, 80, 440).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+        PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1;
+        factor.evaluateError(pose, H1);
+
+        Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
+            [&factor](const Pose2& p) {
+                return factor.evaluateError(p);},
+                pose);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+    }
+
+
+}
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+
diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp
new file mode 100644
index 000000000..22ec67561
--- /dev/null
+++ b/gtsam/slam/tests/testPlanarSFMFactor.cpp
@@ -0,0 +1,243 @@
+/**
+ * @file testPlanarSFMFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarSFMFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarSFMFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+using symbol_shorthand::C;
+using symbol_shorthand::K;
+
+TEST(PlanarSFMFactor, error1) {
+    // landmark is on the camera bore (facing +x)
+    Point3 landmark(1, 0, 0);
+    // so px is (cx, cy)
+    Point2 measured(200, 200);
+    // offset is identity
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    Values values;
+    Pose2 pose(0, 0, 0);
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    // du/dx etc for the pose2d
+    Matrix23 H1Expected = (Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        0, 0, 200, 0, 200, 0,//
+        0, -200, 0, 0, 0, 200).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    // du wrt calibration
+    // on-bore, f doesn't matter
+    // but c does
+    Matrix29 H3Expected = (Matrix29() <<//
+        0, 0, 0, 1, 0, 0, 0, 0, 0,//
+        0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarSFMFactor, error2) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    Matrix23 H1Expected = (Matrix23() <<//
+        -200, 200, 400,//
+        -200, 0, 200).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        -200, -200, 400, -200, 200, 0,//
+        200, -400, 200, -200, 0, 200).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    Matrix29 H3Expected = (Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarSFMFactor, error3) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    Matrix23 H1Expected = (Matrix23() <<//
+        -360, 280, 640,//
+        -360, 80, 440).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        -200, -440, 640, -360, 280, 80,//
+        200, -640, 440, -360, 80, 280).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    Matrix29 H3Expected = (Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+
+
+TEST(PlanarSFMFactor, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);    
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+        PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1, H2, H3;
+        factor.evaluateError(pose, offset, calib, H1, H2, H3);
+
+        Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+        CHECK(assert_equal(expectedH2, H2, 1e-6));
+        CHECK(assert_equal(expectedH3, H3, 1e-6));
+    }
+}
+
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

From a32bb7bf096d6ccd7432563de28b2fdfd96deb39 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 23:39:45 -0500
Subject: [PATCH 136/362] Export

---
 gtsam/geometry/Pose3.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 7faf433bb..f317cd4e8 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -240,18 +240,16 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 
 /* ************************************************************************* */
 namespace pose3 {
-class ExpmapFunctor : public so3::DexpFunctor {
- private:
-  static constexpr double one_twenty_fourth = 1.0 / 24.0;
-
+class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
+ protected:
   // Constant used in computeQ
   double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
 
  public:
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
       : so3::DexpFunctor(omega, nearZeroApprox) {
-        F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2;
-      }
+    F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
+  }
 
   // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
   // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
@@ -262,6 +260,9 @@ class ExpmapFunctor : public so3::DexpFunctor {
     return -0.5 * V + C * (W * V + V * W - WVW) +
            F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
   }
+
+ protected:
+  static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
 };
 }  // namespace pose3
 

From bcfb7d8444ae4fd1f8883c27e7f81a6b559c2a27 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 23:42:59 -0500
Subject: [PATCH 137/362] Fix test cases

---
 gtsam/geometry/tests/testSO3.cpp | 50 +++++++++++++++-----------------
 1 file changed, 24 insertions(+), 26 deletions(-)

diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index e8de2e93d..3ecabe84b 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -304,26 +304,25 @@ TEST(SO3, JacobianLogmap) {
 }
 
 namespace test_cases {
-std::vector<Vector3> nearZeros{
-    {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
-std::vector<Vector3> omegas{
+std::vector<Vector3> small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
+std::vector<Vector3> large{
     {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
+auto omegas = [](bool nearZero) { return nearZero ? small : large; };
 std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
 }  // namespace test_cases
 
 //******************************************************************************
-TEST(SO3, Cross) {
+TEST(SO3, CrossB) {
   Matrix aH1;
   for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZero).cross(v);
+          return so3::DexpFunctor(omega, nearZero).crossB(v);
         };
-    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
-    for (Vector3 omega : omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
-        local.cross(v, aH1);
+      for (const Vector3& v : test_cases::vs) {
+        local.crossB(v, aH1);
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
       }
     }
@@ -331,18 +330,17 @@ TEST(SO3, Cross) {
 }
 
 //******************************************************************************
-TEST(SO3, DoubleCross) {
+TEST(SO3, DoubleCrossC) {
   Matrix aH1;
   for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZero).doubleCross(v);
+          return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
         };
-    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
-    for (Vector3 omega : omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
-        local.doubleCross(v, aH1);
+      for (const Vector3& v : test_cases::vs) {
+        local.doubleCrossC(v, aH1);
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
       }
     }
@@ -357,9 +355,9 @@ TEST(SO3, ApplyDexp) {
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZero).applyDexp(v);
         };
-    for (Vector3 omega : test_cases::omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
+      for (const Vector3& v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -378,14 +376,14 @@ TEST(SO3, ApplyLeftJacobian) {
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
         };
-    for (Vector3 omega : test_cases::omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
-        CHECK(assert_equal(Vector3(local.leftJacobian() * v),
-                           local.applyLeftJacobian(v, aH1, aH2)));
-        CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
-        CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
-        CHECK(assert_equal(local.leftJacobian(), aH2));
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
+                            local.applyLeftJacobian(v, aH1, aH2)));
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(local.leftJacobian(), aH2));
       }
     }
   }
@@ -399,10 +397,10 @@ TEST(SO3, ApplyInvDexp) {
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
         };
-    for (Vector3 omega : test_cases::omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
       Matrix invDexp = local.dexp().inverse();
-      for (Vector3 v : test_cases::vs) {
+      for (const Vector3& v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(invDexp * v),
                             local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));

From d547fe2ec137ce3f6727d84bd9f1e5b9d7508867 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 23:43:59 -0500
Subject: [PATCH 138/362] Remove all nearZero paths

---
 gtsam/geometry/SO3.cpp | 52 ++++++++++--------------------------------
 gtsam/geometry/SO3.h   | 50 +++++++++++++++++++---------------------
 2 files changed, 35 insertions(+), 67 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index e341911b9..ef1fa374b 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
   E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
 }
 
-Matrix3 DexpFunctor::rightJacobian() const {
-  if (nearZero) {
-    return I_3x3 - B * W;  // + C * WW;
-  } else {
-    return I_3x3 - B * W + C * WW;
-  }
-}
-
-Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
+Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
   // Wv = omega x * v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
@@ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
   return B * Wv;
 }
 
-Vector3 DexpFunctor::doubleCross(const Vector3& v,
-                                 OptionalJacobian<3, 3> H) const {
+Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
+                                  OptionalJacobian<3, 3> H) const {
   // WWv = omega x (omega x * v)
   Matrix3 doubleCrossJacobian;
   const Vector3 WWv =
@@ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
 // Multiplies v with left Jacobian through vector operations only.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  if (nearZero) {
-    if (H1) *H1 = 0.5 * skewSymmetric(v);
-    if (H2) *H2 = I_3x3 - 0.5 * W;
-    return v - 0.5 * gtsam::cross(omega, v);
-  } else {
   Matrix3 D_BWv_omega, D_CWWv_omega;
-    const Vector3 BWv = cross(v, D_BWv_omega);
-    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
-    if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
+  const Vector3 BWv = crossB(v, D_BWv_omega);
+  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
+  if (H1) *H1 = -D_BWv_omega + D_CWWv_omega;
   if (H2) *H2 = rightJacobian();
   return v - BWv + CWWv;
-  }
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
@@ -158,29 +144,15 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   return c;
 }
 
-Matrix3 DexpFunctor::leftJacobian() const {
-  if (nearZero) {
-    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
-  } else {
-    return I_3x3 + B * W + C * WW;
-  }
-}
-
 Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
                                        OptionalJacobian<3, 3> H1,
                                        OptionalJacobian<3, 3> H2) const {
-  if (nearZero) {
-    if (H1) *H1 = - 0.5 * skewSymmetric(v);
-    if (H2) *H2 = I_3x3 + 0.5 * W;
-    return v + 0.5 * gtsam::cross(omega, v);
-  } else {
-    Matrix3 D_BWv_omega, D_CWWv_omega;
-    const Vector3 BWv = cross(v, D_BWv_omega);
-    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
-    if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
-    if (H2) *H2 = leftJacobian();
-    return v + BWv + CWWv;
-  }
+  Matrix3 D_BWv_omega, D_CWWv_omega;
+  const Vector3 BWv = crossB(v, D_BWv_omega);
+  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
+  if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
+  if (H2) *H2 = leftJacobian();
+  return v + BWv + CWWv;
 }
 
 }  // namespace so3
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 6403c3ae0..efb947e73 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor {
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class DexpFunctor : public ExpmapFunctor {
+class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
  protected:
   const Vector3 omega;
   double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
@@ -164,15 +164,8 @@ class DexpFunctor : public ExpmapFunctor {
   double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
  public:
-  /// Computes B * (omega x v).
-  Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
-
-  /// Computes C * (omega x (omega x v)).
-  Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
-
   /// Constructor with element of Lie algebra so(3)
-  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
-                                    bool nearZeroApprox = false);
+  explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@@ -180,28 +173,31 @@ class DexpFunctor : public ExpmapFunctor {
   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
   // This maps a perturbation v in the tangent space to
   // a perturbation on the manifold Expmap(dexp * v)
-  GTSAM_EXPORT Matrix3 rightJacobian() const;
-
-  /// differential of expmap == right Jacobian
-  GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
-
-  /// Multiplies with dexp(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
-                                 OptionalJacobian<3, 3> H1 = {},
-                                 OptionalJacobian<3, 3> H2 = {}) const;
-
-  /// Multiplies with dexp().inverse(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
-                                    OptionalJacobian<3, 3> H1 = {},
-                                    OptionalJacobian<3, 3> H2 = {}) const;
+  Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
 
   // Compute the left Jacobian for Exponential map in SO(3)
-  GTSAM_EXPORT Matrix3 leftJacobian() const;
+  Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
+
+  /// differential of expmap == right Jacobian
+  inline Matrix3 dexp() const { return rightJacobian(); }
+
+  /// Computes B * (omega x v).
+  Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Computes C * (omega x (omega x v)).
+  Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Multiplies with dexp(), with optional derivatives
+  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                    OptionalJacobian<3, 3> H2 = {}) const;
+
+  /// Multiplies with dexp().inverse(), with optional derivatives
+  Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                       OptionalJacobian<3, 3> H2 = {}) const;
 
   /// Multiplies with leftJacobian(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
-                                         OptionalJacobian<3, 3> H1 = {},
-                                         OptionalJacobian<3, 3> H2 = {}) const;
+  Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                            OptionalJacobian<3, 3> H2 = {}) const;
 
  protected:
   static constexpr double one_sixth = 1.0 / 6.0;

From 0c1f087dba7374e13440461e260ee47086d39c31 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 00:10:26 -0500
Subject: [PATCH 139/362] Final touches

---
 gtsam/geometry/Pose3.cpp |  7 ++-----
 gtsam/geometry/SO3.cpp   | 25 ++++++++++++-------------
 gtsam/geometry/SO3.h     | 27 ++++++++++++---------------
 3 files changed, 26 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f317cd4e8..5f0e8e3ce 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -240,12 +240,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 
 /* ************************************************************************* */
 namespace pose3 {
-class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
- protected:
+struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
   // Constant used in computeQ
   double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
 
- public:
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
       : so3::DexpFunctor(omega, nearZeroApprox) {
     F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
@@ -253,7 +251,7 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
 
   // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
   // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
-  // how to compute mess below from its derivatives in w and v.
+  // how to compute mess below from applyLeftJacobian derivatives in w and v.
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
@@ -261,7 +259,6 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
            F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
   }
 
- protected:
   static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
 };
 }  // namespace pose3
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index ef1fa374b..c54306ae5 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -26,7 +26,6 @@
 
 #include <Eigen/SVD>
 #include <cmath>
-#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -94,7 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
 }
 
 Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
-  // Wv = omega x * v
+  // Wv = omega x v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
     // Apply product rule:
@@ -123,10 +122,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
 // Multiplies v with left Jacobian through vector operations only.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  Matrix3 D_BWv_omega, D_CWWv_omega;
-  const Vector3 BWv = crossB(v, D_BWv_omega);
-  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
-  if (H1) *H1 = -D_BWv_omega + D_CWWv_omega;
+  Matrix3 D_BWv_w, D_CWWv_w;
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
+  if (H1) *H1 = -D_BWv_w + D_CWWv_w;
   if (H2) *H2 = rightJacobian();
   return v - BWv + CWWv;
 }
@@ -136,9 +135,9 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   const Matrix3 invDexp = rightJacobian().inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
-    Matrix3 D_dexpv_omega;
-    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_omega;
+    Matrix3 D_dexpv_w;
+    applyDexp(c, D_dexpv_w);  // get derivative H of forward mapping
+    *H1 = -invDexp * D_dexpv_w;
   }
   if (H2) *H2 = invDexp;
   return c;
@@ -147,10 +146,10 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
                                        OptionalJacobian<3, 3> H1,
                                        OptionalJacobian<3, 3> H2) const {
-  Matrix3 D_BWv_omega, D_CWWv_omega;
-  const Vector3 BWv = crossB(v, D_BWv_omega);
-  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
-  if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
+  Matrix3 D_BWv_w, D_CWWv_w;
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
+  if (H1) *H1 = D_BWv_w + D_CWWv_w;
   if (H2) *H2 = leftJacobian();
   return v + BWv + CWWv;
 }
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index efb947e73..f831aa426 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -132,18 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 // functor also implements dedicated methods to apply dexp and/or inv(dexp).
 
 /// Functor implementing Exponential map
-class GTSAM_EXPORT ExpmapFunctor {
- protected:
+struct GTSAM_EXPORT ExpmapFunctor {
   const double theta2, theta;
   const Matrix3 W, WW;
   bool nearZero;
+
   // Ethan Eade's constants:
-  double A; // A = sin(theta) / theta or 1 for theta->0
-  double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
+  double A;  // A = sin(theta) / theta or 1 for theta->0
+  double B;  // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
 
-  void init(bool nearZeroApprox = false);
-
- public:
   /// Constructor with element of Lie algebra so(3)
   explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
@@ -152,33 +149,34 @@ class GTSAM_EXPORT ExpmapFunctor {
 
   /// Rodrigues formula
   SO3 expmap() const;
+
+ protected:
+  void init(bool nearZeroApprox = false);
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
- protected:
+struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
   double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
   // Constants used in cross and doubleCross
   double D;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
   double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
- public:
   /// Constructor with element of Lie algebra so(3)
   explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
   // Information Theory, and Lie Groups", Volume 2, 2008.
-  //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
-  // This maps a perturbation v in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v)
+  //   Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
+  // This maps a perturbation dxi=(w,v) in the tangent space to
+  // a perturbation on the manifold Expmap(dexp * xi)
   Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
 
   // Compute the left Jacobian for Exponential map in SO(3)
   Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
 
-  /// differential of expmap == right Jacobian
+  /// Differential of expmap == right Jacobian
   inline Matrix3 dexp() const { return rightJacobian(); }
 
   /// Computes B * (omega x v).
@@ -199,7 +197,6 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
                             OptionalJacobian<3, 3> H2 = {}) const;
 
- protected:
   static constexpr double one_sixth = 1.0 / 6.0;
   static constexpr double _one_twelfth = -1.0 / 12.0;
   static constexpr double _one_sixtieth = -1.0 / 60.0;

From 37f6de744df609a6cbc6f347d9d82bd690cf6358 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Dec 2024 10:28:55 -0500
Subject: [PATCH 140/362] use c++11 method for string to int

---
 gtsam/discrete/TableFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 63e9e5d6b..3cfae56ec 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -260,7 +260,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
       ss << keyValueForIndex(key, i);
     }
     // k will be in reverse key order already
-    uint64_t k = std::strtoull(ss.str().c_str(), NULL, 10);
+    uint64_t k = std::stoll(ss.str().c_str());
     pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
 

From 76c953784704c084ffba34e4d3fdc233a67b2ba4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 12:48:34 -0500
Subject: [PATCH 141/362] inverse Jacobians

---
 gtsam/geometry/SO3.cpp           | 32 +++++++++++++++++++++-----------
 gtsam/geometry/SO3.h             | 25 ++++++++++++++++++++-----
 gtsam/geometry/tests/testSO3.cpp | 19 ++++++++++++++++++-
 3 files changed, 59 insertions(+), 17 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index c54306ae5..6499d4139 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -87,19 +87,29 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  C = nearZero ? one_sixth : (1 - A) / theta2;
-  D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
-  E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
+  if (!nearZero) {
+    C = (1 - A) / theta2;
+    D = (1.0 - A / (2.0 * B)) / theta2;
+    E = (2.0 * B - A) / theta2;
+    F = (3.0 * C - B) / theta2;
+  } else {
+    // Limit as theta -> 0
+    // TODO(Frank): flipping signs here does not trigger any tests: harden!
+    C = one_sixth;
+    D = one_twelfth;
+    E = one_twelfth;
+    F = one_sixtieth;
+  }
 }
 
 Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
   // Wv = omega x v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
-    // Apply product rule:
-    // D * omega.transpose() is 1x3 Jacobian of B with respect to omega
-    // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v)
-    *H = Wv * D * omega.transpose() - B * skewSymmetric(v);
+    // Apply product rule to (B Wv)
+    // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
+    // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
+    *H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
   }
   return B * Wv;
 }
@@ -111,10 +121,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
   const Vector3 WWv =
       gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
   if (H) {
-    // Apply product rule:
-    // E * omega.transpose() is 1x3 Jacobian of C with respect to omega
-    // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v)
-    *H = WWv * E * omega.transpose() + C * doubleCrossJacobian;
+    // Apply product rule to (C WWv)
+    // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
+    // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
+    *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
   }
   return C * WWv;
 }
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index f831aa426..488dea12e 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -157,10 +157,16 @@ struct GTSAM_EXPORT ExpmapFunctor {
 /// Functor that implements Exponential map *and* its derivatives
 struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
-  double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
+
+  // Ethan's C constant used in Jacobians
+  double C;  // (1 - A) / theta^2 or 1/6 for theta->0
+
+  // Constant used in inverse Jacobians
+  double D;  // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0
+
   // Constants used in cross and doubleCross
-  double D;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
-  double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
+  double E;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
+  double F;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
   /// Constructor with element of Lie algebra so(3)
   explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
@@ -179,6 +185,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   /// Differential of expmap == right Jacobian
   inline Matrix3 dexp() const { return rightJacobian(); }
 
+  /// Inverse of right Jacobian
+  Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
+
+  // Inverse of left Jacobian
+  Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
+
+  /// Synonym for rightJacobianInverse
+  inline Matrix3 invDexp() const { return rightJacobianInverse(); }
+
   /// Computes B * (omega x v).
   Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
@@ -198,8 +213,8 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
                             OptionalJacobian<3, 3> H2 = {}) const;
 
   static constexpr double one_sixth = 1.0 / 6.0;
-  static constexpr double _one_twelfth = -1.0 / 12.0;
-  static constexpr double _one_sixtieth = -1.0 / 60.0;
+  static constexpr double one_twelfth = 1.0 / 12.0;
+  static constexpr double one_sixtieth = 1.0 / 60.0;
 };
 }  //  namespace so3
 
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 3ecabe84b..96872eae9 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
+#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;
@@ -304,13 +305,29 @@ TEST(SO3, JacobianLogmap) {
 }
 
 namespace test_cases {
-std::vector<Vector3> small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
+std::vector<Vector3> small{{0, 0, 0},                                 //
+                           {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
+                           {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
 std::vector<Vector3> large{
     {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
 auto omegas = [](bool nearZero) { return nearZero ? small : large; };
 std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
 }  // namespace test_cases
 
+//******************************************************************************
+TEST(SO3, JacobianInverses) {
+  Matrix HR, HL;
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
+                                   local.rightJacobianInverse()));
+      EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
+                                   local.leftJacobianInverse()));
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, CrossB) {
   Matrix aH1;

From 6c84a2b539c144ed06c9a3aec5fa2ba697a29b5d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 12:48:52 -0500
Subject: [PATCH 142/362] Use X to map left to right

---
 gtsam/geometry/Pose3.cpp | 20 +++++++-------------
 1 file changed, 7 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 5f0e8e3ce..cb8de111e 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -22,6 +22,7 @@
 #include <cmath>
 #include <iostream>
 #include <string>
+#include "gtsam/geometry/Rot3.h"
 
 namespace gtsam {
 
@@ -241,25 +242,18 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 /* ************************************************************************* */
 namespace pose3 {
 struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
-  // Constant used in computeQ
-  double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
-
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
       : so3::DexpFunctor(omega, nearZeroApprox) {
-    F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
   }
 
-  // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
-  // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
-  // how to compute mess below from applyLeftJacobian derivatives in w and v.
+  // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative.
   Matrix3 computeQ(const Vector3& v) const {
-    const Matrix3 V = skewSymmetric(v);
-    const Matrix3 WVW = W * V * W;
-    return -0.5 * V + C * (W * V + V * W - WVW) +
-           F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
+    // X translate from left to right for our right expmap convention:
+    Matrix X = rightJacobian() * leftJacobianInverse();
+    Matrix3 H;
+    applyLeftJacobian(v, H);
+    return X * H;
   }
-
-  static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
 };
 }  // namespace pose3
 

From 98697251bd9d8558258493f18c266c032b2c1dc3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 12:57:27 -0500
Subject: [PATCH 143/362] applyLeftJacobianInverse

---
 gtsam/geometry/SO3.cpp           | 26 ++++++++++++++++++------
 gtsam/geometry/SO3.h             |  7 ++++++-
 gtsam/geometry/tests/testSO3.cpp | 34 ++++++++++++++++++++++++++------
 3 files changed, 54 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 6499d4139..93bf072c5 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -142,14 +142,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = rightJacobian().inverse();
-  const Vector3 c = invDexp * v;
+  const Matrix3 invJr = rightJacobianInverse();
+  const Vector3 c = invJr * v;
   if (H1) {
-    Matrix3 D_dexpv_w;
-    applyDexp(c, D_dexpv_w);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_w;
+    Matrix3 H;
+    applyDexp(c, H);  // get derivative H of forward mapping
+    *H1 = -invJr * H;
   }
-  if (H2) *H2 = invDexp;
+  if (H2) *H2 = invJr;
   return c;
 }
 
@@ -164,6 +164,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
   return v + BWv + CWWv;
 }
 
+Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
+                                              OptionalJacobian<3, 3> H1,
+                                              OptionalJacobian<3, 3> H2) const {
+  const Matrix3 invJl = leftJacobianInverse();
+  const Vector3 c = invJl * v;
+  if (H1) {
+    Matrix3 H;
+    applyLeftJacobian(c, H);  // get derivative H of forward mapping
+    *H1 = -invJl * H;
+  }
+  if (H2) *H2 = invJl;
+  return c;
+}
+
 }  // namespace so3
 
 //******************************************************************************
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 488dea12e..2845fcce7 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -162,7 +162,7 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   double C;  // (1 - A) / theta^2 or 1/6 for theta->0
 
   // Constant used in inverse Jacobians
-  double D;  // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0
+  double D;  // (1 - A/2B) / theta2 or 1/12 for theta->0
 
   // Constants used in cross and doubleCross
   double E;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
@@ -212,6 +212,11 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
                             OptionalJacobian<3, 3> H2 = {}) const;
 
+  /// Multiplies with leftJacobianInverse(), with optional derivatives
+  Vector3 applyLeftJacobianInverse(const Vector3& v,
+                                   OptionalJacobian<3, 3> H1 = {},
+                                   OptionalJacobian<3, 3> H2 = {}) const;
+
   static constexpr double one_sixth = 1.0 / 6.0;
   static constexpr double one_twelfth = 1.0 / 12.0;
   static constexpr double one_sixtieth = 1.0 / 60.0;
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 96872eae9..d3313b7f1 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -385,6 +385,28 @@ TEST(SO3, ApplyDexp) {
   }
 }
 
+//******************************************************************************
+TEST(SO3, ApplyInvDexp) {
+  Matrix aH1, aH2;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      Matrix invJr = local.rightJacobianInverse();
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(
+            assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(invJr, aH2));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyLeftJacobian) {
   Matrix aH1, aH2;
@@ -407,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) {
 }
 
 //******************************************************************************
-TEST(SO3, ApplyInvDexp) {
+TEST(SO3, ApplyLeftJacobianInverse) {
   Matrix aH1, aH2;
   for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
         };
     for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      Matrix invDexp = local.dexp().inverse();
+      Matrix invJl = local.leftJacobianInverse();
       for (const Vector3& v : test_cases::vs) {
-        EXPECT(assert_equal(Vector3(invDexp * v),
-                            local.applyInvDexp(v, aH1, aH2)));
+        EXPECT(assert_equal(Vector3(invJl * v),
+                            local.applyLeftJacobianInverse(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
         EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
-        EXPECT(assert_equal(invDexp, aH2));
+        EXPECT(assert_equal(invJl, aH2));
       }
     }
   }

From b11387167d9d8988e6a35c920518863c9d14fe44 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 13:28:19 -0500
Subject: [PATCH 144/362] No more functor for Q

---
 gtsam/geometry/Pose3.cpp           | 59 +++++++++++++-----------------
 gtsam/geometry/tests/testPose3.cpp | 11 ------
 2 files changed, 26 insertions(+), 44 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index cb8de111e..9f81204d6 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -22,6 +22,7 @@
 #include <cmath>
 #include <iostream>
 #include <string>
+
 #include "gtsam/geometry/Rot3.h"
 
 namespace gtsam {
@@ -112,7 +113,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
 
 /* ************************************************************************* */
 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
+                       OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
   if (Hxi) {
     Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
@@ -159,7 +160,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
 /* ************************************************************************* */
 Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
   return Pose3(interpolate<Rot3>(R_, T.R_, t),
-                interpolate<Point3>(t_, T.t_, t));
+               interpolate<Point3>(t_, T.t_, t));
 }
 
 /* ************************************************************************* */
@@ -239,30 +240,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 #endif
 }
 
-/* ************************************************************************* */
-namespace pose3 {
-struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
-      : so3::DexpFunctor(omega, nearZeroApprox) {
-  }
-
-  // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative.
-  Matrix3 computeQ(const Vector3& v) const {
-    // X translate from left to right for our right expmap convention:
-    Matrix X = rightJacobian() * leftJacobianInverse();
-    Matrix3 H;
-    applyLeftJacobian(v, H);
-    return X * H;
-  }
-};
-}  // namespace pose3
-
 /* ************************************************************************* */
 Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
                                            double nearZeroThreshold) {
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  return pose3::ExpmapFunctor(w).computeQ(v);
+  Matrix3 Q;
+  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
+  return Q;
 }
 
 /* ************************************************************************* */
@@ -273,13 +258,22 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   const double theta2 = w.dot(w);
   bool nearZero = (theta2 <= nearZeroThreshold);
 
-  if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
-
-  if (nearZero) {
-    return v + 0.5 * w.cross(v);
+  if (Q) {
+    // Instantiate functor for Dexp-related operations:
+    so3::DexpFunctor local(w, nearZero);
+    // X translate from left to right for our right expmap convention:
+    Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
+    Matrix3 H;
+    Vector t = local.applyLeftJacobian(v, H);
+    *Q = X * H;
+    return t;
+  } else if (nearZero) {
+    // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6
+    Vector3 Wv = w.cross(v);
+    return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth;
   } else {
-    // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but
-    // formulas below convey geometric insight and creating functor is not free.
+    // NOTE(Frank): if Q is not asked we use formulas below instead of calling
+    // applyLeftJacobian(v), as they convey geometric insight and are faster.
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
@@ -314,7 +308,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
 }
 
 /* ************************************************************************* */
-
 const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
   if (Hself) {
     *Hself << I_3x3, Z_3x3;
@@ -332,14 +325,14 @@ Matrix4 Pose3::matrix() const {
 
 /* ************************************************************************* */
 Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
-                                                 OptionalJacobian<6, 6> HaTb) const {
+                               OptionalJacobian<6, 6> HaTb) const {
   const Pose3& wTa = *this;
   return wTa.compose(aTb, Hself, HaTb);
 }
 
 /* ************************************************************************* */
 Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
-                                               OptionalJacobian<6, 6> HwTb) const {
+                             OptionalJacobian<6, 6> HwTb) const {
   if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
   if (HwTb) *HwTb = I_6x6;
   const Pose3& wTa = *this;
@@ -348,7 +341,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
 
 /* ************************************************************************* */
 Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
-    OptionalJacobian<3, 3> Hpoint) const {
+                            OptionalJacobian<3, 3> Hpoint) const {
   // Only get matrix once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 R = R_.matrix();
@@ -372,7 +365,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const {
 
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
-    OptionalJacobian<3, 3> Hpoint) const {
+                          OptionalJacobian<3, 3> Hpoint) const {
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 Rt = R_.transpose();
@@ -478,7 +471,7 @@ std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
 std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
   if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
     throw std::invalid_argument(
-      "Pose3:Align expects 3*N matrices of equal shape.");
+        "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
   for (Eigen::Index j = 0; j < a.cols(); j++) {
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 4ff4f9a85..3b3de6792 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) {
   }
 }
 
-TEST( Pose3, ExpmapDerivativeQr) {
-  Vector6 w = Vector6::Random();
-  w.head<3>().normalize();
-  w.head<3>() = w.head<3>() * 0.9e-2;
-  Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
-  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
-}
-
 /* ************************************************************************* */
 TEST( Pose3, LogmapDerivative) {
   Matrix6 actualH;

From 2aa36d4f7ab89e59138fbaa2c9914643069b77ef Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 14:08:02 -0500
Subject: [PATCH 145/362] Modernize ExmapTranslation

---
 gtsam/geometry/Pose3.cpp         | 56 +++++++++++++++++---------------
 gtsam/geometry/Pose3.h           | 10 +++---
 gtsam/geometry/tests/testSO3.cpp |  1 +
 gtsam/navigation/NavState.cpp    | 27 ++++++++-------
 4 files changed, 50 insertions(+), 44 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 9f81204d6..6fbd2468f 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -164,22 +164,22 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
 }
 
 /* ************************************************************************* */
-/** Modified from Murray94book version (which assumes w and v normalized?) */
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
   // Get angular velocity omega and translational velocity v from twist xi
   const Vector3 w = xi.head<3>(), v = xi.tail<3>();
 
   // Compute rotation using Expmap
-  Matrix3 Jw;
-  Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr);
+  Matrix3 Jr;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
 
-  // Compute translation and optionally its Jacobian in w
+  // Compute translation and optionally its Jacobian Q in w
+  // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
   Matrix3 Q;
-  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
+  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
 
   if (Hxi) {
-    *Hxi << Jw, Z_3x3,
-             Q, Jw;
+    *Hxi << Jr, Z_3x3,  //
+        Q, Jr;
   }
 
   return Pose3(R, t);
@@ -251,35 +251,39 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
 }
 
 /* ************************************************************************* */
+// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
+//   t_parallel = w * w.dot(v);  // translation parallel to axis
+//   w_cross_v = w.cross(v);     // translation orthogonal to axis
+//   t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
+// but functor does not need R, deals automatically with the case where theta2
+// is near zero, and also gives us the machinery for the Jacobians.
 Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
                                  OptionalJacobian<3, 3> Q,
-                                 const std::optional<Rot3>& R,
+                                 OptionalJacobian<3, 3> J,
                                  double nearZeroThreshold) {
   const double theta2 = w.dot(w);
   bool nearZero = (theta2 <= nearZeroThreshold);
 
+  // Instantiate functor for Dexp-related operations:
+  so3::DexpFunctor local(w, nearZero);
+
+  // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
+  // don't need Jacobians, and returns Jacobian of t with respect to w if asked.
+  Matrix3 H;
+  Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
+
+  // We return Jacobians for use in Expmap, so we multiply with X, that
+  // translates from left to right for our right expmap convention:
   if (Q) {
-    // Instantiate functor for Dexp-related operations:
-    so3::DexpFunctor local(w, nearZero);
-    // X translate from left to right for our right expmap convention:
     Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
-    Matrix3 H;
-    Vector t = local.applyLeftJacobian(v, H);
     *Q = X * H;
-    return t;
-  } else if (nearZero) {
-    // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6
-    Vector3 Wv = w.cross(v);
-    return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth;
-  } else {
-    // NOTE(Frank): if Q is not asked we use formulas below instead of calling
-    // applyLeftJacobian(v), as they convey geometric insight and are faster.
-    Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
-    Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
-    Rot3 rotation = R.value_or(Rot3::Expmap(w));
-    Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2;
-    return t;
   }
+
+  if (J) {
+    *J = local.rightJacobian();  // = X * local.leftJacobian();
+  }
+
+  return t;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index d7c280c80..04504de65 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -223,17 +223,19 @@ public:
       const Vector6& xi, double nearZeroThreshold = 1e-5);
 
   /**
-   * Compute the translation part of the exponential map, with derivative.
+   * Compute the translation part of the exponential map, with Jacobians.
    * @param w 3D angular velocity
    * @param v 3D velocity
    * @param Q Optionally, compute 3x3 Jacobian wrpt w
-   * @param R Optionally, precomputed as Rot3::Expmap(w)
+   * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
    * @param nearZeroThreshold threshold for small values
-   * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix
+   * @note This function returns Jacobians Q and J corresponding to the bottom
+   * blocks of the SE(3) exponential, and translated from left to right from the
+   * applyLeftJacobian Jacobians.
    */
   static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
                                    OptionalJacobian<3, 3> Q = {},
-                                   const std::optional<Rot3>& R = {},
+                                   OptionalJacobian<3, 3> J = {},
                                    double nearZeroThreshold = 1e-5);
 
   using LieGroup<Pose3, 6>::inverse; // version with derivative
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index d3313b7f1..200642456 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -304,6 +304,7 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
+//******************************************************************************
 namespace test_cases {
 std::vector<Vector3> small{{0, 0, 0},                                 //
                            {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index b9d48a3cb..c9cd244d3 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -114,24 +114,23 @@ NavState NavState::inverse() const {
 
 //------------------------------------------------------------------------------
 NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
-  // Get angular velocity w, translational velocity v, and acceleration a
-  Vector3 w = xi.head<3>();
-  Vector3 rho = xi.segment<3>(3);
-  Vector3 nu = xi.tail<3>();
+  // Get angular velocity w and components rho and nu from xi
+  Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
 
   // Compute rotation using Expmap
-  Rot3 R = Rot3::Expmap(w);
+  Matrix3 Jr;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
 
-  // Compute translations and optionally their Jacobians
+  // Compute translations and optionally their Jacobians Q in w
+  // The Jacobians with respect to rho and nu are equal to Jr
   Matrix3 Qt, Qv;
-  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R);
-  Vector3 v = Pose3::ExpmapTranslation(w,  nu, Hxi ? &Qv : nullptr, R);
+  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
+  Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
 
   if (Hxi) {
-    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-    *Hxi << Jw, Z_3x3, Z_3x3,
-            Qt,    Jw, Z_3x3,
-            Qv, Z_3x3,    Jw;
+    *Hxi << Jr, Z_3x3, Z_3x3,  //
+        Qt, Jr, Z_3x3,         //
+        Qv, Z_3x3, Jr;
   }
 
   return NavState(R, t, v);
@@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
   
   Matrix3 Qt, Qv;
   const Rot3 R = Rot3::Expmap(w);
-  Pose3::ExpmapTranslation(w, rho, Qt, R);
-  Pose3::ExpmapTranslation(w,  nu, Qv, R);
+  Pose3::ExpmapTranslation(w, rho, Qt);
+  Pose3::ExpmapTranslation(w,  nu, Qv);
   const Matrix3 Jw = Rot3::LogmapDerivative(w);
   const Matrix3 Qt2 = -Jw * Qt * Jw;
   const Matrix3 Qv2 = -Jw * Qv * Jw;

From df0f597ed763d028b170690846f6eddcecf0b46f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Dec 2024 14:28:27 -0500
Subject: [PATCH 146/362] debug conversion

---
 gtsam/discrete/TableFactor.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 3cfae56ec..cb60a9818 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -260,7 +260,9 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
       ss << keyValueForIndex(key, i);
     }
     // k will be in reverse key order already
-    uint64_t k = std::stoll(ss.str().c_str());
+    uint64_t k;
+    ss >> k;
+    std::cout << "ss: " << ss.str() << ", k=" << k << std::endl;
     pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
 

From 17cae8c45336cdb62bf2bfcbc7c6fe6f7cdc1d39 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Dec 2024 15:22:25 -0500
Subject: [PATCH 147/362] print more to debug

---
 gtsam/discrete/TableFactor.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index cb60a9818..38b4ddd30 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -262,16 +262,21 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
     // k will be in reverse key order already
     uint64_t k;
     ss >> k;
-    std::cout << "ss: " << ss.str() << ", k=" << k << std::endl;
+    std::cout << "ss: " << ss.str() << ", k=" << k
+              << ", v=" << sparse_table_.coeff(i) << std::endl;
     pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
 
-  // Sort based on key so we get values in reverse key order.
+  // Sort based on key assignment so we get values in reverse key order.
   std::sort(
       pair_table.begin(), pair_table.end(),
       [](const std::pair<uint64_t, double>& a,
          const std::pair<uint64_t, double>& b) { return a.first <= b.first; });
 
+  std::cout << "Sorted pair_table:" << std::endl;
+  for (auto&& [k, v] : pair_table) {
+    std::cout << "k=" << k << ", v=" << v << std::endl;
+  }
   // Create the table vector
   std::vector<double> table;
   std::for_each(pair_table.begin(), pair_table.end(),

From 72306efe9830f24ed741db8b614d0c665ccf9452 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Dec 2024 18:20:16 -0500
Subject: [PATCH 148/362] strict less than check

---
 gtsam/discrete/TableFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 38b4ddd30..0a93e7b5d 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -271,7 +271,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   std::sort(
       pair_table.begin(), pair_table.end(),
       [](const std::pair<uint64_t, double>& a,
-         const std::pair<uint64_t, double>& b) { return a.first <= b.first; });
+         const std::pair<uint64_t, double>& b) { return a.first < b.first; });
 
   std::cout << "Sorted pair_table:" << std::endl;
   for (auto&& [k, v] : pair_table) {

From db5b9dee65442245a8de1708fb1a21d72ee16e55 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 18:35:17 -0500
Subject: [PATCH 149/362] Taylor expansion

---
 gtsam/geometry/SO3.cpp             |  25 ++++---
 gtsam/geometry/SO3.h               |  16 ++---
 gtsam/geometry/tests/testPose3.cpp | 101 +++++++++++++++++------------
 3 files changed, 83 insertions(+), 59 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 93bf072c5..41f7ce810 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -33,6 +33,15 @@ namespace gtsam {
 //******************************************************************************
 namespace so3 {
 
+static constexpr double one_6th = 1.0 / 6.0;
+static constexpr double one_12th = 1.0 / 12.0;
+static constexpr double one_24th = 1.0 / 24.0;
+static constexpr double one_60th = 1.0 / 60.0;
+static constexpr double one_120th = 1.0 / 120.0;
+static constexpr double one_180th = 1.0 / 180.0;
+static constexpr double one_720th = 1.0 / 720.0;
+static constexpr double one_1260th = 1.0 / 1260.0;
+
 GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
   Matrix99 H;
   auto R = Q.matrix();
@@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
         2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
     B = one_minus_cos / theta2;
   } else {
-    // Limits as theta -> 0:
-    A = 1.0;
-    B = 0.5;
+    // Taylor expansion at 0
+    A = 1.0 - theta2 * one_6th;
+    B = 0.5 - theta2 * one_24th;
   }
 }
 
@@ -93,12 +102,12 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     E = (2.0 * B - A) / theta2;
     F = (3.0 * C - B) / theta2;
   } else {
-    // Limit as theta -> 0
+    // Taylor expansion at 0
     // TODO(Frank): flipping signs here does not trigger any tests: harden!
-    C = one_sixth;
-    D = one_twelfth;
-    E = one_twelfth;
-    F = one_sixtieth;
+    C = one_6th - theta2 * one_120th;
+    D = one_12th + theta2 * one_720th;
+    E = one_12th - theta2 * one_180th;
+    F = one_60th - theta2 * one_1260th;
   }
 }
 
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 2845fcce7..6e2e38101 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -138,8 +138,8 @@ struct GTSAM_EXPORT ExpmapFunctor {
   bool nearZero;
 
   // Ethan Eade's constants:
-  double A;  // A = sin(theta) / theta or 1 for theta->0
-  double B;  // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
+  double A;  // A = sin(theta) / theta
+  double B;  // B = (1 - cos(theta))
 
   /// Constructor with element of Lie algebra so(3)
   explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
@@ -159,14 +159,14 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
 
   // Ethan's C constant used in Jacobians
-  double C;  // (1 - A) / theta^2 or 1/6 for theta->0
+  double C;  // (1 - A) / theta^2
 
   // Constant used in inverse Jacobians
-  double D;  // (1 - A/2B) / theta2 or 1/12 for theta->0
+  double D;  // (1 - A/2B) / theta2
 
   // Constants used in cross and doubleCross
-  double E;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
-  double F;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
+  double E;  // (2B - A) / theta2
+  double F;  // (3C - B) / theta2
 
   /// Constructor with element of Lie algebra so(3)
   explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
@@ -216,10 +216,6 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   Vector3 applyLeftJacobianInverse(const Vector3& v,
                                    OptionalJacobian<3, 3> H1 = {},
                                    OptionalJacobian<3, 3> H2 = {}) const;
-
-  static constexpr double one_sixth = 1.0 / 6.0;
-  static constexpr double one_twelfth = 1.0 / 12.0;
-  static constexpr double one_sixtieth = 1.0 / 60.0;
 };
 }  //  namespace so3
 
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 3b3de6792..ce74091f0 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -835,38 +835,7 @@ TEST(Pose3, Align2) {
 }
 
 /* ************************************************************************* */
-TEST( Pose3, ExpmapDerivative1) {
-  Matrix6 actualH;
-  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
-  Pose3::Expmap(w,actualH);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  EXPECT(assert_equal(expectedH, actualH));
-}
-
-/* ************************************************************************* */
-TEST( Pose3, ExpmapDerivative2) {
-  Matrix6 actualH;
-  Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0;
-  Pose3::Expmap(w,actualH);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  EXPECT(assert_equal(expectedH, actualH));
-}
-
-/* ************************************************************************* */
-TEST( Pose3, ExpmapDerivative3) {
-  Matrix6 actualH;
-  Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0;
-  Pose3::Expmap(w,actualH);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  // Small angle approximation is not as precise as numerical derivative?
-  EXPECT(assert_equal(expectedH, actualH, 1e-5));
-}
-
-/* ************************************************************************* */
-TEST(Pose3, ExpmapDerivative4) {
+TEST(Pose3, ExpmapDerivative) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@@ -900,15 +869,65 @@ TEST(Pose3, ExpmapDerivative4) {
   }
 }
 
-/* ************************************************************************* */
-TEST( Pose3, LogmapDerivative) {
-  Matrix6 actualH;
-  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
-  Pose3 p = Pose3::Expmap(w);
-  EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
-  Matrix expectedH = numericalDerivative21<Vector6, Pose3,
-      OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
-  EXPECT(assert_equal(expectedH, actualH));
+//******************************************************************************
+namespace test_cases {
+std::vector<Vector3> small{{0, 0, 0},                                 //
+                           {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
+                           {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
+std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0},    {0, 1, 0},
+                           {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
+auto omegas = [](bool nearZero) { return nearZero ? small : large; };
+std::vector<Vector3> vs{{1, 0, 0},    {0, 1, 0}, {0, 0, 1},
+                        {.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}};
+}  // namespace test_cases
+
+//******************************************************************************
+TEST(Pose3, ExpmapDerivatives) {
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& w : test_cases::omegas(nearZero)) {
+      for (Vector3 v : test_cases::vs) {
+        const Vector6 xi = (Vector6() << w, v).finished();
+        const Matrix6 expectedH =
+            numericalDerivative21<Pose3, Vector6, OptionalJacobian<6, 6> >(
+                &Pose3::Expmap, xi, {});
+        Matrix actualH;
+        Pose3::Expmap(xi, actualH);
+        EXPECT(assert_equal(expectedH, actualH));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+// Check logmap for all small values, as we don't want wrapping.
+TEST(Pose3, Logmap) {
+  static constexpr bool nearZero = true;
+  for (const Vector3& w : test_cases::omegas(nearZero)) {
+    for (Vector3 v : test_cases::vs) {
+      const Vector6 xi = (Vector6() << w, v).finished();
+      Pose3 pose = Pose3::Expmap(xi);
+      EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5));
+    }
+  }
+}
+
+//******************************************************************************
+// Check logmap derivatives for all values
+TEST(Pose3, LogmapDerivatives) {
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& w : test_cases::omegas(nearZero)) {
+      for (Vector3 v : test_cases::vs) {
+        const Vector6 xi = (Vector6() << w, v).finished();
+        Pose3 pose = Pose3::Expmap(xi);
+        const Matrix6 expectedH =
+            numericalDerivative21<Vector6, Pose3, OptionalJacobian<6, 6> >(
+                &Pose3::Logmap, pose, {});
+        Matrix actualH;
+        Pose3::Logmap(pose, actualH);
+        EXPECT(assert_equal(expectedH, actualH));
+      }
+    }
+  }
 }
 
 /* ************************************************************************* */

From 49c2a040095a4d4d682a2c846f8a79bb8f87584c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 18:51:29 -0500
Subject: [PATCH 150/362] Address review comments

---
 gtsam/geometry/Pose3.cpp         | 13 +++++++------
 gtsam/geometry/Pose3.h           |  2 +-
 gtsam/geometry/SO3.h             |  4 ++++
 gtsam/geometry/tests/testSO3.cpp |  1 -
 gtsam/navigation/NavState.cpp    |  2 +-
 5 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 6fbd2468f..16dda3d1e 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -11,20 +11,19 @@
 
 /**
  * @file  Pose3.cpp
- * @brief 3D Pose
+ * @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
  */
 
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/concepts.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/concepts.h>
 
 #include <cmath>
 #include <iostream>
 #include <string>
 
-#include "gtsam/geometry/Rot3.h"
-
 namespace gtsam {
 
 /** instantiate concept checks */
@@ -164,6 +163,8 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
 }
 
 /* ************************************************************************* */
+// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's
+// elegant Lie group document, at https://www.ethaneade.org/lie.pdf.
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
   // Get angular velocity omega and translational velocity v from twist xi
   const Vector3 w = xi.head<3>(), v = xi.tail<3>();
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 04504de65..81a9848e2 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -11,7 +11,7 @@
 
 /**
  *@file  Pose3.h
- *@brief 3D Pose
+ * @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
  */
 
 // \callgraph
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 6e2e38101..163ae6623 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -132,6 +132,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 // functor also implements dedicated methods to apply dexp and/or inv(dexp).
 
 /// Functor implementing Exponential map
+/// Math is based on Ethan Eade's elegant Lie group document, at
+/// https://www.ethaneade.org/lie.pdf.
 struct GTSAM_EXPORT ExpmapFunctor {
   const double theta2, theta;
   const Matrix3 W, WW;
@@ -155,6 +157,8 @@ struct GTSAM_EXPORT ExpmapFunctor {
 };
 
 /// Functor that implements Exponential map *and* its derivatives
+/// Math extends Ethan theme of elegant I + aW + bWW expressions.
+/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83).
 struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
 
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 200642456..c6fd52161 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
-#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;
diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index c9cd244d3..bd482132e 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -114,7 +114,7 @@ NavState NavState::inverse() const {
 
 //------------------------------------------------------------------------------
 NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
-  // Get angular velocity w and components rho and nu from xi
+  // Get angular velocity w and components rho (for t) and nu (for v) from xi
   Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
 
   // Compute rotation using Expmap

From 14c79986d3416320e110f567b0def090b9ae9c1f Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Mon, 16 Dec 2024 18:04:40 -0800
Subject: [PATCH 151/362] consolidate factors in one file

---
 gtsam/slam/PlanarProjectionFactor.h           | 360 +++++++++++++-----
 gtsam/slam/PlanarSFMFactor.h                  | 165 --------
 gtsam/slam/slam.i                             |  36 +-
 .../slam/tests/testPlanarProjectionFactor.cpp | 224 ++++++++++-
 gtsam/slam/tests/testPlanarSFMFactor.cpp      | 243 ------------
 5 files changed, 499 insertions(+), 529 deletions(-)
 delete mode 100644 gtsam/slam/PlanarSFMFactor.h
 delete mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp

diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
index 49d1f7cac..285b64900 100644
--- a/gtsam/slam/PlanarProjectionFactor.h
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -1,24 +1,13 @@
 /**
- * Similar to GenericProjectionFactor, but:
- *
- * * 2d pose variable (robot on the floor)
- * * constant landmarks
- * * batched input
- * * numeric differentiation
+ * ProjectionFactor, but with pose2 (robot on the floor)
  *
  * This factor is useful for high-school robotics competitions,
  * which run robots on the floor, with use fixed maps and fiducial
- * markers.  The camera offset and calibration are fixed, perhaps
- * found using PlanarSFMFactor.
- *
- * The python version of this factor uses batches, to save on calls
- * across the C++/python boundary, but here the only extra cost
- * is instantiating the camera, so there's no batch.
+ * markers.
  *
  * @see https://www.firstinspires.org/
- * @see PlanarSFMFactor.h
  *
- * @file PlanarSFMFactor.h
+ * @file PlanarProjectionFactor.h
  * @brief for planar smoothing
  * @date Dec 2, 2024
  * @author joel@truher.org
@@ -39,116 +28,291 @@
 
 
 namespace gtsam {
+
     /**
-     * @class PlanarProjectionFactor
-     * @brief Camera projection for robot on the floor.
+     * @class PlanarProjectionFactorBase
+     * @brief Camera projection for robot on the floor.  Measurement is camera pixels.
      */
-    class PlanarProjectionFactor : public NoiseModelFactorN<Pose2> {
-        typedef NoiseModelFactorN<Pose2> Base;
-        static const Pose3 CAM_COORD;
-
+    class PlanarProjectionFactorBase {
     protected:
-
-        Point3 landmark_; // landmark
-        Point2 measured_; // pixel measurement
-        Pose3 offset_; // camera offset to robot pose
-        Cal3DS2 calib_; // camera calibration
-
-    public:
-        // Provide access to the Matrix& version of evaluateError:
-        using Base::evaluateError;
-
-        PlanarProjectionFactor() {}
+        PlanarProjectionFactorBase() {}
 
         /**
-         * @param landmarks point in the world
          * @param measured corresponding point in the camera frame
-         * @param offset constant camera offset from pose
-         * @param calib constant camera calibration
-         * @param model stddev of the measurements, ~one pixel?
          * @param poseKey index of the robot pose in the z=0 plane
          */
-        PlanarProjectionFactor(
+        PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}
+
+        /**
+         * @param landmark point3
+         * @param pose
+         * @param offset oriented parallel to pose2 zero i.e. +x
+         * @param calib
+         * @param H1 landmark jacobian
+         * @param H2 pose jacobian
+         * @param H3 offset jacobian
+         * @param H4 calib jacobian
+         */
+        Point2 h(
             const Point3& landmark,
-            const Point2& measured,
+            const Pose2& pose,
             const Pose3& offset,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model,
-            Key poseKey)
-            : NoiseModelFactorN(model, poseKey),
-            landmark_(landmark),
-            measured_(measured),
-            offset_(offset),
-            calib_(calib)
-        {
-            assert(2 == model->dim());
-        }
-
-        ~PlanarProjectionFactor() override {}
-
-        /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this)));
-        }
-
-        Point2 h2(const Pose2& pose, OptionalMatrixType H1) const {
-            // this is x-forward z-up
-            gtsam::Matrix H0;
-            Pose3 offset_pose = Pose3(pose).compose(offset_, H0);
-            // this is z-forward y-down
-            gtsam::Matrix H00;
-            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
-            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
-            if (H1) {
-                // Dpose is 2x6 (R,t)
-                // H1 is 2x3 (x, y, theta)
-                gtsam::Matrix Dpose;
-                Point2 result = camera.project(landmark_, Dpose);
-                Dpose = Dpose * H00 * H0;
-                *H1 = Matrix::Zero(2, 3);
-                (*H1)(0, 0) = Dpose(0, 3); // du/dx
-                (*H1)(1, 0) = Dpose(1, 3); // dv/dx
-                (*H1)(0, 1) = Dpose(0, 4); // du/dy
-                (*H1)(1, 1) = Dpose(1, 4); // dv/dy
-                (*H1)(0, 2) = Dpose(0, 2); // du/dyaw
-                (*H1)(1, 2) = Dpose(1, 2); // dv/dyaw
-                return result;
-            }
-            else {
-                return camera.project(landmark_, {}, {}, {});
-            }
-        }
-
-        Vector evaluateError(
-            const Pose2& pose,
-            OptionalMatrixType H1 = OptionalNone) const override {
+            OptionalMatrixType H1, // 2x3 (x, y, z)
+            OptionalMatrixType H2, // 2x3 (x, y, theta)
+            OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta)
+            OptionalMatrixType H4  // 2x9
+        ) const {
             try {
-                return h2(pose, H1) - measured_;
+                // this is x-forward z-up
+                gtsam::Matrix H0; // 6x6
+                Pose3 offset_pose = Pose3(pose).compose(offset, H0);
+                // this is z-forward y-down
+                gtsam::Matrix H00; // 6x6
+                Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
+                if (H2 || H3) {
+                    // Dpose is for pose3, 2x6 (R,t)
+                    gtsam::Matrix Dpose;
+                    Point2 result = camera.project(landmark, Dpose, H1, H4);
+                    gtsam::Matrix DposeOffset = Dpose * H00; // 2x6
+                    if (H3)
+                        *H3 = DposeOffset; // with Eigen this is a deep copy (!)
+                    if (H2) {
+                        gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
+                        *H2 = Matrix::Zero(2, 3);
+                        (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx
+                        (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx
+                        (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy
+                        (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy
+                        (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw
+                        (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw
+                    }
+                    return result;
+                } else {
+                    return camera.project(landmark, {}, {}, {});
+                }
             } catch (CheiralityException& e) {
                 std::cout << "****** CHIRALITY EXCEPTION ******\n";
-                std::cout << "landmark " << landmark_ << "\n";
-                std::cout << "pose " << pose << "\n";
-                std::cout << "offset " << offset_ << "\n";
-                std::cout << "calib " << calib_ << "\n";
                 if (H1) *H1 = Matrix::Zero(2, 3);
+                if (H2) *H2 = Matrix::Zero(2, 3);
+                if (H3) *H3 = Matrix::Zero(2, 6);
+                if (H4) *H4 = Matrix::Zero(2, 9);
                 // return a large error
-                return Matrix::Constant(2, 1, 2.0 * calib_.fx());
+                return Matrix::Constant(2, 1, 2.0 * calib.fx());
             }
         }
+
+        Point2 measured_; // pixel measurement
+
+    private:
+        static const Pose3 CAM_COORD;
     };
 
     // camera "zero" is facing +z; this turns it to face +x
-    const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3(
+    const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3(
         Rot3(0, 0, 1,//
             -1, 0, 0, //
             0, -1, 0),
         Vector3(0, 0, 0)
     );
 
-    template<>
-    struct traits<PlanarProjectionFactor> :
-        public Testable<PlanarProjectionFactor > {
+    /**
+     * @class PlanarProjectionFactor1
+     * @brief One variable: the pose.
+     * Landmark, camera offset, camera calibration are constant.
+     * This is intended for localization within a known map.
+     */
+    class PlanarProjectionFactor1
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2> {
+    public:
+        typedef NoiseModelFactorN<Pose2> Base;
+        using Base::evaluateError;
+        PlanarProjectionFactor1() {}
+
+        ~PlanarProjectionFactor1() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
+        }
+
+
+        /**
+         * @param landmark point3 in the world
+         * @param measured corresponding point2 in the camera frame
+         * @param offset constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         */
+        PlanarProjectionFactor1(
+            const Point3& landmark,
+            const Point2& measured,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model,
+            Key poseKey)
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, poseKey),
+            landmark_(landmark),
+            offset_(offset),
+            calib_(calib) {
+            assert(2 == model->dim());
+        }
+
+        /**
+         * @param pose estimated pose2
+         * @param H1 pose jacobian
+         */
+        Vector evaluateError(
+            const Pose2& pose,
+            OptionalMatrixType H1 = OptionalNone) const override {
+            return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_;
+        }
+
+    private:
+        Point3 landmark_; // landmark
+        Pose3 offset_; // camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
     };
 
-} // namespace gtsam
+    template<>
+    struct traits<PlanarProjectionFactor1> :
+        public Testable<PlanarProjectionFactor1> {};
+
+    /**
+     * @class PlanarProjectionFactor2
+     * @brief Two unknowns: the pose and the landmark.
+     * Camera offset and calibration are constant.
+     * This is similar to GeneralSFMFactor, used for SLAM.
+    */
+    class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Point3, Pose2> {
+    public:
+        typedef NoiseModelFactorN<Point3, Pose2> Base;
+        using Base::evaluateError;
+
+        PlanarProjectionFactor2() {}
+
+        ~PlanarProjectionFactor2() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
+        }
+
+        /**
+         * @param measured corresponding point in the camera frame
+         * @param offset constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param landmarkKey index of the landmark point3
+         */
+        PlanarProjectionFactor2(
+            const Point2& measured,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model,
+            Key landmarkKey,
+            Key poseKey)
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, landmarkKey, poseKey),
+            offset_(offset),
+            calib_(calib) {
+            assert(2 == model->dim());
+        }
+
+        /**
+         * @param landmark estimated landmark
+         * @param pose estimated pose2
+         * @param H1 landmark jacobian
+         * @param H2 pose jacobian
+         */
+        Vector evaluateError(
+            const Point3& landmark,
+            const Pose2& pose,
+            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType H2 = OptionalNone) const override {
+            return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_;
+        }
+
+    private:
+        Pose3 offset_; // camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor2> :
+        public Testable<PlanarProjectionFactor2> {};
+
+    /**
+     * @class PlanarProjectionFactor3
+     * @brief Three unknowns: the pose, the camera offset, and the camera calibration.
+     * Landmark is constant.
+     * This is intended to be used for camera calibration.
+    */
+    class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
+    public:
+        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;
+        using Base::evaluateError;
+
+        PlanarProjectionFactor3() {}
+
+        ~PlanarProjectionFactor3() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
+        }
+
+        /**
+         * @param landmark point3 in the world
+         * @param measured corresponding point2 in the camera frame
+         * @param model stddev of the measurements, ~one pixel?
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param offsetKey index of camera offset from pose
+         * @param calibKey index of camera calibration         */
+        PlanarProjectionFactor3(
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model,
+            Key poseKey,
+            Key offsetKey,
+            Key calibKey)
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
+            landmark_(landmark) {
+            assert(2 == model->dim());
+        }
+
+        /**
+         * @param pose estimated pose2
+         * @param offset pose3 offset from pose2 +x
+         * @param calib calibration
+         * @param H1 pose jacobian
+         * @param H2 offset jacobian
+         * @param H3 calibration jacobian
+         */
+        Vector evaluateError(
+            const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType H2 = OptionalNone,
+            OptionalMatrixType H3 = OptionalNone) const override {
+            return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_;
+        }
+
+    private:
+        Point3 landmark_; // landmark
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor3> :
+        public Testable<PlanarProjectionFactor3> {};
+
+} // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h
deleted file mode 100644
index bdcbb6ee2..000000000
--- a/gtsam/slam/PlanarSFMFactor.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/**
- * Similar to GeneralSFMFactor, but:
- *
- * * 2d pose variable (robot on the floor)
- * * camera offset variable
- * * constant landmarks
- * * batched input
- * * numeric differentiation
- *
- * This factor is useful to find camera calibration and placement, in
- * a sort of "autocalibrate" mode.  Once a satisfactory solution is
- * found, the PlanarProjectionFactor should be used for localization.
- *
- * The python version of this factor uses batches, to save on calls
- * across the C++/python boundary, but here the only extra cost
- * is instantiating the camera, so there's no batch.
- *
- * @see https://www.firstinspires.org/
- * @see PlanarProjectionFactor.h
- *
- * @file PlanarSFMFactor.h
- * @brief for planar smoothing with unknown calibration
- * @date Dec 2, 2024
- * @author joel@truher.org
- */
-#pragma once
-
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/Lie.h>
-
-#include <gtsam/geometry/Cal3DS2.h>
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Rot3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/base/numericalDerivative.h>
-
-
-namespace gtsam {
-    /**
-     * @class PlanarSFMFactor
-     * @brief Camera calibration for robot on the floor.
-     */
-    class PlanarSFMFactor : public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
-    private:
-        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;   
-        static const Pose3 CAM_COORD;
-
-    protected:
-
-        Point3 landmark_; // landmark
-        Point2 measured_; // pixel measurement
-
-    public:
-        // Provide access to the Matrix& version of evaluateError:
-        using Base::evaluateError;
-
-        PlanarSFMFactor() {}
-        /**
-         * @param landmarks point in the world
-         * @param measured corresponding point in the camera frame
-         * @param model stddev of the measurements, ~one pixel?
-         * @param poseKey index of the robot pose2 in the z=0 plane
-         * @param offsetKey index of the 3d camera offset from the robot pose
-         * @param calibKey index of the camera calibration
-         */
-        PlanarSFMFactor(
-            const Point3& landmark,
-            const Point2& measured,
-            const SharedNoiseModel& model,
-            Key poseKey,
-            Key offsetKey,
-            Key calibKey)
-            : NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
-            landmark_(landmark), measured_(measured)
-        {
-            assert(2 == model->dim());
-        }
-
-        ~PlanarSFMFactor() override {}
-
-        /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this)));
-        }
-
-        Point2 h2(const Pose2& pose,
-            const Pose3& offset,
-            const Cal3DS2& calib,
-            OptionalMatrixType H1,
-            OptionalMatrixType H2,
-            OptionalMatrixType H3
-            ) const {
-            // this is x-forward z-up
-            gtsam::Matrix H0;
-            Pose3 offset_pose = Pose3(pose).compose(offset, H0);
-            // this is z-forward y-down
-            gtsam::Matrix H00;
-            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
-            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
-            if (H1 || H2) {
-                gtsam::Matrix Dpose;
-                Point2 result = camera.project(landmark_, Dpose, {}, H3);
-                gtsam::Matrix DposeOffset = Dpose * H00;
-                if (H2)
-                    *H2 = DposeOffset; // a deep copy
-                if (H1) {
-                    gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
-                    *H1 = Matrix::Zero(2,3);
-                    (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx
-                    (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx
-                    (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy
-                    (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy
-                    (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw
-                    (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw
-                }
-                return result;
-            } else {
-                return camera.project(landmark_, {}, {}, {});
-            }    
-        }
-
-        Vector evaluateError(
-            const Pose2& pose,
-            const Pose3& offset,
-            const Cal3DS2& calib,
-            OptionalMatrixType H1 = OptionalNone,
-            OptionalMatrixType H2 = OptionalNone,
-            OptionalMatrixType H3 = OptionalNone
-        ) const override {
-            try {
-                return h2(pose,offset,calib,H1,H2,H3) - measured_;
-            }
-            catch (CheiralityException& e) {
-                std::cout << "****** CHIRALITY EXCEPTION ******\n";
-                std::cout << "landmark " << landmark_ << "\n";
-                std::cout << "pose " << pose << "\n";
-                std::cout << "offset " << offset << "\n";
-                std::cout << "calib " << calib << "\n";
-                if (H1) *H1 = Matrix::Zero(2, 3);
-                if (H2) *H2 = Matrix::Zero(2, 6);
-                if (H3) *H3 = Matrix::Zero(2, 9);
-                // return a large error
-                return Matrix::Constant(2, 1, 2.0 * calib.fx());
-            }
-        }
-    };
-
-    // camera "zero" is facing +z; this turns it to face +x
-    const Pose3 PlanarSFMFactor::CAM_COORD = Pose3(
-        Rot3(0, 0, 1,//
-            -1, 0, 0, //
-            0, -1, 0),
-        Vector3(0, 0, 0)
-    );
-
-    template<>
-    struct traits<PlanarSFMFactor> :
-        public Testable<PlanarSFMFactor > {
-    };
-
-} // namespace gtsam
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index d69b9890e..992c9aa6f 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -25,8 +25,8 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/PlanarProjectionFactor.h>
-virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor {
-  PlanarProjectionFactor(
+virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor1(
             const gtsam::Point3& landmark,
             const gtsam::Point2& measured,
             const gtsam::Pose3& offset,
@@ -35,6 +35,26 @@ virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor {
             size_t poseKey);
   void serialize() const;
 };
+virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor2(
+            const gtsam::Point2& measured,
+            const gtsam::Pose3& offset,
+            const gtsam::Cal3DS2& calib,
+            const gtsam::noiseModel::Base* model,
+            size_t landmarkKey,
+            size_t poseKey);
+  void serialize() const;
+};
+virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor3(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey,
+            size_t offsetKey,
+            size_t calibKey));
+  void serialize() const;
+};
 
 #include <gtsam/slam/ProjectionFactor.h>
 template <POSE, LANDMARK, CALIBRATION>
@@ -79,18 +99,6 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                        gtsam::Cal3Unified>
     GenericProjectionFactorCal3Unified;
 
-#include <gtsam/slam/PlanarSFMFactor.h>
-virtual class PlanarSFMFactor  : gtsam::NoiseModelFactor {
-  PlanarSFMFactor(
-            const gtsam::Point3& landmark,
-            const gtsam::Point2& measured,
-            const gtsam::noiseModel::Base* model,
-            size_t poseKey,
-            size_t offsetKey,
-            size_t calibKey);
-  void serialize() const;
-};
-
 #include <gtsam/slam/GeneralSFMFactor.h>
 template <CAMERA, LANDMARK>
 virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index eafe2041f..21961cdad 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -24,8 +24,10 @@
 using namespace std;
 using namespace gtsam;
 using symbol_shorthand::X;
+using symbol_shorthand::C;
+using symbol_shorthand::K;
 
-TEST(PlanarProjectionFactor, error1) {
+TEST(PlanarProjectionFactor1, error1) {
     // landmark is on the camera bore (which faces +x)
     Point3 landmark(1, 0, 0);
     // so pixel measurement is (cx, cy)
@@ -37,7 +39,7 @@ TEST(PlanarProjectionFactor, error1) {
     Pose2 pose(0, 0, 0);
     values.insert(X(0), pose);
 
-    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
 
     CHECK_EQUAL(2, factor.dim());
     CHECK(factor.active(values));
@@ -55,7 +57,7 @@ TEST(PlanarProjectionFactor, error1) {
     CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
 }
 
-TEST(PlanarProjectionFactor, error2) {
+TEST(PlanarProjectionFactor1, error2) {
     // landmark is in the upper left corner
     Point3 landmark(1, 1, 1);
     // upper left corner in pixels
@@ -63,7 +65,7 @@ TEST(PlanarProjectionFactor, error2) {
     Pose3 offset;
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -85,7 +87,7 @@ TEST(PlanarProjectionFactor, error2) {
     CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
 }
 
-TEST(PlanarProjectionFactor, error3) {
+TEST(PlanarProjectionFactor1, error3) {
     // landmark is in the upper left corner
     Point3 landmark(1, 1, 1);
     // upper left corner in pixels
@@ -94,7 +96,7 @@ TEST(PlanarProjectionFactor, error3) {
     // distortion
     Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -116,7 +118,7 @@ TEST(PlanarProjectionFactor, error3) {
     CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
 }
 
-TEST(PlanarProjectionFactor, jacobian) {
+TEST(PlanarProjectionFactor1, jacobian) {
     // test many jacobians with many randoms
 
     std::default_random_engine g;
@@ -129,7 +131,7 @@ TEST(PlanarProjectionFactor, jacobian) {
         Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+        PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
 
         Pose2 pose(s(g), s(g), s(g));
 
@@ -143,8 +145,213 @@ TEST(PlanarProjectionFactor, jacobian) {
                 pose);
         CHECK(assert_equal(expectedH1, H1, 1e-6));
     }
+}
 
 
+
+TEST(PlanarProjectionFactor3, error1) {
+    // landmark is on the camera bore (facing +x)
+    Point3 landmark(1, 0, 0);
+    // so px is (cx, cy)
+    Point2 measured(200, 200);
+    // offset is identity
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    Values values;
+    Pose2 pose(0, 0, 0);
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    // du/dx etc for the pose2d
+    Matrix23 H1Expected = (Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        0, 0, 200, 0, 200, 0,//
+        0, -200, 0, 0, 0, 200).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    // du wrt calibration
+    // on-bore, f doesn't matter
+    // but c does
+    Matrix29 H3Expected = (Matrix29() <<//
+        0, 0, 0, 1, 0, 0, 0, 0, 0,//
+        0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor3, error2) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    Matrix23 H1Expected = (Matrix23() <<//
+        -200, 200, 400,//
+        -200, 0, 200).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        -200, -200, 400, -200, 200, 0,//
+        200, -400, 200, -200, 0, 200).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    Matrix29 H3Expected = (Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor3, error3) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    CHECK(assert_equal(Vector2(0, 0), actual));
+
+    const Matrix& H1Actual = actualHs.at(0);
+    const Matrix& H2Actual = actualHs.at(1);
+    const Matrix& H3Actual = actualHs.at(2);
+
+    CHECK_EQUAL(2, H1Actual.rows());
+    CHECK_EQUAL(3, H1Actual.cols());
+    CHECK_EQUAL(2, H2Actual.rows());
+    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK_EQUAL(2, H3Actual.rows());
+    CHECK_EQUAL(9, H3Actual.cols());
+
+    Matrix23 H1Expected = (Matrix23() <<//
+        -360, 280, 640,//
+        -360, 80, 440).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    Matrix26 H2Expected = (Matrix26() <<//
+        -200, -440, 640, -360, 280, 80,//
+        200, -640, 440, -360, 80, 280).finished();
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
+
+    Matrix29 H3Expected = (Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+
+TEST(PlanarProjectionFactor3, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);    
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+        PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1, H2, H3;
+        factor.evaluateError(pose, offset, calib, H1, H2, H3);
+
+        Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+        CHECK(assert_equal(expectedH2, H2, 1e-6));
+        CHECK(assert_equal(expectedH3, H3, 1e-6));
+    }
 }
 
 /* ************************************************************************* */
@@ -153,4 +360,3 @@ int main() {
     return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-
diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp
deleted file mode 100644
index 22ec67561..000000000
--- a/gtsam/slam/tests/testPlanarSFMFactor.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/**
- * @file testPlanarSFMFactor.cpp
- * @date Dec 3, 2024
- * @author joel@truher.org
- * @brief unit tests for PlanarSFMFactor
- */
-
-#include <random>
-
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/Cal3DS2.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Rot2.h>
-#include <gtsam/geometry/Rot3.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/slam/PlanarSFMFactor.h>
-
-#include <CppUnitLite/TestHarness.h>
-
-using namespace std;
-using namespace gtsam;
-using symbol_shorthand::X;
-using symbol_shorthand::C;
-using symbol_shorthand::K;
-
-TEST(PlanarSFMFactor, error1) {
-    // landmark is on the camera bore (facing +x)
-    Point3 landmark(1, 0, 0);
-    // so px is (cx, cy)
-    Point2 measured(200, 200);
-    // offset is identity
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    Values values;
-    Pose2 pose(0, 0, 0);
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
-
-    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
-
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(3);
-
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    // du/dx etc for the pose2d
-    Matrix23 H1Expected = (Matrix23() <<//
-        0, 200, 200,//
-        0, 0, 0).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
-
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        0, 0, 200, 0, 200, 0,//
-        0, -200, 0, 0, 0, 200).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    // du wrt calibration
-    // on-bore, f doesn't matter
-    // but c does
-    Matrix29 H3Expected = (Matrix29() <<//
-        0, 0, 0, 1, 0, 0, 0, 0, 0,//
-        0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
-}
-
-TEST(PlanarSFMFactor, error2) {
-    Point3 landmark(1, 1, 1);
-    Point2 measured(0, 0);
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-
-    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
-    Values values;
-    Pose2 pose(0, 0, 0);
-
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
-
-
-    CHECK_EQUAL(2, model->dim());
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(3);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    Matrix23 H1Expected = (Matrix23() <<//
-        -200, 200, 400,//
-        -200, 0, 200).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
-
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        -200, -200, 400, -200, 200, 0,//
-        200, -400, 200, -200, 0, 200).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    Matrix29 H3Expected = (Matrix29() <<//
-        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
-        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
-
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
-}
-
-TEST(PlanarSFMFactor, error3) {
-    Point3 landmark(1, 1, 1);
-    Point2 measured(0, 0);
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-
-    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
-    Values values;
-    Pose2 pose(0, 0, 0);
-
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
-
-
-    CHECK_EQUAL(2, model->dim());
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(3);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    Matrix23 H1Expected = (Matrix23() <<//
-        -360, 280, 640,//
-        -360, 80, 440).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
-
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        -200, -440, 640, -360, 280, 80,//
-        200, -640, 440, -360, 80, 280).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    Matrix29 H3Expected = (Matrix29() <<//
-        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
-        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
-
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
-}
-
-
-
-TEST(PlanarSFMFactor, jacobian) {
-    // test many jacobians with many randoms
-
-    std::default_random_engine g;
-    std::uniform_real_distribution<double> s(-0.3, 0.3);    
-    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-
-    for (int i = 0; i < 1000; ++i) {
-        Point3 landmark(2 + s(g), s(g), s(g));
-        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
-        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
-        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-
-        PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
-
-        Pose2 pose(s(g), s(g), s(g));
-
-        // actual H
-        Matrix H1, H2, H3;
-        factor.evaluateError(pose, offset, calib, H1, H2, H3);
-
-        Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
-            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
-                return factor.evaluateError(p, o, c);},
-                pose, offset, calib);
-        Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
-            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
-                return factor.evaluateError(p, o, c);},
-                pose, offset, calib);
-        Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
-            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
-                return factor.evaluateError(p, o, c);},
-                pose, offset, calib);
-        CHECK(assert_equal(expectedH1, H1, 1e-6));
-        CHECK(assert_equal(expectedH2, H2, 1e-6));
-        CHECK(assert_equal(expectedH3, H3, 1e-6));
-    }
-}
-
-
-/* ************************************************************************* */
-int main() {
-    TestResult tr;
-    return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */
-

From ca958ccbc70eda61d5d56d101fa0101cac2e099c Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Mon, 16 Dec 2024 19:06:23 -0800
Subject: [PATCH 152/362] maybe fix clang?

---
 gtsam/slam/tests/testPlanarProjectionFactor.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index 21961cdad..03a0b7bed 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -141,7 +141,7 @@ TEST(PlanarProjectionFactor1, jacobian) {
 
         Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
             [&factor](const Pose2& p) {
-                return factor.evaluateError(p);},
+                return factor.evaluateError(p, {});},
                 pose);
         CHECK(assert_equal(expectedH1, H1, 1e-6));
     }
@@ -338,15 +338,15 @@ TEST(PlanarProjectionFactor3, jacobian) {
 
         Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
             [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
-                return factor.evaluateError(p, o, c);},
+                return factor.evaluateError(p, o, c, {}, {}, {});},
                 pose, offset, calib);
         Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
             [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
-                return factor.evaluateError(p, o, c);},
+                return factor.evaluateError(p, o, c, {}, {}, {});},
                 pose, offset, calib);
         Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
             [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
-                return factor.evaluateError(p, o, c);},
+                return factor.evaluateError(p, o, c, {}, {}, {});},
                 pose, offset, calib);
         CHECK(assert_equal(expectedH1, H1, 1e-6));
         CHECK(assert_equal(expectedH2, H2, 1e-6));

From 70288bc32a573cb2040d2f6f0fc115b1e5a094b2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Dec 2024 22:18:12 -0500
Subject: [PATCH 153/362] remove print statements

---
 gtsam/discrete/TableFactor.cpp | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 0a93e7b5d..d9bfc42c2 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -262,8 +262,6 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
     // k will be in reverse key order already
     uint64_t k;
     ss >> k;
-    std::cout << "ss: " << ss.str() << ", k=" << k
-              << ", v=" << sparse_table_.coeff(i) << std::endl;
     pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
 
@@ -273,10 +271,6 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
       [](const std::pair<uint64_t, double>& a,
          const std::pair<uint64_t, double>& b) { return a.first < b.first; });
 
-  std::cout << "Sorted pair_table:" << std::endl;
-  for (auto&& [k, v] : pair_table) {
-    std::cout << "k=" << k << ", v=" << v << std::endl;
-  }
   // Create the table vector
   std::vector<double> table;
   std::for_each(pair_table.begin(), pair_table.end(),

From 8e473c04e82eacbef373cc8efdc8be59eb01e4e1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 22:46:16 -0500
Subject: [PATCH 154/362] Address Quaternion on Ubuntu accuracy

---
 gtsam/geometry/tests/testPose3.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index ce74091f0..b04fb1982 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -906,7 +906,7 @@ TEST(Pose3, Logmap) {
     for (Vector3 v : test_cases::vs) {
       const Vector6 xi = (Vector6() << w, v).finished();
       Pose3 pose = Pose3::Expmap(xi);
-      EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5));
+      EXPECT(assert_equal(xi, Pose3::Logmap(pose)));
     }
   }
 }
@@ -924,7 +924,13 @@ TEST(Pose3, LogmapDerivatives) {
                 &Pose3::Logmap, pose, {});
         Matrix actualH;
         Pose3::Logmap(pose, actualH);
+#ifdef GTSAM_USE_QUATERNIONS
+        // TODO(Frank): Figure out why quaternions are not as accurate.
+        // Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS.
+        EXPECT(assert_equal(expectedH, actualH, 1e-7));
+#else
         EXPECT(assert_equal(expectedH, actualH));
+#endif
       }
     }
   }

From 11f7eb642295d997012ca942695a5be7e1e79cb4 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Mon, 16 Dec 2024 19:47:42 -0800
Subject: [PATCH 155/362] fix typo, fix python CI?

---
 gtsam/slam/slam.i | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 992c9aa6f..e141cb50c 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -52,7 +52,7 @@ virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor {
             const gtsam::noiseModel::Base* model,
             size_t poseKey,
             size_t offsetKey,
-            size_t calibKey));
+            size_t calibKey);
   void serialize() const;
 };
 

From 004cab1542c807e16218bc8cf85c4feb759ca8cb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 17 Dec 2024 00:14:17 -0500
Subject: [PATCH 156/362] add serialize() to various preintegration classes

---
 gtsam/navigation/navigation.i | 28 +++++++++++++++++++---------
 1 file changed, 19 insertions(+), 9 deletions(-)

diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
index 831788073..3e7aa0fd6 100644
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams {
 
   std::optional<gtsam::Vector> getOmegaCoriolis() const;
   std::optional<gtsam::Pose3> getBodyPSensor() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/PreintegrationParams.h>
@@ -170,22 +173,26 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
   gtsam::Matrix getBiasAccCovariance() const ;
   gtsam::Matrix getBiasOmegaCovariance() const ;
   gtsam::Matrix getBiasAccOmegaInit() const;
- 
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class PreintegratedCombinedMeasurements {
-// Constructors
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
-				    const gtsam::imuBias::ConstantBias& bias);
+  // Constructors
+  PreintegratedCombinedMeasurements(
+      const gtsam::PreintegrationCombinedParams* params);
+  PreintegratedCombinedMeasurements(
+      const gtsam::PreintegrationCombinedParams* params,
+      const gtsam::imuBias::ConstantBias& bias);
   // Testable
   void print(string s = "Preintegrated Measurements:") const;
   bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
-      double tol);
+              double tol);
 
   // Standard Interface
-  void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
-      double deltaT);
+  void integrateMeasurement(gtsam::Vector measuredAcc,
+                            gtsam::Vector measuredOmega, double deltaT);
   void resetIntegration();
   void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
 
@@ -197,7 +204,10 @@ class PreintegratedCombinedMeasurements {
   gtsam::imuBias::ConstantBias biasHat() const;
   gtsam::Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
-      const gtsam::imuBias::ConstantBias& bias) const;
+                          const gtsam::imuBias::ConstantBias& bias) const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class CombinedImuFactor: gtsam::NoiseModelFactor {

From c5554c836f79ec52bebadf31659dde5baf48d004 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 17 Dec 2024 00:15:09 -0500
Subject: [PATCH 157/362] python tests

---
 python/gtsam/tests/test_Serialization.py | 47 ++++++++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 python/gtsam/tests/test_Serialization.py

diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py
new file mode 100644
index 000000000..be8dd7bec
--- /dev/null
+++ b/python/gtsam/tests/test_Serialization.py
@@ -0,0 +1,47 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+KalmanFilter unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+from copy import deepcopy
+
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+import gtsam
+
+
+class TestSerialization(GtsamTestCase):
+    """Tests for serialization of various GTSAM objects."""
+
+    def test_PreintegratedImuMeasurements(self):
+        """
+        Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy.
+        """
+        params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81]))
+        pim = gtsam.PreintegratedImuMeasurements(params)
+
+        # If serialization failed, then this will throw an error
+        pim2 = deepcopy(pim)
+        self.assertEqual(pim, pim2)
+
+    def test_PreintegratedCombinedMeasurements(self):
+        """
+        Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy.
+        """
+        params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81]))
+        pim = gtsam.PreintegratedCombinedMeasurements(params)
+
+        # If serialization failed, then this will throw an error
+        pim2 = deepcopy(pim)
+        self.assertEqual(pim, pim2)
+
+
+if __name__ == "__main__":
+    unittest.main()

From dfcbba822e673fa60527409cf33db7b8731cf08b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 17 Dec 2024 09:20:33 -0500
Subject: [PATCH 158/362] move default AHRSFactor constructor to public for
 wrapper

---
 gtsam/navigation/AHRSFactor.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h
index 05439bafc..4289b4969 100644
--- a/gtsam/navigation/AHRSFactor.h
+++ b/gtsam/navigation/AHRSFactor.h
@@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
   PreintegratedAhrsMeasurements _PIM_;
 
-  /** Default constructor - only use for serialization */
-  AHRSFactor() {}
-
 public:
 
   // Provide access to the Matrix& version of evaluateError:
@@ -154,6 +151,9 @@ public:
   typedef std::shared_ptr<AHRSFactor> shared_ptr;
 #endif
 
+  /** Default constructor - only use for serialization */
+  AHRSFactor() {}
+
   /**
    * Constructor
    * @param rot_i previous rot key

From 8f77bfa13b9688358abc476f467afdb3c9940493 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 17 Dec 2024 09:22:47 -0500
Subject: [PATCH 159/362] serialize navigation factors

---
 gtsam/navigation/navigation.i            | 29 +++++++++++++++++++++++-
 python/gtsam/tests/test_Serialization.py | 13 +++++++++++
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
index 3e7aa0fd6..adb8bf2bb 100644
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@@ -151,6 +151,9 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
   gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
       const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
       const gtsam::imuBias::ConstantBias& bias);
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
@@ -206,7 +209,7 @@ class PreintegratedCombinedMeasurements {
   gtsam::NavState predict(const gtsam::NavState& state_i,
                           const gtsam::imuBias::ConstantBias& bias) const;
 
-  // enabling serialization functionality
+  // enable serialization functionality
   void serialize() const;
 };
 
@@ -221,6 +224,9 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
       const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
       const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::imuBias::ConstantBias& bias_j);
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/AHRSFactor.h>
@@ -247,6 +253,9 @@ class PreintegratedAhrsMeasurements {
   // Standard Interface
   void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT);
   void resetIntegration() ;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class AHRSFactor : gtsam::NonlinearFactor {
@@ -263,6 +272,9 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
   gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias,
       const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
       gtsam::Vector omegaCoriolis) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/AttitudeFactor.h>
@@ -276,6 +288,9 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor {
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nRef() const;
   gtsam::Unit3 bMeasured() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
@@ -290,6 +305,9 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nRef() const;
   gtsam::Unit3 bMeasured() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/GPSFactor.h>
@@ -304,6 +322,9 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class GPSFactor2 : gtsam::NonlinearFactor {
@@ -317,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/BarometricFactor.h>
@@ -334,6 +358,9 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
   const double& measurementIn() const;
   double heightOut(double n) const;
   double baroOut(const double& meters) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/Scenario.h>
diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py
index be8dd7bec..3e22e6603 100644
--- a/python/gtsam/tests/test_Serialization.py
+++ b/python/gtsam/tests/test_Serialization.py
@@ -12,6 +12,7 @@ import unittest
 from copy import deepcopy
 
 import numpy as np
+from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
@@ -31,6 +32,18 @@ class TestSerialization(GtsamTestCase):
         pim2 = deepcopy(pim)
         self.assertEqual(pim, pim2)
 
+    def test_ImuFactor(self):
+        """
+        Test the serialization of `ImuFactor` by performing a deepcopy.
+        """
+        params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81]))
+        pim = gtsam.PreintegratedImuMeasurements(params)
+        imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)
+
+        # If serialization failed, then this will throw an error
+        imu_odom2 = deepcopy(imu_odom)
+        self.assertEqual(imu_odom, imu_odom2)
+
     def test_PreintegratedCombinedMeasurements(self):
         """
         Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy.

From 60b00ebdda6da38bf26c35d112cee6d964bd5da8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 17 Dec 2024 10:08:19 -0500
Subject: [PATCH 160/362] improve testing for deep copy

---
 python/gtsam/tests/test_Serialization.py | 30 ++++++++++--------------
 python/gtsam/tests/test_pickle.py        | 10 ++++----
 python/gtsam/utils/test_case.py          | 16 +++++++++++--
 3 files changed, 32 insertions(+), 24 deletions(-)

diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py
index 3e22e6603..e935f1f62 100644
--- a/python/gtsam/tests/test_Serialization.py
+++ b/python/gtsam/tests/test_Serialization.py
@@ -5,11 +5,11 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-KalmanFilter unit tests.
-Author: Frank Dellaert & Duy Nguyen Ta (Python)
+Serialization and deep copy tests.
+
+Author: Varun Agrawal
 """
 import unittest
-from copy import deepcopy
 
 import numpy as np
 from gtsam.symbol_shorthand import B, V, X
@@ -18,42 +18,36 @@ from gtsam.utils.test_case import GtsamTestCase
 import gtsam
 
 
-class TestSerialization(GtsamTestCase):
-    """Tests for serialization of various GTSAM objects."""
+class TestDeepCopy(GtsamTestCase):
+    """Tests for deep copy of various GTSAM objects."""
 
     def test_PreintegratedImuMeasurements(self):
         """
-        Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy.
+        Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy.
         """
         params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81]))
         pim = gtsam.PreintegratedImuMeasurements(params)
 
-        # If serialization failed, then this will throw an error
-        pim2 = deepcopy(pim)
-        self.assertEqual(pim, pim2)
+        self.assertDeepCopyEquality(pim)
 
     def test_ImuFactor(self):
         """
-        Test the serialization of `ImuFactor` by performing a deepcopy.
+        Test the deep copy of `ImuFactor` by performing a deepcopy.
         """
         params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81]))
         pim = gtsam.PreintegratedImuMeasurements(params)
-        imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)
+        imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)
 
-        # If serialization failed, then this will throw an error
-        imu_odom2 = deepcopy(imu_odom)
-        self.assertEqual(imu_odom, imu_odom2)
+        self.assertDeepCopyEquality(imu_factor)
 
     def test_PreintegratedCombinedMeasurements(self):
         """
-        Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy.
+        Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy.
         """
         params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81]))
         pim = gtsam.PreintegratedCombinedMeasurements(params)
 
-        # If serialization failed, then this will throw an error
-        pim2 = deepcopy(pim)
-        self.assertEqual(pim, pim2)
+        self.assertDeepCopyEquality(pim)
 
 
 if __name__ == "__main__":
diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py
index a6a5745bc..e51617b00 100644
--- a/python/gtsam/tests/test_pickle.py
+++ b/python/gtsam/tests/test_pickle.py
@@ -9,24 +9,26 @@ Unit tests to check pickling.
 
 Author: Ayush Baid
 """
-from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 
-
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import (Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3,
+                   Pose3, Rot3, SfmTrack, Unit3)
+
+
 class TestPickle(GtsamTestCase):
     """Tests pickling on some of the classes."""
 
     def test_cal3Bundler_roundtrip(self):
         obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70)
         self.assertEqualityOnPickleRoundtrip(obj)
-    
+
     def test_pinholeCameraCal3Bundler_roundtrip(self):
         obj = PinholeCameraCal3Bundler(
             Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)),
             Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70),
         )
         self.assertEqualityOnPickleRoundtrip(obj)
-    
+
     def test_rot3_roundtrip(self):
         obj = Rot3.RzRyRx(0, 0.05, 0.1)
         self.assertEqualityOnPickleRoundtrip(obj)
diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py
index 50af004f4..74eaff1db 100644
--- a/python/gtsam/utils/test_case.py
+++ b/python/gtsam/utils/test_case.py
@@ -10,6 +10,7 @@ Author: Frank Dellaert
 """
 import pickle
 import unittest
+from copy import deepcopy
 
 
 class GtsamTestCase(unittest.TestCase):
@@ -28,8 +29,8 @@ class GtsamTestCase(unittest.TestCase):
         else:
             equal = actual.equals(expected, tol)
         if not equal:
-            raise self.failureException(
-                "Values are not equal:\n{}!={}".format(actual, expected))
+            raise self.failureException("Values are not equal:\n{}!={}".format(
+                actual, expected))
 
     def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None:
         """ Performs a round-trip using pickle and asserts equality.
@@ -41,3 +42,14 @@ class GtsamTestCase(unittest.TestCase):
         """
         roundTripObj = pickle.loads(pickle.dumps(obj))
         self.gtsamAssertEquals(roundTripObj, obj)
+
+    def assertDeepCopyEquality(self, obj):
+        """Perform assertion by checking if a
+        deep copied version of `obj` is equal to itself.
+
+        Args:
+            obj: The object to check is deep-copyable.
+        """
+        # If deep copy failed, then this will throw an error
+        obj2 = deepcopy(obj)
+        self.gtsamAssertEquals(obj, obj2)

From e4538d5b3e4ecddfabc8c8d81dc09553772f7147 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Tue, 17 Dec 2024 22:39:13 -0800
Subject: [PATCH 161/362] address some comments

---
 gtsam/geometry/Pose3.cpp                      |  11 ++
 gtsam/geometry/Pose3.h                        |   2 +
 gtsam/slam/PlanarProjectionFactor.h           | 168 +++++++++---------
 gtsam/slam/slam.i                             |  36 ++--
 .../slam/tests/testPlanarProjectionFactor.cpp |  24 ++-
 5 files changed, 132 insertions(+), 109 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index bb1483432..5d7732947 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -42,6 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
   return Pose3(R, t);
 }
 
+Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
+  if (H) *H << (gtsam::Matrix(6, 3) << //
+    0., 0., 0., //
+    0., 0., 0.,//
+    0., 0., 1.,//
+    1., 0., 0.,//
+    0., 1., 0.,//
+    0., 0., 0.).finished();
+  return Pose3(p);
+}
+
 /* ************************************************************************* */
 Pose3 Pose3::inverse() const {
   Rot3 Rt = R_.inverse();
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index d7c280c80..baac21ccb 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -78,6 +78,8 @@ public:
                       OptionalJacobian<6, 3> HR = {},
                       OptionalJacobian<6, 3> Ht = {});
 
+  static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});
+
   /**
    *  Create Pose3 by aligning two point pairs
    *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
index 285b64900..21f24da93 100644
--- a/gtsam/slam/PlanarProjectionFactor.h
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -38,69 +38,67 @@ namespace gtsam {
         PlanarProjectionFactorBase() {}
 
         /**
-         * @param measured corresponding point in the camera frame
-         * @param poseKey index of the robot pose in the z=0 plane
+         * @param measured pixels in the camera frame
          */
         PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}
 
         /**
-         * @param landmark point3
-         * @param pose
-         * @param offset oriented parallel to pose2 zero i.e. +x
-         * @param calib
-         * @param H1 landmark jacobian
-         * @param H2 pose jacobian
-         * @param H3 offset jacobian
-         * @param H4 calib jacobian
+         * Predict the projection of the landmark in camera pixels.
+         *
+         * @param landmark point3 of the target
+         * @param wTb "world to body": planar pose2 of vehicle body frame in world frame
+         * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x
+         * @param calib camera calibration with distortion
+         * @param Hlandmark jacobian
+         * @param HwTb jacobian
+         * @param HbTc jacobian
+         * @param Hcalib jacobian
          */
-        Point2 h(
+        Point2 predict(
             const Point3& landmark,
-            const Pose2& pose,
-            const Pose3& offset,
+            const Pose2& wTb,
+            const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType H1, // 2x3 (x, y, z)
-            OptionalMatrixType H2, // 2x3 (x, y, theta)
-            OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta)
-            OptionalMatrixType H4  // 2x9
+            OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z)
+            OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta)
+            OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta)
+            OptionalMatrixType Hcalib = nullptr // 2x9
         ) const {
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             try {
-                // this is x-forward z-up
+#endif
+                gtsam::Matrix Hp; // 6x3
                 gtsam::Matrix H0; // 6x6
-                Pose3 offset_pose = Pose3(pose).compose(offset, H0);
+                // this is x-forward z-up
+                Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr);
                 // this is z-forward y-down
                 gtsam::Matrix H00; // 6x6
-                Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+                Pose3 camera_pose = wTc.compose(CAM_COORD, H00);
                 PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
-                if (H2 || H3) {
+                if (HwTb || HbTc) {
                     // Dpose is for pose3, 2x6 (R,t)
                     gtsam::Matrix Dpose;
-                    Point2 result = camera.project(landmark, Dpose, H1, H4);
+                    Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
                     gtsam::Matrix DposeOffset = Dpose * H00; // 2x6
-                    if (H3)
-                        *H3 = DposeOffset; // with Eigen this is a deep copy (!)
-                    if (H2) {
-                        gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
-                        *H2 = Matrix::Zero(2, 3);
-                        (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx
-                        (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx
-                        (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy
-                        (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy
-                        (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw
-                        (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw
-                    }
+                    if (HbTc)
+                        *HbTc = DposeOffset; // with Eigen this is a deep copy (!)
+                    if (HwTb) 
+                        *HwTb =  DposeOffset * H0 * Hp;
                     return result;
                 } else {
                     return camera.project(landmark, {}, {}, {});
                 }
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             } catch (CheiralityException& e) {
                 std::cout << "****** CHIRALITY EXCEPTION ******\n";
-                if (H1) *H1 = Matrix::Zero(2, 3);
-                if (H2) *H2 = Matrix::Zero(2, 3);
-                if (H3) *H3 = Matrix::Zero(2, 6);
-                if (H4) *H4 = Matrix::Zero(2, 9);
+                if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3);
+                if (HwTb) *HwTb = Matrix::Zero(2, 3);
+                if (HbTc) *HbTc = Matrix::Zero(2, 6);
+                if (Hcalib) *Hcalib = Matrix::Zero(2, 9);
                 // return a large error
                 return Matrix::Constant(2, 1, 2.0 * calib.fx());
             }
+#endif
         }
 
         Point2 measured_; // pixel measurement
@@ -140,41 +138,42 @@ namespace gtsam {
 
 
         /**
+         * @brief constructor for known landmark, offset, and calibration
+         * @param poseKey index of the robot pose2 in the z=0 plane
          * @param landmark point3 in the world
          * @param measured corresponding point2 in the camera frame
-         * @param offset constant camera offset from pose
+         * @param bTc "body to camera": constant camera offset from pose
          * @param calib constant camera calibration
          * @param model stddev of the measurements, ~one pixel?
-         * @param poseKey index of the robot pose2 in the z=0 plane
          */
         PlanarProjectionFactor1(
+            Key poseKey,
             const Point3& landmark,
             const Point2& measured,
-            const Pose3& offset,
+            const Pose3& bTc,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model,
-            Key poseKey)
+            const SharedNoiseModel& model)
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey),
             landmark_(landmark),
-            offset_(offset),
+            bTc_(bTc),
             calib_(calib) {
             assert(2 == model->dim());
         }
 
         /**
-         * @param pose estimated pose2
-         * @param H1 pose jacobian
+         * @param wTb "world to body": estimated pose2
+         * @param HwTb jacobian
          */
         Vector evaluateError(
-            const Pose2& pose,
-            OptionalMatrixType H1 = OptionalNone) const override {
-            return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_;
+            const Pose2& wTb,
+            OptionalMatrixType HwTb = OptionalNone) const override {
+            return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
         }
 
     private:
         Point3 landmark_; // landmark
-        Pose3 offset_; // camera offset to robot pose
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
         Cal3DS2 calib_; // camera calibration
     };
 
@@ -188,9 +187,10 @@ namespace gtsam {
      * Camera offset and calibration are constant.
      * This is similar to GeneralSFMFactor, used for SLAM.
     */
-    class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Point3, Pose2> {
+    class PlanarProjectionFactor2 
+    : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
     public:
-        typedef NoiseModelFactorN<Point3, Pose2> Base;
+        typedef NoiseModelFactorN<Pose2, Point3> Base;
         using Base::evaluateError;
 
         PlanarProjectionFactor2() {}
@@ -204,43 +204,44 @@ namespace gtsam {
         }
 
         /**
-         * @param measured corresponding point in the camera frame
-         * @param offset constant camera offset from pose
-         * @param calib constant camera calibration
-         * @param model stddev of the measurements, ~one pixel?
+         * @brief constructor for variable landmark, known offset and calibration
          * @param poseKey index of the robot pose2 in the z=0 plane
          * @param landmarkKey index of the landmark point3
+         * @param measured corresponding point in the camera frame
+         * @param bTc "body to camera": constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
          */
         PlanarProjectionFactor2(
-            const Point2& measured,
-            const Pose3& offset,
-            const Cal3DS2& calib,
-            const SharedNoiseModel& model,
+            Key poseKey,
             Key landmarkKey,
-            Key poseKey)
+            const Point2& measured,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model)
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, landmarkKey, poseKey),
-            offset_(offset),
+            bTc_(bTc),
             calib_(calib) {
             assert(2 == model->dim());
         }
 
         /**
+         * @param wTb "world to body": estimated pose2
          * @param landmark estimated landmark
-         * @param pose estimated pose2
-         * @param H1 landmark jacobian
-         * @param H2 pose jacobian
+         * @param HwTb jacobian
+         * @param Hlandmark jacobian
          */
         Vector evaluateError(
+            const Pose2& wTb,
             const Point3& landmark,
-            const Pose2& pose,
-            OptionalMatrixType H1 = OptionalNone,
-            OptionalMatrixType H2 = OptionalNone) const override {
-            return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_;
+            OptionalMatrixType HwTb = OptionalNone,
+            OptionalMatrixType Hlandmark = OptionalNone) const override {
+            return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
         }
 
     private:
-        Pose3 offset_; // camera offset to robot pose
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
         Cal3DS2 calib_; // camera calibration
     };
 
@@ -270,6 +271,7 @@ namespace gtsam {
         }
 
         /**
+         * @brief constructor for variable pose, offset, and calibration, known landmark.
          * @param landmark point3 in the world
          * @param measured corresponding point2 in the camera frame
          * @param model stddev of the measurements, ~one pixel?
@@ -277,12 +279,12 @@ namespace gtsam {
          * @param offsetKey index of camera offset from pose
          * @param calibKey index of camera calibration         */
         PlanarProjectionFactor3(
-            const Point3& landmark,
-            const Point2& measured,
-            const SharedNoiseModel& model,
             Key poseKey,
             Key offsetKey,
-            Key calibKey)
+            Key calibKey,
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model)
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
             landmark_(landmark) {
@@ -290,21 +292,21 @@ namespace gtsam {
         }
 
         /**
-         * @param pose estimated pose2
-         * @param offset pose3 offset from pose2 +x
+         * @param wTb "world to body": estimated pose2
+         * @param bTc "body to camera": pose3 offset from pose2 +x
          * @param calib calibration
-         * @param H1 pose jacobian
+         * @param HwTb pose jacobian
          * @param H2 offset jacobian
          * @param H3 calibration jacobian
          */
         Vector evaluateError(
-            const Pose2& pose,
-            const Pose3& offset,
+            const Pose2& wTb,
+            const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType H1 = OptionalNone,
-            OptionalMatrixType H2 = OptionalNone,
-            OptionalMatrixType H3 = OptionalNone) const override {
-            return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_;
+            OptionalMatrixType HwTb = OptionalNone,
+            OptionalMatrixType HbTc = OptionalNone,
+            OptionalMatrixType Hcalib = OptionalNone) const override {
+            return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
         }
 
     private:
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index e141cb50c..1761a6cc3 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -27,32 +27,32 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 #include <gtsam/slam/PlanarProjectionFactor.h>
 virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor {
   PlanarProjectionFactor1(
-            const gtsam::Point3& landmark,
-            const gtsam::Point2& measured,
-            const gtsam::Pose3& offset,
-            const gtsam::Cal3DS2& calib,
-            const gtsam::noiseModel::Base* model,
-            size_t poseKey);
+    size_t poseKey,
+    const gtsam::Point3& landmark,
+    const gtsam::Point2& measured,
+    const gtsam::Pose3& bTc,
+    const gtsam::Cal3DS2& calib,
+    const gtsam::noiseModel::Base* model);
   void serialize() const;
 };
 virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor {
   PlanarProjectionFactor2(
-            const gtsam::Point2& measured,
-            const gtsam::Pose3& offset,
-            const gtsam::Cal3DS2& calib,
-            const gtsam::noiseModel::Base* model,
-            size_t landmarkKey,
-            size_t poseKey);
+    size_t poseKey,
+    size_t landmarkKey,
+    const gtsam::Point2& measured,
+    const gtsam::Pose3& bTc,
+    const gtsam::Cal3DS2& calib,
+    const gtsam::noiseModel::Base* model);
   void serialize() const;
 };
 virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor {
   PlanarProjectionFactor3(
-            const gtsam::Point3& landmark,
-            const gtsam::Point2& measured,
-            const gtsam::noiseModel::Base* model,
-            size_t poseKey,
-            size_t offsetKey,
-            size_t calibKey);
+    size_t poseKey,
+    size_t offsetKey,
+    size_t calibKey,
+    const gtsam::Point3& landmark,
+    const gtsam::Point2& measured,
+    const gtsam::noiseModel::Base* model);
   void serialize() const;
 };
 
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index 03a0b7bed..67ebd5c25 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -39,7 +39,8 @@ TEST(PlanarProjectionFactor1, error1) {
     Pose2 pose(0, 0, 0);
     values.insert(X(0), pose);
 
-    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(
+        X(0), landmark, measured, offset, calib, model);
 
     CHECK_EQUAL(2, factor.dim());
     CHECK(factor.active(values));
@@ -65,7 +66,8 @@ TEST(PlanarProjectionFactor1, error2) {
     Pose3 offset;
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(
+        X(0), landmark, measured, offset, calib, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -96,7 +98,8 @@ TEST(PlanarProjectionFactor1, error3) {
     // distortion
     Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(
+        X(0), landmark, measured, offset, calib, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -131,7 +134,8 @@ TEST(PlanarProjectionFactor1, jacobian) {
         Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+        PlanarProjectionFactor1 factor(
+            X(0), landmark, measured, offset, calib, model);
 
         Pose2 pose(s(g), s(g), s(g));
 
@@ -164,7 +168,8 @@ TEST(PlanarProjectionFactor3, error1) {
     values.insert(C(0), offset);
     values.insert(K(0), calib);
 
-    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    PlanarProjectionFactor3 factor(
+        X(0), C(0), K(0), landmark, measured, model);
 
     CHECK_EQUAL(2, factor.dim());
     CHECK(factor.active(values));
@@ -213,7 +218,8 @@ TEST(PlanarProjectionFactor3, error2) {
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
 
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    PlanarProjectionFactor3 factor(
+        X(0), C(0), K(0), landmark, measured, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -267,7 +273,8 @@ TEST(PlanarProjectionFactor3, error3) {
     Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    PlanarProjectionFactor3 factor(
+        X(0), C(0), K(0), landmark, measured, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -328,7 +335,8 @@ TEST(PlanarProjectionFactor3, jacobian) {
         Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+        PlanarProjectionFactor3 factor(
+            X(0), C(0), K(0), landmark, measured, model);
 
         Pose2 pose(s(g), s(g), s(g));
 

From 3690937159aa79de1570f4cac8db0ac53f8235c0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 18 Dec 2024 08:11:42 -0500
Subject: [PATCH 162/362] add comments

---
 gtsam/discrete/TableFactor.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index d9bfc42c2..22548da07 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -252,6 +252,11 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
+  // Record key assignment and value pairs in pair_table.
+  // The assignments are stored in descending order of keys so that the order of
+  // the values matches what is expected by a DecisionTree.
+  // This is why we reverse the keys and then
+  // query for the key value/assignment.
   DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend());
   std::vector<std::pair<uint64_t, double>> pair_table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
@@ -265,13 +270,16 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
     pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
 
-  // Sort based on key assignment so we get values in reverse key order.
+  // Sort the pair_table (of assignment-value pairs) based on assignment so we
+  // get values in reverse key order.
   std::sort(
       pair_table.begin(), pair_table.end(),
       [](const std::pair<uint64_t, double>& a,
          const std::pair<uint64_t, double>& b) { return a.first < b.first; });
 
-  // Create the table vector
+  // Create the table vector by extracting the values from pair_table.
+  // The pair_table has already been sorted in the desired order,
+  // so the values will be in descending key order.
   std::vector<double> table;
   std::for_each(pair_table.begin(), pair_table.end(),
                 [&table](const std::pair<uint64_t, double>& pair) {

From 4437baf01327e3f643893ba1e92c8586117f8800 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 18 Dec 2024 12:30:33 -0500
Subject: [PATCH 163/362] expose GTSAM_ENABLE_TIMING

---
 cmake/GtsamBuildTypes.cmake          | 6 ++++++
 cmake/HandleGeneralOptions.cmake     | 1 +
 cmake/HandlePrintConfiguration.cmake | 1 +
 3 files changed, 8 insertions(+)

diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 2aad58abb..ab6d149fe 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -178,6 +178,12 @@ foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES})
   append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC   ${build_type})
 endforeach()
 
+# Check if timing is enabled and add appropriate definition flag
+if(GTSAM_ENABLE_TIMING AND(NOT ${CMAKE_BUILD_TYPE} EQUAL "Timing"))
+  message(STATUS "Enabling timing for non-timing build")
+  list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE "ENABLE_TIMING")
+endif()
+
 # Linker flags:
 set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING  "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
 set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING  "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
index 8c56ae242..0266cf3f0 100644
--- a/cmake/HandleGeneralOptions.cmake
+++ b/cmake/HandleGeneralOptions.cmake
@@ -32,6 +32,7 @@ option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Qu
 option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
 option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
 option(GTSAM_DT_MERGING                     "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON)
+option(GTSAM_ENABLE_TIMING                  "Enable the timing tools (gttic/gttoc)" OFF)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS      "Enable/Disable expensive consistency checks" OFF)
 option(GTSAM_ENABLE_MEMORY_SANITIZER        "Enable/Disable memory sanitizer" OFF)
 option(GTSAM_WITH_TBB                       "Use Intel Threaded Building Blocks (TBB) if available" ON)
diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake
index 42fae90f7..ac68be20f 100644
--- a/cmake/HandlePrintConfiguration.cmake
+++ b/cmake/HandlePrintConfiguration.cmake
@@ -91,6 +91,7 @@ print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER}     "Build with Memory San
 print_enabled_config(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
 print_enabled_config(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap    ")
 print_enabled_config(${GTSAM_DT_MERGING}                  "Enable branch merging in DecisionTree")
+print_enabled_config(${GTSAM_ENABLE_TIMING}               "Enable timing machinery")
 print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43}  "Allow features deprecated in GTSAM 4.3")
 print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")

From 8a30aacf6ca997df0073d633cb3eaa4ff0a5510b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 21 Dec 2024 13:10:29 -0500
Subject: [PATCH 164/362] fix serialization of ImuFactor

---
 python/gtsam/tests/test_Serialization.py | 26 ++++++++++++++++--------
 1 file changed, 18 insertions(+), 8 deletions(-)

diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py
index e935f1f62..4f5c57b0a 100644
--- a/python/gtsam/tests/test_Serialization.py
+++ b/python/gtsam/tests/test_Serialization.py
@@ -23,26 +23,36 @@ class TestDeepCopy(GtsamTestCase):
 
     def test_PreintegratedImuMeasurements(self):
         """
-        Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy.
+        Test the deep copy of `PreintegratedImuMeasurements`.
         """
-        params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81]))
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81)
         pim = gtsam.PreintegratedImuMeasurements(params)
 
         self.assertDeepCopyEquality(pim)
 
     def test_ImuFactor(self):
         """
-        Test the deep copy of `ImuFactor` by performing a deepcopy.
+        Test the deep copy of `ImuFactor`.
         """
-        params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81]))
-        pim = gtsam.PreintegratedImuMeasurements(params)
-        imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81)
+        params.setAccelerometerCovariance(1e-7 * np.eye(3))
+        params.setGyroscopeCovariance(1e-8 * np.eye(3))
+        params.setIntegrationCovariance(1e-9 * np.eye(3))
+        priorBias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3))
+        pim = gtsam.PreintegratedImuMeasurements(params, priorBias)
 
-        self.assertDeepCopyEquality(imu_factor)
+        # Preintegrate a single measurement for serialization to work.
+        pim.integrateMeasurement(measuredAcc=np.zeros(3),
+                                 measuredOmega=np.zeros(3),
+                                 deltaT=0.005)
+
+        factor = gtsam.ImuFactor(0, 1, 2, 3, 4, pim)
+
+        self.assertDeepCopyEquality(factor)
 
     def test_PreintegratedCombinedMeasurements(self):
         """
-        Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy.
+        Test the deep copy of `PreintegratedCombinedMeasurements`.
         """
         params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81]))
         pim = gtsam.PreintegratedCombinedMeasurements(params)

From 60506e2440e825fb7f41ef2331c1993df512ccd3 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 22 Dec 2024 09:25:06 -0800
Subject: [PATCH 165/362] [cephes] use GTSAM_LIBRARY_TYPE

---
 gtsam/3rdparty/cephes/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt
index 190b73db9..aeac7b403 100644
--- a/gtsam/3rdparty/cephes/CMakeLists.txt
+++ b/gtsam/3rdparty/cephes/CMakeLists.txt
@@ -89,7 +89,7 @@ set(CEPHES_SOURCES
     cephes/zetac.c)
 
 # Add library source files
-add_library(cephes-gtsam SHARED ${CEPHES_SOURCES})
+add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES})
 
 # Add include directory (aka headers)
 target_include_directories(

From 3fb1b214524e22c000df0ed7155890c6f1c07a76 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 22 Dec 2024 23:48:10 -0500
Subject: [PATCH 166/362] remove add_definitions since it is deprecated and
 suboptimal

---
 CMakeLists.txt | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e3b462eec..90162fe29 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,14 +71,7 @@ include(GtsamPrinting)
 ############### Decide on BOOST ######################################
 # Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION
 option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON)
-if(GTSAM_ENABLE_BOOST_SERIALIZATION)
-  add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION)
-endif()
-
 option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON)
-if(GTSAM_USE_BOOST_FEATURES)
-  add_definitions(-DGTSAM_USE_BOOST_FEATURES)
-endif()
 
 if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES)
 include(cmake/HandleBoost.cmake)

From 51f3c6dea3d76896897bdf1ca50d8ca97e322b65 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 22 Dec 2024 23:48:50 -0500
Subject: [PATCH 167/362] Optimize build time by adding Boost definitions in
 config.h

---
 gtsam/base/FastSet.h                  | 2 ++
 gtsam/base/serialization.h            | 5 ++++-
 gtsam/base/serializationTestHelpers.h | 5 ++++-
 gtsam/config.h.in                     | 4 ++++
 4 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index a1be08d80..58ddd13a0 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -18,6 +18,8 @@
 
 #pragma once
 
+#include <gtsam/config.h>
+
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/version.hpp>
 #if BOOST_VERSION >= 107400
diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h
index 18612bc22..e829014ae 100644
--- a/gtsam/base/serialization.h
+++ b/gtsam/base/serialization.h
@@ -17,9 +17,12 @@
  * @date Feb 7, 2012
  */
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
+#include <gtsam/config.h>
+
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+
 #include <Eigen/Core>
 #include <fstream>
 #include <sstream>
diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
index 09b36cdc8..fb0701d89 100644
--- a/gtsam/base/serializationTestHelpers.h
+++ b/gtsam/base/serializationTestHelpers.h
@@ -17,9 +17,12 @@
  * @date Feb 7, 2012
  */
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
+#include <gtsam/config.h>
+
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+
 #include <iostream>
 #include <sstream>
 #include <string>
diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 719cbc6cb..8b4903d3a 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -88,3 +88,7 @@
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
 
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP
+
+// Boost flags
+#cmakedefine01 GTSAM_ENABLE_BOOST_SERIALIZATION
+#cmakedefine01 GTSAM_USE_BOOST_FEATURES

From 53cf49b1ba335a46ac97ec7aaff9c99a496cee33 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 23 Dec 2024 15:20:40 -0500
Subject: [PATCH 168/362] code to print timing as CSV

---
 gtsam/base/timing.cpp | 50 +++++++++++++++++++++++++++++++++++++++++++
 gtsam/base/timing.h   | 31 +++++++++++++++++++++++++++
 2 files changed, 81 insertions(+)

diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index b43595066..ecff40ebc 100644
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@@ -106,6 +106,56 @@ void TimingOutline::print(const std::string& outline) const {
 #endif
 }
 
+/* ************************************************************************* */
+void TimingOutline::print_csv_header(bool addLineBreak) const {
+#ifdef GTSAM_USE_BOOST_FEATURES
+  // Order is (CPU time, number of times, wall time, time + children in seconds,
+  // min time, max time)
+  std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << ","
+            << label_ + " wall time(s)" << "," << label_ + " subtree time (s)"
+            << "," << label_ + " min time (s)" << "," << label_ + "max time(s)"
+            << ", ";
+  // Order children
+  typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
+  ChildOrder childOrder;
+  for (const ChildMap::value_type& child : children_) {
+    childOrder[child.second->myOrder_] = child.second;
+  }
+  // Print children
+  for (const ChildOrder::value_type& order_child : childOrder) {
+    order_child.second->print_csv_header();
+  }
+  if (addLineBreak) {
+    std::cout << std::endl;
+  }
+  std::cout.flush();
+#endif
+}
+
+/* ************************************************************************* */
+void TimingOutline::print_csv(bool addLineBreak) const {
+#ifdef GTSAM_USE_BOOST_FEATURES
+  // Order is (CPU time, number of times, wall time, time + children in seconds,
+  // min time, max time)
+  std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","
+            << min() << "," << max() << ", ";
+  // Order children
+  typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
+  ChildOrder childOrder;
+  for (const ChildMap::value_type& child : children_) {
+    childOrder[child.second->myOrder_] = child.second;
+  }
+  // Print children
+  for (const ChildOrder::value_type& order_child : childOrder) {
+    order_child.second->print_csv(false);
+  }
+  if (addLineBreak) {
+    std::cout << std::endl;
+  }
+  std::cout.flush();
+#endif
+}
+
 void TimingOutline::print2(const std::string& outline,
     const double parentTotal) const {
 #ifdef GTSAM_USE_BOOST_FEATURES
diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h
index 99c55a3d7..dfc2928f1 100644
--- a/gtsam/base/timing.h
+++ b/gtsam/base/timing.h
@@ -199,6 +199,29 @@ namespace gtsam {
 #endif
       GTSAM_EXPORT void print(const std::string& outline = "") const;
       GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
+
+      /**
+       * @brief Print the CSV header.
+       * Order is
+       * (CPU time, number of times, wall time, time + children in seconds, min
+       * time, max time)
+       *
+       * @param addLineBreak Flag indicating if a line break should be added at
+       * the end. Only used at the top-leve.
+       */
+      GTSAM_EXPORT void print_csv_header(bool addLineBreak = false) const;
+
+      /**
+       * @brief Print the times recursively from parent to child in CSV format.
+       * For each timing node, the output is
+       * (CPU time, number of times, wall time, time + children in seconds, min
+       * time, max time)
+       *
+       * @param addLineBreak Flag indicating if a line break should be added at
+       * the end. Only used at the top-leve.
+       */
+      GTSAM_EXPORT void print_csv(bool addLineBreak = false) const;
+
       GTSAM_EXPORT const std::shared_ptr<TimingOutline>&
         child(size_t child, const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr);
       GTSAM_EXPORT void tic();
@@ -268,6 +291,14 @@ inline void tictoc_finishedIteration_() {
 inline void tictoc_print_() {
   ::gtsam::internal::gTimingRoot->print(); }
 
+// print timing in CSV format
+inline void tictoc_print_csv_(bool displayHeader = false) {
+  if (displayHeader) {
+    ::gtsam::internal::gTimingRoot->print_csv_header(true);
+  }
+  ::gtsam::internal::gTimingRoot->print_csv(true);
+}
+
 // print mean and standard deviation
 inline void tictoc_print2_() {
   ::gtsam::internal::gTimingRoot->print2(); }

From 427d54dce8d03c89e1f40a26bd9dad346eb4a5b6 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 24 Dec 2024 00:30:15 +0000
Subject: [PATCH 169/362] Bump jinja2 from 3.1.4 to 3.1.5 in
 /wrap/pybind11/docs

Bumps [jinja2](https://github.com/pallets/jinja) from 3.1.4 to 3.1.5.
- [Release notes](https://github.com/pallets/jinja/releases)
- [Changelog](https://github.com/pallets/jinja/blob/main/CHANGES.rst)
- [Commits](https://github.com/pallets/jinja/compare/3.1.4...3.1.5)

---
updated-dependencies:
- dependency-name: jinja2
  dependency-type: indirect
...

Signed-off-by: dependabot[bot] <support@github.com>
---
 wrap/pybind11/docs/requirements.txt | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/wrap/pybind11/docs/requirements.txt b/wrap/pybind11/docs/requirements.txt
index 293db6a06..8dffd36b5 100644
--- a/wrap/pybind11/docs/requirements.txt
+++ b/wrap/pybind11/docs/requirements.txt
@@ -130,9 +130,9 @@ imagesize==1.4.1 \
     --hash=sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b \
     --hash=sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a
     # via sphinx
-jinja2==3.1.4 \
-    --hash=sha256:4a3aee7acbbe7303aede8e9648d13b8bf88a429282aa6122a993f0ac800cb369 \
-    --hash=sha256:bc5dd2abb727a5319567b7a813e6a2e7318c39f4f487cfe6c89c6f9c7d25197d
+jinja2==3.1.5 \
+    --hash=sha256:8fefff8dc3034e27bb80d67c671eb8a9bc424c0ef4c0826edbff304cceff43bb \
+    --hash=sha256:aba0f4dc9ed8013c424088f68a5c226f7d6097ed89b246d7749c2ec4175c6adb
     # via sphinx
 markupsafe==2.1.5 \
     --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \

From 05e01b1b53909f9ca6e91b0aaa9d313e50c10a97 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 24 Dec 2024 12:02:43 -0500
Subject: [PATCH 170/362] remove extra space after comma

---
 gtsam/base/timing.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index ecff40ebc..83556cbd2 100644
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@@ -114,7 +114,7 @@ void TimingOutline::print_csv_header(bool addLineBreak) const {
   std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << ","
             << label_ + " wall time(s)" << "," << label_ + " subtree time (s)"
             << "," << label_ + " min time (s)" << "," << label_ + "max time(s)"
-            << ", ";
+            << ",";
   // Order children
   typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
   ChildOrder childOrder;
@@ -138,7 +138,7 @@ void TimingOutline::print_csv(bool addLineBreak) const {
   // Order is (CPU time, number of times, wall time, time + children in seconds,
   // min time, max time)
   std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","
-            << min() << "," << max() << ", ";
+            << min() << "," << max() << ",";
   // Order children
   typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
   ChildOrder childOrder;

From 0f8fe15e3118fd235ecddd10756a72410a0603bf Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Tue, 24 Dec 2024 09:06:25 -0800
Subject: [PATCH 171/362] remove "cam coord" idea

---
 gtsam/geometry/Pose3.cpp                      |   9 +-
 gtsam/geometry/Pose3.h                        |   3 +-
 gtsam/slam/PlanarProjectionFactor.h           | 114 ++--
 .../slam/tests/testPlanarProjectionFactor.cpp | 535 ++++++++++--------
 4 files changed, 355 insertions(+), 306 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 5d7732947..03464b0db 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -42,14 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
   return Pose3(R, t);
 }
 
-Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
-  if (H) *H << (gtsam::Matrix(6, 3) << //
+// Pose2 constructor Jacobian is always the same.
+static const Matrix63 Hpose2 = (Matrix63() << //
     0., 0., 0., //
     0., 0., 0.,//
     0., 0., 1.,//
     1., 0., 0.,//
     0., 1., 0.,//
     0., 0., 0.).finished();
+
+Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
+  if (H) *H << Hpose2;
   return Pose3(p);
 }
 
@@ -520,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   return os;
 }
 
-} // namespace gtsam
+} // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index baac21ccb..ba00c1863 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -78,6 +78,7 @@ public:
                       OptionalJacobian<6, 3> HR = {},
                       OptionalJacobian<6, 3> Ht = {});
 
+  /** Construct from Pose2 in the xy plane, with derivative. */
   static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});
 
   /**
@@ -460,4 +461,4 @@ struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
 template <typename T>
 struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
 
-}  // namespace gtsam
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
index 21f24da93..22cea2214 100644
--- a/gtsam/slam/PlanarProjectionFactor.h
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -59,31 +59,26 @@ namespace gtsam {
             const Pose2& wTb,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z)
-            OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta)
-            OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta)
-            OptionalMatrixType Hcalib = nullptr // 2x9
+            OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z)
+            OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta)
+            OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta)
+            OptionalJacobian<2, 9> Hcalib = {}
         ) const {
 #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             try {
 #endif
-                gtsam::Matrix Hp; // 6x3
-                gtsam::Matrix H0; // 6x6
-                // this is x-forward z-up
-                Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr);
-                // this is z-forward y-down
-                gtsam::Matrix H00; // 6x6
-                Pose3 camera_pose = wTc.compose(CAM_COORD, H00);
-                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
+                Matrix63 Hp; // 6x3
+                Matrix66 H0; // 6x6
+                Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr);
+                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(wTc, calib);
                 if (HwTb || HbTc) {
-                    // Dpose is for pose3, 2x6 (R,t)
-                    gtsam::Matrix Dpose;
+                    // Dpose is for pose3 (R,t)
+                    Matrix26 Dpose;
                     Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
-                    gtsam::Matrix DposeOffset = Dpose * H00; // 2x6
                     if (HbTc)
-                        *HbTc = DposeOffset; // with Eigen this is a deep copy (!)
-                    if (HwTb) 
-                        *HwTb =  DposeOffset * H0 * Hp;
+                        *HbTc = Dpose;
+                    if (HwTb)
+                        *HwTb = Dpose * H0 * Hp;
                     return result;
                 } else {
                     return camera.project(landmark, {}, {}, {});
@@ -91,10 +86,10 @@ namespace gtsam {
 #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             } catch (CheiralityException& e) {
                 std::cout << "****** CHIRALITY EXCEPTION ******\n";
-                if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3);
-                if (HwTb) *HwTb = Matrix::Zero(2, 3);
-                if (HbTc) *HbTc = Matrix::Zero(2, 6);
-                if (Hcalib) *Hcalib = Matrix::Zero(2, 9);
+                if (Hlandmark) Hlandmark->setZero();
+                if (HwTb) HwTb->setZero();
+                if (HbTc) HbTc->setZero();
+                if (Hcalib) Hcalib->setZero();
                 // return a large error
                 return Matrix::Constant(2, 1, 2.0 * calib.fx());
             }
@@ -102,18 +97,8 @@ namespace gtsam {
         }
 
         Point2 measured_; // pixel measurement
-
-    private:
-        static const Pose3 CAM_COORD;
     };
 
-    // camera "zero" is facing +z; this turns it to face +x
-    const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3(
-        Rot3(0, 0, 1,//
-            -1, 0, 0, //
-            0, -1, 0),
-        Vector3(0, 0, 0)
-    );
 
     /**
      * @class PlanarProjectionFactor1
@@ -131,9 +116,9 @@ namespace gtsam {
         ~PlanarProjectionFactor1() override {}
 
         /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
         }
 
 
@@ -152,22 +137,18 @@ namespace gtsam {
             const Point2& measured,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model)
+            const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey),
             landmark_(landmark),
             bTc_(bTc),
-            calib_(calib) {
-            assert(2 == model->dim());
-        }
+            calib_(calib) {}
 
         /**
          * @param wTb "world to body": estimated pose2
          * @param HwTb jacobian
          */
-        Vector evaluateError(
-            const Pose2& wTb,
-            OptionalMatrixType HwTb = OptionalNone) const override {
+        Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override {
             return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
         }
 
@@ -187,8 +168,8 @@ namespace gtsam {
      * Camera offset and calibration are constant.
      * This is similar to GeneralSFMFactor, used for SLAM.
     */
-    class PlanarProjectionFactor2 
-    : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
+    class PlanarProjectionFactor2
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
     public:
         typedef NoiseModelFactorN<Pose2, Point3> Base;
         using Base::evaluateError;
@@ -198,9 +179,9 @@ namespace gtsam {
         ~PlanarProjectionFactor2() override {}
 
         /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
         }
 
         /**
@@ -218,13 +199,11 @@ namespace gtsam {
             const Point2& measured,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model)
+            const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, landmarkKey, poseKey),
             bTc_(bTc),
-            calib_(calib) {
-            assert(2 == model->dim());
-        }
+            calib_(calib) {}
 
         /**
          * @param wTb "world to body": estimated pose2
@@ -235,8 +214,8 @@ namespace gtsam {
         Vector evaluateError(
             const Pose2& wTb,
             const Point3& landmark,
-            OptionalMatrixType HwTb = OptionalNone,
-            OptionalMatrixType Hlandmark = OptionalNone) const override {
+            OptionalMatrixType HwTb,
+            OptionalMatrixType Hlandmark) const override {
             return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
         }
 
@@ -265,47 +244,46 @@ namespace gtsam {
         ~PlanarProjectionFactor3() override {}
 
         /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
         }
 
         /**
          * @brief constructor for variable pose, offset, and calibration, known landmark.
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param offsetKey index of camera offset from pose
+         * @param calibKey index of camera calibration
          * @param landmark point3 in the world
          * @param measured corresponding point2 in the camera frame
          * @param model stddev of the measurements, ~one pixel?
-         * @param poseKey index of the robot pose2 in the z=0 plane
-         * @param offsetKey index of camera offset from pose
-         * @param calibKey index of camera calibration         */
+         */
         PlanarProjectionFactor3(
             Key poseKey,
             Key offsetKey,
             Key calibKey,
             const Point3& landmark,
             const Point2& measured,
-            const SharedNoiseModel& model)
+            const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
-            landmark_(landmark) {
-            assert(2 == model->dim());
-        }
+            landmark_(landmark) {}
 
         /**
          * @param wTb "world to body": estimated pose2
          * @param bTc "body to camera": pose3 offset from pose2 +x
          * @param calib calibration
          * @param HwTb pose jacobian
-         * @param H2 offset jacobian
-         * @param H3 calibration jacobian
+         * @param HbTc offset jacobian
+         * @param Hcalib calibration jacobian
          */
         Vector evaluateError(
             const Pose2& wTb,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType HwTb = OptionalNone,
-            OptionalMatrixType HbTc = OptionalNone,
-            OptionalMatrixType Hcalib = OptionalNone) const override {
+            OptionalMatrixType HwTb,
+            OptionalMatrixType HbTc,
+            OptionalMatrixType Hcalib) const override {
             return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
         }
 
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index 67ebd5c25..1cc002797 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -16,6 +16,10 @@
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PlanarProjectionFactor.h>
 
@@ -26,124 +30,101 @@ using namespace gtsam;
 using symbol_shorthand::X;
 using symbol_shorthand::C;
 using symbol_shorthand::K;
+using symbol_shorthand::L;
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, error1) {
-    // landmark is on the camera bore (which faces +x)
+    // Example: center projection and Jacobian
     Point3 landmark(1, 0, 0);
-    // so pixel measurement is (cx, cy)
     Point2 measured(200, 200);
-    Pose3 offset;
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    Values values;
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
     Pose2 pose(0, 0, 0);
-    values.insert(X(0), pose);
-
-    PlanarProjectionFactor1 factor(
-        X(0), landmark, measured, offset, calib, model);
-
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(1);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    const Matrix23 H1Expected = (Matrix23() << //
-        0, 200, 200,//
-        0, 0, 0).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+    Matrix H;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
+    CHECK(assert_equal((Matrix23() << //
+        0, 200, 200, //
+        0, 0, 0).finished(), H, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, error2) {
-    // landmark is in the upper left corner
+    // Example: upper left corner projection and Jacobian
     Point3 landmark(1, 1, 1);
-    // upper left corner in pixels
     Point2 measured(0, 0);
-    Pose3 offset;
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(
-        X(0), landmark, measured, offset, calib, model);
-    Values values;
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
     Pose2 pose(0, 0, 0);
-
-    values.insert(X(0), pose);
-
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(1);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    Matrix23 H1Expected = (Matrix23() << //
+    Matrix H;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
+    CHECK(assert_equal((Matrix23() << //
         -200, 200, 400, //
-        -200, 0, 200).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+        -200, 0, 200).finished(), H, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, error3) {
-    // landmark is in the upper left corner
+    // Example: upper left corner projection and Jacobian with distortion
     Point3 landmark(1, 1, 1);
-    // upper left corner in pixels
     Point2 measured(0, 0);
-    Pose3 offset;
-    // distortion
-    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(
-        X(0), landmark, measured, offset, calib, model);
-    Values values;
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
     Pose2 pose(0, 0, 0);
-
-    values.insert(X(0), pose);
-
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(1);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    Matrix23 H1Expected = (Matrix23() << //
+    Matrix H;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
+    CHECK(assert_equal((Matrix23() << //
         -360, 280, 640, //
-        -360, 80, 440).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+        -360, 80, 440).finished(), H, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, jacobian) {
-    // test many jacobians with many randoms
-
-    std::default_random_engine g;
-    std::uniform_real_distribution<double> s(-0.3, 0.3);
+    // Verify Jacobians with numeric derivative
+    std::default_random_engine rng(42);
+    std::uniform_real_distribution<double> dist(-0.3, 0.3);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // center of the random camera poses
+    Pose3 centerOffset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
 
     for (int i = 0; i < 1000; ++i) {
-        Point3 landmark(2 + s(g), s(g), s(g));
-        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
-        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
+        Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
+        Pose3 offset = centerOffset.compose(
+            Pose3(
+                Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
+                Point3(dist(rng), dist(rng), dist(rng))));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-
-        PlanarProjectionFactor1 factor(
-            X(0), landmark, measured, offset, calib, model);
-
-        Pose2 pose(s(g), s(g), s(g));
-
-        // actual H
+        PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
+        Pose2 pose(dist(rng), dist(rng), dist(rng));
         Matrix H1;
         factor.evaluateError(pose, H1);
-
-        Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
+        auto expectedH1 = numericalDerivative11<Vector, Pose2>(
             [&factor](const Pose2& p) {
                 return factor.evaluateError(p, {});},
                 pose);
@@ -151,194 +132,182 @@ TEST(PlanarProjectionFactor1, jacobian) {
     }
 }
 
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, solve) {
+    // Example localization
 
+    SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // pose model is wide, so the solver finds the right answer.
+    SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10));
 
-TEST(PlanarProjectionFactor3, error1) {
-    // landmark is on the camera bore (facing +x)
-    Point3 landmark(1, 0, 0);
-    // so px is (cx, cy)
-    Point2 measured(200, 200);
-    // offset is identity
-    Pose3 offset;
+    // landmarks
+    Point3 l0(1, 0.1, 1);
+    Point3 l1(1, -0.1, 1);
+
+    // camera pixels
+    Point2 p0(180, 0);
+    Point2 p1(220, 0);
+
+    // body
+    Pose2 x0(0, 0, 0);
+
+    // camera z looking at +x with (xy) antiparallel to (yz)
+    Pose3 c0(
+        Rot3(0, 0, 1, //
+            -1, 0, 0, //
+            0, -1, 0), //
+        Vector3(0, 0, 0));
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    Values values;
-    Pose2 pose(0, 0, 0);
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
 
-    PlanarProjectionFactor3 factor(
-        X(0), C(0), K(0), landmark, measured, model);
+    NonlinearFactorGraph graph;
+    graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel));
+    graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel));
+    graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
 
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(3);
+    Values initialEstimate;
+    initialEstimate.insert(X(0), x0);
 
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-    CHECK(assert_equal(Vector2(0, 0), actual));
+    // run the optimizer
+    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
 
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
+    // verify that the optimizer found the right pose.
+    CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
 
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
+    // covariance
+    Marginals marginals(graph, result);
+    Matrix cov = marginals.marginalCovariance(X(0));
+    CHECK(assert_equal((Matrix33() << //
+        0.000012, 0.000000, 0.000000, //
+        0.000000, 0.001287, -.001262, //
+        0.000000, -.001262, 0.001250).finished(), cov, 3e-6));
 
-    // du/dx etc for the pose2d
-    Matrix23 H1Expected = (Matrix23() <<//
-        0, 200, 200,//
-        0, 0, 0).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+    // pose stddev
+    Vector3 sigma = cov.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector3() << //
+        0.0035,
+        0.0359,
+        0.0354
+        ).finished(), sigma, 1e-4));
 
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        0, 0, 200, 0, 200, 0,//
-        0, -200, 0, 0, 0, 200).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    // du wrt calibration
-    // on-bore, f doesn't matter
-    // but c does
-    Matrix29 H3Expected = (Matrix29() <<//
-        0, 0, 0, 1, 0, 0, 0, 0, 0,//
-        0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
 }
 
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, error1) {
+    // Example: center projection and Jacobian
+    Point3 landmark(1, 0, 0);
+    Point2 measured(200, 200);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
+    Pose2 pose(0, 0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
+    CHECK(assert_equal((Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        0, -200, 0, -200, 0, 0,//
+        200, -0, 0, 0, -200, 0).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
+        0, 0, 0, 1, 0, 0, 0, 0, 0,//
+        0, 0, 0, 0, 1, 0, 0, 0, 0).finished(), H3, 1e-6));
+}
+
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor3, error2) {
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(
-        X(0), C(0), K(0), landmark, measured, model);
-    Values values;
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
     Pose2 pose(0, 0, 0);
-
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
-
-
-    CHECK_EQUAL(2, model->dim());
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(3);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3);
     CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    Matrix23 H1Expected = (Matrix23() <<//
+    CHECK(assert_equal((Matrix23() <<//
         -200, 200, 400,//
-        -200, 0, 200).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
-
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        -200, -200, 400, -200, 200, 0,//
-        200, -400, 200, -200, 0, 200).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    Matrix29 H3Expected = (Matrix29() <<//
+        -200, 0, 200).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        200, -400, -200, -200, 0, -200,//
+        400, -200, 200, 0, -200, -200).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
         -1, 0, -1, 1, 0, -400, -800, 400, 800,//
-        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
-
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor3, error3) {
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(
-        X(0), C(0), K(0), landmark, measured, model);
-    Values values;
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
     Pose2 pose(0, 0, 0);
-
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
-
-
-    CHECK_EQUAL(2, model->dim());
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(3);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    Matrix23 H1Expected = (Matrix23() <<//
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
+    CHECK(assert_equal((Matrix23() <<//
         -360, 280, 640,//
-        -360, 80, 440).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
-
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        -200, -440, 640, -360, 280, 80,//
-        200, -640, 440, -360, 80, 280).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    Matrix29 H3Expected = (Matrix29() <<//
+        -360, 80, 440).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        440, -640, -200, -280, -80, -360,//
+        640, -440, 200, -80, -280, -360).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
         -1, 0, -1, 1, 0, -400, -800, 400, 800,//
-        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
-
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
 }
 
-
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor3, jacobian) {
-    // test many jacobians with many randoms
+    // Verify Jacobians with numeric derivative
 
-    std::default_random_engine g;
-    std::uniform_real_distribution<double> s(-0.3, 0.3);    
+    std::default_random_engine rng(42);
+    std::uniform_real_distribution<double> dist(-0.3, 0.3);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // center of the random camera poses
+    Pose3 centerOffset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
 
     for (int i = 0; i < 1000; ++i) {
-        Point3 landmark(2 + s(g), s(g), s(g));
-        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
-        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
+        Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
+        Pose3 offset = centerOffset.compose(
+            Pose3(
+                Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
+                Point3(dist(rng), dist(rng), dist(rng))));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor3 factor(
-            X(0), C(0), K(0), landmark, measured, model);
+        PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
 
-        Pose2 pose(s(g), s(g), s(g));
+        Pose2 pose(dist(rng), dist(rng), dist(rng));
 
         // actual H
         Matrix H1, H2, H3;
@@ -362,6 +331,104 @@ TEST(PlanarProjectionFactor3, jacobian) {
     }
 }
 
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, solveOffset) {
+    // Example localization
+    SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
+    // offset model is wide, so the solver finds the right answer.
+    SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10));
+    SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001));
+
+    // landmarks
+    Point3 l0(1, 0, 1);
+    Point3 l1(1, 0, 0);
+    Point3 l2(1, -1, 1);
+    Point3 l3(2, 2, 1);
+
+    // camera pixels
+    Point2 p0(200, 200);
+    Point2 p1(200, 400);
+    Point2 p2(400, 200);
+    Point2 p3(0, 200);
+
+    // body
+    Pose2 x0(0, 0, 0);
+
+    // camera z looking at +x with (xy) antiparallel to (yz)
+    Pose3 c0(
+        Rot3(0, 0, 1, //
+            -1, 0, 0, //
+            0, -1, 0), //
+        Vector3(0, 0, 1)); // note z offset
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    NonlinearFactorGraph graph;
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel));
+    graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
+    graph.add(PriorFactor<Pose3>(C(0), c0, cNoise));
+    graph.add(PriorFactor<Cal3DS2>(K(0), calib, kNoise));
+
+    Values initialEstimate;
+    initialEstimate.insert(X(0), x0);
+    initialEstimate.insert(C(0), c0);
+    initialEstimate.insert(K(0), calib);
+
+    // run the optimizer
+    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
+
+    // verify that the optimizer found the right pose.
+    CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
+
+    // verify the camera is pointing at +x
+    Pose3 cc0 = result.at<Pose3>(C(0));
+    CHECK(assert_equal(c0, cc0, 5e-3));
+
+    // verify the calibration
+    CHECK(assert_equal(calib, result.at<Cal3DS2>(K(0)), 2e-3));
+
+    Marginals marginals(graph, result);
+    Matrix x0cov = marginals.marginalCovariance(X(0));
+
+    // narrow prior => ~zero cov
+    CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4));
+
+    Matrix c0cov = marginals.marginalCovariance(C(0));
+
+    // invert the camera offset to get covariance in body coordinates
+    Matrix66 HcTb = cc0.inverse().AdjointMap().inverse();
+    Matrix c0cov2 = HcTb * c0cov * HcTb.transpose();
+
+    // camera-frame stddev
+    Vector6 c0sigma = c0cov.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector6() << //
+        0.009,
+        0.011,
+        0.004,
+        0.012,
+        0.012,
+        0.011
+        ).finished(), c0sigma, 1e-3));
+
+    // body frame stddev
+    Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector6() << //
+        0.004,
+        0.009,
+        0.011,
+        0.012,
+        0.012,
+        0.012
+        ).finished(), bTcSigma, 1e-3));
+
+    // narrow prior => ~zero cov
+    CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3));
+}
+
 /* ************************************************************************* */
 int main() {
     TestResult tr;

From 69765496c5935edaa71791ac2a69b41a407b2acb Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Tue, 24 Dec 2024 14:27:11 -0800
Subject: [PATCH 172/362] fix windows test

---
 gtsam/slam/tests/testPlanarProjectionFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index 1cc002797..603eb555a 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -128,7 +128,7 @@ TEST(PlanarProjectionFactor1, jacobian) {
             [&factor](const Pose2& p) {
                 return factor.evaluateError(p, {});},
                 pose);
-        CHECK(assert_equal(expectedH1, H1, 1e-6));
+        CHECK(assert_equal(expectedH1, H1, 2e-6));
     }
 }
 

From 2827a1e04167707c74f0e51816d1e658b6e8eb58 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Tue, 24 Dec 2024 14:30:41 -0800
Subject: [PATCH 173/362] fix test names

---
 .../slam/tests/testPlanarProjectionFactor.cpp | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index 603eb555a..acfa95618 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -33,7 +33,7 @@ using symbol_shorthand::K;
 using symbol_shorthand::L;
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor1, error1) {
+TEST(PlanarProjectionFactor1, Error1) {
     // Example: center projection and Jacobian
     Point3 landmark(1, 0, 0);
     Point2 measured(200, 200);
@@ -55,7 +55,7 @@ TEST(PlanarProjectionFactor1, error1) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor1, error2) {
+TEST(PlanarProjectionFactor1, Error2) {
     // Example: upper left corner projection and Jacobian
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
@@ -77,7 +77,7 @@ TEST(PlanarProjectionFactor1, error2) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor1, error3) {
+TEST(PlanarProjectionFactor1, Error3) {
     // Example: upper left corner projection and Jacobian with distortion
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
@@ -99,7 +99,7 @@ TEST(PlanarProjectionFactor1, error3) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor1, jacobian) {
+TEST(PlanarProjectionFactor1, Jacobian) {
     // Verify Jacobians with numeric derivative
     std::default_random_engine rng(42);
     std::uniform_real_distribution<double> dist(-0.3, 0.3);
@@ -133,7 +133,7 @@ TEST(PlanarProjectionFactor1, jacobian) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor1, solve) {
+TEST(PlanarProjectionFactor1, Solve) {
     // Example localization
 
     SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
@@ -193,7 +193,7 @@ TEST(PlanarProjectionFactor1, solve) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor3, error1) {
+TEST(PlanarProjectionFactor3, Error1) {
     // Example: center projection and Jacobian
     Point3 landmark(1, 0, 0);
     Point2 measured(200, 200);
@@ -223,7 +223,7 @@ TEST(PlanarProjectionFactor3, error1) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor3, error2) {
+TEST(PlanarProjectionFactor3, Error2) {
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
@@ -253,7 +253,7 @@ TEST(PlanarProjectionFactor3, error2) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor3, error3) {
+TEST(PlanarProjectionFactor3, Error3) {
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
@@ -282,7 +282,7 @@ TEST(PlanarProjectionFactor3, error3) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor3, jacobian) {
+TEST(PlanarProjectionFactor3, Jacobian) {
     // Verify Jacobians with numeric derivative
 
     std::default_random_engine rng(42);
@@ -332,7 +332,7 @@ TEST(PlanarProjectionFactor3, jacobian) {
 }
 
 /* ************************************************************************* */
-TEST(PlanarProjectionFactor3, solveOffset) {
+TEST(PlanarProjectionFactor3, SolveOffset) {
     // Example localization
     SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
     SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));

From 1203f0c7024a197207c39f0995c61eaa860c06d5 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Tue, 24 Dec 2024 14:32:44 -0800
Subject: [PATCH 174/362] add newlines

---
 gtsam/geometry/Pose3.cpp            | 2 +-
 gtsam/geometry/Pose3.h              | 2 +-
 gtsam/slam/PlanarProjectionFactor.h | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 03464b0db..f3dacb163 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -523,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   return os;
 }
 
-} // namespace gtsam
\ No newline at end of file
+} // namespace gtsam
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index ba00c1863..edb6094fa 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -461,4 +461,4 @@ struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
 template <typename T>
 struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
index 22cea2214..d53cd0ae1 100644
--- a/gtsam/slam/PlanarProjectionFactor.h
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -295,4 +295,4 @@ namespace gtsam {
     struct traits<PlanarProjectionFactor3> :
         public Testable<PlanarProjectionFactor3> {};
 
-} // namespace gtsam
\ No newline at end of file
+} // namespace gtsam

From 993ac90e43012f8b2008ad5e74144a2d26622a91 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Tue, 24 Dec 2024 15:27:49 -0800
Subject: [PATCH 175/362] fix windows test

---
 gtsam/slam/tests/testPlanarProjectionFactor.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
index acfa95618..5f65c9b5e 100644
--- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -128,7 +128,7 @@ TEST(PlanarProjectionFactor1, Jacobian) {
             [&factor](const Pose2& p) {
                 return factor.evaluateError(p, {});},
                 pose);
-        CHECK(assert_equal(expectedH1, H1, 2e-6));
+        CHECK(assert_equal(expectedH1, H1, 5e-6));
     }
 }
 
@@ -325,9 +325,9 @@ TEST(PlanarProjectionFactor3, Jacobian) {
             [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
                 return factor.evaluateError(p, o, c, {}, {}, {});},
                 pose, offset, calib);
-        CHECK(assert_equal(expectedH1, H1, 1e-6));
-        CHECK(assert_equal(expectedH2, H2, 1e-6));
-        CHECK(assert_equal(expectedH3, H3, 1e-6));
+        CHECK(assert_equal(expectedH1, H1, 5e-6));
+        CHECK(assert_equal(expectedH2, H2, 5e-6));
+        CHECK(assert_equal(expectedH3, H3, 5e-6));
     }
 }
 

From 748db19795d6c51d9d18b74355540544081e8ceb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 25 Dec 2024 13:13:21 -0500
Subject: [PATCH 176/362] rename normalization to normalizer

---
 gtsam/discrete/DiscreteFactorGraph.cpp           |  7 +++----
 gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 10 +++++-----
 2 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index d0bf21047..169259a36 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -126,10 +126,10 @@ namespace gtsam {
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:
-    auto normalization = product.max(product.size());
+    auto normalizer = product.max(product.size());
 
     // Normalize the product factor to prevent underflow.
-    product = product / (*normalization);
+    product = product / (*normalizer);
 
     return product;
   }
@@ -207,8 +207,7 @@ namespace gtsam {
     return dag.argmax();
   }
 
-  DiscreteValues DiscreteFactorGraph::optimize(
-      const Ordering& ordering) const {
+  DiscreteValues DiscreteFactorGraph::optimize(const Ordering& ordering) const {
     gttic(DiscreteFactorGraph_optimize);
     DiscreteLookupDAG dag = maxProduct(ordering);
     return dag.argmax();
diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 0d71c12ba..cbcf5234e 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -117,9 +117,9 @@ TEST(DiscreteFactorGraph, test) {
       *std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
 
   // Normalize newFactor by max for comparison with expected
-  auto normalization = newFactor.max(newFactor.size());
+  auto normalizer = newFactor.max(newFactor.size());
 
-  newFactor = newFactor / *normalization;
+  newFactor = newFactor / *normalizer;
 
   // Check Conditional
   CHECK(conditional);
@@ -131,9 +131,9 @@ TEST(DiscreteFactorGraph, test) {
   CHECK(&newFactor);
   DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
   // Normalize by max.
-  normalization = expectedFactor.max(expectedFactor.size());
-  // Ensure normalization is correct.
-  expectedFactor = expectedFactor / *normalization;
+  normalizer = expectedFactor.max(expectedFactor.size());
+  // Ensure normalizer is correct.
+  expectedFactor = expectedFactor / *normalizer;
   EXPECT(assert_equal(expectedFactor, newFactor));
 
   // Test using elimination tree

From c2e8867e82b68201db094dca4dd17ec8688ed039 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 25 Dec 2024 13:14:22 -0500
Subject: [PATCH 177/362] avoid extra conversion step

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 8832cbb34..b9051554a 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -282,7 +282,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
       auto dc = hc->asDiscrete();
       if (!dc) throwRuntimeError("discreteElimination", dc);
-      dfg.push_back(hc->asDiscrete());
+      dfg.push_back(dc);
     } else {
       throwRuntimeError("discreteElimination", f);
     }

From 3694f7aeb36405273b6bf5d79967e8bea652f7c4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 25 Dec 2024 13:27:54 -0500
Subject: [PATCH 178/362] undo change to TableFactor::toDecisionTreeFactor
 since it is incorrect for certain larger scale cases

---
 gtsam/discrete/TableFactor.cpp | 35 +++-------------------------------
 1 file changed, 3 insertions(+), 32 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 22548da07..bf9662e34 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -252,41 +252,12 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
-  // Record key assignment and value pairs in pair_table.
-  // The assignments are stored in descending order of keys so that the order of
-  // the values matches what is expected by a DecisionTree.
-  // This is why we reverse the keys and then
-  // query for the key value/assignment.
-  DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend());
-  std::vector<std::pair<uint64_t, double>> pair_table;
+  std::vector<double> table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
-    std::stringstream ss;
-    for (auto&& [key, _] : rdkeys) {
-      ss << keyValueForIndex(key, i);
-    }
-    // k will be in reverse key order already
-    uint64_t k;
-    ss >> k;
-    pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
+    table.push_back(sparse_table_.coeff(i));
   }
 
-  // Sort the pair_table (of assignment-value pairs) based on assignment so we
-  // get values in reverse key order.
-  std::sort(
-      pair_table.begin(), pair_table.end(),
-      [](const std::pair<uint64_t, double>& a,
-         const std::pair<uint64_t, double>& b) { return a.first < b.first; });
-
-  // Create the table vector by extracting the values from pair_table.
-  // The pair_table has already been sorted in the desired order,
-  // so the values will be in descending key order.
-  std::vector<double> table;
-  std::for_each(pair_table.begin(), pair_table.end(),
-                [&table](const std::pair<uint64_t, double>& pair) {
-                  table.push_back(pair.second);
-                });
-
-  AlgebraicDecisionTree<Key> tree(rdkeys, table);
+  AlgebraicDecisionTree<Key> tree(dkeys, table);
   DecisionTreeFactor f(dkeys, tree);
   return f;
 }

From ae213dd46477fd39e8399f466ff6b21d11909713 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 26 Dec 2024 23:44:00 -0500
Subject: [PATCH 179/362] replace #ifdef with #if for
 GTSAM_ENABLE_BOOST_SERIALIZATION

---
 gtsam/base/ConcurrentMap.h                    |  4 +--
 gtsam/base/FastList.h                         |  4 +--
 gtsam/base/FastMap.h                          |  4 +--
 gtsam/base/FastSet.h                          |  4 +--
 gtsam/base/GenericValue.h                     |  2 +-
 gtsam/base/MatrixSerialization.h              |  2 +-
 gtsam/base/SymmetricBlockMatrix.h             |  4 +--
 gtsam/base/Value.h                            |  6 ++--
 gtsam/base/VectorSerialization.h              |  2 +-
 gtsam/base/VerticalBlockMatrix.h              |  2 +-
 gtsam/base/serialization.h                    |  2 +-
 gtsam/base/serializationTestHelpers.h         |  2 +-
 gtsam/base/std_optional_serialization.h       |  2 +-
 gtsam/discrete/DecisionTree-inl.h             |  4 +--
 gtsam/discrete/DecisionTree.h                 |  6 ++--
 gtsam/discrete/DecisionTreeFactor.h           |  2 +-
 gtsam/discrete/DiscreteBayesNet.h             |  2 +-
 gtsam/discrete/DiscreteConditional.h          |  2 +-
 gtsam/discrete/DiscreteFactor.h               |  2 +-
 gtsam/discrete/DiscreteKey.h                  |  4 +--
 gtsam/discrete/DiscreteLookupDAG.h            |  2 +-
 gtsam/geometry/BearingRange.h                 |  4 +--
 gtsam/geometry/Cal3.h                         |  2 +-
 gtsam/geometry/Cal3Bundler.h                  |  2 +-
 gtsam/geometry/Cal3DS2.h                      |  2 +-
 gtsam/geometry/Cal3DS2_Base.h                 |  2 +-
 gtsam/geometry/Cal3Fisheye.h                  |  2 +-
 gtsam/geometry/Cal3Unified.h                  |  2 +-
 gtsam/geometry/Cal3_S2.h                      |  2 +-
 gtsam/geometry/Cal3_S2Stereo.h                |  2 +-
 gtsam/geometry/Cal3f.h                        |  2 +-
 gtsam/geometry/CalibratedCamera.h             |  6 ++--
 gtsam/geometry/CameraSet.h                    |  2 +-
 gtsam/geometry/EssentialMatrix.h              |  2 +-
 gtsam/geometry/PinholeCamera.h                |  2 +-
 gtsam/geometry/PinholePose.h                  |  4 +--
 gtsam/geometry/PinholeSet.h                   |  2 +-
 gtsam/geometry/Point2.h                       |  2 +-
 gtsam/geometry/Point3.h                       |  2 +-
 gtsam/geometry/Pose2.h                        |  2 +-
 gtsam/geometry/Pose3.h                        |  2 +-
 gtsam/geometry/Rot2.h                         |  2 +-
 gtsam/geometry/Rot3.h                         |  2 +-
 gtsam/geometry/SO3.h                          |  2 +-
 gtsam/geometry/SO4.h                          |  2 +-
 gtsam/geometry/SOn.h                          |  6 ++--
 gtsam/geometry/Similarity3.h                  |  2 +-
 gtsam/geometry/SphericalCamera.h              |  6 ++--
 gtsam/geometry/StereoCamera.h                 |  2 +-
 gtsam/geometry/StereoPoint2.h                 |  4 +--
 gtsam/geometry/Unit3.h                        |  2 +-
 gtsam/geometry/tests/testBearingRange.cpp     |  2 +-
 gtsam/geometry/tests/testUnit3.cpp            |  2 +-
 gtsam/geometry/triangulation.h                |  4 +--
 gtsam/hybrid/HybridBayesNet.h                 |  2 +-
 gtsam/hybrid/HybridBayesTree.h                |  2 +-
 gtsam/hybrid/HybridConditional.h              |  2 +-
 gtsam/hybrid/HybridFactor.h                   |  2 +-
 gtsam/hybrid/HybridGaussianConditional.h      |  2 +-
 gtsam/hybrid/HybridGaussianFactor.h           |  2 +-
 gtsam/hybrid/HybridGaussianProductFactor.h    |  2 +-
 gtsam/inference/BayesTree.h                   |  2 +-
 gtsam/inference/BayesTreeCliqueBase.h         |  2 +-
 gtsam/inference/Conditional.h                 |  2 +-
 gtsam/inference/Factor.h                      |  4 +--
 gtsam/inference/FactorGraph.h                 |  4 +--
 gtsam/inference/LabeledSymbol.h               |  2 +-
 gtsam/inference/Ordering.h                    |  2 +-
 gtsam/inference/Symbol.h                      |  4 +--
 gtsam/inference/VariableIndex.h               |  2 +-
 gtsam/linear/GaussianBayesNet.h               |  2 +-
 gtsam/linear/GaussianConditional.h            |  2 +-
 gtsam/linear/GaussianFactor.h                 |  2 +-
 gtsam/linear/GaussianFactorGraph.h            |  2 +-
 gtsam/linear/HessianFactor.h                  |  2 +-
 gtsam/linear/JacobianFactor.h                 |  6 ++--
 gtsam/linear/LossFunctions.h                  | 28 +++++++++----------
 gtsam/linear/NoiseModel.h                     | 16 +++++------
 gtsam/linear/SubgraphBuilder.cpp              |  4 +--
 gtsam/linear/SubgraphBuilder.h                |  6 ++--
 gtsam/linear/VectorValues.h                   |  2 +-
 gtsam/navigation/AHRSFactor.h                 |  4 +--
 gtsam/navigation/AttitudeFactor.h             |  6 ++--
 gtsam/navigation/BarometricFactor.h           |  2 +-
 gtsam/navigation/CombinedImuFactor.cpp        |  2 +-
 gtsam/navigation/CombinedImuFactor.h          |  4 +--
 gtsam/navigation/GPSFactor.h                  |  4 +--
 gtsam/navigation/ImuBias.h                    |  4 +--
 gtsam/navigation/ImuFactor.h                  |  6 ++--
 gtsam/navigation/MagPoseFactor.h              |  2 +-
 gtsam/navigation/ManifoldPreintegration.h     |  2 +-
 gtsam/navigation/NavState.h                   |  2 +-
 gtsam/navigation/PreintegratedRotation.h      |  4 +--
 gtsam/navigation/PreintegrationBase.h         |  2 +-
 .../navigation/PreintegrationCombinedParams.h |  2 +-
 gtsam/navigation/PreintegrationParams.h       |  2 +-
 gtsam/navigation/TangentPreintegration.h      |  2 +-
 gtsam/nonlinear/CustomFactor.h                |  2 +-
 gtsam/nonlinear/ExpressionFactor.h            |  4 +--
 gtsam/nonlinear/FunctorizedFactor.h           |  4 +--
 gtsam/nonlinear/ISAM2.h                       |  2 +-
 gtsam/nonlinear/ISAM2Clique.h                 |  2 +-
 gtsam/nonlinear/LinearContainerFactor.h       |  2 +-
 gtsam/nonlinear/NonlinearEquality.h           |  6 ++--
 gtsam/nonlinear/NonlinearFactor.h             |  6 ++--
 gtsam/nonlinear/NonlinearFactorGraph.h        |  2 +-
 gtsam/nonlinear/PriorFactor.h                 |  2 +-
 gtsam/nonlinear/Values.h                      |  4 +--
 gtsam/sam/BearingFactor.h                     |  2 +-
 gtsam/sam/BearingRangeFactor.h                |  2 +-
 gtsam/sam/RangeFactor.h                       |  4 +--
 gtsam/sfm/SfmData.h                           |  2 +-
 gtsam/sfm/SfmTrack.h                          |  2 +-
 gtsam/sfm/TranslationFactor.h                 |  4 +--
 gtsam/slam/AntiFactor.h                       |  2 +-
 gtsam/slam/BetweenFactor.h                    |  4 +--
 gtsam/slam/BoundingConstraint.h               |  4 +--
 gtsam/slam/EssentialMatrixConstraint.h        |  2 +-
 gtsam/slam/GeneralSFMFactor.h                 |  6 ++--
 gtsam/slam/PoseRotationPrior.h                |  2 +-
 gtsam/slam/PoseTranslationPrior.h             |  2 +-
 gtsam/slam/ProjectionFactor.h                 |  2 +-
 gtsam/slam/ReferenceFrameFactor.h             |  2 +-
 gtsam/slam/SmartFactorBase.h                  |  4 +--
 gtsam/slam/SmartFactorParams.h                |  2 +-
 gtsam/slam/SmartProjectionFactor.h            |  2 +-
 gtsam/slam/SmartProjectionPoseFactor.h        |  2 +-
 gtsam/slam/SmartProjectionRigFactor.h         |  2 +-
 gtsam/slam/StereoFactor.h                     |  2 +-
 gtsam/slam/TriangulationFactor.h              |  2 +-
 gtsam/symbolic/SymbolicBayesNet.h             |  2 +-
 gtsam/symbolic/SymbolicBayesTree.h            |  2 +-
 gtsam/symbolic/SymbolicConditional.h          |  2 +-
 gtsam/symbolic/SymbolicFactor.h               |  2 +-
 gtsam/symbolic/SymbolicFactorGraph.h          |  2 +-
 gtsam_unstable/dynamics/PoseRTV.h             |  2 +-
 gtsam_unstable/dynamics/VelocityConstraint3.h |  2 +-
 gtsam_unstable/geometry/BearingS2.h           |  2 +-
 gtsam_unstable/geometry/InvDepthCamera3.h     |  4 +--
 gtsam_unstable/geometry/Pose3Upright.h        |  2 +-
 gtsam_unstable/nonlinear/LinearizedFactor.h   |  6 ++--
 gtsam_unstable/slam/BetweenFactorEM.h         |  2 +-
 gtsam_unstable/slam/BiasedGPSFactor.h         |  2 +-
 .../slam/EquivInertialNavFactor_GlobalVel.h   |  2 +-
 .../EquivInertialNavFactor_GlobalVel_NoBias.h |  2 +-
 .../slam/GaussMarkov1stOrderFactor.h          |  2 +-
 .../slam/InertialNavFactor_GlobalVelocity.h   |  2 +-
 gtsam_unstable/slam/InvDepthFactor3.h         |  2 +-
 gtsam_unstable/slam/InvDepthFactorVariant1.h  |  2 +-
 gtsam_unstable/slam/InvDepthFactorVariant2.h  |  2 +-
 gtsam_unstable/slam/InvDepthFactorVariant3.h  |  4 +--
 gtsam_unstable/slam/MultiProjectionFactor.h   |  2 +-
 gtsam_unstable/slam/PartialPriorFactor.h      |  2 +-
 gtsam_unstable/slam/PoseBetweenFactor.h       |  2 +-
 gtsam_unstable/slam/PosePriorFactor.h         |  2 +-
 gtsam_unstable/slam/PoseToPointFactor.h       |  2 +-
 gtsam_unstable/slam/ProjectionFactorPPP.h     |  2 +-
 gtsam_unstable/slam/ProjectionFactorPPPC.h    |  2 +-
 .../slam/ProjectionFactorRollingShutter.h     |  2 +-
 gtsam_unstable/slam/RelativeElevationFactor.h |  2 +-
 .../SmartProjectionPoseFactorRollingShutter.h |  2 +-
 .../slam/SmartStereoProjectionFactor.h        |  4 +--
 .../slam/SmartStereoProjectionFactorPP.h      |  2 +-
 .../slam/SmartStereoProjectionPoseFactor.h    |  2 +-
 .../slam/TransformBtwRobotsUnaryFactor.h      |  2 +-
 .../slam/TransformBtwRobotsUnaryFactorEM.h    |  2 +-
 tests/simulated2D.h                           |  6 ++--
 tests/testExpressionFactor.cpp                |  2 +-
 168 files changed, 247 insertions(+), 247 deletions(-)

diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h
index 372f02d06..03687f562 100644
--- a/gtsam/base/ConcurrentMap.h
+++ b/gtsam/base/ConcurrentMap.h
@@ -48,7 +48,7 @@ using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
 
 #endif
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/split_member.hpp>
 #endif
@@ -101,7 +101,7 @@ public:
 #endif
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h
index 414c1e83f..1a3b9ed3f 100644
--- a/gtsam/base/FastList.h
+++ b/gtsam/base/FastList.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/base/FastDefaultAllocator.h>
 #include <list>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/version.hpp>
 #if BOOST_VERSION >= 107400
@@ -79,7 +79,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h
index e8ef3fc4f..75998fd2f 100644
--- a/gtsam/base/FastMap.h
+++ b/gtsam/base/FastMap.h
@@ -19,7 +19,7 @@
 #pragma once
 
 #include <gtsam/base/FastDefaultAllocator.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/map.hpp>
 #endif
@@ -69,7 +69,7 @@ public:
   bool exists(const KEY& e) const { return this->find(e) != this->end(); }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index 58ddd13a0..9a2f62497 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/config.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/version.hpp>
 #if BOOST_VERSION >= 107400
 #include <boost/serialization/library_version_type.hpp>
@@ -125,7 +125,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 87ce7a73d..62ea8ec39 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -173,7 +173,7 @@ public:
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h
index 9cf730230..11b6a417a 100644
--- a/gtsam/base/MatrixSerialization.h
+++ b/gtsam/base/MatrixSerialization.h
@@ -19,7 +19,7 @@
 // \callgraph
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
 #include <gtsam/base/Matrix.h>
diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h
index 1783857cb..378e91144 100644
--- a/gtsam/base/SymmetricBlockMatrix.h
+++ b/gtsam/base/SymmetricBlockMatrix.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/types.h>
 #include <gtsam/dllexport.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <cassert>
@@ -386,7 +386,7 @@ namespace gtsam {
     template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index c36de42ab..09a3f4878 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -21,7 +21,7 @@
 #include <gtsam/config.h>      // Configuration from CMake
 
 #include <gtsam/base/Vector.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/assume_abstract.hpp>
 #endif
@@ -121,7 +121,7 @@ namespace gtsam {
      *       The last two links explain why these export lines have to be in the same source module that includes
      *       any of the archive class headers.
      * */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) {
@@ -132,6 +132,6 @@ namespace gtsam {
 
 } /* namespace gtsam */
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
 #endif
diff --git a/gtsam/base/VectorSerialization.h b/gtsam/base/VectorSerialization.h
index 7acf108f4..39d7e0299 100644
--- a/gtsam/base/VectorSerialization.h
+++ b/gtsam/base/VectorSerialization.h
@@ -17,7 +17,7 @@
  */
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
 #include <gtsam/base/Vector.h>
diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index c8fef76e9..0ff8a8ae2 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -221,7 +221,7 @@ namespace gtsam {
     friend class SymmetricBlockMatrix;
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h
index e829014ae..ef7ed497f 100644
--- a/gtsam/base/serialization.h
+++ b/gtsam/base/serialization.h
@@ -21,7 +21,7 @@
 
 #include <gtsam/config.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 
 #include <Eigen/Core>
 #include <fstream>
diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
index fb0701d89..cc6dadafd 100644
--- a/gtsam/base/serializationTestHelpers.h
+++ b/gtsam/base/serializationTestHelpers.h
@@ -21,7 +21,7 @@
 
 #include <gtsam/config.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 
 #include <iostream>
 #include <sstream>
diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h
index 7e0f2e844..93a5c8dba 100644
--- a/gtsam/base/std_optional_serialization.h
+++ b/gtsam/base/std_optional_serialization.h
@@ -11,7 +11,7 @@
 #pragma once
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <optional>
 #include <boost/config.hpp>
 
diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index bda44bb9d..efc19d9ee 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -144,7 +144,7 @@ namespace gtsam {
    private:
     using Base = DecisionTree<L, Y>::Node;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
@@ -471,7 +471,7 @@ namespace gtsam {
    private:
     using Base = DecisionTree<L, Y>::Node;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 486f798e9..7c8d2742d 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -23,7 +23,7 @@
 #include <gtsam/base/types.h>
 #include <gtsam/discrete/Assignment.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <memory>
@@ -132,7 +132,7 @@ namespace gtsam {
       virtual bool isLeaf() const = 0;
 
      private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template <class ARCHIVE>
@@ -440,7 +440,7 @@ namespace gtsam {
     /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index e1eb2a453..80ee10a7b 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -310,7 +310,7 @@ namespace gtsam {
   /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h
index a5a4621aa..738b91aa5 100644
--- a/gtsam/discrete/DiscreteBayesNet.h
+++ b/gtsam/discrete/DiscreteBayesNet.h
@@ -147,7 +147,7 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
     /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 8cba6dbe7..3ec9ae590 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -275,7 +275,7 @@ class GTSAM_EXPORT DiscreteConditional
                                   bool forceComplete) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 29981e94b..a1fde0f86 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -163,7 +163,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h
index 44cc192ef..d200792c8 100644
--- a/gtsam/discrete/DiscreteKey.h
+++ b/gtsam/discrete/DiscreteKey.h
@@ -21,7 +21,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/inference/Key.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/vector.hpp>
 #endif
 #include <map>
@@ -87,7 +87,7 @@ namespace gtsam {
     /// Check equality to another DiscreteKeys object.
     bool equals(const DiscreteKeys& other, double tol = 0) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h
index f077a13d9..afb4d4208 100644
--- a/gtsam/discrete/DiscreteLookupDAG.h
+++ b/gtsam/discrete/DiscreteLookupDAG.h
@@ -123,7 +123,7 @@ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet<DiscreteLookupTable> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 07229dfca..9c4e42edf 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Manifold.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/OptionalJacobian.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <iostream>
@@ -148,7 +148,7 @@ public:
   /// @{
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 5eecee845..63efb0d49 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3 {
   /// @{
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 01eb9437a..71b5fba1d 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -145,7 +145,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 0ecfc24ae..8c1bbe302 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -104,7 +104,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 6eff04c10..986c9842b 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -156,7 +156,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 4c542c5be..f10a31b65 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 7eb4d2edb..cef2b8f9b 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -138,7 +138,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 568cde41e..8a587af9b 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -132,7 +132,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
   /// @{
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 292519cfd..58c7dd37a 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -143,7 +143,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   /// @{
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h
index 6c3c214b8..b6cf4435b 100644
--- a/gtsam/geometry/Cal3f.h
+++ b/gtsam/geometry/Cal3f.h
@@ -118,7 +118,7 @@ class GTSAM_EXPORT Cal3f : public Cal3 {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index d3d1e1b9a..d364318c9 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -25,7 +25,7 @@
 #include <gtsam/base/Manifold.h>
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/dllexport.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -230,7 +230,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -406,7 +406,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index e3c16f066..36c1e1f54 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -472,7 +472,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index 3986f3063..74b736685 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -180,7 +180,7 @@ class EssentialMatrix {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 0439b2fde..f8599572d 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -324,7 +324,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index f1191dbcc..252d8bc6f 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -212,7 +212,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -425,7 +425,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h
index 55269f9d6..45cdb11e4 100644
--- a/gtsam/geometry/PinholeSet.h
+++ b/gtsam/geometry/PinholeSet.h
@@ -64,7 +64,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 76beea11a..66d57f785 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -19,7 +19,7 @@
 
 #include <gtsam/base/VectorSpace.h>
 #include <gtsam/base/std_optional_serialization.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index d5c32816e..9bc943a43 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -26,7 +26,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 #include <gtsam/base/VectorSerialization.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <numeric>
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index ff9a6399e..3f85a7f42 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -339,7 +339,7 @@ public:
 
  private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
   // Serialization function
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 81a9848e2..eefbd7654 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -406,7 +406,7 @@ public:
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 6bb97ff6c..07f328b96 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -213,7 +213,7 @@ namespace gtsam {
     static Rot2 ClosestTo(const Matrix2& M);
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index d1e330438..71f4cbacd 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -532,7 +532,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 163ae6623..36329a19d 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -98,7 +98,7 @@ template <>
 GTSAM_EXPORT
 Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 template <class Archive>
 /** Serialization function */
 void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h
index 834cab746..43b74f7aa 100644
--- a/gtsam/geometry/SO4.h
+++ b/gtsam/geometry/SO4.h
@@ -78,7 +78,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {});
  */
 GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {});
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 template <class Archive>
 /** Serialization function */
 void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index b0da3b213..e8e1f641d 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -24,7 +24,7 @@
 #include <gtsam/dllexport.h>
 #include <Eigen/Core>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -326,7 +326,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// @name Serialization
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   template <class Archive>
   friend void save(Archive&, SO&, const unsigned int);
   template <class Archive>
@@ -380,7 +380,7 @@ template <>
 GTSAM_EXPORT
 typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 /** Serialization function */
 template<class Archive>
 void serialize(
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 05bd0431e..8f7f30be8 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -203,7 +203,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
 
  private:
 
-  #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  #if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h
index 1ff87cec3..786295d7b 100644
--- a/gtsam/geometry/SphericalCamera.h
+++ b/gtsam/geometry/SphericalCamera.h
@@ -26,7 +26,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -54,7 +54,7 @@ class GTSAM_EXPORT EmptyCal {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
@@ -223,7 +223,7 @@ class GTSAM_EXPORT SphericalCamera {
   static size_t Dim() { return 6; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index 026125572..315e4d267 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -177,7 +177,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 7a1910e42..26c14750b 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/VectorSpace.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -150,7 +150,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index a7304280f..65a304d62 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -198,7 +198,7 @@ private:
 
   /// @name Advanced Interface
   /// @{
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp
index ff2a9a6a4..df4a4dc51 100644
--- a/gtsam/geometry/tests/testBearingRange.cpp
+++ b/gtsam/geometry/tests/testBearingRange.cpp
@@ -56,7 +56,7 @@ TEST(BearingRange, 3D) {
   EXPECT(assert_equal(expected, actual));
 }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 using namespace serializationTestHelpers;
 /* ************************************************************************* */
 TEST(BearingRange, Serialization2D) {
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 0c4c2c725..855e50a42 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -499,7 +499,7 @@ TEST(Unit3, CopyAssign) {
 }
 
 /* ************************************************************************* */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 TEST(actualH, Serialization) {
   Unit3 p(0, 1, 0);
   EXPECT(serializationTestHelpers::equalsObj(p));
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 9087f01c5..749824845 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -622,7 +622,7 @@ struct GTSAM_EXPORT TriangulationParameters {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -687,7 +687,7 @@ class TriangulationResult : public std::optional<Point3> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 96afb87d6..3e07c71ce 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -268,7 +268,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 0fe9ca6ea..06d880f02 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -115,7 +115,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index f3df725ef..a08b3a6ee 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -217,7 +217,7 @@ class GTSAM_EXPORT HybridConditional
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 8d6fef3f4..9fc280322 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -140,7 +140,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index 0d2b1323c..e769662ed 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -253,7 +253,7 @@ class GTSAM_EXPORT HybridGaussianConditional
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 6d1064647..efbba9e51 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -176,7 +176,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   // Private constructor using ConstructorHelper above.
   HybridGaussianFactor(const ConstructorHelper &helper);
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h
index 60a58a3a5..f5e75aa86 100644
--- a/gtsam/hybrid/HybridGaussianProductFactor.h
+++ b/gtsam/hybrid/HybridGaussianProductFactor.h
@@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianProductFactor
   ///@}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index d0ebd916e..03f79c8cf 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -262,7 +262,7 @@ namespace gtsam {
     template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index adffa2f14..0ccb04e90 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -208,7 +208,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index f9da36b7b..31e451714 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -233,7 +233,7 @@ namespace gtsam {
     // Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
     const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 074151e16..19741b22e 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -21,7 +21,7 @@
 
 #pragma once
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <gtsam/base/FastVector.h>
@@ -193,7 +193,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /// @name Serialization
     /// @{
     /** Serialization function */
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index b6046d0bb..ca7321359 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -29,7 +29,7 @@
 
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
 #endif
@@ -420,7 +420,7 @@ class FactorGraph {
   inline bool exists(size_t idx) const { return idx < size() && at(idx); }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h
index 927fb7669..16ac7bccc 100644
--- a/gtsam/inference/LabeledSymbol.h
+++ b/gtsam/inference/LabeledSymbol.h
@@ -142,7 +142,7 @@ class GTSAM_EXPORT LabeledSymbol {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index c3df1b8a0..fced5d4f2 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -252,7 +252,7 @@ private:
   static Ordering ColamdConstrained(
       const VariableIndex& variableIndex, std::vector<int>& cmember);
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h
index 9f2133707..a62c22fac 100644
--- a/gtsam/inference/Symbol.h
+++ b/gtsam/inference/Symbol.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <cstdint>
@@ -124,7 +124,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index 110c0bba4..2f872b25d 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -192,7 +192,7 @@ protected:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index e858bbe43..538d1532e 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -267,7 +267,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 27270fece..14b1ce87f 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -278,7 +278,7 @@ namespace gtsam {
     /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index c5133c6b2..253bbff0f 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -179,7 +179,7 @@ namespace gtsam {
     /// @}
     
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 6a7848f51..67313c086 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -400,7 +400,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 56b3d6537..189020712 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -370,7 +370,7 @@ namespace gtsam {
     friend class NonlinearFactorGraph;
     friend class NonlinearClusterTree;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 407ed1e27..4a5b6d5d1 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -24,7 +24,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/inference/VariableSlots.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/version.hpp>
 #include <boost/serialization/split_member.hpp>
 #endif
@@ -421,7 +421,7 @@ namespace gtsam {
     // be very selective on who can access these private methods:
     template<typename T> friend class ExpressionFactor;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -468,7 +468,7 @@ struct traits<JacobianFactor> : public Testable<JacobianFactor> {
 
 } // \ namespace gtsam
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
 #endif
 
diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
index a8902e11b..989557d87 100644
--- a/gtsam/linear/LossFunctions.h
+++ b/gtsam/linear/LossFunctions.h
@@ -24,7 +24,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/dllexport.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/extended_type_info.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/version.hpp>
@@ -129,7 +129,7 @@ class GTSAM_EXPORT Base {
   void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -161,7 +161,7 @@ class GTSAM_EXPORT Null : public Base {
   static shared_ptr Create();
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -195,7 +195,7 @@ class GTSAM_EXPORT Fair : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -230,7 +230,7 @@ class GTSAM_EXPORT Huber : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -270,7 +270,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -306,7 +306,7 @@ class GTSAM_EXPORT Tukey : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -341,7 +341,7 @@ class GTSAM_EXPORT Welsch : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -380,7 +380,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   double c_;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -420,7 +420,7 @@ class GTSAM_EXPORT DCS : public Base {
   double c_;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -460,7 +460,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -496,7 +496,7 @@ class GTSAM_EXPORT AsymmetricTukey : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -532,7 +532,7 @@ class GTSAM_EXPORT AsymmetricCauchy : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -577,7 +577,7 @@ class GTSAM_EXPORT Custom : public Base {
   inline Custom() = default;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 34fa63e4c..47a84654a 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -24,7 +24,7 @@
 #include <gtsam/dllexport.h>
 #include <gtsam/linear/LossFunctions.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/extended_type_info.hpp>
 #include <boost/serialization/singleton.hpp>
@@ -141,7 +141,7 @@ namespace gtsam {
       virtual double weight(const Vector& v) const { return 1.0; }
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -280,7 +280,7 @@ namespace gtsam {
       double negLogConstant() const;
 
      private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -375,7 +375,7 @@ namespace gtsam {
       }
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -520,7 +520,7 @@ namespace gtsam {
       shared_ptr unit() const;
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -593,7 +593,7 @@ namespace gtsam {
       inline double sigma() const { return sigma_; }
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -648,7 +648,7 @@ namespace gtsam {
       void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -739,7 +739,7 @@ namespace gtsam {
         const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
index 834fc8d12..9520b826b 100644
--- a/gtsam/linear/SubgraphBuilder.cpp
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -25,7 +25,7 @@
 #include <gtsam/linear/SubgraphBuilder.h>
 #include <gtsam/base/kruskal.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/vector.hpp>
@@ -71,7 +71,7 @@ vector<size_t> Subgraph::edgeIndices() const {
   return eid;
 }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 /****************************************************************************/
 void Subgraph::save(const std::string &fn) const {
   std::ofstream os(fn.c_str());
diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h
index f9ddd4c9a..df77ea9de 100644
--- a/gtsam/linear/SubgraphBuilder.h
+++ b/gtsam/linear/SubgraphBuilder.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/types.h>
 #include <gtsam/dllexport.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/version.hpp>
 #include <boost/serialization/nvp.hpp>
 #endif
@@ -51,7 +51,7 @@ class GTSAM_EXPORT Subgraph {
     friend std::ostream &operator<<(std::ostream &os, const Edge &edge);
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     friend class boost::serialization::access;
     template <class Archive>
     void serialize(Archive &ar, const unsigned int /*version*/) {
@@ -87,7 +87,7 @@ class GTSAM_EXPORT Subgraph {
   friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive &ar, const unsigned int /*version*/) {
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 7fbd43ffc..e60209a83 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -371,7 +371,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h
index 4289b4969..496fcde3d 100644
--- a/gtsam/navigation/AHRSFactor.h
+++ b/gtsam/navigation/AHRSFactor.h
@@ -120,7 +120,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -208,7 +208,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 2fada724d..e342e451e 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -73,7 +73,7 @@ class AttitudeFactor {
   const Unit3& bRef() const { return bMeasured_; }
 #endif
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -138,7 +138,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN<Rot3>,
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -220,7 +220,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN<Pose3>,
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h
index 70cae8d36..545928ca9 100644
--- a/gtsam/navigation/BarometricFactor.h
+++ b/gtsam/navigation/BarometricFactor.h
@@ -97,7 +97,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
     }
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp
index a2eb9e0c0..64ee86be9 100644
--- a/gtsam/navigation/CombinedImuFactor.cpp
+++ b/gtsam/navigation/CombinedImuFactor.cpp
@@ -21,7 +21,7 @@
  **/
 
 #include <gtsam/navigation/CombinedImuFactor.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/export.hpp>
 #endif
 
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 0ffb386a0..b4c3ae8dc 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -169,7 +169,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -281,7 +281,7 @@ class GTSAM_EXPORT CombinedImuFactor
                        OptionalMatrixType H6) const override;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index cd89dd464..6af184360 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -97,7 +97,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -166,7 +166,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index 6df87eda6..02a020c42 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -20,7 +20,7 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/VectorSpace.h>
 #include <iosfwd>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -142,7 +142,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 7254838fd..b19989a77 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -142,7 +142,7 @@ public:
 #endif
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -244,7 +244,7 @@ public:
 
  private:
   /** Serialization function */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -316,7 +316,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h
index ba9a08d78..ea0b30c75 100644
--- a/gtsam/navigation/MagPoseFactor.h
+++ b/gtsam/navigation/MagPoseFactor.h
@@ -132,7 +132,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function.
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h
index 40691c445..43228044a 100644
--- a/gtsam/navigation/ManifoldPreintegration.h
+++ b/gtsam/navigation/ManifoldPreintegration.h
@@ -113,7 +113,7 @@ public:
   /// @}
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index 95ee71fe9..e246cbe27 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -269,7 +269,7 @@ public:
 private:
   /// @{
   /// serialization
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 49b1aa4c1..a887ef321 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -85,7 +85,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   std::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -227,7 +227,7 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index e4a790b9d..de3a380cf 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -173,7 +173,7 @@ class GTSAM_EXPORT PreintegrationBase {
       OptionalJacobian<9, 6> H5 = {}) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h
index 6be05c082..b384a36e7 100644
--- a/gtsam/navigation/PreintegrationCombinedParams.h
+++ b/gtsam/navigation/PreintegrationCombinedParams.h
@@ -84,7 +84,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
   const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index 8435d0bad..6ea9e73b3 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -71,7 +71,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
 
 protected:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h
index e16ad9310..7f5c63d50 100644
--- a/gtsam/navigation/TangentPreintegration.h
+++ b/gtsam/navigation/TangentPreintegration.h
@@ -127,7 +127,7 @@ public:
   /// @}
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h
index c4015db37..5f8a19ff0 100644
--- a/gtsam/nonlinear/CustomFactor.h
+++ b/gtsam/nonlinear/CustomFactor.h
@@ -88,7 +88,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index d9af1933c..4f6648849 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -200,7 +200,7 @@ protected:
  }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
  /// Save to an archive: just saves the base class
  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
@@ -287,7 +287,7 @@ private:
     return expression(keys);
   }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
index 128de5847..ca253179b 100644
--- a/gtsam/nonlinear/FunctorizedFactor.h
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -119,7 +119,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -231,7 +231,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index d3abd95fd..f71b4b7da 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -340,7 +340,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   void updateDelta(bool forceFullSolve = false) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h
index fde9dd3f2..39bce432a 100644
--- a/gtsam/nonlinear/ISAM2Clique.h
+++ b/gtsam/nonlinear/ISAM2Clique.h
@@ -146,7 +146,7 @@ class GTSAM_EXPORT ISAM2Clique
   void restoreFromOriginals(const Vector& originalValues,
                             VectorValues* delta) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h
index dfbf221c3..f63ecb0c6 100644
--- a/gtsam/nonlinear/LinearContainerFactor.h
+++ b/gtsam/nonlinear/LinearContainerFactor.h
@@ -163,7 +163,7 @@ public:
   void initializeLinearizationPoint(const Values& linearizationPoint);
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index f81486b30..538a95208 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -180,7 +180,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -273,7 +273,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -344,7 +344,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 101743b78..1af01f60d 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -29,7 +29,7 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/utilities.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/base_object.hpp>
 #endif
 #include <cstddef>
@@ -303,7 +303,7 @@ public:
   shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -715,7 +715,7 @@ protected:
     }
   }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index bf56d835f..8e3443dc7 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -250,7 +250,7 @@ namespace gtsam {
     std::shared_ptr<HessianFactor> linearizeToHessianFactor(
         const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index 159373459..0979be3df 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -105,7 +105,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 2fe64d9c9..7e950479a 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -29,7 +29,7 @@
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/VectorSpace.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/unique_ptr.hpp>
 #endif
 
@@ -393,7 +393,7 @@ namespace gtsam {
       return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
     }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h
index 01b2baf18..97f23ed8f 100644
--- a/gtsam/sam/BearingFactor.h
+++ b/gtsam/sam/BearingFactor.h
@@ -75,7 +75,7 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
 
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index d5467be47..aa08fea17 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -92,7 +92,7 @@ class BearingRangeFactor
 
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index 193a89a5a..0ccc54afd 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -80,7 +80,7 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
@@ -164,7 +164,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /** Serialization function */
   template <typename ARCHIVE>
diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h
index 7161ab2f9..dbe336979 100644
--- a/gtsam/sfm/SfmData.h
+++ b/gtsam/sfm/SfmData.h
@@ -125,7 +125,7 @@ struct GTSAM_EXPORT SfmData {
   /// @name Serialization
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /** Serialization function */
   template <class Archive>
diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h
index 2b494ed7c..bdfa79cef 100644
--- a/gtsam/sfm/SfmTrack.h
+++ b/gtsam/sfm/SfmTrack.h
@@ -160,7 +160,7 @@ struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
   /// @name Serialization
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h
index cfb9cff20..20bf005c1 100644
--- a/gtsam/sfm/TranslationFactor.h
+++ b/gtsam/sfm/TranslationFactor.h
@@ -78,7 +78,7 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
@@ -149,7 +149,7 @@ class BilinearAngleTranslationFactor
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h
index 3b031b334..11a0b1530 100644
--- a/gtsam/slam/AntiFactor.h
+++ b/gtsam/slam/AntiFactor.h
@@ -106,7 +106,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 3e2478e73..297703d4a 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -135,7 +135,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -176,7 +176,7 @@ namespace gtsam {
   private:
 
     /** Serialization function */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index a496971e2..85d1f6255 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -83,7 +83,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -161,7 +161,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index 45e54aa5d..6b57a8f27 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -91,7 +91,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 581624b38..06c832eb4 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -36,7 +36,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/timing.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <iostream>
@@ -182,7 +182,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -283,7 +283,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index 8ce90988b..47ea4c69f 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -91,7 +91,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h
index c5257f45c..378610825 100644
--- a/gtsam/slam/PoseTranslationPrior.h
+++ b/gtsam/slam/PoseTranslationPrior.h
@@ -90,7 +90,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index f40d8f5bb..baea29eed 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -188,7 +188,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h
index 0ae6ea88a..21617e27a 100644
--- a/gtsam/slam/ReferenceFrameFactor.h
+++ b/gtsam/slam/ReferenceFrameFactor.h
@@ -122,7 +122,7 @@ public:
   Key local_key() const { return this->key3(); }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index d3e879c33..e641bf5d1 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -30,7 +30,7 @@
 #include <gtsam/geometry/CameraSet.h>
 
 #include <optional>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
@@ -450,7 +450,7 @@ protected:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION///
 /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h
index 4523d0e0e..de3245038 100644
--- a/gtsam/slam/SmartFactorParams.h
+++ b/gtsam/slam/SmartFactorParams.h
@@ -118,7 +118,7 @@ struct SmartProjectionParams {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index c54efecc4..3c8843b39 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -466,7 +466,7 @@ protected:
 
  private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h
index b88c1c628..855ae8c08 100644
--- a/gtsam/slam/SmartProjectionPoseFactor.h
+++ b/gtsam/slam/SmartProjectionPoseFactor.h
@@ -148,7 +148,7 @@ public:
 
  private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h
index 6007c84e2..dd4237299 100644
--- a/gtsam/slam/SmartProjectionRigFactor.h
+++ b/gtsam/slam/SmartProjectionRigFactor.h
@@ -349,7 +349,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 5adafcf3a..5fd171851 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -175,7 +175,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h
index 3b8486a59..f36e9b385 100644
--- a/gtsam/slam/TriangulationFactor.h
+++ b/gtsam/slam/TriangulationFactor.h
@@ -187,7 +187,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h
index 3102191dc..4bba7cb69 100644
--- a/gtsam/symbolic/SymbolicBayesNet.h
+++ b/gtsam/symbolic/SymbolicBayesNet.h
@@ -101,7 +101,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h
index 275b9560d..2b9e2f7ad 100644
--- a/gtsam/symbolic/SymbolicBayesTree.h
+++ b/gtsam/symbolic/SymbolicBayesTree.h
@@ -62,7 +62,7 @@ namespace gtsam {
     bool equals(const This& other, double tol = 1e-9) const;
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h
index 5cc4e9cfc..3cc3e60cc 100644
--- a/gtsam/symbolic/SymbolicConditional.h
+++ b/gtsam/symbolic/SymbolicConditional.h
@@ -126,7 +126,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h
index 190609ecb..fd19740de 100644
--- a/gtsam/symbolic/SymbolicFactor.h
+++ b/gtsam/symbolic/SymbolicFactor.h
@@ -151,7 +151,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h
index 4122031d0..bbcd59f5e 100644
--- a/gtsam/symbolic/SymbolicFactorGraph.h
+++ b/gtsam/symbolic/SymbolicFactorGraph.h
@@ -145,7 +145,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h
index 0e0d9ae27..95b7cfff3 100644
--- a/gtsam_unstable/dynamics/PoseRTV.h
+++ b/gtsam_unstable/dynamics/PoseRTV.h
@@ -157,7 +157,7 @@ public:
   /// @}
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h
index 9f2ec89d2..9751b30a7 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint3.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint3.h
@@ -51,7 +51,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h
index cb1d055df..0c3f40df8 100644
--- a/gtsam_unstable/geometry/BearingS2.h
+++ b/gtsam_unstable/geometry/BearingS2.h
@@ -90,7 +90,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
   // Serialization function
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h
index 45b27efe3..d1486209e 100644
--- a/gtsam_unstable/geometry/InvDepthCamera3.h
+++ b/gtsam_unstable/geometry/InvDepthCamera3.h
@@ -19,7 +19,7 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -175,7 +175,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h
index a77715fc6..20e0fde5d 100644
--- a/gtsam_unstable/geometry/Pose3Upright.h
+++ b/gtsam_unstable/geometry/Pose3Upright.h
@@ -130,7 +130,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
   // Serialization function
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h
index 1ff45ef5f..7aa596be1 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.h
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.h
@@ -59,7 +59,7 @@ public:
   const Values& linearizationPoint() const { return lin_points_; }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -148,7 +148,7 @@ public:
   Vector error_vector(const Values& c) const;
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /** Serialization function */
   template<class ARCHIVE>
@@ -279,7 +279,7 @@ public:
 
 private:
   /** Serialization function */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index f1337c0e8..6cdce415e 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -415,7 +415,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h
index f2b669f4f..1c278b111 100644
--- a/gtsam_unstable/slam/BiasedGPSFactor.h
+++ b/gtsam_unstable/slam/BiasedGPSFactor.h
@@ -94,7 +94,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index 0accb8f42..b759ceb56 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -704,7 +704,7 @@ public:
     }
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
index a94c95335..56f2e2cee 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
@@ -573,7 +573,7 @@ public:
     }
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
index b91ed2afe..c9c4141b1 100644
--- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
+++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
@@ -113,7 +113,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
index 8fda5456b..95fde2f75 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -411,7 +411,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h
index 64a1366a2..f47c56bc9 100644
--- a/gtsam_unstable/slam/InvDepthFactor3.h
+++ b/gtsam_unstable/slam/InvDepthFactor3.h
@@ -113,7 +113,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h
index a49b2090c..638b77440 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -133,7 +133,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h
index 7abbbe89a..01f087df0 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant2.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h
@@ -140,7 +140,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h
index 8664002e8..f71df8425 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -137,7 +137,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -269,7 +269,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /// Serialization function
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h
index 8e2ea0cf7..28e9c0215 100644
--- a/gtsam_unstable/slam/MultiProjectionFactor.h
+++ b/gtsam_unstable/slam/MultiProjectionFactor.h
@@ -213,7 +213,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h
index 0a735a2e1..645d0305a 100644
--- a/gtsam_unstable/slam/PartialPriorFactor.h
+++ b/gtsam_unstable/slam/PartialPriorFactor.h
@@ -142,7 +142,7 @@ namespace gtsam {
     const std::vector<size_t>& indices() const { return indices_; }
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h
index aa383967d..1e0526e22 100644
--- a/gtsam_unstable/slam/PoseBetweenFactor.h
+++ b/gtsam_unstable/slam/PoseBetweenFactor.h
@@ -119,7 +119,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h
index 4a201ad89..60df7e948 100644
--- a/gtsam_unstable/slam/PosePriorFactor.h
+++ b/gtsam_unstable/slam/PosePriorFactor.h
@@ -101,7 +101,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h
index be454cb70..911445fa5 100644
--- a/gtsam_unstable/slam/PoseToPointFactor.h
+++ b/gtsam_unstable/slam/PoseToPointFactor.h
@@ -92,7 +92,7 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
   const POINT& measured() const { return measured_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h
index 38467c6cc..126900bc2 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPP.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPP.h
@@ -171,7 +171,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h
index df63330df..cea74842f 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPPC.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h
@@ -151,7 +151,7 @@ class ProjectionFactorPPPC
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h
index c5eb58f14..778713733 100644
--- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h
+++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h
@@ -194,7 +194,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
   inline bool throwCheirality() const { return throwCheirality_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h
index d24415c3b..13bd5eba0 100644
--- a/gtsam_unstable/slam/RelativeElevationFactor.h
+++ b/gtsam_unstable/slam/RelativeElevationFactor.h
@@ -65,7 +65,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
index 4a2047ee1..f28ce642b 100644
--- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
+++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
@@ -453,7 +453,7 @@ class SmartProjectionPoseFactorRollingShutter
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 52d6eda05..7b06e8827 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -30,7 +30,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam_unstable/dllexport.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/optional.hpp>
 #endif
 
@@ -504,7 +504,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index 189c501bb..7482682fa 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -285,7 +285,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
index 77a15c246..93d8877a7 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -138,7 +138,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
   Base::Cameras cameras(const Values& values) const override;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
index 2a257ff57..cbc8eba52 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
@@ -216,7 +216,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index c47cf3b7d..352a9ff37 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -414,7 +414,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/tests/simulated2D.h b/tests/simulated2D.h
index 612dd1891..78fc26708 100644
--- a/tests/simulated2D.h
+++ b/tests/simulated2D.h
@@ -160,7 +160,7 @@ namespace simulated2D {
     /// Default constructor
     GenericPrior() { }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -210,7 +210,7 @@ namespace simulated2D {
     /// Default constructor
     GenericOdometry() { }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -261,7 +261,7 @@ namespace simulated2D {
     /// Default constructor
     GenericMeasurement() { }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index 4c2ac99b1..845c28e5a 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -688,7 +688,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>

From 7c9d04fb65b3d595a8d1b08a54d023d72cc87576 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 Dec 2024 12:02:21 -0500
Subject: [PATCH 180/362] conditional switch for hybrid timing

---
 gtsam/config.h.in                          |  4 +++
 gtsam/discrete/DiscreteFactorGraph.cpp     | 30 ++++++++++++----
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 41 ++++++++++++++++++++--
 gtsam/hybrid/HybridGaussianISAM.cpp        |  6 ++++
 4 files changed, 73 insertions(+), 8 deletions(-)

diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 8b4903d3a..db6dd2b34 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -42,6 +42,10 @@
 // Whether to enable merging of equal leaf nodes in the Discrete Decision Tree.
 #cmakedefine GTSAM_DT_MERGING
 
+// Whether to enable timing in hybrid factor graph machinery
+// #cmakedefine01 GTSAM_HYBRID_TIMING
+#define GTSAM_HYBRID_TIMING
+
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB
 
diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 169259a36..2037dd951 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -121,15 +121,25 @@ namespace gtsam {
   static DecisionTreeFactor ProductAndNormalize(
       const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
-    gttic(product);
+#if GTSAM_HYBRID_TIMING
+    gttic_(DiscreteProduct);
+#endif
     DecisionTreeFactor product = factors.product();
-    gttoc(product);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(DiscreteProduct);
+#endif
 
     // Max over all the potentials by pretending all keys are frontal:
     auto normalizer = product.max(product.size());
 
+#if GTSAM_HYBRID_TIMING
+    gttic_(DiscreteNormalize);
+#endif
     // Normalize the product factor to prevent underflow.
     product = product / (*normalizer);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(DiscreteNormalize);
+#endif
 
     return product;
   }
@@ -220,9 +230,13 @@ namespace gtsam {
     DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // sum out frontals, this is the factor on the separator
-    gttic(sum);
+#if GTSAM_HYBRID_TIMING
+    gttic_(EliminateDiscreteSum);
+#endif
     DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
-    gttoc(sum);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(EliminateDiscreteSum);
+#endif
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     Ordering orderedKeys;
@@ -232,10 +246,14 @@ namespace gtsam {
                        sum->keys().end());
 
     // now divide product/sum to get conditional
-    gttic(divide);
+#if GTSAM_HYBRID_TIMING
+    gttic_(EliminateDiscreteToDiscreteConditional);
+#endif
     auto conditional =
         std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
-    gttoc(divide);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(EliminateDiscreteToDiscreteConditional);
+#endif
 
     return {conditional, sum};
   }
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index b9051554a..703684c78 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -282,14 +282,28 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
       auto dc = hc->asDiscrete();
       if (!dc) throwRuntimeError("discreteElimination", dc);
-      dfg.push_back(dc);
+#if GTSAM_HYBRID_TIMING
+      gttic_(ConvertConditionalToTableFactor);
+#endif
+      // Convert DiscreteConditional to TableFactor
+      auto tdc = std::make_shared<TableFactor>(*dc);
+#if GTSAM_HYBRID_TIMING
+      gttoc_(ConvertConditionalToTableFactor);
+#endif
+      dfg.push_back(tdc);
     } else {
       throwRuntimeError("discreteElimination", f);
     }
   }
 
+#if GTSAM_HYBRID_TIMING
+  gttic_(EliminateDiscrete);
+#endif
   // NOTE: This does sum-product. For max-product, use EliminateForMPE.
   auto result = EliminateDiscrete(dfg, frontalKeys);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(EliminateDiscrete);
+#endif
 
   return {std::make_shared<HybridConditional>(result.first), result.second};
 }
@@ -319,8 +333,19 @@ static std::shared_ptr<Factor> createDiscreteFactor(
     }
   };
 
+#if GTSAM_HYBRID_TIMING
+  gttic_(DiscreteBoundaryErrors);
+#endif
   AlgebraicDecisionTree<Key> errors(eliminationResults, calculateError);
-  return DiscreteFactorFromErrors(discreteSeparator, errors);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(DiscreteBoundaryErrors);
+  gttic_(DiscreteBoundaryResult);
+#endif
+  auto result = DiscreteFactorFromErrors(discreteSeparator, errors);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(DiscreteBoundaryResult);
+#endif
+  return result;
 }
 
 /* *******************************************************************************/
@@ -360,12 +385,18 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
   // the discrete separator will be *all* the discrete keys.
   DiscreteKeys discreteSeparator = GetDiscreteKeys(*this);
 
+#if GTSAM_HYBRID_TIMING
+  gttic_(HybridCollectProductFactor);
+#endif
   // Collect all the factors to create a set of Gaussian factor graphs in a
   // decision tree indexed by all discrete keys involved. Just like any hybrid
   // factor, every assignment also has a scalar error, in this case the sum of
   // all errors in the graph. This error is assignment-specific and accounts for
   // any difference in noise models used.
   HybridGaussianProductFactor productFactor = collectProductFactor();
+#if GTSAM_HYBRID_TIMING
+  gttoc_(HybridCollectProductFactor);
+#endif
 
   // Check if a factor is null
   auto isNull = [](const GaussianFactor::shared_ptr &ptr) { return !ptr; };
@@ -393,8 +424,14 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
     return {conditional, conditional->negLogConstant(), factor, scalar};
   };
 
+#if GTSAM_HYBRID_TIMING
+  gttic_(HybridEliminate);
+#endif
   // Perform elimination!
   const ResultTree eliminationResults(productFactor, eliminate);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(HybridEliminate);
+#endif
 
   // If there are no more continuous parents we create a DiscreteFactor with the
   // error for each discrete choice. Otherwise, create a HybridGaussianFactor
diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp
index 28116df45..f99d95c01 100644
--- a/gtsam/hybrid/HybridGaussianISAM.cpp
+++ b/gtsam/hybrid/HybridGaussianISAM.cpp
@@ -104,7 +104,13 @@ void HybridGaussianISAM::updateInternal(
       elimination_ordering, function, std::cref(index));
 
   if (maxNrLeaves) {
+#if GTSAM_HYBRID_TIMING
+    gttic_(HybridBayesTreePrune);
+#endif
     bayesTree->prune(*maxNrLeaves);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(HybridBayesTreePrune);
+#endif
   }
 
   // Re-add into Bayes tree data structures

From 4d96af76e01f1b490c8cb2aea08a1323b1924659 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 Dec 2024 13:45:29 -0500
Subject: [PATCH 181/362] update config.h.in

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

diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index db6dd2b34..5d63624e7 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -44,7 +44,7 @@
 
 // Whether to enable timing in hybrid factor graph machinery
 // #cmakedefine01 GTSAM_HYBRID_TIMING
-#define GTSAM_HYBRID_TIMING
+#define GTSAM_HYBRID_TIMING 1
 
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB

From b7b48ba5302a7045bd9677f957d0977790542ddd Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 29 Dec 2024 11:06:22 -0800
Subject: [PATCH 182/362] Fix lingering boost flags

---
 gtsam/base/concepts.h                         |  2 +-
 gtsam/base/tests/testMatrix.cpp               |  4 ++--
 gtsam/base/tests/testVector.cpp               |  2 +-
 gtsam/base/timing.cpp                         | 22 +++++++++----------
 gtsam/base/timing.h                           |  8 +++----
 .../nonlinear/LevenbergMarquardtOptimizer.cpp |  6 ++---
 6 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h
index c9f76ca9f..6ff1a44d4 100644
--- a/gtsam/base/concepts.h
+++ b/gtsam/base/concepts.h
@@ -8,7 +8,7 @@
 
 #pragma once
 
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #include <boost/concept_check.hpp>
 #include <boost/concept/assert.hpp>
 #include <boost/concept/requires.hpp>
diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp
index 3fbf11746..4c8808722 100644
--- a/gtsam/base/tests/testMatrix.cpp
+++ b/gtsam/base/tests/testMatrix.cpp
@@ -1151,7 +1151,7 @@ TEST(Matrix, Matrix24IsVectorSpace) {
 }
 
 TEST(Matrix, RowMajorIsVectorSpace) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
   GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
 #endif
@@ -1166,7 +1166,7 @@ TEST(Matrix, VectorIsVectorSpace) {
 }
 
 TEST(Matrix, RowVectorIsVectorSpace) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   typedef Eigen::Matrix<double, 1, -1> RowVector;
   GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
   GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector5>);
diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp
index d95b14292..46d44bb80 100644
--- a/gtsam/base/tests/testVector.cpp
+++ b/gtsam/base/tests/testVector.cpp
@@ -272,7 +272,7 @@ TEST(Vector, VectorIsVectorSpace) {
 }
 
 TEST(Vector, RowVectorIsVectorSpace) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   typedef Eigen::Matrix<double,1,-1> RowVector;
   GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
 #endif
diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index b43595066..07fb7b61b 100644
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@@ -57,7 +57,7 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) {
 TimingOutline::TimingOutline(const std::string& label, size_t id) :
     id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
         0), lastChildOrder_(0), label_(label) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
   timer_.stop();
 #endif
@@ -66,7 +66,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) :
 
 /* ************************************************************************* */
 size_t TimingOutline::time() const {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   size_t time = 0;
   bool hasChildren = false;
   for(const ChildMap::value_type& child: children_) {
@@ -84,7 +84,7 @@ size_t TimingOutline::time() const {
 
 /* ************************************************************************* */
 void TimingOutline::print(const std::string& outline) const {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   std::string formattedLabel = label_;
   std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' ');
   std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU ("
@@ -108,7 +108,7 @@ void TimingOutline::print(const std::string& outline) const {
 
 void TimingOutline::print2(const std::string& outline,
     const double parentTotal) const {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2;
   const double selfTotal = self(), selfMean = selfTotal / double(n_);
   const double childTotal = secs();
@@ -153,7 +153,7 @@ void TimingOutline::print2(const std::string& outline,
 /* ************************************************************************* */
 const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
     const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   assert(thisPtr.lock().get() == this);
   std::shared_ptr<TimingOutline>& result = children_[child];
   if (!result) {
@@ -172,7 +172,7 @@ const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
 /* ************************************************************************* */
 void TimingOutline::tic() {
 // Disable this entire function if we are not using boost
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
   assert(timer_.is_stopped());
   timer_.start();
@@ -191,7 +191,7 @@ void TimingOutline::tic() {
 /* ************************************************************************* */
 void TimingOutline::toc() {
 // Disable this entire function if we are not using boost
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
 
@@ -225,7 +225,7 @@ void TimingOutline::toc() {
 
 /* ************************************************************************* */
 void TimingOutline::finishedIteration() {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   if (tIt_ > tMax_)
     tMax_ = tIt_;
   if (tMin_ == 0 || tIt_ < tMin_)
@@ -240,7 +240,7 @@ void TimingOutline::finishedIteration() {
 /* ************************************************************************* */
 size_t getTicTocID(const char *descriptionC) {
 // disable anything which refers to TimingOutline as well, for good measure
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   const std::string description(descriptionC);
   // Global (static) map from strings to ID numbers and current next ID number
   static size_t nextId = 0;
@@ -263,7 +263,7 @@ size_t getTicTocID(const char *descriptionC) {
 /* ************************************************************************* */
 void tic(size_t id, const char *labelC) {
 // disable anything which refers to TimingOutline as well, for good measure
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   const std::string label(labelC);
   std::shared_ptr<TimingOutline> node = //
       gCurrentTimer.lock()->child(id, label, gCurrentTimer);
@@ -275,7 +275,7 @@ void tic(size_t id, const char *labelC) {
 /* ************************************************************************* */
 void toc(size_t id, const char *labelC) {
 // disable anything which refers to TimingOutline as well, for good measure
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   const std::string label(labelC);
   std::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
   if (id != current->id_) {
diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h
index 99c55a3d7..cc38f32b8 100644
--- a/gtsam/base/timing.h
+++ b/gtsam/base/timing.h
@@ -21,7 +21,7 @@
 #include <gtsam/dllexport.h>
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #include <boost/version.hpp>
 #endif
 
@@ -107,7 +107,7 @@
 //   have matching gttic/gttoc statments.  You may want to consider reorganizing your timing
 //   outline to match the scope of your code.
 
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 // Automatically use the new Boost timers if version is recent enough.
 #if BOOST_VERSION >= 104800
 #  ifndef GTSAM_DISABLE_NEW_TIMERS
@@ -165,7 +165,7 @@ namespace gtsam {
       ChildMap children_; ///< subtrees
 
 // disable all timers if not using boost
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
       boost::timer::cpu_timer timer_;
 #else
@@ -183,7 +183,7 @@ namespace gtsam {
       GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId);
       GTSAM_EXPORT size_t time() const; ///< time taken, including children
       double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
       double self() const { return double(t_)     / 1000000.0;} ///< self time only, in seconds
       double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds
       double min()  const { return double(tMin_)  / 1000000.0;} ///< min time, in seconds
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 99682fb77..f6711f0f0 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -26,7 +26,7 @@
 #include <gtsam/linear/linearExceptions.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/Vector.h>
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #include <gtsam/base/timing.h>
 #endif
 
@@ -123,7 +123,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
   auto currentState = static_cast<const State*>(state_.get());
   bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA);
 
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
   boost::timer::cpu_timer lamda_iteration_timer;
   lamda_iteration_timer.start();
@@ -222,7 +222,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
   } // if (systemSolvedSuccessfully)
 
   if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
 // do timing
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
     double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall;

From 846c29fa2edd91045c28c9f7384db08ce1c38f28 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 29 Dec 2024 10:46:20 -0800
Subject: [PATCH 183/362] Fix deperecated copies and redundant moves

---
 gtsam/base/ConcurrentMap.h               | 2 ++
 gtsam/base/FastList.h                    | 2 ++
 gtsam/base/FastMap.h                     | 2 ++
 gtsam/base/FastSet.h                     | 2 ++
 gtsam/base/GenericValue.h                | 9 +++++----
 gtsam/base/Value.h                       | 3 +++
 gtsam/discrete/SignatureParser.cpp       | 4 ++--
 gtsam/geometry/Pose2.h                   | 5 ++++-
 gtsam/geometry/Pose3.h                   | 7 ++++---
 gtsam/geometry/Rot2.h                    | 5 ++++-
 gtsam/linear/ConjugateGradientSolver.h   | 2 ++
 gtsam/linear/HessianFactor.cpp           | 2 +-
 gtsam/linear/JacobianFactor.h            | 2 ++
 gtsam/linear/NoiseModel.cpp              | 2 +-
 gtsam/linear/VectorValues.h              | 2 ++
 gtsam/nonlinear/ExpressionFactor.h       | 2 +-
 gtsam/slam/FrobeniusFactor.cpp           | 2 +-
 gtsam_unstable/geometry/Pose3Upright.h   | 1 +
 gtsam_unstable/slam/Mechanization_bRn2.h | 4 +---
 19 files changed, 42 insertions(+), 18 deletions(-)

diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h
index 03687f562..8ce44dda3 100644
--- a/gtsam/base/ConcurrentMap.h
+++ b/gtsam/base/ConcurrentMap.h
@@ -85,6 +85,8 @@ public:
   /** Copy constructor from the base map class */
   ConcurrentMap(const Base& x) : Base(x) {}
 
+  ConcurrentMap& operator=(const ConcurrentMap& other) = default;
+
   /** Handy 'exists' function */
   bool exists(const KEY& e) const { return this->count(e); }
 
diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h
index 1a3b9ed3f..821575ca9 100644
--- a/gtsam/base/FastList.h
+++ b/gtsam/base/FastList.h
@@ -62,6 +62,8 @@ public:
   /// Construct from c++11 initializer list:
   FastList(std::initializer_list<VALUE> l) : Base(l) {}
 
+  FastList& operator=(const FastList& other) = default;
+
 #ifdef GTSAM_ALLOCATOR_BOOSTPOOL
   /** Copy constructor from a standard STL container */
   FastList(const std::list<VALUE>& x) {
diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h
index 75998fd2f..30052908d 100644
--- a/gtsam/base/FastMap.h
+++ b/gtsam/base/FastMap.h
@@ -54,6 +54,8 @@ public:
   /** Copy constructor from another FastMap */
   FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
 
+  FastMap& operator=(const FastMap<KEY,VALUE>& x) = default;
+
   /** Copy constructor from the base map class */
   FastMap(const Base& x) : Base(x) {}
 
diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index 9a2f62497..1a2627e24 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -80,6 +80,8 @@ public:
   Base(x) {
   }
 
+  FastSet& operator=(const FastSet& other) = default;
+
 #ifdef GTSAM_ALLOCATOR_BOOSTPOOL
   /** Copy constructor from a standard STL container */
   FastSet(const std::set<VALUE>& x) {
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 62ea8ec39..396ca16ba 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -56,9 +56,10 @@ public:
   GenericValue(){}
 
   /// Construct from value
-  GenericValue(const T& value) :
-      value_(value) {
-  }
+  GenericValue(const T& value) : Value(),
+      value_(value) {}
+
+  GenericValue(const GenericValue& other) = default;
 
   /// Return a constant value
   const T& value() const {
@@ -112,7 +113,7 @@ public:
      * Clone this value (normal clone on the heap, delete with 'delete' operator)
      */
     std::shared_ptr<Value> clone() const override {
-		return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
+      return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
     }
 
     /// Generic Value interface version of retract
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 09a3f4878..e80b9d4a4 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -38,6 +38,9 @@ namespace gtsam {
    */
   class GTSAM_EXPORT Value {
   public:
+    // todo - not sure if valid
+    Value() = default;
+    Value(const Value& other) = default;
 
     /** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */
     virtual Value* clone_() const = 0;
diff --git a/gtsam/discrete/SignatureParser.cpp b/gtsam/discrete/SignatureParser.cpp
index 31e425392..8e0221f01 100644
--- a/gtsam/discrete/SignatureParser.cpp
+++ b/gtsam/discrete/SignatureParser.cpp
@@ -38,7 +38,7 @@ std::optional<Row> static ParseConditional(const std::string& token) {
   } catch (...) {
     return std::nullopt;
   }
-  return std::move(row);
+  return row;
 }
 
 std::optional<Table> static ParseConditionalTable(
@@ -62,7 +62,7 @@ std::optional<Table> static ParseConditionalTable(
       }
     }
   }
-  return std::move(table);
+  return table;
 }
 
 std::vector<std::string> static Tokenize(const std::string& str) {
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 3f85a7f42..da7bc922e 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -60,7 +60,10 @@ public:
   }
 
   /** copy constructor */
-  Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
+  Pose2(const Pose2& pose) = default;
+  //  : r_(pose.r_), t_(pose.t_) {}
+
+  Pose2& operator=(const Pose2& other) = default;
 
   /**
    * construct from (x,y,theta)
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 5805ce41b..54bdd7f4c 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -55,9 +55,10 @@ public:
   Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
 
   /** Copy constructor */
-  Pose3(const Pose3& pose) :
-      R_(pose.R_), t_(pose.t_) {
-  }
+  Pose3(const Pose3& pose) = default;
+  //  :
+  //     R_(pose.R_), t_(pose.t_) {
+  // }
 
   /** Construct from R,t */
   Pose3(const Rot3& R, const Point3& t) :
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 07f328b96..7a6f25c4d 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -52,11 +52,14 @@ namespace gtsam {
     Rot2() : c_(1.0), s_(0.0) {}
     
     /** copy constructor */
-    Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
+    Rot2(const Rot2& r) = default;
+    //  : Rot2(r.c_, r.s_) {}
 
     /// Constructor from angle in radians == exponential map at identity
     Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
 
+    // Rot2& operator=(const gtsam::Rot2& other) = default;
+
     /// Named constructor from angle in radians
     static Rot2 fromAngle(double theta) {
       return Rot2(theta);
diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h
index 46d49ca4f..aaec039e9 100644
--- a/gtsam/linear/ConjugateGradientSolver.h
+++ b/gtsam/linear/ConjugateGradientSolver.h
@@ -69,6 +69,8 @@ struct GTSAM_EXPORT ConjugateGradientParameters
         epsilon_abs(p.epsilon_abs),
         blas_kernel(GTSAM) {}
 
+  ConjugateGradientParameters& operator=(const ConjugateGradientParameters& other) = default;
+
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
   inline size_t getMinIterations() const { return minIterations; }
   inline size_t getMaxIterations() const { return maxIterations; }
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 4e8ef804c..1172dc281 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -379,7 +379,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const {
   shared_ptr result = std::make_shared<This>(*this);
   // Negate the information matrix of the result
   result->info_.negate();
-  return std::move(result);
+  return result;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 4a5b6d5d1..a9933374f 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -117,6 +117,8 @@ namespace gtsam {
     /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
     explicit JacobianFactor(const HessianFactor& hf);
 
+    JacobianFactor& operator=(const JacobianFactor& jf) = default;
+
     /** default constructor for I/O */
     JacobianFactor();
 
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 39666b46b..2f693bbc4 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -64,7 +64,7 @@ std::optional<Vector> checkIfDiagonal(const Matrix& M) {
     Vector diagonal(n);
     for (j = 0; j < n; j++)
       diagonal(j) = M(j, j);
-    return std::move(diagonal);
+    return diagonal;
   }
 }
 
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index e60209a83..4728706c5 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -119,6 +119,8 @@ namespace gtsam {
     /// Constructor from Vector, with Scatter
     VectorValues(const Vector& c, const Scatter& scatter);
 
+    VectorValues& operator=(const VectorValues& other) = default;
+
     /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
     static VectorValues Zero(const VectorValues& other);
 
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 4f6648849..df89f5b44 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -149,7 +149,7 @@ protected:
       noiseModel_->WhitenSystem(Ab.matrix(), b);
     }
 
-    return std::move(factor);
+    return factor;
   }
 
   /// @return a deep copy of this factor
diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp
index e70f64d96..982430d81 100644
--- a/gtsam/slam/FrobeniusFactor.cpp
+++ b/gtsam/slam/FrobeniusFactor.cpp
@@ -61,7 +61,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
     return noiseModel::Robust::Create(
         noiseModel::mEstimator::Huber::Create(1.345), isoModel);
   } else {
-    return std::move(isoModel);
+    return isoModel;
   }
 }
 
diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h
index 20e0fde5d..464f46928 100644
--- a/gtsam_unstable/geometry/Pose3Upright.h
+++ b/gtsam_unstable/geometry/Pose3Upright.h
@@ -43,6 +43,7 @@ public:
   Pose3Upright(const Rot2& bearing, const Point3& t);
   Pose3Upright(double x, double y, double z, double theta);
   Pose3Upright(const Pose2& pose, double z);
+  Pose3Upright& operator=(const Pose3Upright& x) = default;
 
   /// Down-converts from a full Pose3
   Pose3Upright(const Pose3& fullpose);
diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h
index 4714f2b6f..3d91f6b0e 100644
--- a/gtsam_unstable/slam/Mechanization_bRn2.h
+++ b/gtsam_unstable/slam/Mechanization_bRn2.h
@@ -35,9 +35,7 @@ public:
   }
 
   /// Copy constructor
-  Mechanization_bRn2(const Mechanization_bRn2& other) :
-      bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) {
-  }
+  Mechanization_bRn2(const Mechanization_bRn2& other) = default;
 
   /// gravity in the body frame
   Vector3 b_g(double g_e) const {

From 02d461e35920f9379915be3fd0d885c8fdbda63a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 Dec 2024 22:49:58 -0500
Subject: [PATCH 184/362] make a cmake flag

---
 cmake/HandleGeneralOptions.cmake | 1 +
 gtsam/config.h.in                | 3 +--
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
index 0266cf3f0..43659718b 100644
--- a/cmake/HandleGeneralOptions.cmake
+++ b/cmake/HandleGeneralOptions.cmake
@@ -33,6 +33,7 @@ option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP
 option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
 option(GTSAM_DT_MERGING                     "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON)
 option(GTSAM_ENABLE_TIMING                  "Enable the timing tools (gttic/gttoc)" OFF)
+option(GTSAM_HYBRID_TIMING                  "Enable the timing of hybrid factor graph machinery" OFF)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS      "Enable/Disable expensive consistency checks" OFF)
 option(GTSAM_ENABLE_MEMORY_SANITIZER        "Enable/Disable memory sanitizer" OFF)
 option(GTSAM_WITH_TBB                       "Use Intel Threaded Building Blocks (TBB) if available" ON)
diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 5d63624e7..58b93ee1c 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -43,8 +43,7 @@
 #cmakedefine GTSAM_DT_MERGING
 
 // Whether to enable timing in hybrid factor graph machinery
-// #cmakedefine01 GTSAM_HYBRID_TIMING
-#define GTSAM_HYBRID_TIMING 1
+#cmakedefine01 GTSAM_HYBRID_TIMING
 
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB

From 34fba6823af8290984054d3c2f55f27cfe1eae8a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 Dec 2024 14:29:16 -0500
Subject: [PATCH 185/362] use TableFactor instead of DecisionTreeFactor in
 discrete elimination

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 44 +++++++++++++++++---------
 gtsam/discrete/DiscreteFactorGraph.h   |  3 +-
 2 files changed, 31 insertions(+), 16 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 2037dd951..68892b1a4 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -64,10 +64,18 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor DiscreteFactorGraph::product() const {
-    DecisionTreeFactor result;
+  TableFactor DiscreteFactorGraph::product() const {
+    TableFactor result;
     for (const sharedFactor& factor : *this) {
-      if (factor) result = (*factor) * result;
+      if (factor) {
+        if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
+          result = result * (*f);
+        }
+        else if (auto dtf =
+                       std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
+          result = TableFactor(result * (*dtf));
+        }
+      }
     }
     return result;
   }
@@ -116,15 +124,14 @@ namespace gtsam {
    * product to prevent underflow.
    *
    * @param factors The factors to multiply as a DiscreteFactorGraph.
-   * @return DecisionTreeFactor
+   * @return TableFactor
    */
-  static DecisionTreeFactor ProductAndNormalize(
-      const DiscreteFactorGraph& factors) {
+  static TableFactor ProductAndNormalize(const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
 #if GTSAM_HYBRID_TIMING
     gttic_(DiscreteProduct);
 #endif
-    DecisionTreeFactor product = factors.product();
+    TableFactor product = factors.product();
 #if GTSAM_HYBRID_TIMING
     gttoc_(DiscreteProduct);
 #endif
@@ -149,11 +156,11 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DecisionTreeFactor product = ProductAndNormalize(factors);
+    TableFactor product = ProductAndNormalize(factors);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
-    DecisionTreeFactor::shared_ptr max = product.max(frontalKeys);
+    TableFactor::shared_ptr max = product.max(frontalKeys);
     gttoc(max);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
@@ -166,8 +173,8 @@ namespace gtsam {
     // Make lookup with product
     gttic(lookup);
     size_t nrFrontals = frontalKeys.size();
-    auto lookup =
-        std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
+    auto lookup = std::make_shared<DiscreteLookupTable>(
+        nrFrontals, orderedKeys, product.toDecisionTreeFactor());
     gttoc(lookup);
 
     return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
@@ -227,13 +234,13 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DecisionTreeFactor product = ProductAndNormalize(factors);
+    TableFactor product = ProductAndNormalize(factors);
 
     // sum out frontals, this is the factor on the separator
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteSum);
 #endif
-    DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
+    TableFactor::shared_ptr sum = product.sum(frontalKeys);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteSum);
 #endif
@@ -246,11 +253,18 @@ namespace gtsam {
                        sum->keys().end());
 
     // now divide product/sum to get conditional
+#if GTSAM_HYBRID_TIMING
+    gttic_(EliminateDiscreteDivide);
+#endif
+    auto c = product / (*sum);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(EliminateDiscreteDivide);
+#endif
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteToDiscreteConditional);
 #endif
-    auto conditional =
-        std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
+    auto conditional = std::make_shared<DiscreteConditional>(
+        orderedKeys.size(), c.toDecisionTreeFactor());
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteToDiscreteConditional);
 #endif
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index c57d2258c..f1575cd7e 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -25,6 +25,7 @@
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/FastSet.h>
+#include <gtsam/discrete/TableFactor.h>
 
 #include <string>
 #include <utility>
@@ -147,7 +148,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
   DiscreteKeys discreteKeys() const;
 
   /** return product of all factors as a single factor */
-  DecisionTreeFactor product() const;
+  TableFactor product() const;
 
   /** 
    * Evaluates the factor graph given values, returns the joint probability of

From de652eafc268d4780f3e850f7b2c5898fd3973a3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 Dec 2024 14:38:54 -0500
Subject: [PATCH 186/362] initial DiscreteTableConditional

---
 gtsam/discrete/DiscreteTableConditional.cpp | 181 ++++++++++++++++
 gtsam/discrete/DiscreteTableConditional.h   | 224 ++++++++++++++++++++
 2 files changed, 405 insertions(+)
 create mode 100644 gtsam/discrete/DiscreteTableConditional.cpp
 create mode 100644 gtsam/discrete/DiscreteTableConditional.h

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
new file mode 100644
index 000000000..b09e2738f
--- /dev/null
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -0,0 +1,181 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 DiscreteTableConditional.cpp
+ *  @date Dec 22, 2024
+ *  @author Varun Agrawal
+ */
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
+#include <gtsam/discrete/Ring.h>
+#include <gtsam/discrete/Signature.h>
+#include <gtsam/hybrid/HybridValues.h>
+
+#include <algorithm>
+#include <cassert>
+#include <random>
+#include <set>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+using namespace std;
+using std::pair;
+using std::stringstream;
+using std::vector;
+namespace gtsam {
+
+/* ************************************************************************** */
+DiscreteTableConditional::DiscreteTableConditional(const size_t nrFrontals,
+                                                   const TableFactor& f)
+    : BaseConditional(nrFrontals, DecisionTreeFactor(f.discreteKeys(), ADT())),
+      sparse_table_((f / (*f.sum(nrFrontals))).sparseTable()) {
+  // sparse_table_ = sparse_table_.prune();
+}
+
+/* ************************************************************************** */
+DiscreteTableConditional::DiscreteTableConditional(
+    size_t nrFrontals, const DiscreteKeys& keys,
+    const Eigen::SparseVector<double>& potentials)
+    : BaseConditional(nrFrontals, keys, DecisionTreeFactor(keys, ADT())),
+      sparse_table_(potentials) {}
+
+/* ************************************************************************** */
+DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
+                                                   const TableFactor& marginal)
+    : BaseConditional(joint.size() - marginal.size(),
+                      joint.discreteKeys() & marginal.discreteKeys(), ADT()),
+      sparse_table_((joint / marginal).sparseTable()) {}
+
+/* ************************************************************************** */
+DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
+                                                   const TableFactor& marginal,
+                                                   const Ordering& orderedKeys)
+    : DiscreteTableConditional(joint, marginal) {
+  keys_.clear();
+  keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
+}
+
+/* ************************************************************************** */
+DiscreteTableConditional::DiscreteTableConditional(const Signature& signature)
+    : BaseConditional(1, DecisionTreeFactor()),
+      sparse_table_(TableFactor(signature.discreteKeys(), signature.cpt())
+                        .sparseTable()) {}
+
+/* ************************************************************************** */
+DiscreteTableConditional DiscreteTableConditional::operator*(
+    const DiscreteTableConditional& other) const {
+  // Take union of frontal keys
+  std::set<Key> newFrontals;
+  for (auto&& key : this->frontals()) newFrontals.insert(key);
+  for (auto&& key : other.frontals()) newFrontals.insert(key);
+
+  // Check if frontals overlapped
+  if (nrFrontals() + other.nrFrontals() > newFrontals.size())
+    throw std::invalid_argument(
+        "DiscreteTableConditional::operator* called with overlapping frontal "
+        "keys.");
+
+  // Now, add cardinalities.
+  DiscreteKeys discreteKeys;
+  for (auto&& key : frontals())
+    discreteKeys.emplace_back(key, cardinality(key));
+  for (auto&& key : other.frontals())
+    discreteKeys.emplace_back(key, other.cardinality(key));
+
+  // Sort
+  std::sort(discreteKeys.begin(), discreteKeys.end());
+
+  // Add parents to set, to make them unique
+  std::set<DiscreteKey> parents;
+  for (auto&& key : this->parents())
+    if (!newFrontals.count(key)) parents.emplace(key, cardinality(key));
+  for (auto&& key : other.parents())
+    if (!newFrontals.count(key)) parents.emplace(key, other.cardinality(key));
+
+  // Finally, add parents to keys, in order
+  for (auto&& dk : parents) discreteKeys.push_back(dk);
+
+  TableFactor a(this->discreteKeys(), this->sparse_table_),
+      b(other.discreteKeys(), other.sparse_table_);
+  TableFactor product = a * other;
+  return DiscreteTableConditional(newFrontals.size(), product);
+}
+
+/* ************************************************************************** */
+void DiscreteTableConditional::print(const string& s,
+                                     const KeyFormatter& formatter) const {
+  cout << s << " P( ";
+  for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
+    cout << formatter(*it) << " ";
+  }
+  if (nrParents()) {
+    cout << "| ";
+    for (const_iterator it = beginParents(); it != endParents(); ++it) {
+      cout << formatter(*it) << " ";
+    }
+  }
+  cout << "):\n";
+  // BaseFactor::print("", formatter);
+  cout << endl;
+}
+
+/* ************************************************************************** */
+bool DiscreteTableConditional::equals(const DiscreteFactor& other,
+                                      double tol) const {
+  if (!dynamic_cast<const DiscreteConditional*>(&other)) {
+    return false;
+  } else {
+    const DiscreteConditional& f(
+        static_cast<const DiscreteConditional&>(other));
+    return DiscreteConditional::equals(f, tol);
+  }
+}
+
+/* ************************************************************************** */
+TableFactor::shared_ptr DiscreteTableConditional::likelihood(
+    const DiscreteValues& frontalValues) const {
+  throw std::runtime_error("Likelihood not implemented");
+}
+
+/* ****************************************************************************/
+TableFactor::shared_ptr DiscreteTableConditional::likelihood(
+    size_t frontal) const {
+  throw std::runtime_error("Likelihood not implemented");
+}
+
+/* ************************************************************************** */
+size_t DiscreteTableConditional::argmax(
+    const DiscreteValues& parentsValues) const {
+  // Initialize
+  size_t maxValue = 0;
+  double maxP = 0;
+  DiscreteValues values = parentsValues;
+
+  assert(nrFrontals() == 1);
+  Key j = firstFrontalKey();
+  for (size_t value = 0; value < cardinality(j); value++) {
+    values[j] = value;
+    double pValueS = (*this)(values);
+    // Update MPE solution if better
+    if (pValueS > maxP) {
+      maxP = pValueS;
+      maxValue = value;
+    }
+  }
+  return maxValue;
+}
+
+}  // namespace gtsam
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
new file mode 100644
index 000000000..28e35277d
--- /dev/null
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -0,0 +1,224 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 DiscreteTableConditional.h
+ *  @date Dec 22, 2024
+ *  @author Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/Signature.h>
+#include <gtsam/discrete/TableFactor.h>
+#include <gtsam/inference/Conditional-inst.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace gtsam {
+
+/**
+ * Discrete Conditional Density which uses a SparseTable as the internal
+ * representation, similar to the TableFactor.
+ *
+ * @ingroup discrete
+ */
+class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
+  Eigen::SparseVector<double> sparse_table_;
+
+ public:
+  // typedefs needed to play nice with gtsam
+  typedef DiscreteTableConditional This;     ///< Typedef to this class
+  typedef std::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
+  typedef DiscreteConditional
+      BaseConditional;  ///< Typedef to our conditional base class
+
+  using Values = DiscreteValues;  ///< backwards compatibility
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// Default constructor needed for serialization.
+  DiscreteTableConditional() {}
+
+  /// Construct from factor, taking the first `nFrontals` keys as frontals.
+  DiscreteTableConditional(size_t nFrontals, const TableFactor& f);
+
+  /**
+   * Construct from DiscreteKeys and SparseVector, taking the first
+   * `nFrontals` keys as frontals, in the order given.
+   */
+  DiscreteTableConditional(size_t nFrontals, const DiscreteKeys& keys,
+                           const Eigen::SparseVector<double>& potentials);
+
+  /** Construct from signature */
+  explicit DiscreteTableConditional(const Signature& signature);
+
+  /**
+   * Construct from key, parents, and a Signature::Table specifying the
+   * conditional probability table (CPT) in 00 01 10 11 order. For
+   * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
+   *
+   * Example: DiscreteTableConditional P(D, {B,E}, table);
+   */
+  DiscreteTableConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+                           const Signature::Table& table)
+      : DiscreteTableConditional(Signature(key, parents, table)) {}
+
+  /**
+   * Construct from key, parents, and a vector<double> specifying the
+   * conditional probability table (CPT) in 00 01 10 11 order. For
+   * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
+   *
+   * Example: DiscreteTableConditional P(D, {B,E}, table);
+   */
+  DiscreteTableConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+                           const std::vector<double>& table)
+      : DiscreteTableConditional(
+            1, TableFactor(DiscreteKeys{key} & parents, table)) {}
+
+  /**
+   * Construct from key, parents, and a string specifying the conditional
+   * probability table (CPT) in 00 01 10 11 order. For three-valued, it would
+   * be 00 01 02 10 11 12 20 21 22, etc....
+   *
+   * The string is parsed into a Signature::Table.
+   *
+   * Example: DiscreteTableConditional P(D, {B,E}, "9/1 2/8 3/7 1/9");
+   */
+  DiscreteTableConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+                           const std::string& spec)
+      : DiscreteTableConditional(Signature(key, parents, spec)) {}
+
+  /// No-parent specialization; can also use DiscreteDistribution.
+  DiscreteTableConditional(const DiscreteKey& key, const std::string& spec)
+      : DiscreteTableConditional(Signature(key, {}, spec)) {}
+
+  /**
+   * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
+   * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
+   */
+  DiscreteTableConditional(const TableFactor& joint,
+                           const TableFactor& marginal);
+
+  /**
+   * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
+   * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
+   * Makes sure the keys are ordered as given. Does not check orderedKeys.
+   */
+  DiscreteTableConditional(const TableFactor& joint,
+                           const TableFactor& marginal,
+                           const Ordering& orderedKeys);
+
+  /**
+   * @brief Combine two conditionals, yielding a new conditional with the union
+   * of the frontal keys, ordered by gtsam::Key.
+   *
+   * The two conditionals must make a valid Bayes net fragment, i.e.,
+   * the frontal variables cannot overlap, and must be acyclic:
+   * Example of correct use:
+   *   P(A,B) = P(A|B) * P(B)
+   *   P(A,B|C) = P(A|B) * P(B|C)
+   *   P(A,B,C) = P(A,B|C) * P(C)
+   * Example of incorrect use:
+   *   P(A|B) * P(A|C) = ?
+   *   P(A|B) * P(B|A) = ?
+   * We check for overlapping frontals, but do *not* check for cyclic.
+   */
+  DiscreteTableConditional operator*(
+      const DiscreteTableConditional& other) const;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// GTSAM-style print
+  void print(
+      const std::string& s = "Discrete Conditional: ",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+
+  /// GTSAM-style equals
+  bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// Log-probability is just -error(x).
+  double logProbability(const DiscreteValues& x) const { return -error(x); }
+
+  /// print index signature only
+  void printSignature(
+      const std::string& s = "Discrete Conditional: ",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const {
+    static_cast<const BaseConditional*>(this)->print(s, formatter);
+  }
+
+  /** Convert to a likelihood factor by providing value before bar. */
+  TableFactor::shared_ptr likelihood(const DiscreteValues& frontalValues) const;
+
+  /** Single variable version of likelihood. */
+  TableFactor::shared_ptr likelihood(size_t frontal) const;
+
+  /**
+   * @brief Return assignment for single frontal variable that maximizes value.
+   * @param parentsValues Known assignments for the parents.
+   * @return maximizing assignment for the frontal variable.
+   */
+  size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+  /// Return all assignments for frontal variables.
+  std::vector<DiscreteValues> frontalAssignments() const;
+
+  /// Return all assignments for frontal *and* parent variables.
+  std::vector<DiscreteValues> allAssignments() const;
+
+  /// @}
+  /// @name HybridValues methods.
+  /// @{
+
+  using BaseConditional::operator();  ///< HybridValues version
+
+  /**
+   * Calculate log-probability log(evaluate(x)) for HybridValues `x`.
+   * This is actually just -error(x).
+   */
+  double logProbability(const HybridValues& x) const override {
+    return -error(x);
+  }
+
+  /// @}
+
+ private:
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
+  }
+#endif
+};
+// DiscreteTableConditional
+
+// traits
+template <>
+struct traits<DiscreteTableConditional>
+    : public Testable<DiscreteTableConditional> {};
+
+}  // namespace gtsam

From b57e4482322a94ff6db9d9d4df7bf281f8f541aa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 Dec 2024 22:55:17 -0500
Subject: [PATCH 187/362] DiscreteConditional evaluate method for conditionals

---
 gtsam/discrete/DiscreteConditional.cpp | 7 ++++++-
 gtsam/discrete/DiscreteConditional.h   | 3 +++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index eeb5dca3f..7db602795 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -24,13 +24,13 @@
 #include <gtsam/hybrid/HybridValues.h>
 
 #include <algorithm>
+#include <cassert>
 #include <random>
 #include <set>
 #include <stdexcept>
 #include <string>
 #include <utility>
 #include <vector>
-#include <cassert>
 
 using namespace std;
 using std::pair;
@@ -478,6 +478,11 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
   return this->evaluate(x.discrete());
 }
 
+/* ************************************************************************* */
+double DiscreteConditional::evaluate(const Assignment<Key>& values) const {
+  return BaseFactor::evaluate(values);
+}
+
 /* ************************************************************************* */
 double DiscreteConditional::negLogConstant() const { return 0.0; }
 
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 3ec9ae590..c44a59577 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -249,6 +249,9 @@ class GTSAM_EXPORT DiscreteConditional
    */
   double evaluate(const HybridValues& x) const override;
 
+  /// Evaluate the conditional given values.
+  virtual double evaluate(const Assignment<Key>& values) const override;
+
   using BaseConditional::operator();  ///< HybridValues version
 
   /**

From d18f23c47b89a873bd2e40f425b0c5609b6c2120 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 Dec 2024 23:02:26 -0500
Subject: [PATCH 188/362] setData method

---
 gtsam/discrete/DiscreteConditional.cpp | 5 +++++
 gtsam/discrete/DiscreteConditional.h   | 3 +++
 2 files changed, 8 insertions(+)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 7db602795..78738dd54 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -483,6 +483,11 @@ double DiscreteConditional::evaluate(const Assignment<Key>& values) const {
   return BaseFactor::evaluate(values);
 }
 
+/* ************************************************************************* */
+double DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
+  this->root_ = dc->root_;
+}
+
 /* ************************************************************************* */
 double DiscreteConditional::negLogConstant() const { return 0.0; }
 
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index c44a59577..318024faa 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -270,6 +270,9 @@ class GTSAM_EXPORT DiscreteConditional
    */
   double negLogConstant() const override;
 
+  /// Set the data from another DiscreteConditional.
+  virtual void setData(const DiscreteConditional::shared_ptr& dc);
+
   /// @}
 
  protected:

From 4ff70141f8208db49f9deab3b51222eb66f63874 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:10:37 -0500
Subject: [PATCH 189/362] use a TableFactor as the underlying data
 representation for DiscreteTableConditional since it provides a clean
 abstraction

---
 gtsam/discrete/DiscreteTableConditional.cpp | 17 ++++++-----------
 gtsam/discrete/DiscreteTableConditional.h   |  7 +++++--
 2 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index b09e2738f..a4fdcef5d 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -41,23 +41,21 @@ namespace gtsam {
 DiscreteTableConditional::DiscreteTableConditional(const size_t nrFrontals,
                                                    const TableFactor& f)
     : BaseConditional(nrFrontals, DecisionTreeFactor(f.discreteKeys(), ADT())),
-      sparse_table_((f / (*f.sum(nrFrontals))).sparseTable()) {
-  // sparse_table_ = sparse_table_.prune();
-}
+      table_(f / (*f.sum(nrFrontals))) {}
 
 /* ************************************************************************** */
 DiscreteTableConditional::DiscreteTableConditional(
     size_t nrFrontals, const DiscreteKeys& keys,
     const Eigen::SparseVector<double>& potentials)
     : BaseConditional(nrFrontals, keys, DecisionTreeFactor(keys, ADT())),
-      sparse_table_(potentials) {}
+      table_(TableFactor(keys, potentials)) {}
 
 /* ************************************************************************** */
 DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
                                                    const TableFactor& marginal)
     : BaseConditional(joint.size() - marginal.size(),
                       joint.discreteKeys() & marginal.discreteKeys(), ADT()),
-      sparse_table_((joint / marginal).sparseTable()) {}
+      table_(joint / marginal) {}
 
 /* ************************************************************************** */
 DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
@@ -71,8 +69,7 @@ DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
 /* ************************************************************************** */
 DiscreteTableConditional::DiscreteTableConditional(const Signature& signature)
     : BaseConditional(1, DecisionTreeFactor()),
-      sparse_table_(TableFactor(signature.discreteKeys(), signature.cpt())
-                        .sparseTable()) {}
+      table_(TableFactor(signature.discreteKeys(), signature.cpt())) {}
 
 /* ************************************************************************** */
 DiscreteTableConditional DiscreteTableConditional::operator*(
@@ -108,9 +105,7 @@ DiscreteTableConditional DiscreteTableConditional::operator*(
   // Finally, add parents to keys, in order
   for (auto&& dk : parents) discreteKeys.push_back(dk);
 
-  TableFactor a(this->discreteKeys(), this->sparse_table_),
-      b(other.discreteKeys(), other.sparse_table_);
-  TableFactor product = a * other;
+  TableFactor product = this->table_ * other.table();
   return DiscreteTableConditional(newFrontals.size(), product);
 }
 
@@ -128,7 +123,7 @@ void DiscreteTableConditional::print(const string& s,
     }
   }
   cout << "):\n";
-  // BaseFactor::print("", formatter);
+  table_.print("", formatter);
   cout << endl;
 }
 
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index 28e35277d..fae0c0761 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -29,13 +29,16 @@
 namespace gtsam {
 
 /**
- * Discrete Conditional Density which uses a SparseTable as the internal
+ * Discrete Conditional Density which uses a SparseVector as the internal
  * representation, similar to the TableFactor.
  *
  * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
-  Eigen::SparseVector<double> sparse_table_;
+ private:
+  TableFactor table_;
+
+  typedef Eigen::SparseVector<double>::InnerIterator SparseIt;
 
  public:
   // typedefs needed to play nice with gtsam

From b39b20084a0102eeb875906c81b7d58335a2ba78 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:11:02 -0500
Subject: [PATCH 190/362] fix return type

---
 gtsam/discrete/DiscreteConditional.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 78738dd54..055503b8e 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -484,7 +484,7 @@ double DiscreteConditional::evaluate(const Assignment<Key>& values) const {
 }
 
 /* ************************************************************************* */
-double DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
+void DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
   this->root_ = dc->root_;
 }
 

From d9faa820def958dc71e9a52d0306a9c97193f876 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:16:44 -0500
Subject: [PATCH 191/362] add evaluate and getter

---
 gtsam/discrete/DiscreteConditional.cpp    | 14 ++++++++++++++
 gtsam/discrete/DiscreteConditional.h      | 12 ++++++++++++
 gtsam/discrete/DiscreteTableConditional.h |  8 ++++++++
 3 files changed, 34 insertions(+)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 055503b8e..981986ea1 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -488,6 +488,20 @@ void DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
   this->root_ = dc->root_;
 }
 
+/* ************************************************************************* */
+DiscreteConditional::shared_ptr DiscreteConditional::max(
+    const Ordering& keys) const {
+  auto m = *BaseFactor::max(keys);
+  return std::make_shared<DiscreteConditional>(m.discreteKeys().size(), m);
+}
+
+/* ************************************************************************* */
+DiscreteConditional::shared_ptr DiscreteConditional::prune(
+    size_t maxNrAssignments) const {
+  return std::make_shared<DiscreteConditional>(
+      this->nrFrontals(), BaseFactor::prune(maxNrAssignments));
+}
+
 /* ************************************************************************* */
 double DiscreteConditional::negLogConstant() const { return 0.0; }
 
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 318024faa..12b5d457c 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -214,6 +214,15 @@ class GTSAM_EXPORT DiscreteConditional
    */
   size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
 
+  /**
+   * @brief Create new conditional by maximizing over all
+   * values with the same separator.
+   *
+   * @param keys The keys to sum over.
+   * @return DiscreteConditional::shared_ptr
+   */
+  virtual DiscreteConditional::shared_ptr max(const Ordering& keys) const;
+
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -273,6 +282,9 @@ class GTSAM_EXPORT DiscreteConditional
   /// Set the data from another DiscreteConditional.
   virtual void setData(const DiscreteConditional::shared_ptr& dc);
 
+  /// Prune the conditional
+  virtual DiscreteConditional::shared_ptr prune(size_t maxNrAssignments) const;
+
   /// @}
 
  protected:
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index fae0c0761..8a03dc361 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -205,6 +205,14 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
     return -error(x);
   }
 
+  /// Return the underlying TableFactor
+  TableFactor table() const { return table_; }
+
+  /// Evaluate the conditional given the values.
+  virtual double evaluate(const Assignment<Key>& values) const override {
+    return table_.evaluate(values);
+  }
+
   /// @}
 
  private:

From 60945c8e3225ba48df7e967faec779f887fd2fe8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:19:22 -0500
Subject: [PATCH 192/362] add override methods to DiscreteTableConditional

---
 gtsam/discrete/DiscreteTableConditional.cpp | 27 +++++++++++++++++++++
 gtsam/discrete/DiscreteTableConditional.h   | 17 +++++++++++++
 2 files changed, 44 insertions(+)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index a4fdcef5d..2bad12d2b 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -151,6 +151,33 @@ TableFactor::shared_ptr DiscreteTableConditional::likelihood(
   throw std::runtime_error("Likelihood not implemented");
 }
 
+/* ****************************************************************************/
+DiscreteConditional::shared_ptr DiscreteTableConditional::max(
+    const Ordering& keys) const override {
+  auto m = *table_.max(keys);
+
+  return std::make_shared<DiscreteTableConditional>(m.discreteKeys().size(), m);
+}
+
+/* ****************************************************************************/
+void DiscreteTableConditional::setData(
+    const DiscreteConditional::shared_ptr& dc) override {
+  if (auto dtc = std::dynamic_pointer_cast<DiscreteTableConditional>(dc)) {
+    this->table_ = dtc->table_;
+  } else {
+    this->table_ = TableFactor(dc->discreteKeys(), *dc);
+  }
+}
+
+/* ****************************************************************************/
+DiscreteConditional::shared_ptr DiscreteTableConditional::prune(
+    size_t maxNrAssignments) const {
+  TableFactor pruned = table_.prune(maxNrAssignments);
+
+  return std::make_shared<DiscreteTableConditional>(
+      this->nrFrontals(), this->discreteKeys(), pruned.sparseTable());
+}
+
 /* ************************************************************************** */
 size_t DiscreteTableConditional::argmax(
     const DiscreteValues& parentsValues) const {
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index 8a03dc361..f34cad2a3 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -181,6 +181,16 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
    */
   size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
 
+  /**
+   * @brief Create new conditional by maximizing over all
+   * values with the same separator.
+   *
+   * @param keys The keys to sum over.
+   * @return DiscreteConditional::shared_ptr
+   */
+  virtual DiscreteConditional::shared_ptr max(
+      const Ordering& keys) const override;
+
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -213,6 +223,13 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
     return table_.evaluate(values);
   }
 
+  /// Set the underlying data from the DiscreteConditional
+  virtual void setData(const DiscreteConditional::shared_ptr& dc) override;
+
+  /// Prune the conditional
+  virtual DiscreteConditional::shared_ptr prune(
+      size_t maxNrAssignments) const override;
+
   /// @}
 
  private:

From e46e9d67c56a99e196de79853bde6b5f29c63a02 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:20:09 -0500
Subject: [PATCH 193/362] use DiscreteTableConditional in EliminateDiscrete

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 17 ++++++-----------
 1 file changed, 6 insertions(+), 11 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 68892b1a4..3fcdf7bc6 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -19,6 +19,7 @@
 
 #include <gtsam/discrete/DiscreteBayesTree.h>
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
@@ -70,8 +71,7 @@ namespace gtsam {
       if (factor) {
         if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
           result = result * (*f);
-        }
-        else if (auto dtf =
+        } else if (auto dtf =
                        std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
           result = TableFactor(result * (*dtf));
         }
@@ -253,18 +253,13 @@ namespace gtsam {
                        sum->keys().end());
 
     // now divide product/sum to get conditional
-#if GTSAM_HYBRID_TIMING
-    gttic_(EliminateDiscreteDivide);
-#endif
-    auto c = product / (*sum);
-#if GTSAM_HYBRID_TIMING
-    gttoc_(EliminateDiscreteDivide);
-#endif
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteToDiscreteConditional);
 #endif
-    auto conditional = std::make_shared<DiscreteConditional>(
-        orderedKeys.size(), c.toDecisionTreeFactor());
+    // auto conditional = std::make_shared<DiscreteConditional>(
+    //     orderedKeys.size(), (product / (*sum)).toDecisionTreeFactor());
+    auto conditional =
+        std::make_shared<DiscreteTableConditional>(product, *sum, orderedKeys);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteToDiscreteConditional);
 #endif

From b7b273468c9bb399d9a888d35ff96ff7d382d52c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:20:43 -0500
Subject: [PATCH 194/362] small cleanup

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 3fcdf7bc6..338453404 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -256,8 +256,6 @@ namespace gtsam {
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteToDiscreteConditional);
 #endif
-    // auto conditional = std::make_shared<DiscreteConditional>(
-    //     orderedKeys.size(), (product / (*sum)).toDecisionTreeFactor());
     auto conditional =
         std::make_shared<DiscreteTableConditional>(product, *sum, orderedKeys);
 #if GTSAM_HYBRID_TIMING

From 214043d60d5f01514ea5bdae2fa827951d438038 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:26:20 -0500
Subject: [PATCH 195/362] use DiscreteConditional shared_ptr for dynamic
 dispatch

---
 gtsam/hybrid/HybridBayesNet.cpp            |  4 ++--
 gtsam/hybrid/HybridBayesTree.cpp           |  9 +++++----
 gtsam/hybrid/HybridGaussianConditional.cpp | 10 +++++-----
 gtsam/hybrid/HybridGaussianConditional.h   |  3 ++-
 4 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 623b82eea..7691bb209 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -56,11 +56,11 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   }
 
   // Prune the joint. NOTE: again, possibly quite expensive.
-  const DecisionTreeFactor pruned = joint.prune(maxNrLeaves);
+  const DiscreteConditional::shared_ptr pruned = joint.prune(maxNrLeaves);
 
   // Create a the result starting with the pruned joint.
   HybridBayesNet result;
-  result.emplace_shared<DiscreteConditional>(pruned.size(), pruned);
+  result.push_back(std::move(pruned));
 
   /* To prune, we visitWith every leaf in the HybridGaussianConditional.
    * For each leaf, using the assignment we can check the discrete decision tree
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 1b633e024..ce2ddda81 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -181,14 +181,15 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
 void HybridBayesTree::prune(const size_t maxNrLeaves) {
   auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete();
 
-  DecisionTreeFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves);
-  discreteProbs->root_ = prunedDiscreteProbs.root_;
+  DiscreteConditional::shared_ptr prunedDiscreteProbs =
+      discreteProbs->prune(maxNrLeaves);
+  discreteProbs->setData(prunedDiscreteProbs);
 
   /// Helper struct for pruning the hybrid bayes tree.
   struct HybridPrunerData {
     /// The discrete decision tree after pruning.
-    DecisionTreeFactor prunedDiscreteProbs;
-    HybridPrunerData(const DecisionTreeFactor& prunedDiscreteProbs,
+    DiscreteConditional::shared_ptr prunedDiscreteProbs;
+    HybridPrunerData(const DiscreteConditional::shared_ptr& prunedDiscreteProbs,
                      const HybridBayesTree::sharedNode& parentClique)
         : prunedDiscreteProbs(prunedDiscreteProbs) {}
 
diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 54346679e..8883217ba 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -304,18 +304,18 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
 
 /* *******************************************************************************/
 HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
-    const DecisionTreeFactor &discreteProbs) const {
-  // Find keys in discreteProbs.keys() but not in this->keys():
+    const DiscreteConditional::shared_ptr &discreteProbs) const {
+  // Find keys in discreteProbs->keys() but not in this->keys():
   std::set<Key> mine(this->keys().begin(), this->keys().end());
-  std::set<Key> theirs(discreteProbs.keys().begin(),
-                       discreteProbs.keys().end());
+  std::set<Key> theirs(discreteProbs->keys().begin(),
+                       discreteProbs->keys().end());
   std::vector<Key> diff;
   std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
                       std::back_inserter(diff));
 
   // Find maximum probability value for every combination of our keys.
   Ordering keys(diff);
-  auto max = discreteProbs.max(keys);
+  auto max = discreteProbs->max(keys);
 
   // Check the max value for every combination of our keys.
   // If the max value is 0.0, we can prune the corresponding conditional.
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index e769662ed..fd9c0d7a3 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -23,6 +23,7 @@
 #include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
@@ -235,7 +236,7 @@ class GTSAM_EXPORT HybridGaussianConditional
    * @return Shared pointer to possibly a pruned HybridGaussianConditional
    */
   HybridGaussianConditional::shared_ptr prune(
-      const DecisionTreeFactor &discreteProbs) const;
+      const DiscreteConditional::shared_ptr &discreteProbs) const;
 
   /// Return true if the conditional has already been pruned.
   bool pruned() const { return pruned_; }

From dfec8409feb891b522d27c32a414fb4e5bdafc77 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:27:04 -0500
Subject: [PATCH 196/362] use TableFactor for discrete elimination

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 703684c78..cf00a2209 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -25,7 +25,9 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
@@ -241,18 +243,18 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
 /* ************************************************************************ */
 /**
  * @brief Take negative log-values, shift them so that the minimum value is 0,
- * and then exponentiate to create a DecisionTreeFactor (not normalized yet!).
+ * and then exponentiate to create a TableFactor (not normalized yet!).
  *
  * @param errors DecisionTree of (unnormalized) errors.
- * @return DecisionTreeFactor::shared_ptr
+ * @return TableFactor::shared_ptr
  */
-static DecisionTreeFactor::shared_ptr DiscreteFactorFromErrors(
+static TableFactor::shared_ptr DiscreteFactorFromErrors(
     const DiscreteKeys &discreteKeys,
     const AlgebraicDecisionTree<Key> &errors) {
   double min_log = errors.min();
   AlgebraicDecisionTree<Key> potentials(
       errors, [&min_log](const double x) { return exp(-(x - min_log)); });
-  return std::make_shared<DecisionTreeFactor>(discreteKeys, potentials);
+  return std::make_shared<TableFactor>(discreteKeys, potentials);
 }
 
 /* ************************************************************************ */
@@ -285,12 +287,17 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
       gttic_(ConvertConditionalToTableFactor);
 #endif
-      // Convert DiscreteConditional to TableFactor
-      auto tdc = std::make_shared<TableFactor>(*dc);
+      if (auto dtc = std::dynamic_pointer_cast<DiscreteTableConditional>(dc)) {
+        /// Get the underlying TableFactor
+        dfg.push_back(dtc->table());
+      } else {
+        // Convert DiscreteConditional to TableFactor
+        auto tdc = std::make_shared<TableFactor>(*dc);
+        dfg.push_back(tdc);
+      }
 #if GTSAM_HYBRID_TIMING
       gttoc_(ConvertConditionalToTableFactor);
 #endif
-      dfg.push_back(tdc);
     } else {
       throwRuntimeError("discreteElimination", f);
     }

From 5019153e12a30e1908bf542e0a5d4444faaa0f9a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 00:27:49 -0500
Subject: [PATCH 197/362] small cleanup

---
 gtsam/discrete/DiscreteTableConditional.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index 2bad12d2b..cdfc36556 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -153,7 +153,7 @@ TableFactor::shared_ptr DiscreteTableConditional::likelihood(
 
 /* ****************************************************************************/
 DiscreteConditional::shared_ptr DiscreteTableConditional::max(
-    const Ordering& keys) const override {
+    const Ordering& keys) const {
   auto m = *table_.max(keys);
 
   return std::make_shared<DiscreteTableConditional>(m.discreteKeys().size(), m);
@@ -161,7 +161,7 @@ DiscreteConditional::shared_ptr DiscreteTableConditional::max(
 
 /* ****************************************************************************/
 void DiscreteTableConditional::setData(
-    const DiscreteConditional::shared_ptr& dc) override {
+    const DiscreteConditional::shared_ptr& dc) {
   if (auto dtc = std::dynamic_pointer_cast<DiscreteTableConditional>(dc)) {
     this->table_ = dtc->table_;
   } else {

From 623bd63ec89a809ab5cb3b767759b38d3932f162 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 10:34:03 -0500
Subject: [PATCH 198/362] fix hybrid tests

---
 gtsam/hybrid/tests/testHybridBayesNet.cpp             | 2 +-
 gtsam/hybrid/tests/testHybridGaussianConditional.cpp  | 9 ++++++---
 gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 3 +--
 3 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 135da5dc7..b9bc29e47 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -454,7 +454,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
   }
 
   size_t maxNrLeaves = 3;
-  auto prunedDecisionTree = joint.prune(maxNrLeaves);
+  auto prunedDecisionTree = *joint.prune(maxNrLeaves);
 
 #ifdef GTSAM_DT_MERGING
   EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/,
diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
index 350bc9184..0bfc49fcb 100644
--- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
@@ -261,7 +261,8 @@ TEST(HybridGaussianConditional, Prune) {
       potentials[i] = 1;
       const DecisionTreeFactor decisionTreeFactor(keys, potentials);
       // Prune the HybridGaussianConditional
-      const auto pruned = hgc.prune(decisionTreeFactor);
+      const auto pruned = hgc.prune(std::make_shared<DiscreteConditional>(
+          keys.size(), decisionTreeFactor));
       // Check that the pruned HybridGaussianConditional has 1 conditional
       EXPECT_LONGS_EQUAL(1, pruned->nrComponents());
     }
@@ -271,7 +272,8 @@ TEST(HybridGaussianConditional, Prune) {
                                          0, 0, 0.5, 0};
     const DecisionTreeFactor decisionTreeFactor(keys, potentials);
 
-    const auto pruned = hgc.prune(decisionTreeFactor);
+    const auto pruned = hgc.prune(
+        std::make_shared<DiscreteConditional>(keys.size(), decisionTreeFactor));
 
     // Check that the pruned HybridGaussianConditional has 2 conditionals
     EXPECT_LONGS_EQUAL(2, pruned->nrComponents());
@@ -286,7 +288,8 @@ TEST(HybridGaussianConditional, Prune) {
                                          0,   0, 0.5, 0};
     const DecisionTreeFactor decisionTreeFactor(keys, potentials);
 
-    const auto pruned = hgc.prune(decisionTreeFactor);
+    const auto pruned = hgc.prune(
+        std::make_shared<DiscreteConditional>(keys.size(), decisionTreeFactor));
 
     // Check that the pruned HybridGaussianConditional has 3 conditionals
     EXPECT_LONGS_EQUAL(3, pruned->nrComponents());
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 07f70e95c..6e844dbcb 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -368,10 +368,9 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
   EXPECT_LONGS_EQUAL(1, hybridGaussianConditional->nrParents());
 
   // This is now a discreteFactor
-  auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
+  auto discreteFactor = dynamic_pointer_cast<TableFactor>(factorOnModes);
   CHECK(discreteFactor);
   EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
-  EXPECT(discreteFactor->root_->isLeaf() == false);
 }
 
 /****************************************************************************

From 9f85d4cc2dbbf33cd8c1f8d124fe93559cd1c6da Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 13:23:41 -0500
Subject: [PATCH 199/362] fix equals

---
 gtsam/discrete/DiscreteTableConditional.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index cdfc36556..f50d7fbeb 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -130,12 +130,14 @@ void DiscreteTableConditional::print(const string& s,
 /* ************************************************************************** */
 bool DiscreteTableConditional::equals(const DiscreteFactor& other,
                                       double tol) const {
-  if (!dynamic_cast<const DiscreteConditional*>(&other)) {
+  auto dtc = dynamic_cast<const DiscreteTableConditional*>(&other);
+  if (!dtc) {
     return false;
   } else {
     const DiscreteConditional& f(
         static_cast<const DiscreteConditional&>(other));
-    return DiscreteConditional::equals(f, tol);
+    return table_.equals(dtc->table_, tol) &&
+           DiscreteConditional::equals(f, tol);
   }
 }
 

From 9cacb9876eae75551c5537f33ca43f01f275523a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 13:27:55 -0500
Subject: [PATCH 200/362] undo changes to DiscreteFactorGraph

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 35 +++++++++++---------------
 gtsam/discrete/DiscreteFactorGraph.h   |  3 +--
 2 files changed, 15 insertions(+), 23 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 338453404..2037dd951 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -19,7 +19,6 @@
 
 #include <gtsam/discrete/DiscreteBayesTree.h>
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
@@ -65,17 +64,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  TableFactor DiscreteFactorGraph::product() const {
-    TableFactor result;
+  DecisionTreeFactor DiscreteFactorGraph::product() const {
+    DecisionTreeFactor result;
     for (const sharedFactor& factor : *this) {
-      if (factor) {
-        if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
-          result = result * (*f);
-        } else if (auto dtf =
-                       std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
-          result = TableFactor(result * (*dtf));
-        }
-      }
+      if (factor) result = (*factor) * result;
     }
     return result;
   }
@@ -124,14 +116,15 @@ namespace gtsam {
    * product to prevent underflow.
    *
    * @param factors The factors to multiply as a DiscreteFactorGraph.
-   * @return TableFactor
+   * @return DecisionTreeFactor
    */
-  static TableFactor ProductAndNormalize(const DiscreteFactorGraph& factors) {
+  static DecisionTreeFactor ProductAndNormalize(
+      const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
 #if GTSAM_HYBRID_TIMING
     gttic_(DiscreteProduct);
 #endif
-    TableFactor product = factors.product();
+    DecisionTreeFactor product = factors.product();
 #if GTSAM_HYBRID_TIMING
     gttoc_(DiscreteProduct);
 #endif
@@ -156,11 +149,11 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    TableFactor product = ProductAndNormalize(factors);
+    DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
-    TableFactor::shared_ptr max = product.max(frontalKeys);
+    DecisionTreeFactor::shared_ptr max = product.max(frontalKeys);
     gttoc(max);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
@@ -173,8 +166,8 @@ namespace gtsam {
     // Make lookup with product
     gttic(lookup);
     size_t nrFrontals = frontalKeys.size();
-    auto lookup = std::make_shared<DiscreteLookupTable>(
-        nrFrontals, orderedKeys, product.toDecisionTreeFactor());
+    auto lookup =
+        std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
     gttoc(lookup);
 
     return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
@@ -234,13 +227,13 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    TableFactor product = ProductAndNormalize(factors);
+    DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // sum out frontals, this is the factor on the separator
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteSum);
 #endif
-    TableFactor::shared_ptr sum = product.sum(frontalKeys);
+    DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteSum);
 #endif
@@ -257,7 +250,7 @@ namespace gtsam {
     gttic_(EliminateDiscreteToDiscreteConditional);
 #endif
     auto conditional =
-        std::make_shared<DiscreteTableConditional>(product, *sum, orderedKeys);
+        std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteToDiscreteConditional);
 #endif
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index f1575cd7e..c57d2258c 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -25,7 +25,6 @@
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/FastSet.h>
-#include <gtsam/discrete/TableFactor.h>
 
 #include <string>
 #include <utility>
@@ -148,7 +147,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
   DiscreteKeys discreteKeys() const;
 
   /** return product of all factors as a single factor */
-  TableFactor product() const;
+  DecisionTreeFactor product() const;
 
   /** 
    * Evaluates the factor graph given values, returns the joint probability of

From c6e9bfc8241c715f08afb8db9542acee79a9a7c0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 14:08:25 -0500
Subject: [PATCH 201/362] remove unused methods

---
 gtsam/discrete/DiscreteTableConditional.cpp | 34 ---------------
 gtsam/discrete/DiscreteTableConditional.h   | 47 +--------------------
 2 files changed, 2 insertions(+), 79 deletions(-)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index f50d7fbeb..bcb65628a 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -141,18 +141,6 @@ bool DiscreteTableConditional::equals(const DiscreteFactor& other,
   }
 }
 
-/* ************************************************************************** */
-TableFactor::shared_ptr DiscreteTableConditional::likelihood(
-    const DiscreteValues& frontalValues) const {
-  throw std::runtime_error("Likelihood not implemented");
-}
-
-/* ****************************************************************************/
-TableFactor::shared_ptr DiscreteTableConditional::likelihood(
-    size_t frontal) const {
-  throw std::runtime_error("Likelihood not implemented");
-}
-
 /* ****************************************************************************/
 DiscreteConditional::shared_ptr DiscreteTableConditional::max(
     const Ordering& keys) const {
@@ -180,26 +168,4 @@ DiscreteConditional::shared_ptr DiscreteTableConditional::prune(
       this->nrFrontals(), this->discreteKeys(), pruned.sparseTable());
 }
 
-/* ************************************************************************** */
-size_t DiscreteTableConditional::argmax(
-    const DiscreteValues& parentsValues) const {
-  // Initialize
-  size_t maxValue = 0;
-  double maxP = 0;
-  DiscreteValues values = parentsValues;
-
-  assert(nrFrontals() == 1);
-  Key j = firstFrontalKey();
-  for (size_t value = 0; value < cardinality(j); value++) {
-    values[j] = value;
-    double pValueS = (*this)(values);
-    // Update MPE solution if better
-    if (pValueS > maxP) {
-      maxP = pValueS;
-      maxValue = value;
-    }
-  }
-  return maxValue;
-}
-
 }  // namespace gtsam
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index f34cad2a3..7bd419f7b 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -158,28 +158,8 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   /// @name Standard Interface
   /// @{
 
-  /// Log-probability is just -error(x).
-  double logProbability(const DiscreteValues& x) const { return -error(x); }
-
-  /// print index signature only
-  void printSignature(
-      const std::string& s = "Discrete Conditional: ",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const {
-    static_cast<const BaseConditional*>(this)->print(s, formatter);
-  }
-
-  /** Convert to a likelihood factor by providing value before bar. */
-  TableFactor::shared_ptr likelihood(const DiscreteValues& frontalValues) const;
-
-  /** Single variable version of likelihood. */
-  TableFactor::shared_ptr likelihood(size_t frontal) const;
-
-  /**
-   * @brief Return assignment for single frontal variable that maximizes value.
-   * @param parentsValues Known assignments for the parents.
-   * @return maximizing assignment for the frontal variable.
-   */
-  size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
+  /// Return the underlying TableFactor
+  TableFactor table() const { return table_; }
 
   /**
    * @brief Create new conditional by maximizing over all
@@ -195,29 +175,6 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   /// @name Advanced Interface
   /// @{
 
-  /// Return all assignments for frontal variables.
-  std::vector<DiscreteValues> frontalAssignments() const;
-
-  /// Return all assignments for frontal *and* parent variables.
-  std::vector<DiscreteValues> allAssignments() const;
-
-  /// @}
-  /// @name HybridValues methods.
-  /// @{
-
-  using BaseConditional::operator();  ///< HybridValues version
-
-  /**
-   * Calculate log-probability log(evaluate(x)) for HybridValues `x`.
-   * This is actually just -error(x).
-   */
-  double logProbability(const HybridValues& x) const override {
-    return -error(x);
-  }
-
-  /// Return the underlying TableFactor
-  TableFactor table() const { return table_; }
-
   /// Evaluate the conditional given the values.
   virtual double evaluate(const Assignment<Key>& values) const override {
     return table_.evaluate(values);

From f95ae52aff0385b947a878beb3c3eb6258ddc2dd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 14:16:00 -0500
Subject: [PATCH 202/362] Use TableFactor everywhere in hybrid elimination

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

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 703684c78..4fcd420b1 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -20,12 +20,12 @@
 
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/Assignment.h>
-#include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
@@ -241,18 +241,18 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
 /* ************************************************************************ */
 /**
  * @brief Take negative log-values, shift them so that the minimum value is 0,
- * and then exponentiate to create a DecisionTreeFactor (not normalized yet!).
+ * and then exponentiate to create a TableFactor (not normalized yet!).
  *
  * @param errors DecisionTree of (unnormalized) errors.
- * @return DecisionTreeFactor::shared_ptr
+ * @return TableFactor::shared_ptr
  */
-static DecisionTreeFactor::shared_ptr DiscreteFactorFromErrors(
+static TableFactor::shared_ptr DiscreteFactorFromErrors(
     const DiscreteKeys &discreteKeys,
     const AlgebraicDecisionTree<Key> &errors) {
   double min_log = errors.min();
   AlgebraicDecisionTree<Key> potentials(
       errors, [&min_log](const double x) { return exp(-(x - min_log)); });
-  return std::make_shared<DecisionTreeFactor>(discreteKeys, potentials);
+  return std::make_shared<TableFactor>(discreteKeys, potentials);
 }
 
 /* ************************************************************************ */

From 71ea8c5d4c22289c328c5bdad054037a9793b679 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 14:55:56 -0500
Subject: [PATCH 203/362] fix tests

---
 gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp  | 6 +++---
 gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 3 +--
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 31e36101b..1942e9234 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -114,10 +114,10 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
   EXPECT(HybridConditional::CheckInvariants(*result.first, values));
 
   // Check that factor is discrete and correct
-  auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
+  auto factor = std::dynamic_pointer_cast<TableFactor>(result.second);
   CHECK(factor);
   // regression test
-  EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5));
+  EXPECT(assert_equal(TableFactor{m1, "1 1"}, *factor, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -329,7 +329,7 @@ TEST(HybridBayesNet, Switching) {
 
   // Check the remaining factor for x1
   CHECK(factor_x1);
-  auto phi_x1 = std::dynamic_pointer_cast<DecisionTreeFactor>(factor_x1);
+  auto phi_x1 = std::dynamic_pointer_cast<TableFactor>(factor_x1);
   CHECK(phi_x1);
   EXPECT_LONGS_EQUAL(1, phi_x1->keys().size());  // m0
   // We can't really check the error of the decision tree factor phi_x1, because
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 07f70e95c..6e844dbcb 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -368,10 +368,9 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
   EXPECT_LONGS_EQUAL(1, hybridGaussianConditional->nrParents());
 
   // This is now a discreteFactor
-  auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
+  auto discreteFactor = dynamic_pointer_cast<TableFactor>(factorOnModes);
   CHECK(discreteFactor);
   EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
-  EXPECT(discreteFactor->root_->isLeaf() == false);
 }
 
 /****************************************************************************

From 42f8e54c2a31127251401e410ceea932c38127a2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 14:27:01 -0500
Subject: [PATCH 204/362] customize discrete elimination in Hybrid

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 70 +++++++++++++++++++++-
 1 file changed, 68 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 4fcd420b1..8b0a2349f 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -255,6 +255,48 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors(
   return std::make_shared<TableFactor>(discreteKeys, potentials);
 }
 
+/**
+ * @brief Multiply all the `factors` and normalize the
+ * product to prevent underflow.
+ *
+ * @param factors The factors to multiply as a DiscreteFactorGraph.
+ * @return TableFactor
+ */
+static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
+  // PRODUCT: multiply all factors
+#if GTSAM_HYBRID_TIMING
+  gttic_(DiscreteProduct);
+#endif
+  TableFactor product;
+  for (const sharedFactor &factor : factors) {
+    if (factor) {
+      if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
+        product = product * (*f);
+      } else if (auto dtf =
+                     std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
+        product = TableFactor(product * (*dtf));
+      }
+    }
+  }
+#if GTSAM_HYBRID_TIMING
+  gttoc_(DiscreteProduct);
+#endif
+
+  // Max over all the potentials by pretending all keys are frontal:
+  auto normalizer = product.max(product.size());
+
+#if GTSAM_HYBRID_TIMING
+  gttic_(DiscreteNormalize);
+#endif
+  // Normalize the product factor to prevent underflow.
+  product = product / (*normalizer);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(DiscreteNormalize);
+#endif
+
+  return product;
+}
+
 /* ************************************************************************ */
 static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
 discreteElimination(const HybridGaussianFactorGraph &factors,
@@ -299,8 +341,32 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscrete);
 #endif
-  // NOTE: This does sum-product. For max-product, use EliminateForMPE.
-  auto result = EliminateDiscrete(dfg, frontalKeys);
+  /**** NOTE: This does sum-product. ****/
+  // Get product factor
+  TableFactor product = ProductAndNormalize(factors);
+
+#if GTSAM_HYBRID_TIMING
+  gttic_(EliminateDiscreteSum);
+#endif
+  // All the discrete variables should form a single clique,
+  // so we can sum out on all the variables as frontals.
+  // This should give an empty separator.
+  Ordering orderedKeys(product.keys());
+  DecisionTreeFactor::shared_ptr sum = product.sum(orderedKeys);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(EliminateDiscreteSum);
+#endif
+
+#if GTSAM_HYBRID_TIMING
+  gttic_(EliminateDiscreteToDiscreteConditional);
+#endif
+  // Finally, get the conditional
+  auto conditional =
+      std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(EliminateDiscreteToDiscreteConditional);
+#endif
+
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscrete);
 #endif

From 47e76ff03b77a007ca166e07b8aa7a2a042e49e3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 14:27:20 -0500
Subject: [PATCH 205/362] remove GTSAM_HYBRID_TIMING guards

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 24 ------------------------
 1 file changed, 24 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 2037dd951..2b63be660 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -121,25 +121,13 @@ namespace gtsam {
   static DecisionTreeFactor ProductAndNormalize(
       const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
-#if GTSAM_HYBRID_TIMING
-    gttic_(DiscreteProduct);
-#endif
     DecisionTreeFactor product = factors.product();
-#if GTSAM_HYBRID_TIMING
-    gttoc_(DiscreteProduct);
-#endif
 
     // Max over all the potentials by pretending all keys are frontal:
     auto normalizer = product.max(product.size());
 
-#if GTSAM_HYBRID_TIMING
-    gttic_(DiscreteNormalize);
-#endif
     // Normalize the product factor to prevent underflow.
     product = product / (*normalizer);
-#if GTSAM_HYBRID_TIMING
-    gttoc_(DiscreteNormalize);
-#endif
 
     return product;
   }
@@ -230,13 +218,7 @@ namespace gtsam {
     DecisionTreeFactor product = ProductAndNormalize(factors);
 
     // sum out frontals, this is the factor on the separator
-#if GTSAM_HYBRID_TIMING
-    gttic_(EliminateDiscreteSum);
-#endif
     DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
-#if GTSAM_HYBRID_TIMING
-    gttoc_(EliminateDiscreteSum);
-#endif
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     Ordering orderedKeys;
@@ -246,14 +228,8 @@ namespace gtsam {
                        sum->keys().end());
 
     // now divide product/sum to get conditional
-#if GTSAM_HYBRID_TIMING
-    gttic_(EliminateDiscreteToDiscreteConditional);
-#endif
     auto conditional =
         std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
-#if GTSAM_HYBRID_TIMING
-    gttoc_(EliminateDiscreteToDiscreteConditional);
-#endif
 
     return {conditional, sum};
   }

From 0820fcb7b273f9cf025408cb12b3b8e04ff9a21b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 14:50:51 -0500
Subject: [PATCH 206/362] fix types

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 8b0a2349f..8f63d6e71 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -268,7 +268,7 @@ static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
   gttic_(DiscreteProduct);
 #endif
   TableFactor product;
-  for (const sharedFactor &factor : factors) {
+  for (auto &&factor : factors) {
     if (factor) {
       if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
         product = product * (*f);
@@ -343,7 +343,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #endif
   /**** NOTE: This does sum-product. ****/
   // Get product factor
-  TableFactor product = ProductAndNormalize(factors);
+  TableFactor product = ProductAndNormalize(dfg);
 
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteSum);
@@ -352,26 +352,26 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   // so we can sum out on all the variables as frontals.
   // This should give an empty separator.
   Ordering orderedKeys(product.keys());
-  DecisionTreeFactor::shared_ptr sum = product.sum(orderedKeys);
+  TableFactor::shared_ptr sum = product.sum(orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteSum);
 #endif
 
 #if GTSAM_HYBRID_TIMING
-  gttic_(EliminateDiscreteToDiscreteConditional);
+  gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
   // Finally, get the conditional
   auto conditional =
       std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
 #if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscreteToDiscreteConditional);
+  gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif
 
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscrete);
 #endif
 
-  return {std::make_shared<HybridConditional>(result.first), result.second};
+  return {std::make_shared<HybridConditional>(conditional), sum};
 }
 
 /* ************************************************************************ */

From 462a5b8b3aa1b82c0145af675b34fdc34a4f050c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 15:53:44 -0500
Subject: [PATCH 207/362] return DiscreteTableConditional from hybrid
 elimination

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 1ad0cdaf4..8c37298a7 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -271,11 +271,14 @@ static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
   TableFactor product;
   for (auto &&factor : factors) {
     if (factor) {
-      if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
+      if (auto dtc =
+              std::dynamic_pointer_cast<DiscreteTableConditional>(factor)) {
+        product = product * dtc->table();
+      } else if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
         product = product * (*f);
       } else if (auto dtf =
                      std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
-        product = TableFactor(product * (*dtf));
+        product = product * TableFactor(*dtf);
       }
     }
   }
@@ -368,7 +371,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #endif
   // Finally, get the conditional
   auto conditional =
-      std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
+      std::make_shared<DiscreteTableConditional>(product, *sum, orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From 5e1931eb98c4f299b96ef46ce12e0e75e781bb37 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 15:54:07 -0500
Subject: [PATCH 208/362] update testGaussianMixture

---
 gtsam/hybrid/tests/testGaussianMixture.cpp | 27 ++++++++++++++--------
 1 file changed, 17 insertions(+), 10 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 14bef5fbb..2de8d15ec 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
@@ -79,8 +80,9 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   double midway = mu1 - mu0;
   auto eliminationResult =
       gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
-  auto pMid = *eliminationResult->at(0)->asDiscrete();
-  EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid));
+  auto pMid = std::dynamic_pointer_cast<DiscreteTableConditional>(
+      eliminationResult->at(0)->asDiscrete());
+  EXPECT(assert_equal(DiscreteTableConditional(m, "60/40"), *pMid));
 
   // Everywhere else, the result should be a sigmoid.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -90,7 +92,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     // Workflow 1: convert HBN to HFG and solve
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
-    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
+    auto posterior1 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
+        eliminationResult1->at(0)->asDiscrete());
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -99,7 +102,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
         m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)});
     hfg1.push_back(mixing);
     auto eliminationResult2 = hfg1.eliminateSequential();
-    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
+    auto posterior2 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
+        eliminationResult2->at(0)->asDiscrete());
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
@@ -133,13 +137,14 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   // Eliminate the graph!
   auto eliminationResultMax = gfg.eliminateSequential();
 
-  // Equality of posteriors asserts that the elimination is correct (same ratios
-  // for all modes)
+  // Equality of posteriors asserts that the elimination is correct
+  // (same ratios for all modes)
   EXPECT(assert_equal(expectedDiscretePosterior,
                       eliminationResultMax->discretePosterior(vv)));
 
-  auto pMax = *eliminationResultMax->at(0)->asDiscrete();
-  EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
+  auto pMax = *std::dynamic_pointer_cast<DiscreteTableConditional>(
+      eliminationResultMax->at(0)->asDiscrete());
+  EXPECT(assert_equal(DiscreteTableConditional(m, "42/58"), pMax, 1e-4));
 
   // Everywhere else, the result should be a bell curve like function.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -149,7 +154,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     // Workflow 1: convert HBN to HFG and solve
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
-    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
+    auto posterior1 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
+        eliminationResult1->at(0)->asDiscrete());
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -158,7 +164,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
         m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)});
     hfg.push_back(mixing);
     auto eliminationResult2 = hfg.eliminateSequential();
-    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
+    auto posterior2 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
+        eliminationResult2->at(0)->asDiscrete());
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }

From 3119d132ac18c5f8f04f8d9b3ab96cac43022ae5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 16:34:20 -0500
Subject: [PATCH 209/362] remove evaluate method

---
 gtsam/discrete/DiscreteConditional.cpp    | 4 ----
 gtsam/discrete/DiscreteConditional.h      | 3 ---
 gtsam/discrete/DiscreteTableConditional.h | 5 -----
 3 files changed, 12 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 981986ea1..aa7f1d391 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -478,10 +478,6 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
   return this->evaluate(x.discrete());
 }
 
-/* ************************************************************************* */
-double DiscreteConditional::evaluate(const Assignment<Key>& values) const {
-  return BaseFactor::evaluate(values);
-}
 
 /* ************************************************************************* */
 void DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 12b5d457c..98edcb8c9 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -258,9 +258,6 @@ class GTSAM_EXPORT DiscreteConditional
    */
   double evaluate(const HybridValues& x) const override;
 
-  /// Evaluate the conditional given values.
-  virtual double evaluate(const Assignment<Key>& values) const override;
-
   using BaseConditional::operator();  ///< HybridValues version
 
   /**
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index 7bd419f7b..fefbea171 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -175,11 +175,6 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   /// @name Advanced Interface
   /// @{
 
-  /// Evaluate the conditional given the values.
-  virtual double evaluate(const Assignment<Key>& values) const override {
-    return table_.evaluate(values);
-  }
-
   /// Set the underlying data from the DiscreteConditional
   virtual void setData(const DiscreteConditional::shared_ptr& dc) override;
 

From 9e1c0d77c5ab361798cd51963d63602ab27ca740 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 16:34:36 -0500
Subject: [PATCH 210/362] fix constructor and equals

---
 gtsam/discrete/DiscreteTableConditional.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index bcb65628a..9aff487cf 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -68,7 +68,7 @@ DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
 
 /* ************************************************************************** */
 DiscreteTableConditional::DiscreteTableConditional(const Signature& signature)
-    : BaseConditional(1, DecisionTreeFactor()),
+    : BaseConditional(1, DecisionTreeFactor(DiscreteKeys{{1, 1}}, ADT(1))),
       table_(TableFactor(signature.discreteKeys(), signature.cpt())) {}
 
 /* ************************************************************************** */
@@ -137,7 +137,7 @@ bool DiscreteTableConditional::equals(const DiscreteFactor& other,
     const DiscreteConditional& f(
         static_cast<const DiscreteConditional&>(other));
     return table_.equals(dtc->table_, tol) &&
-           DiscreteConditional::equals(f, tol);
+           DiscreteConditional::BaseConditional::equals(f, tol);
   }
 }
 

From 094b76df2d4c43545353c7ec9f70d6dd4eee0f27 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 17:06:57 -0500
Subject: [PATCH 211/362] fix bug in TableFactor when trying to convert to
 DecisionTreeFactor

---
 gtsam/discrete/TableFactor.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index bf9662e34..67ba19d39 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -252,6 +252,11 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
+  // If no keys, then return empty DecisionTreeFactor
+  if (dkeys.size() == 0) {
+    return DecisionTreeFactor(dkeys, AlgebraicDecisionTree<Key>());
+  }
+
   std::vector<double> table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
     table.push_back(sparse_table_.coeff(i));

From bf4c0bd72de2fabc760562b5de908cad5ec751fa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 17:07:25 -0500
Subject: [PATCH 212/362] fix creation of DiscreteConditional

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 ++++----
 gtsam/hybrid/tests/testGaussianMixture.cpp | 1 +
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 8f63d6e71..831e0ccc2 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -351,8 +351,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   // All the discrete variables should form a single clique,
   // so we can sum out on all the variables as frontals.
   // This should give an empty separator.
-  Ordering orderedKeys(product.keys());
-  TableFactor::shared_ptr sum = product.sum(orderedKeys);
+  TableFactor::shared_ptr sum = product.sum(frontalKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteSum);
 #endif
@@ -361,8 +360,9 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
   // Finally, get the conditional
-  auto conditional =
-      std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
+  auto c = product / (*sum);
+  auto conditional = std::make_shared<DiscreteConditional>(
+      frontalKeys.size(), c.toDecisionTreeFactor());
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 14bef5fbb..698c1bbf6 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -162,6 +162,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 0e2e8bb8ced3be38e150e72df03869f7ca49632f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 18:23:37 -0500
Subject: [PATCH 213/362] full discrete elimination

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 831e0ccc2..ca971191c 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -356,13 +356,17 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   gttoc_(EliminateDiscreteSum);
 #endif
 
+  // Ordering keys for the conditional so that frontalKeys are really in front
+  Ordering orderedKeys;
+  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
+  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
+
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
   // Finally, get the conditional
-  auto c = product / (*sum);
   auto conditional = std::make_shared<DiscreteConditional>(
-      frontalKeys.size(), c.toDecisionTreeFactor());
+      product.toDecisionTreeFactor(), sum->toDecisionTreeFactor(), orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From 73f54083a7aeab7c996f865e4fe77d3ab3cdfb1d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 19:34:46 -0500
Subject: [PATCH 214/362] normalize

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index ca971191c..5fd9ddfa6 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -364,9 +364,19 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
+  DecisionTreeFactor joint;
+  // Normalize if we have only 1 key
+  // Needed due to conversion from TableFactor
+  if (product.discreteKeys().size() == 1) {
+    joint = DecisionTreeFactor(product.discreteKeys(),
+                               product.toDecisionTreeFactor().normalize());
+  } else {
+    joint = product.toDecisionTreeFactor();
+  }
+
   // Finally, get the conditional
   auto conditional = std::make_shared<DiscreteConditional>(
-      product.toDecisionTreeFactor(), sum->toDecisionTreeFactor(), orderedKeys);
+      joint, sum->toDecisionTreeFactor(), orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From a71008d7fd572b679c7816f19f707d2d2770c3b8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 20:35:36 -0500
Subject: [PATCH 215/362] new helper constructor for DiscreteConditional

---
 gtsam/discrete/DiscreteConditional.cpp | 11 ++++++++++-
 gtsam/discrete/DiscreteConditional.h   | 11 +++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index eeb5dca3f..8396b10e0 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -24,13 +24,13 @@
 #include <gtsam/hybrid/HybridValues.h>
 
 #include <algorithm>
+#include <cassert>
 #include <random>
 #include <set>
 #include <stdexcept>
 #include <string>
 #include <utility>
 #include <vector>
-#include <cassert>
 
 using namespace std;
 using std::pair;
@@ -47,6 +47,15 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
                                          const DecisionTreeFactor& f)
     : BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {}
 
+/* ************************************************************************** */
+DiscreteConditional::DiscreteConditional(size_t nrFrontals,
+                                         const DecisionTreeFactor& f,
+                                         const Ordering& orderedKeys)
+    : BaseFactor(f), BaseConditional(nrFrontals) {
+  keys_.clear();
+  keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
+}
+
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(size_t nrFrontals,
                                          const DiscreteKeys& keys,
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 3ec9ae590..549504985 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -56,6 +56,17 @@ class GTSAM_EXPORT DiscreteConditional
   /// Construct from factor, taking the first `nFrontals` keys as frontals.
   DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
 
+  /**
+   * @brief Construct from DecisionTreeFactor,
+   * taking the first `nrFrontals` from `orderedKeys`.
+   *
+   * @param nrFrontals The number of frontal variables.
+   * @param f The DecisionTreeFactor to construct from.
+   * @param orderedKeys Ordered list of keys involved in the conditional.
+   */
+  DiscreteConditional(size_t nrFrontals, const DecisionTreeFactor& f,
+                      const Ordering& orderedKeys);
+
   /**
    * Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first
    * `nFrontals` keys as frontals, in the order given.

From 57c426a870023400c5a9f8d7bc43f508a6b4c082 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 20:36:07 -0500
Subject: [PATCH 216/362] simplify discrete conditional computation

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 14 ++------------
 1 file changed, 2 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 5fd9ddfa6..e48052c19 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -364,19 +364,9 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
-  DecisionTreeFactor joint;
-  // Normalize if we have only 1 key
-  // Needed due to conversion from TableFactor
-  if (product.discreteKeys().size() == 1) {
-    joint = DecisionTreeFactor(product.discreteKeys(),
-                               product.toDecisionTreeFactor().normalize());
-  } else {
-    joint = product.toDecisionTreeFactor();
-  }
-
-  // Finally, get the conditional
+  auto c = product / (*sum);
   auto conditional = std::make_shared<DiscreteConditional>(
-      joint, sum->toDecisionTreeFactor(), orderedKeys);
+      c.toDecisionTreeFactor(), frontalKeys.size(), orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From ffa40f71012d484da9e55323f6bb7e21eb801934 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 20:37:04 -0500
Subject: [PATCH 217/362] small fix

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index e48052c19..387a5849f 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -366,7 +366,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #endif
   auto c = product / (*sum);
   auto conditional = std::make_shared<DiscreteConditional>(
-      c.toDecisionTreeFactor(), frontalKeys.size(), orderedKeys);
+      frontalKeys.size(), c.toDecisionTreeFactor(), orderedKeys);
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From ab47adeb1873ba72d9ad2e7bb7cd6a6c94747232 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 20:45:11 -0500
Subject: [PATCH 218/362] fix empty keys case

---
 gtsam/discrete/TableFactor.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 67ba19d39..a833e1c5e 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -254,7 +254,11 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
 
   // If no keys, then return empty DecisionTreeFactor
   if (dkeys.size() == 0) {
-    return DecisionTreeFactor(dkeys, AlgebraicDecisionTree<Key>());
+    AlgebraicDecisionTree<Key> tree;
+    if (sparse_table_.size() != 0) {
+      tree = AlgebraicDecisionTree<Key>(sparse_table_.coeff(0));
+    }
+    return DecisionTreeFactor(dkeys, tree);
   }
 
   std::vector<double> table;

From 6e4d1fa1cc95969113cd8cf0c321cb808c01e0d0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 21:01:28 -0500
Subject: [PATCH 219/362] rename

---
 gtsam/base/timing.cpp | 12 ++++++------
 gtsam/base/timing.h   | 10 +++++-----
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index 83556cbd2..57321b0b0 100644
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@@ -107,7 +107,7 @@ void TimingOutline::print(const std::string& outline) const {
 }
 
 /* ************************************************************************* */
-void TimingOutline::print_csv_header(bool addLineBreak) const {
+void TimingOutline::printCsvHeader(bool addLineBreak) const {
 #ifdef GTSAM_USE_BOOST_FEATURES
   // Order is (CPU time, number of times, wall time, time + children in seconds,
   // min time, max time)
@@ -116,14 +116,14 @@ void TimingOutline::print_csv_header(bool addLineBreak) const {
             << "," << label_ + " min time (s)" << "," << label_ + "max time(s)"
             << ",";
   // Order children
-  typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
+  typedef FastMap<size_t, std::shared_ptr<TimingOutline>> ChildOrder;
   ChildOrder childOrder;
   for (const ChildMap::value_type& child : children_) {
     childOrder[child.second->myOrder_] = child.second;
   }
   // Print children
   for (const ChildOrder::value_type& order_child : childOrder) {
-    order_child.second->print_csv_header();
+    order_child.second->printCsvHeader();
   }
   if (addLineBreak) {
     std::cout << std::endl;
@@ -133,21 +133,21 @@ void TimingOutline::print_csv_header(bool addLineBreak) const {
 }
 
 /* ************************************************************************* */
-void TimingOutline::print_csv(bool addLineBreak) const {
+void TimingOutline::printCsv(bool addLineBreak) const {
 #ifdef GTSAM_USE_BOOST_FEATURES
   // Order is (CPU time, number of times, wall time, time + children in seconds,
   // min time, max time)
   std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","
             << min() << "," << max() << ",";
   // Order children
-  typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
+  typedef FastMap<size_t, std::shared_ptr<TimingOutline>> ChildOrder;
   ChildOrder childOrder;
   for (const ChildMap::value_type& child : children_) {
     childOrder[child.second->myOrder_] = child.second;
   }
   // Print children
   for (const ChildOrder::value_type& order_child : childOrder) {
-    order_child.second->print_csv(false);
+    order_child.second->printCsv(false);
   }
   if (addLineBreak) {
     std::cout << std::endl;
diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h
index dfc2928f1..4f484039d 100644
--- a/gtsam/base/timing.h
+++ b/gtsam/base/timing.h
@@ -209,7 +209,7 @@ namespace gtsam {
        * @param addLineBreak Flag indicating if a line break should be added at
        * the end. Only used at the top-leve.
        */
-      GTSAM_EXPORT void print_csv_header(bool addLineBreak = false) const;
+      GTSAM_EXPORT void printCsvHeader(bool addLineBreak = false) const;
 
       /**
        * @brief Print the times recursively from parent to child in CSV format.
@@ -220,7 +220,7 @@ namespace gtsam {
        * @param addLineBreak Flag indicating if a line break should be added at
        * the end. Only used at the top-leve.
        */
-      GTSAM_EXPORT void print_csv(bool addLineBreak = false) const;
+      GTSAM_EXPORT void printCsv(bool addLineBreak = false) const;
 
       GTSAM_EXPORT const std::shared_ptr<TimingOutline>&
         child(size_t child, const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr);
@@ -292,11 +292,11 @@ inline void tictoc_print_() {
   ::gtsam::internal::gTimingRoot->print(); }
 
 // print timing in CSV format
-inline void tictoc_print_csv_(bool displayHeader = false) {
+inline void tictoc_printCsv_(bool displayHeader = false) {
   if (displayHeader) {
-    ::gtsam::internal::gTimingRoot->print_csv_header(true);
+    ::gtsam::internal::gTimingRoot->printCsvHeader(true);
   }
-  ::gtsam::internal::gTimingRoot->print_csv(true);
+  ::gtsam::internal::gTimingRoot->printCsv(true);
 }
 
 // print mean and standard deviation

From 022ed50824f83ac10a4fb662b1284f1f53875aff Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 31 Dec 2024 21:01:59 -0500
Subject: [PATCH 220/362] move common typedef to top

---
 gtsam/base/timing.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index 57321b0b0..a0710ceda 100644
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@@ -31,7 +31,9 @@
 
 namespace gtsam {
 namespace internal {
-  
+
+using ChildOrder = FastMap<size_t, std::shared_ptr<TimingOutline>>;
+
 // a static shared_ptr to TimingOutline with nullptr as the pointer
 const static std::shared_ptr<TimingOutline> nullTimingOutline;
 
@@ -91,7 +93,6 @@ void TimingOutline::print(const std::string& outline) const {
       << n_ << " times, " << wall() << " wall, " << secs() << " children, min: "
       << min() << " max: " << max() << ")\n";
   // Order children
-  typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
   ChildOrder childOrder;
   for(const ChildMap::value_type& child: children_) {
     childOrder[child.second->myOrder_] = child.second;
@@ -116,7 +117,6 @@ void TimingOutline::printCsvHeader(bool addLineBreak) const {
             << "," << label_ + " min time (s)" << "," << label_ + "max time(s)"
             << ",";
   // Order children
-  typedef FastMap<size_t, std::shared_ptr<TimingOutline>> ChildOrder;
   ChildOrder childOrder;
   for (const ChildMap::value_type& child : children_) {
     childOrder[child.second->myOrder_] = child.second;
@@ -140,7 +140,6 @@ void TimingOutline::printCsv(bool addLineBreak) const {
   std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","
             << min() << "," << max() << ",";
   // Order children
-  typedef FastMap<size_t, std::shared_ptr<TimingOutline>> ChildOrder;
   ChildOrder childOrder;
   for (const ChildMap::value_type& child : children_) {
     childOrder[child.second->myOrder_] = child.second;

From cd27b7f2ac1f4423064a723a03eb490412d6f1ec Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Wed, 25 Dec 2024 00:56:12 -0500
Subject: [PATCH 221/362] Add BUILD_SHARED_LIBS as a configurable option

Update build logic to match and also prevent conflicting options
---
 cmake/HandleGeneralOptions.cmake | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
index 8c56ae242..3282ea682 100644
--- a/cmake/HandleGeneralOptions.cmake
+++ b/cmake/HandleGeneralOptions.cmake
@@ -21,12 +21,13 @@ if (NOT MSVC)
 endif()
 
 # Configurable Options
+option(BUILD_SHARED_LIBS                     "Build shared libraries" ON)
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
     option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
     option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
 endif()
-option(GTSAM_FORCE_SHARED_LIB               "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" ON)
+option(GTSAM_FORCE_SHARED_LIB               "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" OFF)
 option(GTSAM_FORCE_STATIC_LIB               "Force gtsam to be a static library, overriding BUILD_SHARED_LIBS" OFF)
 option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
@@ -46,7 +47,9 @@ option(GTSAM_TANGENT_PREINTEGRATION         "Use new ImuFactor with integration
 option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
 option(GTSAM_SLOW_BUT_CORRECT_EXPMAP        "Use slower but correct expmap for Pose2"  OFF)
 
-if (GTSAM_FORCE_SHARED_LIB)
+if (GTSAM_FORCE_SHARED_LIB AND GTSAM_FORCE_STATIC_LIB)
+    message(FATAL_ERROR "GTSAM_FORCE_SHARED_LIB and GTSAM_FORCE_STATIC_LIB are both true. Please, to unambiguously select the desired library type to use to build GTSAM, set one of GTSAM_FORCE_SHARED_LIB=ON, GTSAM_FORCE_STATIC_LIB=ON, or BUILD_SHARED_LIBS={ON/OFF}")
+elseif (GTSAM_FORCE_SHARED_LIB)
     message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB")
     set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
@@ -55,10 +58,9 @@ elseif (GTSAM_FORCE_STATIC_LIB)
     set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)
 elseif (BUILD_SHARED_LIBS)
-    message(STATUS "GTSAM is a shared library due to BUILD_SHARED_LIBS is ON")
     set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
-elseif((DEFINED BUILD_SHARED_LIBS) AND (NOT BUILD_SHARED_LIBS))
+elseif(NOT BUILD_SHARED_LIBS)
     message(STATUS "GTSAM is a static library due to BUILD_SHARED_LIBS is OFF")
     set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)

From 7b9b14e03e2da46f41b788be5df3f0df6b653971 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Wed, 1 Jan 2025 00:45:04 -0500
Subject: [PATCH 222/362] Fix building with a shared metis

---
 gtsam/3rdparty/metis/libmetis/CMakeLists.txt | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
index 92f931b98..f3b8694f4 100644
--- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
@@ -13,7 +13,8 @@ if(WIN32)
 		set_target_properties(metis-gtsam PROPERTIES
 			PREFIX ""
 			COMPILE_FLAGS /w
-			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
+			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin"
+			WINDOWS_EXPORT_ALL_SYMBOLS ON)
 endif()
 
 if (APPLE)

From e854d15033eafe8e0323c969a787c85f2054aee6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 11:37:49 -0500
Subject: [PATCH 223/362] evaluate needed for correct test results

---
 gtsam/discrete/DiscreteTableConditional.h | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index fefbea171..a8f187d2c 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -161,6 +161,13 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   /// Return the underlying TableFactor
   TableFactor table() const { return table_; }
 
+  using BaseConditional::evaluate;  // HybridValues version
+
+  /// Evaluate the conditional given the values.
+  virtual double evaluate(const Assignment<Key>& values) const override {
+    return table_.evaluate(values);
+  }
+
   /**
    * @brief Create new conditional by maximizing over all
    * values with the same separator.

From ec5d87e1a5684871f6a8f62ddf1855f26daf3c40 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 14:01:43 -0500
Subject: [PATCH 224/362] custom discreteMaxProduct

---
 gtsam/hybrid/HybridBayesNet.cpp | 23 ++++++++++++++++++++++-
 gtsam/hybrid/HybridBayesNet.h   |  4 ++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 7691bb209..66e4011dc 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -19,7 +19,9 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/hybrid/HybridValues.h>
 
 #include <memory>
@@ -119,6 +121,23 @@ GaussianBayesNet HybridBayesNet::choose(
   return gbn;
 }
 
+DiscreteValues HybridBayesNet::discreteMaxProduct(
+    const DiscreteFactorGraph &dfg) const {
+  TableFactor product = TableProductAndNormalize(dfg);
+
+  uint64_t maxIdx = 0;
+  double maxValue = 0.0;
+  Eigen::SparseVector<double> sparseTable = product.sparseTable();
+  for (TableFactor::SparseIt it(sparseTable); it; ++it) {
+    if (it.value() > maxValue) {
+      maxIdx = it.index();
+    }
+  }
+
+  DiscreteValues assignment = product.findAssignments(maxIdx);
+  return assignment;
+}
+
 /* ************************************************************************* */
 HybridValues HybridBayesNet::optimize() const {
   // Collect all the discrete factors to compute MPE
@@ -131,7 +150,7 @@ HybridValues HybridBayesNet::optimize() const {
   }
 
   // Solve for the MPE
-  DiscreteValues mpe = discrete_fg.optimize();
+  DiscreteValues mpe = this->discreteMaxProduct(discrete_fg);
 
   // Given the MPE, compute the optimal continuous values.
   return HybridValues(optimize(mpe), mpe);
@@ -191,6 +210,8 @@ AlgebraicDecisionTree<Key> HybridBayesNet::errorTree(
 
   // Iterate over each conditional.
   for (auto &&conditional : *this) {
+    conditional->print();
+    conditional->errorTree(continuousValues).print("errorTre", DefaultKeyFormatter);
     result = result + conditional->errorTree(continuousValues);
   }
 
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 3e07c71ce..263922636 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -268,6 +268,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// @}
 
  private:
+  /// Helper method to compute the max product assignment
+  /// given a DiscreteFactorGraph
+  DiscreteValues discreteMaxProduct(const DiscreteFactorGraph &dfg) const;
+
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;

From 2a5833bf6a78d5493a6ac273cbcfafa3691fe0b1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 14:02:15 -0500
Subject: [PATCH 225/362] custom ProductAndNormalize for TableFactor

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 13 +++----------
 gtsam/hybrid/HybridGaussianFactorGraph.h   | 10 ++++++++++
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index b9f327021..502a52742 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -24,7 +24,6 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/discrete/TableFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -256,14 +255,8 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors(
   return std::make_shared<TableFactor>(discreteKeys, potentials);
 }
 
-/**
- * @brief Multiply all the `factors` and normalize the
- * product to prevent underflow.
- *
- * @param factors The factors to multiply as a DiscreteFactorGraph.
- * @return TableFactor
- */
-static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
+/* ************************************************************************ */
+TableFactor TableProductAndNormalize(const DiscreteFactorGraph &factors) {
   // PRODUCT: multiply all factors
 #if GTSAM_HYBRID_TIMING
   gttic_(DiscreteProduct);
@@ -352,7 +345,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #endif
   /**** NOTE: This does sum-product. ****/
   // Get product factor
-  TableFactor product = ProductAndNormalize(dfg);
+  TableFactor product = TableProductAndNormalize(dfg);
 
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteSum);
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index e3c1e2d55..9803975cf 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -20,6 +20,7 @@
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
@@ -270,4 +271,13 @@ template <>
 struct traits<HybridGaussianFactorGraph>
     : public Testable<HybridGaussianFactorGraph> {};
 
+/**
+ * @brief Multiply all the `factors` and normalize the
+ * product to prevent underflow.
+ *
+ * @param factors The factors to multiply as a DiscreteFactorGraph.
+ * @return TableFactor
+ */
+TableFactor TableProductAndNormalize(const DiscreteFactorGraph& factors);
+
 }  // namespace gtsam

From 6f19ffd96673edfac3f5faa227de6c8e0d225230 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 15:10:53 -0500
Subject: [PATCH 226/362] fixed maxProduct

---
 gtsam/hybrid/HybridBayesNet.cpp                      | 3 +--
 gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 9 +++++----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 66e4011dc..aa2e0fc24 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -131,6 +131,7 @@ DiscreteValues HybridBayesNet::discreteMaxProduct(
   for (TableFactor::SparseIt it(sparseTable); it; ++it) {
     if (it.value() > maxValue) {
       maxIdx = it.index();
+      maxValue = it.value();
     }
   }
 
@@ -210,8 +211,6 @@ AlgebraicDecisionTree<Key> HybridBayesNet::errorTree(
 
   // Iterate over each conditional.
   for (auto &&conditional : *this) {
-    conditional->print();
-    conditional->errorTree(continuousValues).print("errorTre", DefaultKeyFormatter);
     result = result + conditional->errorTree(continuousValues);
   }
 
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 1942e9234..32a425474 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -650,7 +650,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
+  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "74/26");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
@@ -700,11 +700,12 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
       m1, std::vector{conditional0, conditional1});
 
   // Add prior on m1.
-  expectedBayesNet.emplace_shared<DiscreteConditional>(m1, "1/1");
+  expectedBayesNet.emplace_shared<DiscreteTableConditional>(
+      m1, "0.188638/0.811362");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
-  // EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
+  EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
 
   EXPECT(ratioTest(bn, measurements, *posterior));
 
@@ -736,7 +737,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
+  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "23/77");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();

From e49b40b4c45373a25f36052ba6026d2ceeebf77f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:03:00 -0500
Subject: [PATCH 227/362] remove TableFactor check for another day

---
 gtsam/discrete/TableFactor.cpp | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index a833e1c5e..bf9662e34 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -252,15 +252,6 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
-  // If no keys, then return empty DecisionTreeFactor
-  if (dkeys.size() == 0) {
-    AlgebraicDecisionTree<Key> tree;
-    if (sparse_table_.size() != 0) {
-      tree = AlgebraicDecisionTree<Key>(sparse_table_.coeff(0));
-    }
-    return DecisionTreeFactor(dkeys, tree);
-  }
-
   std::vector<double> table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
     table.push_back(sparse_table_.coeff(i));

From bb4ee207b837aa5a613461b6c20a67e2d01ef29d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:13:13 -0500
Subject: [PATCH 228/362] custom path for empty separator

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 49 +++++++++++-----------
 1 file changed, 24 insertions(+), 25 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 387a5849f..7cc890dc0 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -341,41 +341,40 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscrete);
 #endif
-  /**** NOTE: This does sum-product. ****/
-  // Get product factor
-  TableFactor product = ProductAndNormalize(dfg);
+  // Check if separator is empty
+  Ordering allKeys(dfg.keyVector());
+  Ordering separator;
+  std::set_difference(allKeys.begin(), allKeys.end(), frontalKeys.begin(),
+                      frontalKeys.end(),
+                      std::inserter(separator, separator.begin()));
+
+  // If the separator is empty, we have a clique of all the discrete variables
+  // so we can use the TableFactor for efficiency.
+  if (separator.size() == 0) {
+    // Get product factor
+    TableFactor product = ProductAndNormalize(dfg);
 
 #if GTSAM_HYBRID_TIMING
-  gttic_(EliminateDiscreteSum);
+    gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
-  // All the discrete variables should form a single clique,
-  // so we can sum out on all the variables as frontals.
-  // This should give an empty separator.
-  TableFactor::shared_ptr sum = product.sum(frontalKeys);
+    auto conditional = std::make_shared<DiscreteConditional>(
+        frontalKeys.size(), product.toDecisionTreeFactor());
 #if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscreteSum);
+    gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif
 
-  // Ordering keys for the conditional so that frontalKeys are really in front
-  Ordering orderedKeys;
-  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
-  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
-
+    TableFactor::shared_ptr sum = product.sum(frontalKeys);
 #if GTSAM_HYBRID_TIMING
-  gttic_(EliminateDiscreteFormDiscreteConditional);
-#endif
-  auto c = product / (*sum);
-  auto conditional = std::make_shared<DiscreteConditional>(
-      frontalKeys.size(), c.toDecisionTreeFactor(), orderedKeys);
-#if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscreteFormDiscreteConditional);
+    gttoc_(EliminateDiscrete);
 #endif
 
-#if GTSAM_HYBRID_TIMING
-  gttoc_(EliminateDiscrete);
-#endif
+    return {std::make_shared<HybridConditional>(conditional), sum};
 
-  return {std::make_shared<HybridConditional>(conditional), sum};
+  } else {
+    // Perform sum-product.
+    auto result = EliminateDiscrete(dfg, frontalKeys);
+    return {std::make_shared<HybridConditional>(result.first), result.second};
+  }
 }
 
 /* ************************************************************************ */

From 2894c957b1942d355b4c014b0a176ae6d592d078 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:15:49 -0500
Subject: [PATCH 229/362] clarify TableProduct function

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 7cc890dc0..25047bfad 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -256,13 +256,12 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors(
 }
 
 /**
- * @brief Multiply all the `factors` and normalize the
- * product to prevent underflow.
+ * @brief Multiply all the `factors` using the machinery of the TableFactor.
  *
  * @param factors The factors to multiply as a DiscreteFactorGraph.
  * @return TableFactor
  */
-static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
+static TableFactor TableProduct(const DiscreteFactorGraph &factors) {
   // PRODUCT: multiply all factors
 #if GTSAM_HYBRID_TIMING
   gttic_(DiscreteProduct);
@@ -282,14 +281,13 @@ static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
   gttoc_(DiscreteProduct);
 #endif
 
-  // Max over all the potentials by pretending all keys are frontal:
-  auto normalizer = product.max(product.size());
-
 #if GTSAM_HYBRID_TIMING
   gttic_(DiscreteNormalize);
 #endif
+  // Max over all the potentials by pretending all keys are frontal:
+  auto denominator = product.max(product.size());
   // Normalize the product factor to prevent underflow.
-  product = product / (*normalizer);
+  product = product / (*denominator);
 #if GTSAM_HYBRID_TIMING
   gttoc_(DiscreteNormalize);
 #endif
@@ -352,7 +350,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   // so we can use the TableFactor for efficiency.
   if (separator.size() == 0) {
     // Get product factor
-    TableFactor product = ProductAndNormalize(dfg);
+    TableFactor product = TableProduct(dfg);
 
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteFormDiscreteConditional);

From d22ba290547cb9a81c6e2656bd877bc4cf5a836d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:18:05 -0500
Subject: [PATCH 230/362] remove DiscreteConditional constructor since we no
 longer use it

---
 gtsam/discrete/DiscreteConditional.cpp |  9 ---------
 gtsam/discrete/DiscreteConditional.h   | 11 -----------
 2 files changed, 20 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 8396b10e0..0eea8b4bd 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -47,15 +47,6 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
                                          const DecisionTreeFactor& f)
     : BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {}
 
-/* ************************************************************************** */
-DiscreteConditional::DiscreteConditional(size_t nrFrontals,
-                                         const DecisionTreeFactor& f,
-                                         const Ordering& orderedKeys)
-    : BaseFactor(f), BaseConditional(nrFrontals) {
-  keys_.clear();
-  keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
-}
-
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(size_t nrFrontals,
                                          const DiscreteKeys& keys,
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 549504985..3ec9ae590 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -56,17 +56,6 @@ class GTSAM_EXPORT DiscreteConditional
   /// Construct from factor, taking the first `nFrontals` keys as frontals.
   DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
 
-  /**
-   * @brief Construct from DecisionTreeFactor,
-   * taking the first `nrFrontals` from `orderedKeys`.
-   *
-   * @param nrFrontals The number of frontal variables.
-   * @param f The DecisionTreeFactor to construct from.
-   * @param orderedKeys Ordered list of keys involved in the conditional.
-   */
-  DiscreteConditional(size_t nrFrontals, const DecisionTreeFactor& f,
-                      const Ordering& orderedKeys);
-
   /**
    * Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first
    * `nFrontals` keys as frontals, in the order given.

From ebd523eea4ded155412050ba9e164d23b96ff56e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:23:12 -0500
Subject: [PATCH 231/362] fix naming

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 169259a36..227bb4da3 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -112,13 +112,12 @@ namespace gtsam {
 //  }
 
   /**
-   * @brief Multiply all the `factors` and normalize the
-   * product to prevent underflow.
+   * @brief Multiply all the `factors`.
    *
    * @param factors The factors to multiply as a DiscreteFactorGraph.
    * @return DecisionTreeFactor
    */
-  static DecisionTreeFactor ProductAndNormalize(
+  static DecisionTreeFactor DiscreteProduct(
       const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
     gttic(product);
@@ -126,10 +125,10 @@ namespace gtsam {
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:
-    auto normalizer = product.max(product.size());
+    auto denominator = product.max(product.size());
 
     // Normalize the product factor to prevent underflow.
-    product = product / (*normalizer);
+    product = product / (*denominator);
 
     return product;
   }
@@ -139,7 +138,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DecisionTreeFactor product = ProductAndNormalize(factors);
+    DecisionTreeFactor product = DiscreteProduct(factors);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -217,7 +216,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DecisionTreeFactor product = ProductAndNormalize(factors);
+    DecisionTreeFactor product = DiscreteProduct(factors);
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);

From e56fac2c1b0ae70906295921fa245948937ffb19 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:45:41 -0500
Subject: [PATCH 232/362] fix TableProduct name

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

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 9803975cf..2e1c11dbe 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -278,6 +278,6 @@ struct traits<HybridGaussianFactorGraph>
  * @param factors The factors to multiply as a DiscreteFactorGraph.
  * @return TableFactor
  */
-TableFactor TableProductAndNormalize(const DiscreteFactorGraph& factors);
+TableFactor TableProduct(const DiscreteFactorGraph& factors);
 
 }  // namespace gtsam

From 26e1f088e4ff045adb5d394d14e9694310731935 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:45:55 -0500
Subject: [PATCH 233/362] fix testGaussianMixture

---
 gtsam/hybrid/tests/testGaussianMixture.cpp | 23 ++++++++--------------
 1 file changed, 8 insertions(+), 15 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 64336ae8d..d5137ca38 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -20,7 +20,6 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
@@ -80,9 +79,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   double midway = mu1 - mu0;
   auto eliminationResult =
       gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
-  auto pMid = std::dynamic_pointer_cast<DiscreteTableConditional>(
-      eliminationResult->at(0)->asDiscrete());
-  EXPECT(assert_equal(DiscreteTableConditional(m, "60/40"), *pMid));
+  auto pMid = eliminationResult->at(0)->asDiscrete();
+  EXPECT(assert_equal(DiscreteConditional(m, "60/40"), *pMid));
 
   // Everywhere else, the result should be a sigmoid.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -92,8 +90,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     // Workflow 1: convert HBN to HFG and solve
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
-    auto posterior1 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
-        eliminationResult1->at(0)->asDiscrete());
+    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -102,8 +99,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
         m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)});
     hfg1.push_back(mixing);
     auto eliminationResult2 = hfg1.eliminateSequential();
-    auto posterior2 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
-        eliminationResult2->at(0)->asDiscrete());
+    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
@@ -142,9 +138,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   EXPECT(assert_equal(expectedDiscretePosterior,
                       eliminationResultMax->discretePosterior(vv)));
 
-  auto pMax = *std::dynamic_pointer_cast<DiscreteTableConditional>(
-      eliminationResultMax->at(0)->asDiscrete());
-  EXPECT(assert_equal(DiscreteTableConditional(m, "42/58"), pMax, 1e-4));
+  auto pMax = *eliminationResultMax->at(0)->asDiscrete();
+  EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
 
   // Everywhere else, the result should be a bell curve like function.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -154,8 +149,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     // Workflow 1: convert HBN to HFG and solve
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
-    auto posterior1 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
-        eliminationResult1->at(0)->asDiscrete());
+    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -164,8 +158,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
         m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)});
     hfg.push_back(mixing);
     auto eliminationResult2 = hfg.eliminateSequential();
-    auto posterior2 = *std::dynamic_pointer_cast<DiscreteTableConditional>(
-        eliminationResult2->at(0)->asDiscrete());
+    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }

From c7c42afbaff168bbaf9f1617a9c3ac3da4d70736 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:51:04 -0500
Subject: [PATCH 234/362] undo HybridBayesNet changes

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

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index aa2e0fc24..9eb9bde55 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/hybrid/HybridValues.h>
@@ -121,24 +120,6 @@ GaussianBayesNet HybridBayesNet::choose(
   return gbn;
 }
 
-DiscreteValues HybridBayesNet::discreteMaxProduct(
-    const DiscreteFactorGraph &dfg) const {
-  TableFactor product = TableProductAndNormalize(dfg);
-
-  uint64_t maxIdx = 0;
-  double maxValue = 0.0;
-  Eigen::SparseVector<double> sparseTable = product.sparseTable();
-  for (TableFactor::SparseIt it(sparseTable); it; ++it) {
-    if (it.value() > maxValue) {
-      maxIdx = it.index();
-      maxValue = it.value();
-    }
-  }
-
-  DiscreteValues assignment = product.findAssignments(maxIdx);
-  return assignment;
-}
-
 /* ************************************************************************* */
 HybridValues HybridBayesNet::optimize() const {
   // Collect all the discrete factors to compute MPE
@@ -151,7 +132,7 @@ HybridValues HybridBayesNet::optimize() const {
   }
 
   // Solve for the MPE
-  DiscreteValues mpe = this->discreteMaxProduct(discrete_fg);
+  DiscreteValues mpe = discrete_fg.optimize();
 
   // Given the MPE, compute the optimal continuous values.
   return HybridValues(optimize(mpe), mpe);

From 27e3a04e90561932d65e0e08e94747694092f474 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 19:56:00 -0500
Subject: [PATCH 235/362] fix testHybridGaussianFactorGraph

---
 gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 32a425474..36adf458d 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -650,7 +650,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "74/26");
+  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
@@ -700,7 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
       m1, std::vector{conditional0, conditional1});
 
   // Add prior on m1.
-  expectedBayesNet.emplace_shared<DiscreteTableConditional>(
+  expectedBayesNet.emplace_shared<DiscreteConditional>(
       m1, "0.188638/0.811362");
 
   // Test elimination
@@ -737,7 +737,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "23/77");
+  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();

From cafac6317ea4cfea7d724cb4f001bdbcf25c8d9e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 20:09:18 -0500
Subject: [PATCH 236/362] fix to use DiscreteTableConditional

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

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 0213cd64b..8a25a5128 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -358,8 +358,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
-    auto conditional = std::make_shared<DiscreteConditional>(
-        frontalKeys.size(), product.toDecisionTreeFactor());
+    auto conditional =
+        std::make_shared<DiscreteTableConditional>(frontalKeys.size(), product);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From 35502f3f32cfcf63c0a19ee12e2b5c4f7e567b32 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 20:10:01 -0500
Subject: [PATCH 237/362] custom max-product for HybridBayesTree

---
 gtsam/hybrid/HybridBayesTree.cpp | 26 ++++++++++++++++++++++++--
 1 file changed, 24 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index ce2ddda81..bcd6f48c4 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -41,6 +42,25 @@ bool HybridBayesTree::equals(const This& other, double tol) const {
   return Base::equals(other, tol);
 }
 
+/* ************************************************************************* */
+DiscreteValues HybridBayesTree::discreteMaxProduct(
+    const DiscreteFactorGraph& dfg) const {
+  TableFactor product = TableProduct(dfg);
+
+  uint64_t maxIdx = 0;
+  double maxValue = 0.0;
+  Eigen::SparseVector<double> sparseTable = product.sparseTable();
+  for (TableFactor::SparseIt it(sparseTable); it; ++it) {
+    if (it.value() > maxValue) {
+      maxIdx = it.index();
+      maxValue = it.value();
+    }
+  }
+
+  DiscreteValues assignment = product.findAssignments(maxIdx);
+  return assignment;
+}
+
 /* ************************************************************************* */
 HybridValues HybridBayesTree::optimize() const {
   DiscreteFactorGraph discrete_fg;
@@ -52,8 +72,10 @@ HybridValues HybridBayesTree::optimize() const {
 
   //  The root should be discrete only, we compute the MPE
   if (root_conditional->isDiscrete()) {
-    discrete_fg.push_back(root_conditional->asDiscrete());
-    mpe = discrete_fg.optimize();
+    auto discrete = std::dynamic_pointer_cast<DiscreteTableConditional>(
+        root_conditional->asDiscrete());
+    discrete_fg.push_back(discrete);
+    mpe = discreteMaxProduct(discrete_fg);
   } else {
     throw std::runtime_error(
         "HybridBayesTree root is not discrete-only. Please check elimination "

From 62a6558d856c17750011aca0e6ff3e041a673494 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 20:10:47 -0500
Subject: [PATCH 238/362] fix discreteMaxProduct declaration

---
 gtsam/hybrid/HybridBayesNet.h  | 4 ----
 gtsam/hybrid/HybridBayesTree.h | 4 ++++
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 263922636..3e07c71ce 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -268,10 +268,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// @}
 
  private:
-  /// Helper method to compute the max product assignment
-  /// given a DiscreteFactorGraph
-  DiscreteValues discreteMaxProduct(const DiscreteFactorGraph &dfg) const;
-
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 06d880f02..ec29f7b1e 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -115,6 +115,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
   /// @}
 
  private:
+  /// Helper method to compute the max product assignment
+  /// given a DiscreteFactorGraph
+  DiscreteValues discreteMaxProduct(const DiscreteFactorGraph& dfg) const;
+
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;

From 5d2d87946245c6fb7ddd93ae0fc471f44f158e75 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 20:34:26 -0500
Subject: [PATCH 239/362] make asDiscrete a template

---
 gtsam/hybrid/HybridConditional.h | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index a08b3a6ee..3cf5b80e5 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -166,12 +166,13 @@ class GTSAM_EXPORT HybridConditional
   }
 
   /**
-   * @brief Return conditional as a DiscreteConditional
+   * @brief Return conditional as a DiscreteConditional or specified type T.
    * @return nullptr if not a DiscreteConditional
    * @return DiscreteConditional::shared_ptr
    */
-  DiscreteConditional::shared_ptr asDiscrete() const {
-    return std::dynamic_pointer_cast<DiscreteConditional>(inner_);
+  template <typename T = DiscreteConditional>
+  typename T::shared_ptr asDiscrete() const {
+    return std::dynamic_pointer_cast<T>(inner_);
   }
 
   /// Get the type-erased pointer to the inner type

From 4c5b842c734347a6a1efae6b966593bab73c0b86 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 21:50:26 -0500
Subject: [PATCH 240/362] add checks

---
 gtsam/hybrid/HybridBayesNet.cpp  | 20 +++++++++++++++++---
 gtsam/hybrid/HybridBayesTree.cpp |  3 ++-
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 9eb9bde55..7fa97051a 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -20,7 +20,6 @@
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/hybrid/HybridValues.h>
 
 #include <memory>
@@ -53,7 +52,15 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   // Multiply into one big conditional. NOTE: possibly quite expensive.
   DiscreteConditional joint;
   for (auto &&conditional : marginal) {
-    joint = joint * (*conditional);
+    // The last discrete conditional may be a DiscreteTableConditional
+    if (auto dtc =
+            std::dynamic_pointer_cast<DiscreteTableConditional>(conditional)) {
+      DiscreteConditional dc(dtc->nrFrontals(),
+                             dtc->table().toDecisionTreeFactor());
+      joint = joint * dc;
+    } else {
+      joint = joint * (*conditional);
+    }
   }
 
   // Prune the joint. NOTE: again, possibly quite expensive.
@@ -127,7 +134,14 @@ HybridValues HybridBayesNet::optimize() const {
 
   for (auto &&conditional : *this) {
     if (conditional->isDiscrete()) {
-      discrete_fg.push_back(conditional->asDiscrete());
+      if (auto dtc = conditional->asDiscrete<DiscreteTableConditional>()) {
+        // The number of keys should be small so should not
+        // be expensive to convert to DiscreteConditional.
+        discrete_fg.push_back(DiscreteConditional(
+            dtc->nrFrontals(), dtc->table().toDecisionTreeFactor()));
+      } else {
+        discrete_fg.push_back(conditional->asDiscrete());
+      }
     }
   }
 
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index bcd6f48c4..55a9c7e88 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -201,7 +201,8 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
 
 /* ************************************************************************* */
 void HybridBayesTree::prune(const size_t maxNrLeaves) {
-  auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete();
+  auto discreteProbs =
+      this->roots_.at(0)->conditional()->asDiscrete<DiscreteTableConditional>();
 
   DiscreteConditional::shared_ptr prunedDiscreteProbs =
       discreteProbs->prune(maxNrLeaves);

From e620729c4a77785faf500b7d50ac845dbb065410 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 21:50:40 -0500
Subject: [PATCH 241/362] fix testHybridEstimation

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

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 5b27e2b41..dacdeca08 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -464,14 +464,14 @@ TEST(HybridEstimation, EliminateSequentialRegression) {
 
   // Create expected discrete conditional on m0.
   DiscreteKey m(M(0), 2);
-  DiscreteConditional expected(m % "0.51341712/1");  // regression
+  DiscreteTableConditional expected(m % "0.51341712/1");  // regression
 
   // Eliminate into BN using one ordering
   const Ordering ordering1{X(0), X(1), M(0)};
   HybridBayesNet::shared_ptr bn1 = fg->eliminateSequential(ordering1);
 
   // Check that the discrete conditional matches the expected.
-  auto dc1 = bn1->back()->asDiscrete();
+  auto dc1 = bn1->back()->asDiscrete<DiscreteTableConditional>();
   EXPECT(assert_equal(expected, *dc1, 1e-9));
 
   // Eliminate into BN using a different ordering
@@ -479,7 +479,7 @@ TEST(HybridEstimation, EliminateSequentialRegression) {
   HybridBayesNet::shared_ptr bn2 = fg->eliminateSequential(ordering2);
 
   // Check that the discrete conditional matches the expected.
-  auto dc2 = bn2->back()->asDiscrete();
+  auto dc2 = bn2->back()->asDiscrete<DiscreteTableConditional>();
   EXPECT(assert_equal(expected, *dc2, 1e-9));
 }
 

From d18569be62c00d48777db4f3b6230d52f3a508d7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 21:53:07 -0500
Subject: [PATCH 242/362] fix testGaussianMixture

---
 gtsam/hybrid/tests/testGaussianMixture.cpp | 22 ++++++++++++++--------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index d5137ca38..266b05c95 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
@@ -79,8 +80,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   double midway = mu1 - mu0;
   auto eliminationResult =
       gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
-  auto pMid = eliminationResult->at(0)->asDiscrete();
-  EXPECT(assert_equal(DiscreteConditional(m, "60/40"), *pMid));
+  auto pMid = eliminationResult->at(0)->asDiscrete<DiscreteTableConditional>();
+  EXPECT(assert_equal(DiscreteTableConditional(m, "60/40"), *pMid));
 
   // Everywhere else, the result should be a sigmoid.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -90,7 +91,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     // Workflow 1: convert HBN to HFG and solve
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
-    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
+    auto posterior1 =
+        *eliminationResult1->at(0)->asDiscrete<DiscreteTableConditional>();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -99,7 +101,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
         m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)});
     hfg1.push_back(mixing);
     auto eliminationResult2 = hfg1.eliminateSequential();
-    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
+    auto posterior2 =
+        *eliminationResult2->at(0)->asDiscrete<DiscreteTableConditional>();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
@@ -138,8 +141,9 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   EXPECT(assert_equal(expectedDiscretePosterior,
                       eliminationResultMax->discretePosterior(vv)));
 
-  auto pMax = *eliminationResultMax->at(0)->asDiscrete();
-  EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
+  auto pMax =
+      *eliminationResultMax->at(0)->asDiscrete<DiscreteTableConditional>();
+  EXPECT(assert_equal(DiscreteTableConditional(m, "42/58"), pMax, 1e-4));
 
   // Everywhere else, the result should be a bell curve like function.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -149,7 +153,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     // Workflow 1: convert HBN to HFG and solve
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
-    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
+    auto posterior1 =
+        *eliminationResult1->at(0)->asDiscrete<DiscreteTableConditional>();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -158,7 +163,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
         m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)});
     hfg.push_back(mixing);
     auto eliminationResult2 = hfg.eliminateSequential();
-    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
+    auto posterior2 =
+        *eliminationResult2->at(0)->asDiscrete<DiscreteTableConditional>();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }

From 769e2c785a3800c4f79d4dc35713377503e77c6c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 21:54:50 -0500
Subject: [PATCH 243/362] fix testHybridMotionModel

---
 gtsam/hybrid/tests/testHybridMotionModel.cpp | 39 ++++++++++++--------
 1 file changed, 24 insertions(+), 15 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp
index 747a1b688..4c9843d33 100644
--- a/gtsam/hybrid/tests/testHybridMotionModel.cpp
+++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
@@ -143,8 +144,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Since no measurement on x1, we hedge our bets
     // Importance sampling run with 100k samples gives 50.051/49.949
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "50/50");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete())));
+    DiscreteTableConditional expected(m1, "50/50");
+    EXPECT(assert_equal(expected,
+                        *(bn->at(2)->asDiscrete<DiscreteTableConditional>())));
   }
 
   {
@@ -160,8 +162,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Since we have a measurement on x1, we get a definite result
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "44.3854/55.6146");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+    DiscreteTableConditional expected(m1, "44.3854/55.6146");
+    EXPECT(assert_equal(
+        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.02));
   }
 }
 
@@ -248,8 +251,10 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "48.3158/51.6842");
-    EXPECT(assert_equal(expected, *(eliminated->at(2)->asDiscrete()), 0.002));
+    DiscreteTableConditional expected(m1, "48.3158/51.6842");
+    EXPECT(assert_equal(
+        expected, *(eliminated->at(2)->asDiscrete<DiscreteTableConditional>()),
+        0.02));
   }
 
   {
@@ -263,8 +268,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "55.396/44.604");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+    DiscreteTableConditional expected(m1, "55.396/44.604");
+    EXPECT(assert_equal(
+        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.02));
   }
 }
 
@@ -340,8 +346,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "51.7762/48.2238");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+    DiscreteTableConditional expected(m1, "51.7762/48.2238");
+    EXPECT(assert_equal(
+        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.02));
   }
 
   {
@@ -355,8 +362,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "49.0762/50.9238");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005));
+    DiscreteTableConditional expected(m1, "49.0762/50.9238");
+    EXPECT(assert_equal(
+        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.05));
   }
 }
 
@@ -381,8 +389,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) {
 
   // Values taken from an importance sampling run with 100k samples:
   // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-  DiscreteConditional expected(m1, "8.91527/91.0847");
-  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+  DiscreteTableConditional expected(m1, "8.91527/91.0847");
+  EXPECT(assert_equal(
+      expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.01));
 }
 
 /* ************************************************************************* */
@@ -487,7 +496,7 @@ TEST(HybridGaussianFactorGraph, DifferentMeans) {
         VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
         DiscreteValues{{M(1), 1}});
 
-    EXPECT(assert_equal(expected, actual));
+    // EXPECT(assert_equal(expected, actual));
 
     {
       DiscreteValues dv{{M(1), 0}};

From da22055f35abff88e18864cd8d4c572a290a3d25 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 21:55:01 -0500
Subject: [PATCH 244/362] formatting

---
 gtsam/discrete/DiscreteConditional.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index aa7f1d391..c90002e78 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -478,7 +478,6 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
   return this->evaluate(x.discrete());
 }
 
-
 /* ************************************************************************* */
 void DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
   this->root_ = dc->root_;

From fcc56f5de6a076cee836b42999afe39f1db27bd5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 22:06:06 -0500
Subject: [PATCH 245/362] fix pruning test in testHybridBayesNet

---
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index b9bc29e47..88949f655 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -450,7 +450,15 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
 
   DiscreteConditional joint;
   for (auto&& conditional : posterior->discreteMarginal()) {
-    joint = joint * (*conditional);
+    // The last discrete conditional may be a DiscreteTableConditional
+    if (auto dtc =
+            std::dynamic_pointer_cast<DiscreteTableConditional>(conditional)) {
+      DiscreteConditional dc(dtc->nrFrontals(),
+                             dtc->table().toDecisionTreeFactor());
+      joint = joint * dc;
+    } else {
+      joint = joint * (*conditional);
+    }
   }
 
   size_t maxNrLeaves = 3;

From f80a3a1a1dafe58cecc9be64bccaffa4222ffdc0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 22:10:59 -0500
Subject: [PATCH 246/362] fix testHybridGaussianFactorGraph

---
 gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 36adf458d..8ce419458 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -650,7 +650,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
+  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "74/26");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
@@ -700,7 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
       m1, std::vector{conditional0, conditional1});
 
   // Add prior on m1.
-  expectedBayesNet.emplace_shared<DiscreteConditional>(
+  expectedBayesNet.emplace_shared<DiscreteTableConditional>(
       m1, "0.188638/0.811362");
 
   // Test elimination
@@ -737,7 +737,9 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
+  // Since this is the only discrete conditional, it is added as a
+  // DiscreteTableConditional.
+  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "23/77");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();

From b343a8096544d8a7468969d773094d6a09fd21ca Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 22:26:31 -0500
Subject: [PATCH 247/362] more helper methods in DiscreteTableConditional

---
 gtsam/discrete/DiscreteTableConditional.h | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index a8f187d2c..b722015c6 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -189,6 +189,14 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   virtual DiscreteConditional::shared_ptr prune(
       size_t maxNrAssignments) const override;
 
+  /// Get a DecisionTreeFactor representation.
+  DecisionTreeFactor toDecisionTreeFactor() const override {
+    return table_.toDecisionTreeFactor();
+  }
+
+  /// Get the number of non-zero values.
+  size_t nrValues() const { return table_.sparseTable().nonZeros(); }
+
   /// @}
 
  private:

From e6db6d111cdf69aabe8deebf1cb040a04fb42a73 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 22:27:36 -0500
Subject: [PATCH 248/362] cleaner API

---
 gtsam/hybrid/HybridBayesNet.cpp           | 7 +++----
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 3 +--
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 7fa97051a..20b4428d4 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -55,8 +55,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
     // The last discrete conditional may be a DiscreteTableConditional
     if (auto dtc =
             std::dynamic_pointer_cast<DiscreteTableConditional>(conditional)) {
-      DiscreteConditional dc(dtc->nrFrontals(),
-                             dtc->table().toDecisionTreeFactor());
+      DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
       joint = joint * dc;
     } else {
       joint = joint * (*conditional);
@@ -137,8 +136,8 @@ HybridValues HybridBayesNet::optimize() const {
       if (auto dtc = conditional->asDiscrete<DiscreteTableConditional>()) {
         // The number of keys should be small so should not
         // be expensive to convert to DiscreteConditional.
-        discrete_fg.push_back(DiscreteConditional(
-            dtc->nrFrontals(), dtc->table().toDecisionTreeFactor()));
+        discrete_fg.push_back(DiscreteConditional(dtc->nrFrontals(),
+                                                  dtc->toDecisionTreeFactor()));
       } else {
         discrete_fg.push_back(conditional->asDiscrete());
       }
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 88949f655..e32e96dc7 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -453,8 +453,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
     // The last discrete conditional may be a DiscreteTableConditional
     if (auto dtc =
             std::dynamic_pointer_cast<DiscreteTableConditional>(conditional)) {
-      DiscreteConditional dc(dtc->nrFrontals(),
-                             dtc->table().toDecisionTreeFactor());
+      DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
       joint = joint * dc;
     } else {
       joint = joint * (*conditional);

From fd2820ec90ad307b4096704b3e08959042d26fd8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 22:28:05 -0500
Subject: [PATCH 249/362] fix testHybridNonlinearFactorGraph

---
 gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 6e844dbcb..fd2a99c34 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -512,9 +512,10 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
   // P(m1)
   EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)});
   EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
+  DiscreteTableConditional dtc = *hybridBayesNet->at(4)->asDiscrete<DiscreteTableConditional>();
   EXPECT(
-      dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
-          ->equals(*discreteBayesNet.at(1)));
+      DiscreteConditional(dtc.nrFrontals(), dtc.toDecisionTreeFactor())
+          .equals(*discreteBayesNet.at(1)));
 }
 
 /****************************************************************************

From f42ed21137e78cec7af346dbde2d6027f8293871 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 14:35:13 -0500
Subject: [PATCH 250/362] use faster key storage

---
 gtsam/discrete/TableFactor.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index bf9662e34..459f36ccb 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -87,7 +87,7 @@ static Eigen::SparseVector<double> ComputeSparseTable(
   });
   sparseTable.reserve(nrValues);
 
-  std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
+  KeySet allKeys(dt.keys().begin(), dt.keys().end());
 
   /**
    * @brief Functor which is called by the DecisionTree for each leaf.
@@ -102,13 +102,13 @@ static Eigen::SparseVector<double> ComputeSparseTable(
   auto op = [&](const Assignment<Key>& assignment, double p) {
     if (p > 0) {
       // Get all the keys involved in this assignment
-      std::set<Key> assignmentKeys;
+      KeySet assignmentKeys;
       for (auto&& [k, _] : assignment) {
         assignmentKeys.insert(k);
       }
 
       // Find the keys missing in the assignment
-      std::vector<Key> diff;
+      KeyVector diff;
       std::set_difference(allKeys.begin(), allKeys.end(),
                           assignmentKeys.begin(), assignmentKeys.end(),
                           std::back_inserter(diff));

From 3fca55acc3f21c42a149a4ea3a29559098e0a266 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 15:17:16 -0500
Subject: [PATCH 251/362] add test exposing issue with reverse ordered keys

---
 gtsam/discrete/tests/testTableFactor.cpp | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index a455faaaa..d920f978f 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -173,6 +173,27 @@ TEST(TableFactor, Conversion) {
   TableFactor tf(dtf.discreteKeys(), dtf);
 
   EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor()));
+
+  // Test for correct construction when keys are not in reverse order.
+  // This is possible in conditionals e.g. P(x1 | x0)
+  DiscreteKey X(1, 2), Y(0, 2);
+  DiscreteConditional dtf2(
+      X, {Y}, std::vector<double>{0.33333333, 0.6, 0.66666667, 0.4});
+
+  TableFactor tf2(dtf2);
+  // GTSAM_PRINT(dtf2);
+  // GTSAM_PRINT(tf2);
+  // GTSAM_PRINT(tf2.toDecisionTreeFactor());
+
+  // Check for ADT equality since the order of keys is irrelevant
+  EXPECT(assert_equal<AlgebraicDecisionTree<Key>>(dtf2,
+                                                  tf2.toDecisionTreeFactor()));
+}
+
+/* ************************************************************************* */
+TEST_DISABLED(TableFactor, Empty) {
+  // TableFactor empty({1, 2}, std::vector<double>());
+  // empty.print();
 }
 
 /* ************************************************************************* */

From ff93c8be292941abd90e09450a9e2b3521ada9fa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 15:18:22 -0500
Subject: [PATCH 252/362] use denominators to compute the correct index in
 ComputeSparseTable

---
 gtsam/discrete/TableFactor.cpp | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 459f36ccb..742538c87 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -89,6 +89,14 @@ static Eigen::SparseVector<double> ComputeSparseTable(
 
   KeySet allKeys(dt.keys().begin(), dt.keys().end());
 
+  // Compute denominators to be used in computing sparse table indices
+  std::map<Key, size_t> denominators;
+  double denom = sparseTable.size();
+  for (const DiscreteKey& dkey : dkeys) {
+    denom /= dkey.second;
+    denominators.insert(std::pair<Key, double>(dkey.first, denom));
+  }
+
   /**
    * @brief Functor which is called by the DecisionTree for each leaf.
    * For each leaf value, we use the corresponding assignment to compute a
@@ -127,12 +135,10 @@ static Eigen::SparseVector<double> ComputeSparseTable(
 
         // Generate index and add to the sparse vector.
         Eigen::Index idx = 0;
-        size_t previousCardinality = 1;
         // We go in reverse since a DecisionTree has the highest label first
         for (auto&& it = updatedAssignment.rbegin();
              it != updatedAssignment.rend(); it++) {
-          idx += previousCardinality * it->second;
-          previousCardinality *= dt.cardinality(it->first);
+          idx += it->second * denominators.at(it->first);
         }
         sparseTable.coeffRef(idx) = p;
       }
@@ -252,9 +258,9 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
-  std::vector<double> table;
-  for (auto i = 0; i < sparse_table_.size(); i++) {
-    table.push_back(sparse_table_.coeff(i));
+  std::vector<double> table(sparse_table_.size(), 0.0);
+  for (SparseIt it(sparse_table_); it; ++it) {
+    table[it.index()] = it.value();
   }
 
   AlgebraicDecisionTree<Key> tree(dkeys, table);

From bd32eb8203d2a1ff1bc1e8fb936e751f45072574 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 15:32:11 -0500
Subject: [PATCH 253/362] unit test to expose another bug

---
 gtsam/discrete/tests/testTableFactor.cpp | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index d920f978f..e6c71e15c 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -191,9 +191,18 @@ TEST(TableFactor, Conversion) {
 }
 
 /* ************************************************************************* */
-TEST_DISABLED(TableFactor, Empty) {
-  // TableFactor empty({1, 2}, std::vector<double>());
-  // empty.print();
+TEST(TableFactor, Empty) {
+  DiscreteKey X(1, 2);
+
+  TableFactor single = *TableFactor({X}, "1 1").sum(1);
+  // Should not throw a segfault
+  EXPECT(assert_equal(*DecisionTreeFactor(X, "1 1").sum(1),
+                      single.toDecisionTreeFactor()));
+
+  TableFactor empty = *TableFactor({X}, "0 0").sum(1);
+  // Should not throw a segfault
+  EXPECT(assert_equal(*DecisionTreeFactor(X, "0 0").sum(1),
+                      empty.toDecisionTreeFactor()));
 }
 
 /* ************************************************************************* */

From f9a7eb0937e58b26ed4f96c46df9d4054c76c0c7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 15:33:20 -0500
Subject: [PATCH 254/362] add fix

---
 gtsam/discrete/TableFactor.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 742538c87..a59095d40 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -258,6 +258,16 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
+  // If no keys, then return empty DecisionTreeFactor
+  if (dkeys.size() == 0) {
+    AlgebraicDecisionTree<Key> tree;
+    // We can have an empty sparse_table_ or one with a single value.
+    if (sparse_table_.size() != 0) {
+      tree = AlgebraicDecisionTree<Key>(sparse_table_.coeff(0));
+    }
+    return DecisionTreeFactor(dkeys, tree);
+  }
+
   std::vector<double> table(sparse_table_.size(), 0.0);
   for (SparseIt it(sparse_table_); it; ++it) {
     table[it.index()] = it.value();

From 02d99590334f44887f664ddf9f16571a6170f149 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 16:04:20 -0500
Subject: [PATCH 255/362] small fix

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 25047bfad..34ee3de8c 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -273,7 +273,7 @@ static TableFactor TableProduct(const DiscreteFactorGraph &factors) {
         product = product * (*f);
       } else if (auto dtf =
                      std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
-        product = TableFactor(product * (*dtf));
+        product = product * TableFactor(*dtf);
       }
     }
   }

From 113492f8b5e5027de95ae064d47cb279cf85a84d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 16:07:46 -0500
Subject: [PATCH 256/362] separate function to collect discrete factors

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 34ee3de8c..7762249f1 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -296,14 +296,14 @@ static TableFactor TableProduct(const DiscreteFactorGraph &factors) {
 }
 
 /* ************************************************************************ */
-static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
-discreteElimination(const HybridGaussianFactorGraph &factors,
-                    const Ordering &frontalKeys) {
+static DiscreteFactorGraph CollectDiscreteFactors(
+    const HybridGaussianFactorGraph &factors) {
   DiscreteFactorGraph dfg;
 
   for (auto &f : factors) {
     if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
       dfg.push_back(df);
+
     } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
       // Case where we have a HybridGaussianFactor with no continuous keys.
       // In this case, compute a discrete factor from the remaining error.
@@ -336,6 +336,15 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     }
   }
 
+  return dfg;
+}
+
+/* ************************************************************************ */
+static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
+discreteElimination(const HybridGaussianFactorGraph &factors,
+                    const Ordering &frontalKeys) {
+  DiscreteFactorGraph dfg = CollectDiscreteFactors(factors);
+
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscrete);
 #endif

From 32317d4416bde28ee45e4b986a3bc57cf6b033a9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jan 2025 16:13:24 -0500
Subject: [PATCH 257/362] simplify empty separator check

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 ++++--------
 1 file changed, 4 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 7762249f1..8be5a8af4 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -348,16 +348,12 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscrete);
 #endif
-  // Check if separator is empty
-  Ordering allKeys(dfg.keyVector());
-  Ordering separator;
-  std::set_difference(allKeys.begin(), allKeys.end(), frontalKeys.begin(),
-                      frontalKeys.end(),
-                      std::inserter(separator, separator.begin()));
-
+  // Check if separator is empty.
+  // This is the same as checking if the number of frontal variables
+  // is the same as the number of variables in the DiscreteFactorGraph.
   // If the separator is empty, we have a clique of all the discrete variables
   // so we can use the TableFactor for efficiency.
-  if (separator.size() == 0) {
+  if (frontalKeys.size() == dfg.keys().size()) {
     // Get product factor
     TableFactor product = TableProduct(dfg);
 

From b9293b4e58724f21aa9e9498c59eb776cfa5c9ac Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 11:37:31 -0500
Subject: [PATCH 258/362] fix testHybridGaussianISAM

---
 gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
index 04b44f904..4573edad2 100644
--- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
@@ -141,7 +141,8 @@ TEST(HybridGaussianISAM, IncrementalInference) {
       expectedRemainingGraph->eliminateMultifrontal(discreteOrdering);
 
   // Test the probability values with regression tests.
-  auto discrete = isam[M(1)]->conditional()->asDiscrete();
+  auto discrete =
+      isam[M(1)]->conditional()->asDiscrete<DiscreteTableConditional>();
   EXPECT(assert_equal(0.095292, (*discrete)({{M(0), 0}, {M(1), 0}}), 1e-5));
   EXPECT(assert_equal(0.282758, (*discrete)({{M(0), 1}, {M(1), 0}}), 1e-5));
   EXPECT(assert_equal(0.314175, (*discrete)({{M(0), 0}, {M(1), 1}}), 1e-5));
@@ -221,16 +222,12 @@ TEST(HybridGaussianISAM, ApproxInference) {
     1 1 1 Leaf  0.5
   */
 
-  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteConditional>(
+  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteTableConditional>(
       incrementalHybrid[M(0)]->conditional()->inner());
   EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)}));
 
-  // Get the number of elements which are greater than 0.
-  auto count = [](const double &value, int count) {
-    return value > 0 ? count + 1 : count;
-  };
   // Check that the number of leaves after pruning is 5.
-  EXPECT_LONGS_EQUAL(5, discreteConditional_m0.fold(count, 0));
+  EXPECT_LONGS_EQUAL(5, discreteConditional_m0.nrValues());
 
   // Check that the hybrid nodes of the bayes net match those of the pre-pruning
   // bayes net, at the same positions.
@@ -477,7 +474,9 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   // Test if the optimal discrete mode assignment is (1, 1, 1).
   DiscreteFactorGraph discreteGraph;
-  discreteGraph.push_back(discreteTree);
+  // discreteTree is a DiscreteTableConditional, so we convert to
+  // DecisionTreeFactor for the DiscreteFactorGraph
+  discreteGraph.push_back(discreteTree->toDecisionTreeFactor());
   DiscreteValues optimal_assignment = discreteGraph.optimize();
 
   DiscreteValues expected_assignment;

From 8e36361e523c6fba4a5e90c2e63e64c0db6cc3bf Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 11:37:43 -0500
Subject: [PATCH 259/362] fix testHybridNonlinearISAM

---
 gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index 67cec8319..fa25407ff 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -265,16 +265,12 @@ TEST(HybridNonlinearISAM, ApproxInference) {
     1 1 1 Leaf  0.5
   */
 
-  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteConditional>(
+  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteTableConditional>(
       bayesTree[M(0)]->conditional()->inner());
   EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)}));
 
-  // Get the number of elements which are greater than 0.
-  auto count = [](const double &value, int count) {
-    return value > 0 ? count + 1 : count;
-  };
   // Check that the number of leaves after pruning is 5.
-  EXPECT_LONGS_EQUAL(5, discreteConditional_m0.fold(count, 0));
+  EXPECT_LONGS_EQUAL(5, discreteConditional_m0.nrValues());
 
   // Check that the hybrid nodes of the bayes net match those of the pre-pruning
   // bayes net, at the same positions.
@@ -520,12 +516,13 @@ TEST(HybridNonlinearISAM, NonTrivial) {
 
   // The final discrete graph should not be empty since we have eliminated
   // all continuous variables.
-  auto discreteTree = bayesTree[M(3)]->conditional()->asDiscrete();
+  auto discreteTree =
+      bayesTree[M(3)]->conditional()->asDiscrete<DiscreteTableConditional>();
   EXPECT_LONGS_EQUAL(3, discreteTree->size());
 
   // Test if the optimal discrete mode assignment is (1, 1, 1).
   DiscreteFactorGraph discreteGraph;
-  discreteGraph.push_back(discreteTree);
+  discreteGraph.push_back(discreteTree->toDecisionTreeFactor());
   DiscreteValues optimal_assignment = discreteGraph.optimize();
 
   DiscreteValues expected_assignment;

From 62a35c09cddcdd1423800379f20fecbb68a5cab8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:08:37 -0500
Subject: [PATCH 260/362] serialize table inside TableDistribution

---
 gtsam/discrete/DiscreteTableConditional.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index b722015c6..e35ce925b 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -206,6 +206,7 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
+    ar& BOOST_SERIALIZATION_NVP(table_);
   }
 #endif
 };

From cbcfab4176b0577cc62e3cb77c3522cba90e2015 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:09:25 -0500
Subject: [PATCH 261/362] serialize functions for Eigen::SparseVector

---
 gtsam/base/MatrixSerialization.h | 40 ++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h
index 11b6a417a..43c97097d 100644
--- a/gtsam/base/MatrixSerialization.h
+++ b/gtsam/base/MatrixSerialization.h
@@ -24,6 +24,7 @@
 
 #include <gtsam/base/Matrix.h>
 
+#include <Eigen/Sparse>
 #include <boost/serialization/array.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/split_free.hpp>
@@ -87,6 +88,45 @@ void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
   split_free(ar, m, version);
 }
 
+/******************************************************************************/
+/// Customized functions for serializing Eigen::SparseVector
+template <class Archive, typename _Scalar, int _Options, typename _Index>
+void save(Archive& ar, const Eigen::SparseVector<_Scalar, _Options, _Index>& m,
+          const unsigned int /*version*/) {
+  _Index size = m.size();
+
+  std::vector<std::pair<Eigen::Index, _Scalar>> data;
+  for (typename Eigen::SparseVector<_Scalar, _Options, _Index>::InnerIterator
+           it(m);
+       it; ++it)
+    data.push_back({it.index(), it.value()});
+
+  ar << BOOST_SERIALIZATION_NVP(size);
+  ar << BOOST_SERIALIZATION_NVP(data);
+}
+
+template <class Archive, typename _Scalar, int _Options, typename _Index>
+void load(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
+          const unsigned int /*version*/) {
+  _Index size;
+  ar >> BOOST_SERIALIZATION_NVP(size);
+  m.resize(size);
+
+  std::vector<std::pair<Eigen::Index, _Scalar>> data;
+  ar >> BOOST_SERIALIZATION_NVP(data);
+
+  for (auto&& d : data) {
+    m.coeffRef(d.first) = d.second;
+  }
+}
+
+template <class Archive, typename _Scalar, int _Options, typename _Index>
+void serialize(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
+               const unsigned int version) {
+  split_free(ar, m, version);
+}
+/******************************************************************************/
+
 }  // namespace serialization
 }  // namespace boost
 #endif

From 5c4194e7cd800c8cd232f92a63dbd8e10b935340 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:13:51 -0500
Subject: [PATCH 262/362] add serialization code to TableFactor

---
 gtsam/discrete/TableFactor.h | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 5ddb4ab43..a2fdb4d32 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -31,6 +31,12 @@
 #include <utility>
 #include <vector>
 
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <gtsam/base/MatrixSerialization.h>
+
+#include <boost/serialization/nvp.hpp>
+#endif
+
 namespace gtsam {
 
 class DiscreteConditional;
@@ -342,6 +348,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   double error(const HybridValues& values) const override;
 
   /// @}
+
+ private:
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar& BOOST_SERIALIZATION_NVP(sparse_table_);
+    ar& BOOST_SERIALIZATION_NVP(denominators_);
+    ar& BOOST_SERIALIZATION_NVP(sorted_dkeys_);
+  }
+#endif
 };
 
 // traits

From ef2843c5b213e11463de8217e790a3e6e9cbbd25 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:14:07 -0500
Subject: [PATCH 263/362] test for TableFactor serialization

---
 .../discrete/tests/testSerializationDiscrete.cpp  | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/gtsam/discrete/tests/testSerializationDiscrete.cpp b/gtsam/discrete/tests/testSerializationDiscrete.cpp
index df7df0b7e..9d15d0536 100644
--- a/gtsam/discrete/tests/testSerializationDiscrete.cpp
+++ b/gtsam/discrete/tests/testSerializationDiscrete.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Symbol.h>
 
 using namespace std;
@@ -32,6 +33,7 @@ BOOST_CLASS_EXPORT_GUID(Tree::Leaf, "gtsam_DecisionTreeStringInt_Leaf")
 BOOST_CLASS_EXPORT_GUID(Tree::Choice, "gtsam_DecisionTreeStringInt_Choice")
 
 BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
+BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor");
 
 using ADT = AlgebraicDecisionTree<Key>;
 BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
@@ -79,6 +81,19 @@ TEST(DiscreteSerialization, DecisionTreeFactor) {
   EXPECT(equalsBinary<DecisionTreeFactor>(f));
 }
 
+/* ************************************************************************* */
+// Check serialization for TableFactor
+TEST(DiscreteSerialization, TableFactor) {
+  using namespace serializationTestHelpers;
+
+  DiscreteKey A(Symbol('x', 1), 3);
+  TableFactor tf(A % "1/2/2");
+
+  EXPECT(equalsObj<TableFactor>(tf));
+  EXPECT(equalsXML<TableFactor>(tf));
+  EXPECT(equalsBinary<TableFactor>(tf));
+}
+
 /* ************************************************************************* */
 // Check serialization for DiscreteConditional & DiscreteDistribution
 TEST(DiscreteSerialization, DiscreteConditional) {

From 834288f9748992b24bc4d4f4cffc77c7d8461d8c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:18:24 -0500
Subject: [PATCH 264/362] additional Signature based constructor for
 DecisionTreeFactor

---
 gtsam/discrete/DecisionTreeFactor.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 80ee10a7b..24a699d42 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/discrete/Signature.h>
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
@@ -116,6 +117,10 @@ namespace gtsam {
     DecisionTreeFactor(const DiscreteKey& key, const std::vector<double>& row)
         : DecisionTreeFactor(DiscreteKeys{key}, row) {}
 
+    /// Construct from Signature
+    DecisionTreeFactor(const Signature& signature)
+        : DecisionTreeFactor(signature.discreteKeys(), signature.cpt()) {}
+
     /** Construct from a DiscreteConditional type */
     explicit DecisionTreeFactor(const DiscreteConditional& c);
 

From e6567457b511a6ff993efcf2710c98f72c71bdad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:21:19 -0500
Subject: [PATCH 265/362] update tests

---
 .../discrete/tests/testDecisionTreeFactor.cpp | 22 +++++++------------
 1 file changed, 8 insertions(+), 14 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 756a0cebe..1828db525 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -217,12 +217,6 @@ void maybeSaveDotFile(const DecisionTreeFactor& f, const string& filename) {
 #endif
 }
 
-/** Convert Signature into CPT */
-DecisionTreeFactor create(const Signature& signature) {
-  DecisionTreeFactor p(signature.discreteKeys(), signature.cpt());
-  return p;
-}
-
 /* ************************************************************************* */
 // test Asia Joint
 TEST(DecisionTreeFactor, joint) {
@@ -230,14 +224,14 @@ TEST(DecisionTreeFactor, joint) {
       D(7, 2);
 
   gttic_(asiaCPTs);
-  DecisionTreeFactor pA = create(A % "99/1");
-  DecisionTreeFactor pS = create(S % "50/50");
-  DecisionTreeFactor pT = create(T | A = "99/1 95/5");
-  DecisionTreeFactor pL = create(L | S = "99/1 90/10");
-  DecisionTreeFactor pB = create(B | S = "70/30 40/60");
-  DecisionTreeFactor pE = create((E | T, L) = "F T T T");
-  DecisionTreeFactor pX = create(X | E = "95/5 2/98");
-  DecisionTreeFactor pD = create((D | E, B) = "9/1 2/8 3/7 1/9");
+  DecisionTreeFactor pA(A % "99/1");
+  DecisionTreeFactor pS(S % "50/50");
+  DecisionTreeFactor pT(T | A = "99/1 95/5");
+  DecisionTreeFactor pL(L | S = "99/1 90/10");
+  DecisionTreeFactor pB(B | S = "70/30 40/60");
+  DecisionTreeFactor pE((E | T, L) = "F T T T");
+  DecisionTreeFactor pX(X | E = "95/5 2/98");
+  DecisionTreeFactor pD((D | E, B) = "9/1 2/8 3/7 1/9");
 
   // Create joint
   gttic_(asiaJoint);

From cb9cec30e39895b4745a4727aa896718dcccc467 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:28:21 -0500
Subject: [PATCH 266/362] unit test exposing division bug

---
 gtsam/discrete/tests/testDecisionTreeFactor.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 1828db525..73420c860 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -69,6 +69,15 @@ TEST(DecisionTreeFactor, constructors) {
   EXPECT_DOUBLES_EQUAL(0.8, f4(x121), 1e-9);
 }
 
+/* ************************************************************************* */
+TEST(DecisionTreeFactor, Divide) {
+  DiscreteKey A(0, 2), S(1, 2);
+  DecisionTreeFactor pA(A % "99/1"), pS(S % "50/50");
+  DecisionTreeFactor joint = pA * pS;
+  DecisionTreeFactor s = joint / pA;
+  EXPECT(assert_equal(pS, s));
+}
+
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, Error) {
   // Declare a bunch of keys

From c6c451bee102f624cf25660ec6a820c9d5c1c49c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:28:40 -0500
Subject: [PATCH 267/362] compute correct subset of keys for division

---
 gtsam/discrete/DecisionTreeFactor.h | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 24a699d42..0b94140da 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -161,7 +161,15 @@ namespace gtsam {
 
     /// divide by factor f (safely)
     DecisionTreeFactor operator/(const DecisionTreeFactor& f) const {
-      return apply(f, safe_div);
+      KeyVector diff;
+      std::set_difference(this->keys().begin(), this->keys().end(),
+                          f.keys().begin(), f.keys().end(),
+                          std::back_inserter(diff));
+      DiscreteKeys keys;
+      for (Key key : diff) {
+        keys.push_back({key, this->cardinality(key)});
+      }
+      return DecisionTreeFactor(keys, apply(f, safe_div));
     }
 
     /// Convert into a decision tree

From 3718cb19edd21c718fe280c712a793031b6fd707 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:35:13 -0500
Subject: [PATCH 268/362] use string based constructor

---
 gtsam/discrete/tests/testSerializationDiscrete.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/tests/testSerializationDiscrete.cpp b/gtsam/discrete/tests/testSerializationDiscrete.cpp
index 9d15d0536..b118a00f6 100644
--- a/gtsam/discrete/tests/testSerializationDiscrete.cpp
+++ b/gtsam/discrete/tests/testSerializationDiscrete.cpp
@@ -87,7 +87,7 @@ TEST(DiscreteSerialization, TableFactor) {
   using namespace serializationTestHelpers;
 
   DiscreteKey A(Symbol('x', 1), 3);
-  TableFactor tf(A % "1/2/2");
+  TableFactor tf(A, "1 2 2");
 
   EXPECT(equalsObj<TableFactor>(tf));
   EXPECT(equalsXML<TableFactor>(tf));

From ffe14d39aae1609c4d85b863a3ffd5ebca0089ac Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:42:48 -0500
Subject: [PATCH 269/362] Revert "update tests"

This reverts commit e6567457b511a6ff993efcf2710c98f72c71bdad.
---
 .../discrete/tests/testDecisionTreeFactor.cpp | 22 ++++++++++++-------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 73420c860..61ce9038d 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -226,6 +226,12 @@ void maybeSaveDotFile(const DecisionTreeFactor& f, const string& filename) {
 #endif
 }
 
+/** Convert Signature into CPT */
+DecisionTreeFactor create(const Signature& signature) {
+  DecisionTreeFactor p(signature.discreteKeys(), signature.cpt());
+  return p;
+}
+
 /* ************************************************************************* */
 // test Asia Joint
 TEST(DecisionTreeFactor, joint) {
@@ -233,14 +239,14 @@ TEST(DecisionTreeFactor, joint) {
       D(7, 2);
 
   gttic_(asiaCPTs);
-  DecisionTreeFactor pA(A % "99/1");
-  DecisionTreeFactor pS(S % "50/50");
-  DecisionTreeFactor pT(T | A = "99/1 95/5");
-  DecisionTreeFactor pL(L | S = "99/1 90/10");
-  DecisionTreeFactor pB(B | S = "70/30 40/60");
-  DecisionTreeFactor pE((E | T, L) = "F T T T");
-  DecisionTreeFactor pX(X | E = "95/5 2/98");
-  DecisionTreeFactor pD((D | E, B) = "9/1 2/8 3/7 1/9");
+  DecisionTreeFactor pA = create(A % "99/1");
+  DecisionTreeFactor pS = create(S % "50/50");
+  DecisionTreeFactor pT = create(T | A = "99/1 95/5");
+  DecisionTreeFactor pL = create(L | S = "99/1 90/10");
+  DecisionTreeFactor pB = create(B | S = "70/30 40/60");
+  DecisionTreeFactor pE = create((E | T, L) = "F T T T");
+  DecisionTreeFactor pX = create(X | E = "95/5 2/98");
+  DecisionTreeFactor pD = create((D | E, B) = "9/1 2/8 3/7 1/9");
 
   // Create joint
   gttic_(asiaJoint);

From be7be376a9ef95b12c08b83d72bb602cc93eb852 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:42:56 -0500
Subject: [PATCH 270/362] Revert "additional Signature based constructor for
 DecisionTreeFactor"

This reverts commit 834288f9748992b24bc4d4f4cffc77c7d8461d8c.
---
 gtsam/discrete/DecisionTreeFactor.h | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 0b94140da..804b956fe 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include <gtsam/discrete/Signature.h>
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
@@ -117,10 +116,6 @@ namespace gtsam {
     DecisionTreeFactor(const DiscreteKey& key, const std::vector<double>& row)
         : DecisionTreeFactor(DiscreteKeys{key}, row) {}
 
-    /// Construct from Signature
-    DecisionTreeFactor(const Signature& signature)
-        : DecisionTreeFactor(signature.discreteKeys(), signature.cpt()) {}
-
     /** Construct from a DiscreteConditional type */
     explicit DecisionTreeFactor(const DiscreteConditional& c);
 

From 030207528055d43cdcd81cb7e60f47e20bca6a63 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:09:25 -0500
Subject: [PATCH 271/362] serialize functions for Eigen::SparseVector

---
 gtsam/base/MatrixSerialization.h | 40 ++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h
index 11b6a417a..43c97097d 100644
--- a/gtsam/base/MatrixSerialization.h
+++ b/gtsam/base/MatrixSerialization.h
@@ -24,6 +24,7 @@
 
 #include <gtsam/base/Matrix.h>
 
+#include <Eigen/Sparse>
 #include <boost/serialization/array.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/split_free.hpp>
@@ -87,6 +88,45 @@ void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
   split_free(ar, m, version);
 }
 
+/******************************************************************************/
+/// Customized functions for serializing Eigen::SparseVector
+template <class Archive, typename _Scalar, int _Options, typename _Index>
+void save(Archive& ar, const Eigen::SparseVector<_Scalar, _Options, _Index>& m,
+          const unsigned int /*version*/) {
+  _Index size = m.size();
+
+  std::vector<std::pair<Eigen::Index, _Scalar>> data;
+  for (typename Eigen::SparseVector<_Scalar, _Options, _Index>::InnerIterator
+           it(m);
+       it; ++it)
+    data.push_back({it.index(), it.value()});
+
+  ar << BOOST_SERIALIZATION_NVP(size);
+  ar << BOOST_SERIALIZATION_NVP(data);
+}
+
+template <class Archive, typename _Scalar, int _Options, typename _Index>
+void load(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
+          const unsigned int /*version*/) {
+  _Index size;
+  ar >> BOOST_SERIALIZATION_NVP(size);
+  m.resize(size);
+
+  std::vector<std::pair<Eigen::Index, _Scalar>> data;
+  ar >> BOOST_SERIALIZATION_NVP(data);
+
+  for (auto&& d : data) {
+    m.coeffRef(d.first) = d.second;
+  }
+}
+
+template <class Archive, typename _Scalar, int _Options, typename _Index>
+void serialize(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
+               const unsigned int version) {
+  split_free(ar, m, version);
+}
+/******************************************************************************/
+
 }  // namespace serialization
 }  // namespace boost
 #endif

From 92b5bb190412e2e373ac360150c98927465b2a47 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:13:51 -0500
Subject: [PATCH 272/362] add serialization code to TableFactor

---
 gtsam/discrete/TableFactor.h | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 5ddb4ab43..a2fdb4d32 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -31,6 +31,12 @@
 #include <utility>
 #include <vector>
 
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <gtsam/base/MatrixSerialization.h>
+
+#include <boost/serialization/nvp.hpp>
+#endif
+
 namespace gtsam {
 
 class DiscreteConditional;
@@ -342,6 +348,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   double error(const HybridValues& values) const override;
 
   /// @}
+
+ private:
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar& BOOST_SERIALIZATION_NVP(sparse_table_);
+    ar& BOOST_SERIALIZATION_NVP(denominators_);
+    ar& BOOST_SERIALIZATION_NVP(sorted_dkeys_);
+  }
+#endif
 };
 
 // traits

From 50414689fd0a6b7401d0b1f0389ad25fdb2e2f3d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:14:07 -0500
Subject: [PATCH 273/362] test for TableFactor serialization

---
 .../discrete/tests/testSerializationDiscrete.cpp  | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/gtsam/discrete/tests/testSerializationDiscrete.cpp b/gtsam/discrete/tests/testSerializationDiscrete.cpp
index df7df0b7e..9d15d0536 100644
--- a/gtsam/discrete/tests/testSerializationDiscrete.cpp
+++ b/gtsam/discrete/tests/testSerializationDiscrete.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Symbol.h>
 
 using namespace std;
@@ -32,6 +33,7 @@ BOOST_CLASS_EXPORT_GUID(Tree::Leaf, "gtsam_DecisionTreeStringInt_Leaf")
 BOOST_CLASS_EXPORT_GUID(Tree::Choice, "gtsam_DecisionTreeStringInt_Choice")
 
 BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
+BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor");
 
 using ADT = AlgebraicDecisionTree<Key>;
 BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
@@ -79,6 +81,19 @@ TEST(DiscreteSerialization, DecisionTreeFactor) {
   EXPECT(equalsBinary<DecisionTreeFactor>(f));
 }
 
+/* ************************************************************************* */
+// Check serialization for TableFactor
+TEST(DiscreteSerialization, TableFactor) {
+  using namespace serializationTestHelpers;
+
+  DiscreteKey A(Symbol('x', 1), 3);
+  TableFactor tf(A % "1/2/2");
+
+  EXPECT(equalsObj<TableFactor>(tf));
+  EXPECT(equalsXML<TableFactor>(tf));
+  EXPECT(equalsBinary<TableFactor>(tf));
+}
+
 /* ************************************************************************* */
 // Check serialization for DiscreteConditional & DiscreteDistribution
 TEST(DiscreteSerialization, DiscreteConditional) {

From b28cae275b2c574f3efc5d7eb610336e8b1ff520 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 13:35:13 -0500
Subject: [PATCH 274/362] use string based constructor

---
 gtsam/discrete/tests/testSerializationDiscrete.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/tests/testSerializationDiscrete.cpp b/gtsam/discrete/tests/testSerializationDiscrete.cpp
index 9d15d0536..b118a00f6 100644
--- a/gtsam/discrete/tests/testSerializationDiscrete.cpp
+++ b/gtsam/discrete/tests/testSerializationDiscrete.cpp
@@ -87,7 +87,7 @@ TEST(DiscreteSerialization, TableFactor) {
   using namespace serializationTestHelpers;
 
   DiscreteKey A(Symbol('x', 1), 3);
-  TableFactor tf(A % "1/2/2");
+  TableFactor tf(A, "1 2 2");
 
   EXPECT(equalsObj<TableFactor>(tf));
   EXPECT(equalsXML<TableFactor>(tf));

From 9b1918c085785c20efc07b65ed153a05012c5f8d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 14:51:32 -0500
Subject: [PATCH 275/362] rename from DiscreteTableConditional to
 TableDistribution

---
 gtsam/discrete/DiscreteTableConditional.cpp   | 42 ++++++++--------
 gtsam/discrete/DiscreteTableConditional.h     | 50 +++++++++----------
 gtsam/hybrid/HybridBayesNet.cpp               |  6 +--
 gtsam/hybrid/HybridBayesTree.cpp              |  6 +--
 gtsam/hybrid/HybridGaussianFactorGraph.cpp    |  6 +--
 gtsam/hybrid/HybridGaussianFactorGraph.h      |  2 +-
 gtsam/hybrid/tests/testGaussianMixture.cpp    | 18 +++----
 gtsam/hybrid/tests/testHybridBayesNet.cpp     |  4 +-
 gtsam/hybrid/tests/testHybridEstimation.cpp   |  6 +--
 .../tests/testHybridGaussianFactorGraph.cpp   |  8 +--
 gtsam/hybrid/tests/testHybridGaussianISAM.cpp |  6 +--
 gtsam/hybrid/tests/testHybridMotionModel.cpp  | 30 +++++------
 .../tests/testHybridNonlinearFactorGraph.cpp  |  2 +-
 .../hybrid/tests/testHybridNonlinearISAM.cpp  |  4 +-
 14 files changed, 95 insertions(+), 95 deletions(-)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/DiscreteTableConditional.cpp
index 9aff487cf..87eeb1614 100644
--- a/gtsam/discrete/DiscreteTableConditional.cpp
+++ b/gtsam/discrete/DiscreteTableConditional.cpp
@@ -10,14 +10,14 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file DiscreteTableConditional.cpp
+ *  @file TableDistribution.cpp
  *  @date Dec 22, 2024
  *  @author Varun Agrawal
  */
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/debug.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/discrete/Ring.h>
 #include <gtsam/discrete/Signature.h>
 #include <gtsam/hybrid/HybridValues.h>
@@ -38,42 +38,42 @@ using std::vector;
 namespace gtsam {
 
 /* ************************************************************************** */
-DiscreteTableConditional::DiscreteTableConditional(const size_t nrFrontals,
+TableDistribution::TableDistribution(const size_t nrFrontals,
                                                    const TableFactor& f)
     : BaseConditional(nrFrontals, DecisionTreeFactor(f.discreteKeys(), ADT())),
       table_(f / (*f.sum(nrFrontals))) {}
 
 /* ************************************************************************** */
-DiscreteTableConditional::DiscreteTableConditional(
+TableDistribution::TableDistribution(
     size_t nrFrontals, const DiscreteKeys& keys,
     const Eigen::SparseVector<double>& potentials)
     : BaseConditional(nrFrontals, keys, DecisionTreeFactor(keys, ADT())),
       table_(TableFactor(keys, potentials)) {}
 
 /* ************************************************************************** */
-DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
+TableDistribution::TableDistribution(const TableFactor& joint,
                                                    const TableFactor& marginal)
     : BaseConditional(joint.size() - marginal.size(),
                       joint.discreteKeys() & marginal.discreteKeys(), ADT()),
       table_(joint / marginal) {}
 
 /* ************************************************************************** */
-DiscreteTableConditional::DiscreteTableConditional(const TableFactor& joint,
+TableDistribution::TableDistribution(const TableFactor& joint,
                                                    const TableFactor& marginal,
                                                    const Ordering& orderedKeys)
-    : DiscreteTableConditional(joint, marginal) {
+    : TableDistribution(joint, marginal) {
   keys_.clear();
   keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
 }
 
 /* ************************************************************************** */
-DiscreteTableConditional::DiscreteTableConditional(const Signature& signature)
+TableDistribution::TableDistribution(const Signature& signature)
     : BaseConditional(1, DecisionTreeFactor(DiscreteKeys{{1, 1}}, ADT(1))),
       table_(TableFactor(signature.discreteKeys(), signature.cpt())) {}
 
 /* ************************************************************************** */
-DiscreteTableConditional DiscreteTableConditional::operator*(
-    const DiscreteTableConditional& other) const {
+TableDistribution TableDistribution::operator*(
+    const TableDistribution& other) const {
   // Take union of frontal keys
   std::set<Key> newFrontals;
   for (auto&& key : this->frontals()) newFrontals.insert(key);
@@ -82,7 +82,7 @@ DiscreteTableConditional DiscreteTableConditional::operator*(
   // Check if frontals overlapped
   if (nrFrontals() + other.nrFrontals() > newFrontals.size())
     throw std::invalid_argument(
-        "DiscreteTableConditional::operator* called with overlapping frontal "
+        "TableDistribution::operator* called with overlapping frontal "
         "keys.");
 
   // Now, add cardinalities.
@@ -106,11 +106,11 @@ DiscreteTableConditional DiscreteTableConditional::operator*(
   for (auto&& dk : parents) discreteKeys.push_back(dk);
 
   TableFactor product = this->table_ * other.table();
-  return DiscreteTableConditional(newFrontals.size(), product);
+  return TableDistribution(newFrontals.size(), product);
 }
 
 /* ************************************************************************** */
-void DiscreteTableConditional::print(const string& s,
+void TableDistribution::print(const string& s,
                                      const KeyFormatter& formatter) const {
   cout << s << " P( ";
   for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
@@ -128,9 +128,9 @@ void DiscreteTableConditional::print(const string& s,
 }
 
 /* ************************************************************************** */
-bool DiscreteTableConditional::equals(const DiscreteFactor& other,
+bool TableDistribution::equals(const DiscreteFactor& other,
                                       double tol) const {
-  auto dtc = dynamic_cast<const DiscreteTableConditional*>(&other);
+  auto dtc = dynamic_cast<const TableDistribution*>(&other);
   if (!dtc) {
     return false;
   } else {
@@ -142,17 +142,17 @@ bool DiscreteTableConditional::equals(const DiscreteFactor& other,
 }
 
 /* ****************************************************************************/
-DiscreteConditional::shared_ptr DiscreteTableConditional::max(
+DiscreteConditional::shared_ptr TableDistribution::max(
     const Ordering& keys) const {
   auto m = *table_.max(keys);
 
-  return std::make_shared<DiscreteTableConditional>(m.discreteKeys().size(), m);
+  return std::make_shared<TableDistribution>(m.discreteKeys().size(), m);
 }
 
 /* ****************************************************************************/
-void DiscreteTableConditional::setData(
+void TableDistribution::setData(
     const DiscreteConditional::shared_ptr& dc) {
-  if (auto dtc = std::dynamic_pointer_cast<DiscreteTableConditional>(dc)) {
+  if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(dc)) {
     this->table_ = dtc->table_;
   } else {
     this->table_ = TableFactor(dc->discreteKeys(), *dc);
@@ -160,11 +160,11 @@ void DiscreteTableConditional::setData(
 }
 
 /* ****************************************************************************/
-DiscreteConditional::shared_ptr DiscreteTableConditional::prune(
+DiscreteConditional::shared_ptr TableDistribution::prune(
     size_t maxNrAssignments) const {
   TableFactor pruned = table_.prune(maxNrAssignments);
 
-  return std::make_shared<DiscreteTableConditional>(
+  return std::make_shared<TableDistribution>(
       this->nrFrontals(), this->discreteKeys(), pruned.sparseTable());
 }
 
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/DiscreteTableConditional.h
index e35ce925b..cb99c8677 100644
--- a/gtsam/discrete/DiscreteTableConditional.h
+++ b/gtsam/discrete/DiscreteTableConditional.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file DiscreteTableConditional.h
+ *  @file TableDistribution.h
  *  @date Dec 22, 2024
  *  @author Varun Agrawal
  */
@@ -34,7 +34,7 @@ namespace gtsam {
  *
  * @ingroup discrete
  */
-class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
+class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
  private:
   TableFactor table_;
 
@@ -42,7 +42,7 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
 
  public:
   // typedefs needed to play nice with gtsam
-  typedef DiscreteTableConditional This;     ///< Typedef to this class
+  typedef TableDistribution This;     ///< Typedef to this class
   typedef std::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
   typedef DiscreteConditional
       BaseConditional;  ///< Typedef to our conditional base class
@@ -53,42 +53,42 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   /// @{
 
   /// Default constructor needed for serialization.
-  DiscreteTableConditional() {}
+  TableDistribution() {}
 
   /// Construct from factor, taking the first `nFrontals` keys as frontals.
-  DiscreteTableConditional(size_t nFrontals, const TableFactor& f);
+  TableDistribution(size_t nFrontals, const TableFactor& f);
 
   /**
    * Construct from DiscreteKeys and SparseVector, taking the first
    * `nFrontals` keys as frontals, in the order given.
    */
-  DiscreteTableConditional(size_t nFrontals, const DiscreteKeys& keys,
+  TableDistribution(size_t nFrontals, const DiscreteKeys& keys,
                            const Eigen::SparseVector<double>& potentials);
 
   /** Construct from signature */
-  explicit DiscreteTableConditional(const Signature& signature);
+  explicit TableDistribution(const Signature& signature);
 
   /**
    * Construct from key, parents, and a Signature::Table specifying the
    * conditional probability table (CPT) in 00 01 10 11 order. For
    * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
    *
-   * Example: DiscreteTableConditional P(D, {B,E}, table);
+   * Example: TableDistribution P(D, {B,E}, table);
    */
-  DiscreteTableConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+  TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
                            const Signature::Table& table)
-      : DiscreteTableConditional(Signature(key, parents, table)) {}
+      : TableDistribution(Signature(key, parents, table)) {}
 
   /**
    * Construct from key, parents, and a vector<double> specifying the
    * conditional probability table (CPT) in 00 01 10 11 order. For
    * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
    *
-   * Example: DiscreteTableConditional P(D, {B,E}, table);
+   * Example: TableDistribution P(D, {B,E}, table);
    */
-  DiscreteTableConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+  TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
                            const std::vector<double>& table)
-      : DiscreteTableConditional(
+      : TableDistribution(
             1, TableFactor(DiscreteKeys{key} & parents, table)) {}
 
   /**
@@ -98,21 +98,21 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
    *
    * The string is parsed into a Signature::Table.
    *
-   * Example: DiscreteTableConditional P(D, {B,E}, "9/1 2/8 3/7 1/9");
+   * Example: TableDistribution P(D, {B,E}, "9/1 2/8 3/7 1/9");
    */
-  DiscreteTableConditional(const DiscreteKey& key, const DiscreteKeys& parents,
+  TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
                            const std::string& spec)
-      : DiscreteTableConditional(Signature(key, parents, spec)) {}
+      : TableDistribution(Signature(key, parents, spec)) {}
 
   /// No-parent specialization; can also use DiscreteDistribution.
-  DiscreteTableConditional(const DiscreteKey& key, const std::string& spec)
-      : DiscreteTableConditional(Signature(key, {}, spec)) {}
+  TableDistribution(const DiscreteKey& key, const std::string& spec)
+      : TableDistribution(Signature(key, {}, spec)) {}
 
   /**
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    */
-  DiscreteTableConditional(const TableFactor& joint,
+  TableDistribution(const TableFactor& joint,
                            const TableFactor& marginal);
 
   /**
@@ -120,7 +120,7 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    * Makes sure the keys are ordered as given. Does not check orderedKeys.
    */
-  DiscreteTableConditional(const TableFactor& joint,
+  TableDistribution(const TableFactor& joint,
                            const TableFactor& marginal,
                            const Ordering& orderedKeys);
 
@@ -139,8 +139,8 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
    *   P(A|B) * P(B|A) = ?
    * We check for overlapping frontals, but do *not* check for cyclic.
    */
-  DiscreteTableConditional operator*(
-      const DiscreteTableConditional& other) const;
+  TableDistribution operator*(
+      const TableDistribution& other) const;
 
   /// @}
   /// @name Testable
@@ -210,11 +210,11 @@ class GTSAM_EXPORT DiscreteTableConditional : public DiscreteConditional {
   }
 #endif
 };
-// DiscreteTableConditional
+// TableDistribution
 
 // traits
 template <>
-struct traits<DiscreteTableConditional>
-    : public Testable<DiscreteTableConditional> {};
+struct traits<TableDistribution>
+    : public Testable<TableDistribution> {};
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 20b4428d4..d5f056e42 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -52,9 +52,9 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   // Multiply into one big conditional. NOTE: possibly quite expensive.
   DiscreteConditional joint;
   for (auto &&conditional : marginal) {
-    // The last discrete conditional may be a DiscreteTableConditional
+    // The last discrete conditional may be a TableDistribution
     if (auto dtc =
-            std::dynamic_pointer_cast<DiscreteTableConditional>(conditional)) {
+            std::dynamic_pointer_cast<TableDistribution>(conditional)) {
       DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
       joint = joint * dc;
     } else {
@@ -133,7 +133,7 @@ HybridValues HybridBayesNet::optimize() const {
 
   for (auto &&conditional : *this) {
     if (conditional->isDiscrete()) {
-      if (auto dtc = conditional->asDiscrete<DiscreteTableConditional>()) {
+      if (auto dtc = conditional->asDiscrete<TableDistribution>()) {
         // The number of keys should be small so should not
         // be expensive to convert to DiscreteConditional.
         discrete_fg.push_back(DiscreteConditional(dtc->nrFrontals(),
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 55a9c7e88..82b0876f2 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -72,7 +72,7 @@ HybridValues HybridBayesTree::optimize() const {
 
   //  The root should be discrete only, we compute the MPE
   if (root_conditional->isDiscrete()) {
-    auto discrete = std::dynamic_pointer_cast<DiscreteTableConditional>(
+    auto discrete = std::dynamic_pointer_cast<TableDistribution>(
         root_conditional->asDiscrete());
     discrete_fg.push_back(discrete);
     mpe = discreteMaxProduct(discrete_fg);
@@ -202,7 +202,7 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
 /* ************************************************************************* */
 void HybridBayesTree::prune(const size_t maxNrLeaves) {
   auto discreteProbs =
-      this->roots_.at(0)->conditional()->asDiscrete<DiscreteTableConditional>();
+      this->roots_.at(0)->conditional()->asDiscrete<TableDistribution>();
 
   DiscreteConditional::shared_ptr prunedDiscreteProbs =
       discreteProbs->prune(maxNrLeaves);
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 963b309c4..6aad1bba0 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -265,7 +265,7 @@ TableFactor TableProduct(const DiscreteFactorGraph &factors) {
   for (auto &&factor : factors) {
     if (factor) {
       if (auto dtc =
-              std::dynamic_pointer_cast<DiscreteTableConditional>(factor)) {
+              std::dynamic_pointer_cast<TableDistribution>(factor)) {
         product = product * dtc->table();
       } else if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
         product = product * (*f);
@@ -323,7 +323,7 @@ static DiscreteFactorGraph CollectDiscreteFactors(
 #if GTSAM_HYBRID_TIMING
       gttic_(ConvertConditionalToTableFactor);
 #endif
-      if (auto dtc = std::dynamic_pointer_cast<DiscreteTableConditional>(dc)) {
+      if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(dc)) {
         /// Get the underlying TableFactor
         dfg.push_back(dtc->table());
       } else {
@@ -364,7 +364,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
     auto conditional =
-        std::make_shared<DiscreteTableConditional>(frontalKeys.size(), product);
+        std::make_shared<TableDistribution>(frontalKeys.size(), product);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 2e1c11dbe..b7d815ec6 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 266b05c95..d273dd64f 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
@@ -80,8 +80,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   double midway = mu1 - mu0;
   auto eliminationResult =
       gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
-  auto pMid = eliminationResult->at(0)->asDiscrete<DiscreteTableConditional>();
-  EXPECT(assert_equal(DiscreteTableConditional(m, "60/40"), *pMid));
+  auto pMid = eliminationResult->at(0)->asDiscrete<TableDistribution>();
+  EXPECT(assert_equal(TableDistribution(m, "60/40"), *pMid));
 
   // Everywhere else, the result should be a sigmoid.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -92,7 +92,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
     auto posterior1 =
-        *eliminationResult1->at(0)->asDiscrete<DiscreteTableConditional>();
+        *eliminationResult1->at(0)->asDiscrete<TableDistribution>();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -102,7 +102,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     hfg1.push_back(mixing);
     auto eliminationResult2 = hfg1.eliminateSequential();
     auto posterior2 =
-        *eliminationResult2->at(0)->asDiscrete<DiscreteTableConditional>();
+        *eliminationResult2->at(0)->asDiscrete<TableDistribution>();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
@@ -142,8 +142,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
                       eliminationResultMax->discretePosterior(vv)));
 
   auto pMax =
-      *eliminationResultMax->at(0)->asDiscrete<DiscreteTableConditional>();
-  EXPECT(assert_equal(DiscreteTableConditional(m, "42/58"), pMax, 1e-4));
+      *eliminationResultMax->at(0)->asDiscrete<TableDistribution>();
+  EXPECT(assert_equal(TableDistribution(m, "42/58"), pMax, 1e-4));
 
   // Everywhere else, the result should be a bell curve like function.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -154,7 +154,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     auto eliminationResult1 =
         gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
     auto posterior1 =
-        *eliminationResult1->at(0)->asDiscrete<DiscreteTableConditional>();
+        *eliminationResult1->at(0)->asDiscrete<TableDistribution>();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -164,7 +164,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     hfg.push_back(mixing);
     auto eliminationResult2 = hfg.eliminateSequential();
     auto posterior2 =
-        *eliminationResult2->at(0)->asDiscrete<DiscreteTableConditional>();
+        *eliminationResult2->at(0)->asDiscrete<TableDistribution>();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index e32e96dc7..247474c6b 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -450,9 +450,9 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
 
   DiscreteConditional joint;
   for (auto&& conditional : posterior->discreteMarginal()) {
-    // The last discrete conditional may be a DiscreteTableConditional
+    // The last discrete conditional may be a TableDistribution
     if (auto dtc =
-            std::dynamic_pointer_cast<DiscreteTableConditional>(conditional)) {
+            std::dynamic_pointer_cast<TableDistribution>(conditional)) {
       DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
       joint = joint * dc;
     } else {
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index dacdeca08..1b7f8054f 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -464,14 +464,14 @@ TEST(HybridEstimation, EliminateSequentialRegression) {
 
   // Create expected discrete conditional on m0.
   DiscreteKey m(M(0), 2);
-  DiscreteTableConditional expected(m % "0.51341712/1");  // regression
+  TableDistribution expected(m % "0.51341712/1");  // regression
 
   // Eliminate into BN using one ordering
   const Ordering ordering1{X(0), X(1), M(0)};
   HybridBayesNet::shared_ptr bn1 = fg->eliminateSequential(ordering1);
 
   // Check that the discrete conditional matches the expected.
-  auto dc1 = bn1->back()->asDiscrete<DiscreteTableConditional>();
+  auto dc1 = bn1->back()->asDiscrete<TableDistribution>();
   EXPECT(assert_equal(expected, *dc1, 1e-9));
 
   // Eliminate into BN using a different ordering
@@ -479,7 +479,7 @@ TEST(HybridEstimation, EliminateSequentialRegression) {
   HybridBayesNet::shared_ptr bn2 = fg->eliminateSequential(ordering2);
 
   // Check that the discrete conditional matches the expected.
-  auto dc2 = bn2->back()->asDiscrete<DiscreteTableConditional>();
+  auto dc2 = bn2->back()->asDiscrete<TableDistribution>();
   EXPECT(assert_equal(expected, *dc2, 1e-9));
 }
 
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 8ce419458..a8d37c8f0 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -650,7 +650,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "74/26");
+  expectedBayesNet.emplace_shared<TableDistribution>(mode, "74/26");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
@@ -700,7 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
       m1, std::vector{conditional0, conditional1});
 
   // Add prior on m1.
-  expectedBayesNet.emplace_shared<DiscreteTableConditional>(
+  expectedBayesNet.emplace_shared<TableDistribution>(
       m1, "0.188638/0.811362");
 
   // Test elimination
@@ -738,8 +738,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
 
   // Add prior on mode.
   // Since this is the only discrete conditional, it is added as a
-  // DiscreteTableConditional.
-  expectedBayesNet.emplace_shared<DiscreteTableConditional>(mode, "23/77");
+  // TableDistribution.
+  expectedBayesNet.emplace_shared<TableDistribution>(mode, "23/77");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
index 4573edad2..5edb5ea20 100644
--- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
@@ -142,7 +142,7 @@ TEST(HybridGaussianISAM, IncrementalInference) {
 
   // Test the probability values with regression tests.
   auto discrete =
-      isam[M(1)]->conditional()->asDiscrete<DiscreteTableConditional>();
+      isam[M(1)]->conditional()->asDiscrete<TableDistribution>();
   EXPECT(assert_equal(0.095292, (*discrete)({{M(0), 0}, {M(1), 0}}), 1e-5));
   EXPECT(assert_equal(0.282758, (*discrete)({{M(0), 1}, {M(1), 0}}), 1e-5));
   EXPECT(assert_equal(0.314175, (*discrete)({{M(0), 0}, {M(1), 1}}), 1e-5));
@@ -222,7 +222,7 @@ TEST(HybridGaussianISAM, ApproxInference) {
     1 1 1 Leaf  0.5
   */
 
-  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteTableConditional>(
+  auto discreteConditional_m0 = *dynamic_pointer_cast<TableDistribution>(
       incrementalHybrid[M(0)]->conditional()->inner());
   EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)}));
 
@@ -474,7 +474,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   // Test if the optimal discrete mode assignment is (1, 1, 1).
   DiscreteFactorGraph discreteGraph;
-  // discreteTree is a DiscreteTableConditional, so we convert to
+  // discreteTree is a TableDistribution, so we convert to
   // DecisionTreeFactor for the DiscreteFactorGraph
   discreteGraph.push_back(discreteTree->toDecisionTreeFactor());
   DiscreteValues optimal_assignment = discreteGraph.optimize();
diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp
index 4c9843d33..3c00d607c 100644
--- a/gtsam/hybrid/tests/testHybridMotionModel.cpp
+++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp
@@ -21,7 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
@@ -144,9 +144,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Since no measurement on x1, we hedge our bets
     // Importance sampling run with 100k samples gives 50.051/49.949
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteTableConditional expected(m1, "50/50");
+    TableDistribution expected(m1, "50/50");
     EXPECT(assert_equal(expected,
-                        *(bn->at(2)->asDiscrete<DiscreteTableConditional>())));
+                        *(bn->at(2)->asDiscrete<TableDistribution>())));
   }
 
   {
@@ -162,9 +162,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Since we have a measurement on x1, we get a definite result
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteTableConditional expected(m1, "44.3854/55.6146");
+    TableDistribution expected(m1, "44.3854/55.6146");
     EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.02));
+        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
 }
 
@@ -251,9 +251,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteTableConditional expected(m1, "48.3158/51.6842");
+    TableDistribution expected(m1, "48.3158/51.6842");
     EXPECT(assert_equal(
-        expected, *(eliminated->at(2)->asDiscrete<DiscreteTableConditional>()),
+        expected, *(eliminated->at(2)->asDiscrete<TableDistribution>()),
         0.02));
   }
 
@@ -268,9 +268,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteTableConditional expected(m1, "55.396/44.604");
+    TableDistribution expected(m1, "55.396/44.604");
     EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.02));
+        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
 }
 
@@ -346,9 +346,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteTableConditional expected(m1, "51.7762/48.2238");
+    TableDistribution expected(m1, "51.7762/48.2238");
     EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.02));
+        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
 
   {
@@ -362,9 +362,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteTableConditional expected(m1, "49.0762/50.9238");
+    TableDistribution expected(m1, "49.0762/50.9238");
     EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.05));
+        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.05));
   }
 }
 
@@ -389,9 +389,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) {
 
   // Values taken from an importance sampling run with 100k samples:
   // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-  DiscreteTableConditional expected(m1, "8.91527/91.0847");
+  TableDistribution expected(m1, "8.91527/91.0847");
   EXPECT(assert_equal(
-      expected, *(bn->at(2)->asDiscrete<DiscreteTableConditional>()), 0.01));
+      expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.01));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index fd2a99c34..e020e851f 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -512,7 +512,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
   // P(m1)
   EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)});
   EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
-  DiscreteTableConditional dtc = *hybridBayesNet->at(4)->asDiscrete<DiscreteTableConditional>();
+  TableDistribution dtc = *hybridBayesNet->at(4)->asDiscrete<TableDistribution>();
   EXPECT(
       DiscreteConditional(dtc.nrFrontals(), dtc.toDecisionTreeFactor())
           .equals(*discreteBayesNet.at(1)));
diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index fa25407ff..e6249f4ac 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -265,7 +265,7 @@ TEST(HybridNonlinearISAM, ApproxInference) {
     1 1 1 Leaf  0.5
   */
 
-  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteTableConditional>(
+  auto discreteConditional_m0 = *dynamic_pointer_cast<TableDistribution>(
       bayesTree[M(0)]->conditional()->inner());
   EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)}));
 
@@ -517,7 +517,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
   // The final discrete graph should not be empty since we have eliminated
   // all continuous variables.
   auto discreteTree =
-      bayesTree[M(3)]->conditional()->asDiscrete<DiscreteTableConditional>();
+      bayesTree[M(3)]->conditional()->asDiscrete<TableDistribution>();
   EXPECT_LONGS_EQUAL(3, discreteTree->size());
 
   // Test if the optimal discrete mode assignment is (1, 1, 1).

From e1628e32a488ab4be077c6cc3433b382e4258cd1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 14:52:18 -0500
Subject: [PATCH 276/362] rename source files

---
 .../{DiscreteTableConditional.cpp => TableDistribution.cpp}       | 0
 .../discrete/{DiscreteTableConditional.h => TableDistribution.h}  | 0
 2 files changed, 0 insertions(+), 0 deletions(-)
 rename gtsam/discrete/{DiscreteTableConditional.cpp => TableDistribution.cpp} (100%)
 rename gtsam/discrete/{DiscreteTableConditional.h => TableDistribution.h} (100%)

diff --git a/gtsam/discrete/DiscreteTableConditional.cpp b/gtsam/discrete/TableDistribution.cpp
similarity index 100%
rename from gtsam/discrete/DiscreteTableConditional.cpp
rename to gtsam/discrete/TableDistribution.cpp
diff --git a/gtsam/discrete/DiscreteTableConditional.h b/gtsam/discrete/TableDistribution.h
similarity index 100%
rename from gtsam/discrete/DiscreteTableConditional.h
rename to gtsam/discrete/TableDistribution.h

From 83bb404856ec7ef0fb4d770a43753731b8dfb71b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 14:52:34 -0500
Subject: [PATCH 277/362] export TableDistribution for serialization

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

diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp
index 3be96b751..af4a81fdf 100644
--- a/gtsam/hybrid/tests/testSerializationHybrid.cpp
+++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp
@@ -44,6 +44,8 @@ BOOST_CLASS_EXPORT_GUID(HybridFactor, "gtsam_HybridFactor");
 BOOST_CLASS_EXPORT_GUID(JacobianFactor, "gtsam_JacobianFactor");
 BOOST_CLASS_EXPORT_GUID(GaussianConditional, "gtsam_GaussianConditional");
 BOOST_CLASS_EXPORT_GUID(DiscreteConditional, "gtsam_DiscreteConditional");
+BOOST_CLASS_EXPORT_GUID(TableDistribution,
+                        "gtsam_TableDistribution");
 
 BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
 using ADT = AlgebraicDecisionTree<Key>;

From 2e0695470a89885cdf3bcb7f2a9929aa781cb234 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 14:53:41 -0500
Subject: [PATCH 278/362] improved docstring

---
 gtsam/discrete/TableDistribution.h | 32 ++++++++++++++----------------
 1 file changed, 15 insertions(+), 17 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index cb99c8677..ccd768a83 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -29,9 +29,12 @@
 namespace gtsam {
 
 /**
- * Discrete Conditional Density which uses a SparseVector as the internal
+ * Distribution which uses a SparseVector as the internal
  * representation, similar to the TableFactor.
  *
+ * This is primarily used in the case when we have a clique in the BayesTree
+ * which consists of all the discrete variables, e.g. in hybrid elimination.
+ *
  * @ingroup discrete
  */
 class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
@@ -42,7 +45,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
 
  public:
   // typedefs needed to play nice with gtsam
-  typedef TableDistribution This;     ///< Typedef to this class
+  typedef TableDistribution This;            ///< Typedef to this class
   typedef std::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
   typedef DiscreteConditional
       BaseConditional;  ///< Typedef to our conditional base class
@@ -63,7 +66,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    * `nFrontals` keys as frontals, in the order given.
    */
   TableDistribution(size_t nFrontals, const DiscreteKeys& keys,
-                           const Eigen::SparseVector<double>& potentials);
+                    const Eigen::SparseVector<double>& potentials);
 
   /** Construct from signature */
   explicit TableDistribution(const Signature& signature);
@@ -76,7 +79,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    * Example: TableDistribution P(D, {B,E}, table);
    */
   TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
-                           const Signature::Table& table)
+                    const Signature::Table& table)
       : TableDistribution(Signature(key, parents, table)) {}
 
   /**
@@ -87,9 +90,8 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    * Example: TableDistribution P(D, {B,E}, table);
    */
   TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
-                           const std::vector<double>& table)
-      : TableDistribution(
-            1, TableFactor(DiscreteKeys{key} & parents, table)) {}
+                    const std::vector<double>& table)
+      : TableDistribution(1, TableFactor(DiscreteKeys{key} & parents, table)) {}
 
   /**
    * Construct from key, parents, and a string specifying the conditional
@@ -101,7 +103,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    * Example: TableDistribution P(D, {B,E}, "9/1 2/8 3/7 1/9");
    */
   TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
-                           const std::string& spec)
+                    const std::string& spec)
       : TableDistribution(Signature(key, parents, spec)) {}
 
   /// No-parent specialization; can also use DiscreteDistribution.
@@ -112,17 +114,15 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    */
-  TableDistribution(const TableFactor& joint,
-                           const TableFactor& marginal);
+  TableDistribution(const TableFactor& joint, const TableFactor& marginal);
 
   /**
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
    * Makes sure the keys are ordered as given. Does not check orderedKeys.
    */
-  TableDistribution(const TableFactor& joint,
-                           const TableFactor& marginal,
-                           const Ordering& orderedKeys);
+  TableDistribution(const TableFactor& joint, const TableFactor& marginal,
+                    const Ordering& orderedKeys);
 
   /**
    * @brief Combine two conditionals, yielding a new conditional with the union
@@ -139,8 +139,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    *   P(A|B) * P(B|A) = ?
    * We check for overlapping frontals, but do *not* check for cyclic.
    */
-  TableDistribution operator*(
-      const TableDistribution& other) const;
+  TableDistribution operator*(const TableDistribution& other) const;
 
   /// @}
   /// @name Testable
@@ -214,7 +213,6 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
 
 // traits
 template <>
-struct traits<TableDistribution>
-    : public Testable<TableDistribution> {};
+struct traits<TableDistribution> : public Testable<TableDistribution> {};
 
 }  // namespace gtsam

From 35e1e6102fbb77c6e0f586553e9d99c0c21991ca Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 15:11:07 -0500
Subject: [PATCH 279/362] kill operator* method

---
 gtsam/discrete/TableDistribution.cpp | 38 ----------------------------
 gtsam/discrete/TableDistribution.h   | 19 +-------------
 2 files changed, 1 insertion(+), 56 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 87eeb1614..3fa66f78c 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -71,44 +71,6 @@ TableDistribution::TableDistribution(const Signature& signature)
     : BaseConditional(1, DecisionTreeFactor(DiscreteKeys{{1, 1}}, ADT(1))),
       table_(TableFactor(signature.discreteKeys(), signature.cpt())) {}
 
-/* ************************************************************************** */
-TableDistribution TableDistribution::operator*(
-    const TableDistribution& other) const {
-  // Take union of frontal keys
-  std::set<Key> newFrontals;
-  for (auto&& key : this->frontals()) newFrontals.insert(key);
-  for (auto&& key : other.frontals()) newFrontals.insert(key);
-
-  // Check if frontals overlapped
-  if (nrFrontals() + other.nrFrontals() > newFrontals.size())
-    throw std::invalid_argument(
-        "TableDistribution::operator* called with overlapping frontal "
-        "keys.");
-
-  // Now, add cardinalities.
-  DiscreteKeys discreteKeys;
-  for (auto&& key : frontals())
-    discreteKeys.emplace_back(key, cardinality(key));
-  for (auto&& key : other.frontals())
-    discreteKeys.emplace_back(key, other.cardinality(key));
-
-  // Sort
-  std::sort(discreteKeys.begin(), discreteKeys.end());
-
-  // Add parents to set, to make them unique
-  std::set<DiscreteKey> parents;
-  for (auto&& key : this->parents())
-    if (!newFrontals.count(key)) parents.emplace(key, cardinality(key));
-  for (auto&& key : other.parents())
-    if (!newFrontals.count(key)) parents.emplace(key, other.cardinality(key));
-
-  // Finally, add parents to keys, in order
-  for (auto&& dk : parents) discreteKeys.push_back(dk);
-
-  TableFactor product = this->table_ * other.table();
-  return TableDistribution(newFrontals.size(), product);
-}
-
 /* ************************************************************************** */
 void TableDistribution::print(const string& s,
                                      const KeyFormatter& formatter) const {
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index ccd768a83..8fb1cb60a 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -124,30 +124,13 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   TableDistribution(const TableFactor& joint, const TableFactor& marginal,
                     const Ordering& orderedKeys);
 
-  /**
-   * @brief Combine two conditionals, yielding a new conditional with the union
-   * of the frontal keys, ordered by gtsam::Key.
-   *
-   * The two conditionals must make a valid Bayes net fragment, i.e.,
-   * the frontal variables cannot overlap, and must be acyclic:
-   * Example of correct use:
-   *   P(A,B) = P(A|B) * P(B)
-   *   P(A,B|C) = P(A|B) * P(B|C)
-   *   P(A,B,C) = P(A,B|C) * P(C)
-   * Example of incorrect use:
-   *   P(A|B) * P(A|C) = ?
-   *   P(A|B) * P(B|A) = ?
-   * We check for overlapping frontals, but do *not* check for cyclic.
-   */
-  TableDistribution operator*(const TableDistribution& other) const;
-
   /// @}
   /// @name Testable
   /// @{
 
   /// GTSAM-style print
   void print(
-      const std::string& s = "Discrete Conditional: ",
+      const std::string& s = "Table Distribution: ",
       const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
   /// GTSAM-style equals

From bc449c1a4502b7eb348cdfe691f765495cf99f6f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 15:11:21 -0500
Subject: [PATCH 280/362] formatting

---
 gtsam/discrete/TableDistribution.cpp | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 3fa66f78c..5862c64be 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -17,9 +17,9 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/debug.h>
-#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/discrete/Ring.h>
 #include <gtsam/discrete/Signature.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridValues.h>
 
 #include <algorithm>
@@ -39,7 +39,7 @@ namespace gtsam {
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const size_t nrFrontals,
-                                                   const TableFactor& f)
+                                     const TableFactor& f)
     : BaseConditional(nrFrontals, DecisionTreeFactor(f.discreteKeys(), ADT())),
       table_(f / (*f.sum(nrFrontals))) {}
 
@@ -52,15 +52,15 @@ TableDistribution::TableDistribution(
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const TableFactor& joint,
-                                                   const TableFactor& marginal)
+                                     const TableFactor& marginal)
     : BaseConditional(joint.size() - marginal.size(),
                       joint.discreteKeys() & marginal.discreteKeys(), ADT()),
       table_(joint / marginal) {}
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const TableFactor& joint,
-                                                   const TableFactor& marginal,
-                                                   const Ordering& orderedKeys)
+                                     const TableFactor& marginal,
+                                     const Ordering& orderedKeys)
     : TableDistribution(joint, marginal) {
   keys_.clear();
   keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
@@ -73,7 +73,7 @@ TableDistribution::TableDistribution(const Signature& signature)
 
 /* ************************************************************************** */
 void TableDistribution::print(const string& s,
-                                     const KeyFormatter& formatter) const {
+                              const KeyFormatter& formatter) const {
   cout << s << " P( ";
   for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
     cout << formatter(*it) << " ";
@@ -90,8 +90,7 @@ void TableDistribution::print(const string& s,
 }
 
 /* ************************************************************************** */
-bool TableDistribution::equals(const DiscreteFactor& other,
-                                      double tol) const {
+bool TableDistribution::equals(const DiscreteFactor& other, double tol) const {
   auto dtc = dynamic_cast<const TableDistribution*>(&other);
   if (!dtc) {
     return false;
@@ -112,8 +111,7 @@ DiscreteConditional::shared_ptr TableDistribution::max(
 }
 
 /* ****************************************************************************/
-void TableDistribution::setData(
-    const DiscreteConditional::shared_ptr& dc) {
+void TableDistribution::setData(const DiscreteConditional::shared_ptr& dc) {
   if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(dc)) {
     this->table_ = dtc->table_;
   } else {

From 6b6283c1512467819918c90191aa8372e96a00dd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jan 2025 15:21:49 -0500
Subject: [PATCH 281/362] fix factor construction

---
 gtsam/discrete/tests/testDecisionTreeFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 61ce9038d..dc18e0ab2 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -72,7 +72,7 @@ TEST(DecisionTreeFactor, constructors) {
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, Divide) {
   DiscreteKey A(0, 2), S(1, 2);
-  DecisionTreeFactor pA(A % "99/1"), pS(S % "50/50");
+  DecisionTreeFactor pA = create(A % "99/1"), pS = create(S % "50/50");
   DecisionTreeFactor joint = pA * pS;
   DecisionTreeFactor s = joint / pA;
   EXPECT(assert_equal(pS, s));

From bd30bef1a361488a9ef7a2e0ec223220ace56bb8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 05:16:21 -0500
Subject: [PATCH 282/362] remove constructors that need parents

---
 gtsam/discrete/TableDistribution.cpp | 24 ++++++--------
 gtsam/discrete/TableDistribution.h   | 48 ++--------------------------
 2 files changed, 12 insertions(+), 60 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 5862c64be..b74acbbd1 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -38,16 +38,15 @@ using std::vector;
 namespace gtsam {
 
 /* ************************************************************************** */
-TableDistribution::TableDistribution(const size_t nrFrontals,
-                                     const TableFactor& f)
-    : BaseConditional(nrFrontals, DecisionTreeFactor(f.discreteKeys(), ADT())),
-      table_(f / (*f.sum(nrFrontals))) {}
+TableDistribution::TableDistribution(const TableFactor& f)
+    : BaseConditional(f.keys().size(),
+                      DecisionTreeFactor(f.discreteKeys(), ADT())),
+      table_(f / (*f.sum(f.keys().size()))) {}
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(
-    size_t nrFrontals, const DiscreteKeys& keys,
-    const Eigen::SparseVector<double>& potentials)
-    : BaseConditional(nrFrontals, keys, DecisionTreeFactor(keys, ADT())),
+    const DiscreteKeys& keys, const Eigen::SparseVector<double>& potentials)
+    : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
       table_(TableFactor(keys, potentials)) {}
 
 /* ************************************************************************** */
@@ -66,11 +65,6 @@ TableDistribution::TableDistribution(const TableFactor& joint,
   keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
 }
 
-/* ************************************************************************** */
-TableDistribution::TableDistribution(const Signature& signature)
-    : BaseConditional(1, DecisionTreeFactor(DiscreteKeys{{1, 1}}, ADT(1))),
-      table_(TableFactor(signature.discreteKeys(), signature.cpt())) {}
-
 /* ************************************************************************** */
 void TableDistribution::print(const string& s,
                               const KeyFormatter& formatter) const {
@@ -107,7 +101,7 @@ DiscreteConditional::shared_ptr TableDistribution::max(
     const Ordering& keys) const {
   auto m = *table_.max(keys);
 
-  return std::make_shared<TableDistribution>(m.discreteKeys().size(), m);
+  return std::make_shared<TableDistribution>(m);
 }
 
 /* ****************************************************************************/
@@ -124,8 +118,8 @@ DiscreteConditional::shared_ptr TableDistribution::prune(
     size_t maxNrAssignments) const {
   TableFactor pruned = table_.prune(maxNrAssignments);
 
-  return std::make_shared<TableDistribution>(
-      this->nrFrontals(), this->discreteKeys(), pruned.sparseTable());
+  return std::make_shared<TableDistribution>(this->discreteKeys(),
+                                             pruned.sparseTable());
 }
 
 }  // namespace gtsam
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 8fb1cb60a..a1c463e0e 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -58,58 +58,16 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   /// Default constructor needed for serialization.
   TableDistribution() {}
 
-  /// Construct from factor, taking the first `nFrontals` keys as frontals.
-  TableDistribution(size_t nFrontals, const TableFactor& f);
+  /// Construct from TableFactor.
+  TableDistribution(const TableFactor& f);
 
   /**
    * Construct from DiscreteKeys and SparseVector, taking the first
    * `nFrontals` keys as frontals, in the order given.
    */
-  TableDistribution(size_t nFrontals, const DiscreteKeys& keys,
+  TableDistribution(const DiscreteKeys& keys,
                     const Eigen::SparseVector<double>& potentials);
 
-  /** Construct from signature */
-  explicit TableDistribution(const Signature& signature);
-
-  /**
-   * Construct from key, parents, and a Signature::Table specifying the
-   * conditional probability table (CPT) in 00 01 10 11 order. For
-   * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
-   *
-   * Example: TableDistribution P(D, {B,E}, table);
-   */
-  TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
-                    const Signature::Table& table)
-      : TableDistribution(Signature(key, parents, table)) {}
-
-  /**
-   * Construct from key, parents, and a vector<double> specifying the
-   * conditional probability table (CPT) in 00 01 10 11 order. For
-   * three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
-   *
-   * Example: TableDistribution P(D, {B,E}, table);
-   */
-  TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
-                    const std::vector<double>& table)
-      : TableDistribution(1, TableFactor(DiscreteKeys{key} & parents, table)) {}
-
-  /**
-   * Construct from key, parents, and a string specifying the conditional
-   * probability table (CPT) in 00 01 10 11 order. For three-valued, it would
-   * be 00 01 02 10 11 12 20 21 22, etc....
-   *
-   * The string is parsed into a Signature::Table.
-   *
-   * Example: TableDistribution P(D, {B,E}, "9/1 2/8 3/7 1/9");
-   */
-  TableDistribution(const DiscreteKey& key, const DiscreteKeys& parents,
-                    const std::string& spec)
-      : TableDistribution(Signature(key, parents, spec)) {}
-
-  /// No-parent specialization; can also use DiscreteDistribution.
-  TableDistribution(const DiscreteKey& key, const std::string& spec)
-      : TableDistribution(Signature(key, {}, spec)) {}
-
   /**
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).

From f9e3280d75d61f9d6e3619f071722454281882bb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 05:27:30 -0500
Subject: [PATCH 283/362] add helpful constructors

---
 gtsam/discrete/TableDistribution.cpp | 19 +++++++++++++------
 gtsam/discrete/TableDistribution.h   | 27 +++++++++++++++++++++++++--
 2 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index b74acbbd1..6669cea4a 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -50,6 +50,19 @@ TableDistribution::TableDistribution(
       table_(TableFactor(keys, potentials)) {}
 
 /* ************************************************************************** */
+TableDistribution::TableDistribution(const DiscreteKeys& keys,
+                                     const std::vector<double>& potentials)
+    : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
+      table_(TableFactor(keys, potentials)) {}
+
+/* ************************************************************************** */
+TableDistribution::TableDistribution(const DiscreteKeys& keys,
+                                     const std::string& potentials)
+    : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
+      table_(TableFactor(keys, potentials)) {}
+
+/* **************************************************************************
+ */
 TableDistribution::TableDistribution(const TableFactor& joint,
                                      const TableFactor& marginal)
     : BaseConditional(joint.size() - marginal.size(),
@@ -72,12 +85,6 @@ void TableDistribution::print(const string& s,
   for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
     cout << formatter(*it) << " ";
   }
-  if (nrParents()) {
-    cout << "| ";
-    for (const_iterator it = beginParents(); it != endParents(); ++it) {
-      cout << formatter(*it) << " ";
-    }
-  }
   cout << "):\n";
   table_.print("", formatter);
   cout << endl;
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index a1c463e0e..655774f04 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -62,12 +62,35 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   TableDistribution(const TableFactor& f);
 
   /**
-   * Construct from DiscreteKeys and SparseVector, taking the first
-   * `nFrontals` keys as frontals, in the order given.
+   * Construct from DiscreteKeys and SparseVector.
    */
   TableDistribution(const DiscreteKeys& keys,
                     const Eigen::SparseVector<double>& potentials);
 
+  /**
+   * Construct from DiscreteKeys and std::vector.
+   */
+  TableDistribution(const DiscreteKeys& keys,
+                    const std::vector<double>& potentials);
+
+  /**
+   * Construct from single DiscreteKey and std::vector.
+   */
+  TableDistribution(const DiscreteKey& key,
+                    const std::vector<double>& potentials)
+      : TableDistribution(DiscreteKeys(key), potentials) {}
+
+  /**
+   * Construct from DiscreteKey and std::string.
+   */
+  TableDistribution(const DiscreteKeys& key, const std::string& potentials);
+
+  /**
+   * Construct from single DiscreteKey and std::string.
+   */
+  TableDistribution(const DiscreteKey& key, const std::string& potentials)
+      : TableDistribution(DiscreteKeys(key), potentials) {}
+
   /**
    * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
    * Assumes but *does not check* that f(Y)=sum_X f(X,Y).

From 3abff90fb4111af03448f93941a6822a5228acc2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 05:27:49 -0500
Subject: [PATCH 284/362] fix tests

---
 gtsam/hybrid/tests/testGaussianMixture.cpp           | 7 +++----
 gtsam/hybrid/tests/testHybridEstimation.cpp          | 2 +-
 gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 2 +-
 3 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index d273dd64f..c98485fea 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -81,7 +81,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   auto eliminationResult =
       gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
   auto pMid = eliminationResult->at(0)->asDiscrete<TableDistribution>();
-  EXPECT(assert_equal(TableDistribution(m, "60/40"), *pMid));
+  EXPECT(assert_equal(TableDistribution(m, "60 40"), *pMid));
 
   // Everywhere else, the result should be a sigmoid.
   for (const double shift : {-4, -2, 0, 2, 4}) {
@@ -141,9 +141,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   EXPECT(assert_equal(expectedDiscretePosterior,
                       eliminationResultMax->discretePosterior(vv)));
 
-  auto pMax =
-      *eliminationResultMax->at(0)->asDiscrete<TableDistribution>();
-  EXPECT(assert_equal(TableDistribution(m, "42/58"), pMax, 1e-4));
+  auto pMax = *eliminationResultMax->at(0)->asDiscrete<TableDistribution>();
+  EXPECT(assert_equal(TableDistribution(m, "42 58"), pMax, 1e-4));
 
   // Everywhere else, the result should be a bell curve like function.
   for (const double shift : {-4, -2, 0, 2, 4}) {
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 1b7f8054f..425b29742 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -464,7 +464,7 @@ TEST(HybridEstimation, EliminateSequentialRegression) {
 
   // Create expected discrete conditional on m0.
   DiscreteKey m(M(0), 2);
-  TableDistribution expected(m % "0.51341712/1");  // regression
+  TableDistribution expected(m, "0.51341712 1");  // regression
 
   // Eliminate into BN using one ordering
   const Ordering ordering1{X(0), X(1), M(0)};
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index a8d37c8f0..d54f8a141 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -650,7 +650,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
       mode, std::vector{conditional0, conditional1});
 
   // Add prior on mode.
-  expectedBayesNet.emplace_shared<TableDistribution>(mode, "74/26");
+  expectedBayesNet.emplace_shared<TableDistribution>(mode, "74 26");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();

From 11a740e8e3dd150ec7cc595fddd279966262faa4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 05:28:16 -0500
Subject: [PATCH 285/362] use template

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

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 82b0876f2..088f16350 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -72,8 +72,7 @@ HybridValues HybridBayesTree::optimize() const {
 
   //  The root should be discrete only, we compute the MPE
   if (root_conditional->isDiscrete()) {
-    auto discrete = std::dynamic_pointer_cast<TableDistribution>(
-        root_conditional->asDiscrete());
+    auto discrete = root_conditional->asDiscrete<TableDistribution>();
     discrete_fg.push_back(discrete);
     mpe = discreteMaxProduct(discrete_fg);
   } else {

From b7bddde82b9a3f679048868fb30255b2f7f7f724 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 05:29:15 -0500
Subject: [PATCH 286/362] fix TableDistribution constructor call

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 6aad1bba0..594aa5c40 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -264,8 +264,7 @@ TableFactor TableProduct(const DiscreteFactorGraph &factors) {
   TableFactor product;
   for (auto &&factor : factors) {
     if (factor) {
-      if (auto dtc =
-              std::dynamic_pointer_cast<TableDistribution>(factor)) {
+      if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(factor)) {
         product = product * dtc->table();
       } else if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
         product = product * (*f);
@@ -363,8 +362,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
-    auto conditional =
-        std::make_shared<TableDistribution>(frontalKeys.size(), product);
+    auto conditional = std::make_shared<TableDistribution>(product);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif

From d6bc1e11a6ef757e759c50b0d97357a257b06c88 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 05:48:50 -0500
Subject: [PATCH 287/362] pass DiscreteConditional& for pruning instead of
 shared_ptr

---
 gtsam/hybrid/HybridBayesNet.cpp                      |  5 ++---
 gtsam/hybrid/HybridBayesTree.cpp                     |  2 +-
 gtsam/hybrid/HybridGaussianConditional.cpp           | 10 +++++-----
 gtsam/hybrid/HybridGaussianConditional.h             |  2 +-
 gtsam/hybrid/tests/testHybridGaussianConditional.cpp | 12 ++++++------
 5 files changed, 15 insertions(+), 16 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index d5f056e42..a80c4c0f2 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -53,8 +53,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   DiscreteConditional joint;
   for (auto &&conditional : marginal) {
     // The last discrete conditional may be a TableDistribution
-    if (auto dtc =
-            std::dynamic_pointer_cast<TableDistribution>(conditional)) {
+    if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(conditional)) {
       DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
       joint = joint * dc;
     } else {
@@ -81,7 +80,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   for (auto &&conditional : *this) {
     if (auto hgc = conditional->asHybrid()) {
       // Prune the hybrid Gaussian conditional!
-      auto prunedHybridGaussianConditional = hgc->prune(pruned);
+      auto prunedHybridGaussianConditional = hgc->prune(*pruned);
 
       // Type-erase and add to the pruned Bayes Net fragment.
       result.push_back(prunedHybridGaussianConditional);
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 088f16350..65664e2b1 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -236,7 +236,7 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
         if (!hybridGaussianCond->pruned()) {
           // Imperative
           clique->conditional() = std::make_shared<HybridConditional>(
-              hybridGaussianCond->prune(parentData.prunedDiscreteProbs));
+              hybridGaussianCond->prune(*parentData.prunedDiscreteProbs));
         }
       }
       return parentData;
diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 8883217ba..78e1f5324 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -304,18 +304,18 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
 
 /* *******************************************************************************/
 HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
-    const DiscreteConditional::shared_ptr &discreteProbs) const {
-  // Find keys in discreteProbs->keys() but not in this->keys():
+    const DiscreteConditional &discreteProbs) const {
+  // Find keys in discreteProbs.keys() but not in this->keys():
   std::set<Key> mine(this->keys().begin(), this->keys().end());
-  std::set<Key> theirs(discreteProbs->keys().begin(),
-                       discreteProbs->keys().end());
+  std::set<Key> theirs(discreteProbs.keys().begin(),
+                       discreteProbs.keys().end());
   std::vector<Key> diff;
   std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
                       std::back_inserter(diff));
 
   // Find maximum probability value for every combination of our keys.
   Ordering keys(diff);
-  auto max = discreteProbs->max(keys);
+  auto max = discreteProbs.max(keys);
 
   // Check the max value for every combination of our keys.
   // If the max value is 0.0, we can prune the corresponding conditional.
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index fd9c0d7a3..3b95e0277 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -236,7 +236,7 @@ class GTSAM_EXPORT HybridGaussianConditional
    * @return Shared pointer to possibly a pruned HybridGaussianConditional
    */
   HybridGaussianConditional::shared_ptr prune(
-      const DiscreteConditional::shared_ptr &discreteProbs) const;
+      const DiscreteConditional &discreteProbs) const;
 
   /// Return true if the conditional has already been pruned.
   bool pruned() const { return pruned_; }
diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
index 0bfc49fcb..8bb83cac4 100644
--- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
@@ -261,8 +261,8 @@ TEST(HybridGaussianConditional, Prune) {
       potentials[i] = 1;
       const DecisionTreeFactor decisionTreeFactor(keys, potentials);
       // Prune the HybridGaussianConditional
-      const auto pruned = hgc.prune(std::make_shared<DiscreteConditional>(
-          keys.size(), decisionTreeFactor));
+      const auto pruned =
+          hgc.prune(DiscreteConditional(keys.size(), decisionTreeFactor));
       // Check that the pruned HybridGaussianConditional has 1 conditional
       EXPECT_LONGS_EQUAL(1, pruned->nrComponents());
     }
@@ -272,8 +272,8 @@ TEST(HybridGaussianConditional, Prune) {
                                          0, 0, 0.5, 0};
     const DecisionTreeFactor decisionTreeFactor(keys, potentials);
 
-    const auto pruned = hgc.prune(
-        std::make_shared<DiscreteConditional>(keys.size(), decisionTreeFactor));
+    const auto pruned =
+        hgc.prune(DiscreteConditional(keys.size(), decisionTreeFactor));
 
     // Check that the pruned HybridGaussianConditional has 2 conditionals
     EXPECT_LONGS_EQUAL(2, pruned->nrComponents());
@@ -288,8 +288,8 @@ TEST(HybridGaussianConditional, Prune) {
                                          0,   0, 0.5, 0};
     const DecisionTreeFactor decisionTreeFactor(keys, potentials);
 
-    const auto pruned = hgc.prune(
-        std::make_shared<DiscreteConditional>(keys.size(), decisionTreeFactor));
+    const auto pruned =
+        hgc.prune(DiscreteConditional(keys.size(), decisionTreeFactor));
 
     // Check that the pruned HybridGaussianConditional has 3 conditionals
     EXPECT_LONGS_EQUAL(3, pruned->nrComponents());

From 9a40be6f32ccdcafe50774795b5f8df84bc36d52 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 06:12:01 -0500
Subject: [PATCH 288/362] normalize values in sparse_table so it forms a proper
 distribution

---
 gtsam/discrete/TableDistribution.cpp | 16 +++++++++++++---
 gtsam/discrete/TableFactor.h         |  2 +-
 2 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 6669cea4a..e62d3ecec 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -37,6 +37,12 @@ using std::stringstream;
 using std::vector;
 namespace gtsam {
 
+/// Normalize sparse_table
+static Eigen::SparseVector<double> normalizeSparseTable(
+    const Eigen::SparseVector<double>& sparse_table) {
+  return sparse_table / sparse_table.sum();
+}
+
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const TableFactor& f)
     : BaseConditional(f.keys().size(),
@@ -47,19 +53,23 @@ TableDistribution::TableDistribution(const TableFactor& f)
 TableDistribution::TableDistribution(
     const DiscreteKeys& keys, const Eigen::SparseVector<double>& potentials)
     : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
-      table_(TableFactor(keys, potentials)) {}
+      table_(TableFactor(keys, normalizeSparseTable(potentials))) {}
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const DiscreteKeys& keys,
                                      const std::vector<double>& potentials)
     : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
-      table_(TableFactor(keys, potentials)) {}
+      table_(TableFactor(
+          keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) {
+}
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const DiscreteKeys& keys,
                                      const std::string& potentials)
     : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
-      table_(TableFactor(keys, potentials)) {}
+      table_(TableFactor(
+          keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) {
+}
 
 /* **************************************************************************
  */
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index a2fdb4d32..72778d711 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -86,6 +86,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return DiscreteKey(keys_[i], cardinalities_.at(keys_[i]));
   }
 
+ public:
   /**
    * Convert probability table given as doubles to SparseVector.
    * Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
@@ -97,7 +98,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
                                              const std::string& table);
 
- public:
   // typedefs needed to play nice with gtsam
   typedef TableFactor This;
   typedef DiscreteFactor Base;  ///< Typedef to base class

From 7cb818136f25e6f019b157065a531ee9476121d5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 06:14:00 -0500
Subject: [PATCH 289/362] fix TableDistribution constructors in tests

---
 .../hybrid/tests/testHybridGaussianFactorGraph.cpp |  5 ++---
 gtsam/hybrid/tests/testHybridMotionModel.cpp       | 14 +++++++-------
 2 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index d54f8a141..fb09bb618 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -700,8 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
       m1, std::vector{conditional0, conditional1});
 
   // Add prior on m1.
-  expectedBayesNet.emplace_shared<TableDistribution>(
-      m1, "0.188638/0.811362");
+  expectedBayesNet.emplace_shared<TableDistribution>(m1, "0.188638 0.811362");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
@@ -739,7 +738,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
   // Add prior on mode.
   // Since this is the only discrete conditional, it is added as a
   // TableDistribution.
-  expectedBayesNet.emplace_shared<TableDistribution>(mode, "23/77");
+  expectedBayesNet.emplace_shared<TableDistribution>(mode, "23 77");
 
   // Test elimination
   const auto posterior = fg.eliminateSequential();
diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp
index 3c00d607c..5d307e81f 100644
--- a/gtsam/hybrid/tests/testHybridMotionModel.cpp
+++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp
@@ -144,7 +144,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Since no measurement on x1, we hedge our bets
     // Importance sampling run with 100k samples gives 50.051/49.949
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    TableDistribution expected(m1, "50/50");
+    TableDistribution expected(m1, "50 50");
     EXPECT(assert_equal(expected,
                         *(bn->at(2)->asDiscrete<TableDistribution>())));
   }
@@ -162,7 +162,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Since we have a measurement on x1, we get a definite result
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    TableDistribution expected(m1, "44.3854/55.6146");
+    TableDistribution expected(m1, "44.3854 55.6146");
     EXPECT(assert_equal(
         expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
@@ -251,7 +251,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    TableDistribution expected(m1, "48.3158/51.6842");
+    TableDistribution expected(m1, "48.3158 51.6842");
     EXPECT(assert_equal(
         expected, *(eliminated->at(2)->asDiscrete<TableDistribution>()),
         0.02));
@@ -268,7 +268,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    TableDistribution expected(m1, "55.396/44.604");
+    TableDistribution expected(m1, "55.396 44.604");
     EXPECT(assert_equal(
         expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
@@ -346,7 +346,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    TableDistribution expected(m1, "51.7762/48.2238");
+    TableDistribution expected(m1, "51.7762 48.2238");
     EXPECT(assert_equal(
         expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
@@ -362,7 +362,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    TableDistribution expected(m1, "49.0762/50.9238");
+    TableDistribution expected(m1, "49.0762 50.9238");
     EXPECT(assert_equal(
         expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.05));
   }
@@ -389,7 +389,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) {
 
   // Values taken from an importance sampling run with 100k samples:
   // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-  TableDistribution expected(m1, "8.91527/91.0847");
+  TableDistribution expected(m1, "8.91527 91.0847");
   EXPECT(assert_equal(
       expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.01));
 }

From d39641d8ac7cba1c11c90871e41a6fcfdb024e6c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 14:39:18 -0500
Subject: [PATCH 290/362] get rid of setData and make prune() imperative for
 non-factors

---
 gtsam/discrete/DiscreteConditional.cpp | 13 ++++---------
 gtsam/discrete/DiscreteConditional.h   |  5 +----
 gtsam/discrete/TableDistribution.cpp   | 17 ++---------------
 gtsam/discrete/TableDistribution.h     |  6 +-----
 4 files changed, 8 insertions(+), 33 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index c90002e78..1a345afac 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -478,11 +478,6 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
   return this->evaluate(x.discrete());
 }
 
-/* ************************************************************************* */
-void DiscreteConditional::setData(const DiscreteConditional::shared_ptr& dc) {
-  this->root_ = dc->root_;
-}
-
 /* ************************************************************************* */
 DiscreteConditional::shared_ptr DiscreteConditional::max(
     const Ordering& keys) const {
@@ -491,10 +486,10 @@ DiscreteConditional::shared_ptr DiscreteConditional::max(
 }
 
 /* ************************************************************************* */
-DiscreteConditional::shared_ptr DiscreteConditional::prune(
-    size_t maxNrAssignments) const {
-  return std::make_shared<DiscreteConditional>(
-      this->nrFrontals(), BaseFactor::prune(maxNrAssignments));
+void DiscreteConditional::prune(size_t maxNrAssignments) {
+  // Get as DiscreteConditional so the probabilities are normalized
+  DiscreteConditional pruned(nrFrontals(), BaseFactor::prune(maxNrAssignments));
+  this->root_ = pruned.root_;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 98edcb8c9..35dc346d1 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -276,11 +276,8 @@ class GTSAM_EXPORT DiscreteConditional
    */
   double negLogConstant() const override;
 
-  /// Set the data from another DiscreteConditional.
-  virtual void setData(const DiscreteConditional::shared_ptr& dc);
-
   /// Prune the conditional
-  virtual DiscreteConditional::shared_ptr prune(size_t maxNrAssignments) const;
+  virtual void prune(size_t maxNrAssignments);
 
   /// @}
 
diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index e62d3ecec..bedcee42c 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -122,21 +122,8 @@ DiscreteConditional::shared_ptr TableDistribution::max(
 }
 
 /* ****************************************************************************/
-void TableDistribution::setData(const DiscreteConditional::shared_ptr& dc) {
-  if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(dc)) {
-    this->table_ = dtc->table_;
-  } else {
-    this->table_ = TableFactor(dc->discreteKeys(), *dc);
-  }
-}
-
-/* ****************************************************************************/
-DiscreteConditional::shared_ptr TableDistribution::prune(
-    size_t maxNrAssignments) const {
-  TableFactor pruned = table_.prune(maxNrAssignments);
-
-  return std::make_shared<TableDistribution>(this->discreteKeys(),
-                                             pruned.sparseTable());
+void TableDistribution::prune(size_t maxNrAssignments) {
+  table_ = table_.prune(maxNrAssignments);
 }
 
 }  // namespace gtsam
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 655774f04..ce41835d6 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -145,12 +145,8 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   /// @name Advanced Interface
   /// @{
 
-  /// Set the underlying data from the DiscreteConditional
-  virtual void setData(const DiscreteConditional::shared_ptr& dc) override;
-
   /// Prune the conditional
-  virtual DiscreteConditional::shared_ptr prune(
-      size_t maxNrAssignments) const override;
+  virtual void prune(size_t maxNrAssignments) override;
 
   /// Get a DecisionTreeFactor representation.
   DecisionTreeFactor toDecisionTreeFactor() const override {

From d3780158b1e1f1e615d36ba5adb08cecb11fc21f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 14:39:40 -0500
Subject: [PATCH 291/362] update pruning in BayesNet and BayesTree

---
 gtsam/hybrid/HybridBayesNet.cpp  | 9 +++++----
 gtsam/hybrid/HybridBayesTree.cpp | 7 +++----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index a80c4c0f2..841b74f4f 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -61,12 +61,13 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
     }
   }
 
-  // Prune the joint. NOTE: again, possibly quite expensive.
-  const DiscreteConditional::shared_ptr pruned = joint.prune(maxNrLeaves);
+  // Prune the joint. NOTE: imperative and, again, possibly quite expensive.
+  DiscreteConditional pruned(joint);
+  pruned.prune(maxNrLeaves);
 
   // Create a the result starting with the pruned joint.
   HybridBayesNet result;
-  result.push_back(std::move(pruned));
+  result.push_back(std::make_shared<DiscreteConditional>(pruned));
 
   /* To prune, we visitWith every leaf in the HybridGaussianConditional.
    * For each leaf, using the assignment we can check the discrete decision tree
@@ -80,7 +81,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   for (auto &&conditional : *this) {
     if (auto hgc = conditional->asHybrid()) {
       // Prune the hybrid Gaussian conditional!
-      auto prunedHybridGaussianConditional = hgc->prune(*pruned);
+      auto prunedHybridGaussianConditional = hgc->prune(pruned);
 
       // Type-erase and add to the pruned Bayes Net fragment.
       result.push_back(prunedHybridGaussianConditional);
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 65664e2b1..22777600f 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -200,12 +200,11 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
 
 /* ************************************************************************* */
 void HybridBayesTree::prune(const size_t maxNrLeaves) {
-  auto discreteProbs =
+  auto prunedDiscreteProbs =
       this->roots_.at(0)->conditional()->asDiscrete<TableDistribution>();
 
-  DiscreteConditional::shared_ptr prunedDiscreteProbs =
-      discreteProbs->prune(maxNrLeaves);
-  discreteProbs->setData(prunedDiscreteProbs);
+  // Imperative pruning
+  prunedDiscreteProbs->prune(maxNrLeaves);
 
   /// Helper struct for pruning the hybrid bayes tree.
   struct HybridPrunerData {

From 14f32544d26c685d861233070901fb5955768c82 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 14:39:53 -0500
Subject: [PATCH 292/362] update test

---
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 247474c6b..5c788446c 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -451,8 +451,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
   DiscreteConditional joint;
   for (auto&& conditional : posterior->discreteMarginal()) {
     // The last discrete conditional may be a TableDistribution
-    if (auto dtc =
-            std::dynamic_pointer_cast<TableDistribution>(conditional)) {
+    if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(conditional)) {
       DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
       joint = joint * dc;
     } else {
@@ -461,7 +460,8 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
   }
 
   size_t maxNrLeaves = 3;
-  auto prunedDecisionTree = *joint.prune(maxNrLeaves);
+  DiscreteConditional prunedDecisionTree(joint);
+  prunedDecisionTree.prune(maxNrLeaves);
 
 #ifdef GTSAM_DT_MERGING
   EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/,

From 5a8a9425f9665c5fabab117779522a6b239ebed5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 15:47:16 -0500
Subject: [PATCH 293/362] add argmax method to TableDistribution

---
 gtsam/discrete/TableDistribution.cpp | 17 +++++++++++++++++
 gtsam/discrete/TableDistribution.h   |  7 +++++++
 2 files changed, 24 insertions(+)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index bedcee42c..2e476b0ee 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -121,6 +121,23 @@ DiscreteConditional::shared_ptr TableDistribution::max(
   return std::make_shared<TableDistribution>(m);
 }
 
+/* ************************************************************************ */
+uint64_t TableDistribution::argmax() const {
+  uint64_t maxIdx = 0;
+  double maxValue = 0.0;
+
+  Eigen::SparseVector<double> sparseTable = table_.sparseTable();
+
+  for (SparseIt it(sparseTable); it; ++it) {
+    if (it.value() > maxValue) {
+      maxIdx = it.index();
+      maxValue = it.value();
+    }
+  }
+
+  return maxIdx;
+}
+
 /* ****************************************************************************/
 void TableDistribution::prune(size_t maxNrAssignments) {
   table_ = table_.prune(maxNrAssignments);
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index ce41835d6..9cbca0d26 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -141,6 +141,13 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   virtual DiscreteConditional::shared_ptr max(
       const Ordering& keys) const override;
 
+  /**
+   * @brief Return assignment that maximizes value.
+   *
+   * @return maximizing assignment for the variables.
+   */
+  uint64_t argmax() const;
+
   /// @}
   /// @name Advanced Interface
   /// @{

From 2410d4f442ad9fc3989a2f0d6fe6c4b68fcf024e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 15:47:36 -0500
Subject: [PATCH 294/362] use TableDistribution::argmax in discreteMaxProduct

---
 gtsam/hybrid/HybridBayesTree.cpp | 11 +----------
 1 file changed, 1 insertion(+), 10 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 22777600f..1dc277243 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -47,16 +47,7 @@ DiscreteValues HybridBayesTree::discreteMaxProduct(
     const DiscreteFactorGraph& dfg) const {
   TableFactor product = TableProduct(dfg);
 
-  uint64_t maxIdx = 0;
-  double maxValue = 0.0;
-  Eigen::SparseVector<double> sparseTable = product.sparseTable();
-  for (TableFactor::SparseIt it(sparseTable); it; ++it) {
-    if (it.value() > maxValue) {
-      maxIdx = it.index();
-      maxValue = it.value();
-    }
-  }
-
+  uint64_t maxIdx = TableDistribution(product).argmax();
   DiscreteValues assignment = product.findAssignments(maxIdx);
   return assignment;
 }

From 5e4cf89ba99d50c523f7237187724d6b5c3c5155 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 16:12:09 -0500
Subject: [PATCH 295/362] max returns DiscreteFactor

---
 gtsam/discrete/DiscreteConditional.cpp | 5 ++---
 gtsam/discrete/DiscreteConditional.h   | 6 +++---
 gtsam/discrete/TableDistribution.cpp   | 7 ++-----
 gtsam/discrete/TableDistribution.h     | 7 +++----
 4 files changed, 10 insertions(+), 15 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 1a345afac..e433243e1 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -479,10 +479,9 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
 }
 
 /* ************************************************************************* */
-DiscreteConditional::shared_ptr DiscreteConditional::max(
+DiscreteFactor::shared_ptr DiscreteConditional::max(
     const Ordering& keys) const {
-  auto m = *BaseFactor::max(keys);
-  return std::make_shared<DiscreteConditional>(m.discreteKeys().size(), m);
+  return BaseFactor::max(keys);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 35dc346d1..c92a69050 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -215,13 +215,13 @@ class GTSAM_EXPORT DiscreteConditional
   size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
 
   /**
-   * @brief Create new conditional by maximizing over all
+   * @brief Create new factor by maximizing over all
    * values with the same separator.
    *
    * @param keys The keys to sum over.
-   * @return DiscreteConditional::shared_ptr
+   * @return DiscreteFactor::shared_ptr
    */
-  virtual DiscreteConditional::shared_ptr max(const Ordering& keys) const;
+  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const;
 
   /// @}
   /// @name Advanced Interface
diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 2e476b0ee..241320649 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -114,11 +114,8 @@ bool TableDistribution::equals(const DiscreteFactor& other, double tol) const {
 }
 
 /* ****************************************************************************/
-DiscreteConditional::shared_ptr TableDistribution::max(
-    const Ordering& keys) const {
-  auto m = *table_.max(keys);
-
-  return std::make_shared<TableDistribution>(m);
+DiscreteFactor::shared_ptr TableDistribution::max(const Ordering& keys) const {
+  return table_.max(keys);
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 9cbca0d26..5b36105a1 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -132,14 +132,13 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   }
 
   /**
-   * @brief Create new conditional by maximizing over all
+   * @brief Create new factor by maximizing over all
    * values with the same separator.
    *
    * @param keys The keys to sum over.
-   * @return DiscreteConditional::shared_ptr
+   * @return DiscreteFactor::shared_ptr
    */
-  virtual DiscreteConditional::shared_ptr max(
-      const Ordering& keys) const override;
+  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const override;
 
   /**
    * @brief Return assignment that maximizes value.

From ffc20f86485dfdf95087cbedcd549eef39f3c2ad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 16:42:10 -0500
Subject: [PATCH 296/362] wrap TableDistribution

---
 gtsam/discrete/discrete.i | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index b2e2524f8..2b8881729 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -168,6 +168,29 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional {
   std::vector<double> pmf() const;
 };
 
+#include <gtsam/discrete/TableDistribution.h>
+virtual class TableDistribution : gtsam::DiscreteConditional {
+  TableDistribution();
+  TableDistribution(const gtsam::TableFactor& f);
+  TableDistribution(const gtsam::DiscreteKey& key, std::vector<double> spec);
+  TableDistribution(const gtsam::DiscreteKeys& keys, std::vector<double> spec);
+  TableDistribution(const gtsam::DiscreteKeys& keys, string spec);
+  TableDistribution(const gtsam::DiscreteKey& keys, string spec);
+  TableDistribution(const gtsam::TableFactor& joint,
+                    const gtsam::TableFactor& marginal);
+  TableDistribution(const gtsam::TableFactor& joint,
+                    const gtsam::TableFactor& marginal,
+                    const gtsam::Ordering& orderedKeys);
+
+  void print(string s = "Table Distribution\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+
+  gtsam::TableFactor table() const;
+  double evaluate(const gtsam::DiscreteValues& values) const;
+  size_t nrValues() const;
+};
+
 #include <gtsam/discrete/DiscreteBayesNet.h>
 class DiscreteBayesNet {
   DiscreteBayesNet();

From e9abd5c57e0eeb19412aeeaf9c895606303b70a5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 16:58:02 -0500
Subject: [PATCH 297/362] wrap TableFactor

---
 gtsam/discrete/discrete.i | 21 ++++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 2b8881729..892df4c73 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -168,6 +168,25 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional {
   std::vector<double> pmf() const;
 };
 
+#include <gtsam/discrete/TableFactor.h>
+virtual class TableFactor : gtsam::DiscreteFactor {
+  TableFactor();
+  TableFactor(const gtsam::DiscreteKeys& keys,
+              const gtsam::TableFactor& potentials);
+  TableFactor(const gtsam::DiscreteKeys& keys, std::vector<double>& table);
+  TableFactor(const gtsam::DiscreteKeys& keys, string spec);
+  TableFactor(const gtsam::DiscreteKeys& keys,
+              const gtsam::DecisionTreeFactor& dtf);
+  TableFactor(const gtsam::DecisionTreeFactor& dtf);
+
+  void print(string s = "TableFactor\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+
+  double evaluate(const gtsam::DiscreteValues& values) const;
+  double error(const gtsam::DiscreteValues& values) const;
+};
+
 #include <gtsam/discrete/TableDistribution.h>
 virtual class TableDistribution : gtsam::DiscreteConditional {
   TableDistribution();
@@ -175,7 +194,7 @@ virtual class TableDistribution : gtsam::DiscreteConditional {
   TableDistribution(const gtsam::DiscreteKey& key, std::vector<double> spec);
   TableDistribution(const gtsam::DiscreteKeys& keys, std::vector<double> spec);
   TableDistribution(const gtsam::DiscreteKeys& keys, string spec);
-  TableDistribution(const gtsam::DiscreteKey& keys, string spec);
+  TableDistribution(const gtsam::DiscreteKey& key, string spec);
   TableDistribution(const gtsam::TableFactor& joint,
                     const gtsam::TableFactor& marginal);
   TableDistribution(const gtsam::TableFactor& joint,

From 9a356f102eeec980cd14b8a2a6c88292bc43c1fe Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 16:58:08 -0500
Subject: [PATCH 298/362] typo fix

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

diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 5b36105a1..662602c77 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -83,7 +83,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   /**
    * Construct from DiscreteKey and std::string.
    */
-  TableDistribution(const DiscreteKeys& key, const std::string& potentials);
+  TableDistribution(const DiscreteKeys& keys, const std::string& potentials);
 
   /**
    * Construct from single DiscreteKey and std::string.

From aba691d3d6572d454cd794173ea6b4de26d31f95 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 16:58:18 -0500
Subject: [PATCH 299/362] fix python test

---
 python/gtsam/tests/test_HybridFactorGraph.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 6d609deb0..6edab3449 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -13,14 +13,14 @@ Author: Fan Jiang, Varun Agrawal, Frank Dellaert
 import unittest
 
 import numpy as np
-from gtsam.symbol_shorthand import C, M, X, Z
-from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam import (DiscreteConditional, GaussianConditional,
-                   HybridBayesNet, HybridGaussianConditional,
-                   HybridGaussianFactor, HybridGaussianFactorGraph,
-                   HybridValues, JacobianFactor, noiseModel)
+from gtsam import (DiscreteConditional, GaussianConditional, HybridBayesNet,
+                   HybridGaussianConditional, HybridGaussianFactor,
+                   HybridGaussianFactorGraph, HybridValues, JacobianFactor,
+                   TableDistribution, noiseModel)
+from gtsam.symbol_shorthand import C, M, X, Z
+from gtsam.utils.test_case import GtsamTestCase
 
 DEBUG_MARGINALS = False
 
@@ -51,7 +51,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         self.assertEqual(len(hybridCond.keys()), 2)
 
         discrete_conditional = hbn.at(hbn.size() - 1).inner()
-        self.assertIsInstance(discrete_conditional, DiscreteConditional)
+        self.assertIsInstance(discrete_conditional, TableDistribution)
 
     def test_optimize(self):
         """Test construction of hybrid factor graph."""

From 69b5e7d5275f28392dff6f844514b6ee92454bc8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 17:10:43 -0500
Subject: [PATCH 300/362] return DiscreteValues directly

---
 gtsam/discrete/TableDistribution.cpp | 4 ++--
 gtsam/discrete/TableDistribution.h   | 2 +-
 gtsam/hybrid/HybridBayesTree.cpp     | 3 +--
 3 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 241320649..aa639c126 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -119,7 +119,7 @@ DiscreteFactor::shared_ptr TableDistribution::max(const Ordering& keys) const {
 }
 
 /* ************************************************************************ */
-uint64_t TableDistribution::argmax() const {
+DiscreteValues TableDistribution::argmax() const {
   uint64_t maxIdx = 0;
   double maxValue = 0.0;
 
@@ -132,7 +132,7 @@ uint64_t TableDistribution::argmax() const {
     }
   }
 
-  return maxIdx;
+  return table_.findAssignments(maxIdx);
 }
 
 /* ****************************************************************************/
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 662602c77..65e895a85 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -145,7 +145,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    *
    * @return maximizing assignment for the variables.
    */
-  uint64_t argmax() const;
+  DiscreteValues argmax() const;
 
   /// @}
   /// @name Advanced Interface
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 1dc277243..0df46f262 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -47,8 +47,7 @@ DiscreteValues HybridBayesTree::discreteMaxProduct(
     const DiscreteFactorGraph& dfg) const {
   TableFactor product = TableProduct(dfg);
 
-  uint64_t maxIdx = TableDistribution(product).argmax();
-  DiscreteValues assignment = product.findAssignments(maxIdx);
+  DiscreteValues assignment = TableDistribution(product).argmax();
   return assignment;
 }
 

From 07a68296d5d8ab900883431d1eccc88e8809d47b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 17:18:19 -0500
Subject: [PATCH 301/362] code cleanup

---
 gtsam/discrete/TableDistribution.cpp | 23 -----------------------
 gtsam/discrete/TableDistribution.h   | 20 --------------------
 2 files changed, 43 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index aa639c126..a7883571a 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -49,12 +49,6 @@ TableDistribution::TableDistribution(const TableFactor& f)
                       DecisionTreeFactor(f.discreteKeys(), ADT())),
       table_(f / (*f.sum(f.keys().size()))) {}
 
-/* ************************************************************************** */
-TableDistribution::TableDistribution(
-    const DiscreteKeys& keys, const Eigen::SparseVector<double>& potentials)
-    : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
-      table_(TableFactor(keys, normalizeSparseTable(potentials))) {}
-
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const DiscreteKeys& keys,
                                      const std::vector<double>& potentials)
@@ -71,23 +65,6 @@ TableDistribution::TableDistribution(const DiscreteKeys& keys,
           keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) {
 }
 
-/* **************************************************************************
- */
-TableDistribution::TableDistribution(const TableFactor& joint,
-                                     const TableFactor& marginal)
-    : BaseConditional(joint.size() - marginal.size(),
-                      joint.discreteKeys() & marginal.discreteKeys(), ADT()),
-      table_(joint / marginal) {}
-
-/* ************************************************************************** */
-TableDistribution::TableDistribution(const TableFactor& joint,
-                                     const TableFactor& marginal,
-                                     const Ordering& orderedKeys)
-    : TableDistribution(joint, marginal) {
-  keys_.clear();
-  keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
-}
-
 /* ************************************************************************** */
 void TableDistribution::print(const string& s,
                               const KeyFormatter& formatter) const {
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 65e895a85..39a1c481f 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -61,12 +61,6 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   /// Construct from TableFactor.
   TableDistribution(const TableFactor& f);
 
-  /**
-   * Construct from DiscreteKeys and SparseVector.
-   */
-  TableDistribution(const DiscreteKeys& keys,
-                    const Eigen::SparseVector<double>& potentials);
-
   /**
    * Construct from DiscreteKeys and std::vector.
    */
@@ -91,20 +85,6 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   TableDistribution(const DiscreteKey& key, const std::string& potentials)
       : TableDistribution(DiscreteKeys(key), potentials) {}
 
-  /**
-   * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
-   * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
-   */
-  TableDistribution(const TableFactor& joint, const TableFactor& marginal);
-
-  /**
-   * @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
-   * Assumes but *does not check* that f(Y)=sum_X f(X,Y).
-   * Makes sure the keys are ordered as given. Does not check orderedKeys.
-   */
-  TableDistribution(const TableFactor& joint, const TableFactor& marginal,
-                    const Ordering& orderedKeys);
-
   /// @}
   /// @name Testable
   /// @{

From bcc52becfbeb1af2c4cf966f03ccc3c30a9c91c9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 17:22:58 -0500
Subject: [PATCH 302/362] emplace then prune

---
 gtsam/hybrid/HybridBayesNet.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 841b74f4f..d6fd7e6bd 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -61,13 +61,15 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
     }
   }
 
-  // Prune the joint. NOTE: imperative and, again, possibly quite expensive.
-  DiscreteConditional pruned(joint);
-  pruned.prune(maxNrLeaves);
-
-  // Create a the result starting with the pruned joint.
+  // Create the result starting with the pruned joint.
   HybridBayesNet result;
-  result.push_back(std::make_shared<DiscreteConditional>(pruned));
+  result.emplace_shared<DiscreteConditional>(joint);
+  // Prune the joint. NOTE: imperative and, again, possibly quite expensive.
+  result.back()->asDiscrete()->prune(maxNrLeaves);
+
+  // Get pruned discrete probabilities so
+  // we can prune HybridGaussianConditionals.
+  DiscreteConditional pruned = *result.back()->asDiscrete();
 
   /* To prune, we visitWith every leaf in the HybridGaussianConditional.
    * For each leaf, using the assignment we can check the discrete decision tree

From 77f38742c4dfe9859d47916d1bf9fdcc2b023041 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jan 2025 17:35:36 -0500
Subject: [PATCH 303/362] remove deleted constructors

---
 gtsam/discrete/discrete.i | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 892df4c73..40f1822cf 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -195,11 +195,6 @@ virtual class TableDistribution : gtsam::DiscreteConditional {
   TableDistribution(const gtsam::DiscreteKeys& keys, std::vector<double> spec);
   TableDistribution(const gtsam::DiscreteKeys& keys, string spec);
   TableDistribution(const gtsam::DiscreteKey& key, string spec);
-  TableDistribution(const gtsam::TableFactor& joint,
-                    const gtsam::TableFactor& marginal);
-  TableDistribution(const gtsam::TableFactor& joint,
-                    const gtsam::TableFactor& marginal,
-                    const gtsam::Ordering& orderedKeys);
 
   void print(string s = "Table Distribution\n",
              const gtsam::KeyFormatter& keyFormatter =

From a142556c52064b5adb2926e76e4eb7e238ed0cb5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 08:45:11 -0500
Subject: [PATCH 304/362] move create to the top

---
 .../discrete/tests/testDecisionTreeFactor.cpp | 30 +++++++++----------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index dc18e0ab2..7210622d8 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -30,6 +30,12 @@
 using namespace std;
 using namespace gtsam;
 
+/** Convert Signature into CPT */
+DecisionTreeFactor create(const Signature& signature) {
+  DecisionTreeFactor p(signature.discreteKeys(), signature.cpt());
+  return p;
+}
+
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, ConstructorsMatch) {
   // Declare two keys
@@ -69,15 +75,6 @@ TEST(DecisionTreeFactor, constructors) {
   EXPECT_DOUBLES_EQUAL(0.8, f4(x121), 1e-9);
 }
 
-/* ************************************************************************* */
-TEST(DecisionTreeFactor, Divide) {
-  DiscreteKey A(0, 2), S(1, 2);
-  DecisionTreeFactor pA = create(A % "99/1"), pS = create(S % "50/50");
-  DecisionTreeFactor joint = pA * pS;
-  DecisionTreeFactor s = joint / pA;
-  EXPECT(assert_equal(pS, s));
-}
-
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, Error) {
   // Declare a bunch of keys
@@ -114,6 +111,15 @@ TEST(DecisionTreeFactor, multiplication) {
   CHECK(assert_equal(expected2, actual));
 }
 
+/* ************************************************************************* */
+TEST(DecisionTreeFactor, Divide) {
+  DiscreteKey A(0, 2), S(1, 2);
+  DecisionTreeFactor pA = create(A % "99/1"), pS = create(S % "50/50");
+  DecisionTreeFactor joint = pA * pS;
+  DecisionTreeFactor s = joint / pA;
+  EXPECT(assert_equal(pS, s));
+}
+
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, sum_max) {
   DiscreteKey v0(0, 3), v1(1, 2);
@@ -226,12 +232,6 @@ void maybeSaveDotFile(const DecisionTreeFactor& f, const string& filename) {
 #endif
 }
 
-/** Convert Signature into CPT */
-DecisionTreeFactor create(const Signature& signature) {
-  DecisionTreeFactor p(signature.discreteKeys(), signature.cpt());
-  return p;
-}
-
 /* ************************************************************************* */
 // test Asia Joint
 TEST(DecisionTreeFactor, joint) {

From 5fa04d7622548a8072ac621ff355a8caa622930d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 09:08:57 -0500
Subject: [PATCH 305/362] small improvements

---
 gtsam/discrete/DiscreteConditional.cpp           |  6 ++----
 gtsam/discrete/DiscreteFactorGraph.cpp           |  2 +-
 gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 10 +++++-----
 gtsam/discrete/tests/testTableFactor.cpp         | 14 ++++++++------
 4 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index be0f42bea..26f38e278 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -24,13 +24,13 @@
 #include <gtsam/hybrid/HybridValues.h>
 
 #include <algorithm>
+#include <cassert>
 #include <random>
 #include <set>
 #include <stdexcept>
 #include <string>
 #include <utility>
 #include <vector>
-#include <cassert>
 
 using namespace std;
 using std::pair;
@@ -45,9 +45,7 @@ template class GTSAM_EXPORT
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
                                          const DecisionTreeFactor& f)
-    : BaseFactor(f / (*std::dynamic_pointer_cast<DecisionTreeFactor>(
-                         f.sum(nrFrontals)))),
-      BaseConditional(nrFrontals) {}
+    : BaseFactor(f / f.sum(nrFrontals)), BaseConditional(nrFrontals) {}
 
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(size_t nrFrontals,
diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 8c950050b..678c70e2d 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -128,7 +128,7 @@ namespace gtsam {
     auto denominator = product.max(product.size());
 
     // Normalize the product factor to prevent underflow.
-    product = product / (*denominator);
+    product = product / denominator;
 
     return product;
   }
diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index cbcf5234e..4ee36f0ab 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -117,9 +117,9 @@ TEST(DiscreteFactorGraph, test) {
       *std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
 
   // Normalize newFactor by max for comparison with expected
-  auto normalizer = newFactor.max(newFactor.size());
+  auto denominator = newFactor.max(newFactor.size());
 
-  newFactor = newFactor / *normalizer;
+  newFactor = newFactor / denominator;
 
   // Check Conditional
   CHECK(conditional);
@@ -131,9 +131,9 @@ TEST(DiscreteFactorGraph, test) {
   CHECK(&newFactor);
   DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
   // Normalize by max.
-  normalizer = expectedFactor.max(expectedFactor.size());
-  // Ensure normalizer is correct.
-  expectedFactor = expectedFactor / *normalizer;
+  denominator = expectedFactor.max(expectedFactor.size());
+  // Ensure denominator is correct.
+  expectedFactor = expectedFactor / denominator;
   EXPECT(assert_equal(expectedFactor, newFactor));
 
   // Test using elimination tree
diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 147c3aea9..4f6ec2f39 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -194,15 +194,17 @@ TEST(TableFactor, Conversion) {
 TEST(TableFactor, Empty) {
   DiscreteKey X(1, 2);
 
-  TableFactor single = *TableFactor({X}, "1 1").sum(1);
+  auto single = TableFactor({X}, "1 1").sum(1);
   // Should not throw a segfault
-  EXPECT(assert_equal(*DecisionTreeFactor(X, "1 1").sum(1),
-                      single.toDecisionTreeFactor()));
+  auto expected_single = DecisionTreeFactor(X, "1 1").sum(1);
+  EXPECT(assert_equal(expected_single->toDecisionTreeFactor(),
+                      single->toDecisionTreeFactor()));
 
-  TableFactor empty = *TableFactor({X}, "0 0").sum(1);
+  auto empty = TableFactor({X}, "0 0").sum(1);
   // Should not throw a segfault
-  EXPECT(assert_equal(*DecisionTreeFactor(X, "0 0").sum(1),
-                      empty.toDecisionTreeFactor()));
+  auto expected_empty = DecisionTreeFactor(X, "0 0").sum(1);
+  EXPECT(assert_equal(expected_empty->toDecisionTreeFactor(),
+                      empty->toDecisionTreeFactor()));
 }
 
 /* ************************************************************************* */

From e309bf370bd195d5f4e2171cd451ca58588e9fb1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 11:54:09 -0500
Subject: [PATCH 306/362] improve operator/ documentation and also showcase
 understanding in test

---
 gtsam/discrete/DecisionTreeFactor.h           | 24 +++++++++----------
 .../discrete/tests/testDecisionTreeFactor.cpp | 14 ++++++++++-
 2 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 804b956fe..a5b82f277 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -154,17 +154,17 @@ namespace gtsam {
 
     static double safe_div(const double& a, const double& b);
 
-    /// divide by factor f (safely)
+    /**
+     * @brief Divide by factor f (safely).
+     * Division of a factor \f$f(x, y)\f$ by another factor \f$g(y, z)\f$
+     * results in a function which involves all keys
+     * \f$(\frac{f}{g})(x, y, z) = f(x, y) / g(y, z)\f$
+     * 
+     * @param f The DecisinTreeFactor to divide by.
+     * @return DecisionTreeFactor 
+     */
     DecisionTreeFactor operator/(const DecisionTreeFactor& f) const {
-      KeyVector diff;
-      std::set_difference(this->keys().begin(), this->keys().end(),
-                          f.keys().begin(), f.keys().end(),
-                          std::back_inserter(diff));
-      DiscreteKeys keys;
-      for (Key key : diff) {
-        keys.push_back({key, this->cardinality(key)});
-      }
-      return DecisionTreeFactor(keys, apply(f, safe_div));
+      return apply(f, safe_div);
     }
 
     /// Convert into a decision tree
@@ -181,12 +181,12 @@ namespace gtsam {
     }
 
     /// Create new factor by maximizing over all values with the same separator.
-    shared_ptr max(size_t nrFrontals) const {
+    DiscreteFactor::shared_ptr max(size_t nrFrontals) const {
       return combine(nrFrontals, Ring::max);
     }
 
     /// Create new factor by maximizing over all values with the same separator.
-    shared_ptr max(const Ordering& keys) const {
+    DiscreteFactor::shared_ptr max(const Ordering& keys) const {
       return combine(keys, Ring::max);
     }
 
diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 7210622d8..ba8714783 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -116,8 +116,20 @@ TEST(DecisionTreeFactor, Divide) {
   DiscreteKey A(0, 2), S(1, 2);
   DecisionTreeFactor pA = create(A % "99/1"), pS = create(S % "50/50");
   DecisionTreeFactor joint = pA * pS;
+
   DecisionTreeFactor s = joint / pA;
-  EXPECT(assert_equal(pS, s));
+
+  // Factors are not equal due to difference in keys
+  EXPECT(assert_inequal(pS, s));
+
+  // The underlying data should be the same
+  using ADT = AlgebraicDecisionTree<Key>;
+  EXPECT(assert_equal(ADT(pS), ADT(s)));
+
+  KeySet keys(joint.keys());
+  keys.insert(pA.keys().begin(), pA.keys().end());
+  EXPECT(assert_inequal(KeySet(pS.keys()), keys));
+  
 }
 
 /* ************************************************************************* */

From 268290dbf25195ce2cdb4ef1ed4099eed6338f78 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 13:22:36 -0500
Subject: [PATCH 307/362] multiply method for DiscreteFactor

---
 gtsam/discrete/DecisionTreeFactor.cpp | 12 ++++++++++++
 gtsam/discrete/DecisionTreeFactor.h   |  5 +++++
 gtsam/discrete/DiscreteFactor.h       | 10 ++++++++++
 gtsam/discrete/TableFactor.cpp        | 12 ++++++++++++
 gtsam/discrete/TableFactor.h          |  4 ++++
 5 files changed, 43 insertions(+)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 1ac782b88..cf22fe153 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -62,6 +62,18 @@ namespace gtsam {
     return error(values.discrete());
   }
 
+  /* ************************************************************************ */
+  DiscreteFactor::shared_ptr DecisionTreeFactor::multiply(
+      const DiscreteFactor::shared_ptr& f) const override {
+    DiscreteFactor::shared_ptr result;
+    if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+      result = std::make_shared<TableFactor>((*tf) * TableFactor(*this));
+    } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      result = std::make_shared<DecisionTreeFactor>(this->operator*(*dtf));
+    }
+    return result;
+  }
+
   /* ************************************************************************ */
   double DecisionTreeFactor::safe_div(const double& a, const double& b) {
     // The use for safe_div is when we divide the product factor by the sum
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 8b6d91be7..d3cb55fa5 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -22,6 +22,7 @@
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/Ring.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Ordering.h>
 
 #include <algorithm>
@@ -147,6 +148,10 @@ namespace gtsam {
     /// Calculate error for DiscreteValues `x`, is -log(probability).
     double error(const DiscreteValues& values) const override;
 
+    /// Multiply factors, DiscreteFactor::shared_ptr edition
+    virtual DiscreteFactor::shared_ptr multiply(
+        const DiscreteFactor::shared_ptr& f) const override;
+
     /// multiply two factors
     DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
       return apply(f, Ring::mul);
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 63cd7844c..adc79bbd5 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -130,6 +130,16 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// DecisionTreeFactor
   virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;
 
+  /**
+   * @brief Multiply in a DiscreteFactor and return the result as
+   * DiscreteFactor, both via shared pointers.
+   *
+   * @param df DiscreteFactor shared_ptr
+   * @return DiscreteFactor::shared_ptr
+   */
+  virtual DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const = 0;
+
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
   /// Create new factor by summing all values with the same separator values
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index a59095d40..cfa56b43a 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -254,6 +254,18 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
+/* ************************************************************************ */
+DiscreteFactor::shared_ptr TableFactor::multiply(
+    const DiscreteFactor::shared_ptr& f) const override {
+  DiscreteFactor::shared_ptr result;
+  if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+    result = std::make_shared<TableFactor>(this->operator*(*tf));
+  } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    result = std::make_shared<TableFactor>(this->operator*(TableFactor(*dtf)));
+  }
+  return result;
+}
+
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 16354026d..f7d0f5215 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -179,6 +179,10 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   /// multiply with DecisionTreeFactor
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  virtual DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& f) const override;
+
   static double safe_div(const double& a, const double& b);
 
   /// divide by factor f (safely)

From 2f09e860e1109cf543a92c32d4fca14ce5d6c28e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 13:36:06 -0500
Subject: [PATCH 308/362] remove override from definition

---
 gtsam/discrete/DecisionTreeFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index cf22fe153..c15fd4e2e 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -64,7 +64,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DiscreteFactor::shared_ptr DecisionTreeFactor::multiply(
-      const DiscreteFactor::shared_ptr& f) const override {
+      const DiscreteFactor::shared_ptr& f) const {
     DiscreteFactor::shared_ptr result;
     if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
       result = std::make_shared<TableFactor>((*tf) * TableFactor(*this));

From 5d865a8cc7908f02c206a3d9801af0fbaf8d1eaa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 13:22:36 -0500
Subject: [PATCH 309/362] multiply method for DiscreteFactor

---
 gtsam/discrete/DecisionTreeFactor.cpp | 12 ++++++++++++
 gtsam/discrete/DecisionTreeFactor.h   |  5 +++++
 gtsam/discrete/DiscreteFactor.h       | 10 ++++++++++
 gtsam/discrete/TableFactor.cpp        | 12 ++++++++++++
 gtsam/discrete/TableFactor.h          |  4 ++++
 5 files changed, 43 insertions(+)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 1ac782b88..cf22fe153 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -62,6 +62,18 @@ namespace gtsam {
     return error(values.discrete());
   }
 
+  /* ************************************************************************ */
+  DiscreteFactor::shared_ptr DecisionTreeFactor::multiply(
+      const DiscreteFactor::shared_ptr& f) const override {
+    DiscreteFactor::shared_ptr result;
+    if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+      result = std::make_shared<TableFactor>((*tf) * TableFactor(*this));
+    } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      result = std::make_shared<DecisionTreeFactor>(this->operator*(*dtf));
+    }
+    return result;
+  }
+
   /* ************************************************************************ */
   double DecisionTreeFactor::safe_div(const double& a, const double& b) {
     // The use for safe_div is when we divide the product factor by the sum
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 80ee10a7b..3e70c0df9 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -22,6 +22,7 @@
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/Ring.h>
+#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Ordering.h>
 
 #include <algorithm>
@@ -147,6 +148,10 @@ namespace gtsam {
     /// Calculate error for DiscreteValues `x`, is -log(probability).
     double error(const DiscreteValues& values) const override;
 
+    /// Multiply factors, DiscreteFactor::shared_ptr edition
+    virtual DiscreteFactor::shared_ptr multiply(
+        const DiscreteFactor::shared_ptr& f) const override;
+
     /// multiply two factors
     DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
       return apply(f, Ring::mul);
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index a1fde0f86..c18eaae2f 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -129,6 +129,16 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// DecisionTreeFactor
   virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;
 
+  /**
+   * @brief Multiply in a DiscreteFactor and return the result as
+   * DiscreteFactor, both via shared pointers.
+   *
+   * @param df DiscreteFactor shared_ptr
+   * @return DiscreteFactor::shared_ptr
+   */
+  virtual DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const = 0;
+
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
   /// @}
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index a59095d40..cfa56b43a 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -254,6 +254,18 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
+/* ************************************************************************ */
+DiscreteFactor::shared_ptr TableFactor::multiply(
+    const DiscreteFactor::shared_ptr& f) const override {
+  DiscreteFactor::shared_ptr result;
+  if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+    result = std::make_shared<TableFactor>(this->operator*(*tf));
+  } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    result = std::make_shared<TableFactor>(this->operator*(TableFactor(*dtf)));
+  }
+  return result;
+}
+
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index a2fdb4d32..4b53d7e2b 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -178,6 +178,10 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   /// multiply with DecisionTreeFactor
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  virtual DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& f) const override;
+
   static double safe_div(const double& a, const double& b);
 
   /// divide by factor f (safely)

From 75a4e98715caca504bacf4cc92a768db3dd89303 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 13:36:06 -0500
Subject: [PATCH 310/362] remove override from definition

---
 gtsam/discrete/DecisionTreeFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index cf22fe153..c15fd4e2e 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -64,7 +64,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DiscreteFactor::shared_ptr DecisionTreeFactor::multiply(
-      const DiscreteFactor::shared_ptr& f) const override {
+      const DiscreteFactor::shared_ptr& f) const {
     DiscreteFactor::shared_ptr result;
     if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
       result = std::make_shared<TableFactor>((*tf) * TableFactor(*this));

From 700ad2bae326f3f89ecc78b56d55a74a64c3785a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 13:56:54 -0500
Subject: [PATCH 311/362] remove override from TableFactor definition

---
 gtsam/discrete/TableFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index cfa56b43a..3ca8fecda 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -256,7 +256,7 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 
 /* ************************************************************************ */
 DiscreteFactor::shared_ptr TableFactor::multiply(
-    const DiscreteFactor::shared_ptr& f) const override {
+    const DiscreteFactor::shared_ptr& f) const {
   DiscreteFactor::shared_ptr result;
   if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
     result = std::make_shared<TableFactor>(this->operator*(*tf));

From 260d448887447a9a9f3216a544ad34ddd1dfbd8c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 13:57:07 -0500
Subject: [PATCH 312/362] use new multiply method

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 227bb4da3..0444f47ae 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -65,11 +65,11 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
-    DecisionTreeFactor result;
-    for (const sharedFactor& factor : *this) {
-      if (factor) result = (*factor) * result;
+    DiscreteFactor::shared_ptr result = *this->begin();
+    for (auto it = this->begin() + 1; it != this->end(); ++it) {
+      if (*it) result = result->multiply(*it);
     }
-    return result;
+    return result->toDecisionTreeFactor();
   }
 
   /* ************************************************************************ */

From 453059bd61f4b08acdc43c37b9b098a5579d36a3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 14:03:08 -0500
Subject: [PATCH 313/362] simplify to remove DiscreteProduct static function

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 44 ++++++++++----------------
 1 file changed, 17 insertions(+), 27 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 0444f47ae..b3029111a 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -65,11 +65,23 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
-    DiscreteFactor::shared_ptr result = *this->begin();
+    // PRODUCT: multiply all factors
+    gttic(product);
+    DiscreteFactor::shared_ptr product = *this->begin();
     for (auto it = this->begin() + 1; it != this->end(); ++it) {
-      if (*it) result = result->multiply(*it);
+      if (*it) product = product->multiply(*it);
     }
-    return result->toDecisionTreeFactor();
+    gttoc(product);
+
+    DecisionTreeFactor = result->toDecisionTreeFactor();
+
+    // Max over all the potentials by pretending all keys are frontal:
+    auto denominator = product.max(product.size());
+
+    // Normalize the product factor to prevent underflow.
+    product = product / (*denominator);
+
+    return product;
   }
 
   /* ************************************************************************ */
@@ -111,34 +123,12 @@ namespace gtsam {
 //      }
 //  }
 
-  /**
-   * @brief Multiply all the `factors`.
-   *
-   * @param factors The factors to multiply as a DiscreteFactorGraph.
-   * @return DecisionTreeFactor
-   */
-  static DecisionTreeFactor DiscreteProduct(
-      const DiscreteFactorGraph& factors) {
-    // PRODUCT: multiply all factors
-    gttic(product);
-    DecisionTreeFactor product = factors.product();
-    gttoc(product);
-
-    // Max over all the potentials by pretending all keys are frontal:
-    auto denominator = product.max(product.size());
-
-    // Normalize the product factor to prevent underflow.
-    product = product / (*denominator);
-
-    return product;
-  }
-
   /* ************************************************************************ */
   // Alternate eliminate function for MPE
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DecisionTreeFactor product = DiscreteProduct(factors);
+    DecisionTreeFactor product = factors.product();
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -216,7 +206,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DecisionTreeFactor product = DiscreteProduct(factors);
+    DecisionTreeFactor product = factors.product();
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);

From a02baec0119ca9b670a8b5b64ebecc5db492bcbd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 14:23:34 -0500
Subject: [PATCH 314/362] naive implementation of multiply for unstable

---
 gtsam_unstable/discrete/AllDiff.h       | 7 +++++++
 gtsam_unstable/discrete/BinaryAllDiff.h | 7 +++++++
 gtsam_unstable/discrete/Domain.h        | 7 +++++++
 gtsam_unstable/discrete/SingleValue.h   | 7 +++++++
 4 files changed, 28 insertions(+)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 1180abad4..cfbd76e7c 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -53,6 +53,13 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   /// Multiply into a decisiontree
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
+  }
+
   /// Compute error for each assignment and return as a tree
   AlgebraicDecisionTree<Key> errorTree() const override {
     throw std::runtime_error("AllDiff::error not implemented");
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index e96bfdfde..a1a2bf0a6 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -69,6 +69,13 @@ class BinaryAllDiff : public Constraint {
     return toDecisionTreeFactor() * f;
   }
 
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
+  }
+
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 23a566d24..dea85934f 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -90,6 +90,13 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   /// Multiply into a decisiontree
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
+  }
+
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 3df1209b8..8675c929b 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -63,6 +63,13 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   /// Multiply into a decisiontree
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
+  }
+
   /*
    * Ensure Arc-consistency: just sets domain[j] to {value_}.
    * @param j domain to be checked

From 13bafb0a48bae3c5598b7db112ac2757bd02c431 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 14:28:57 -0500
Subject: [PATCH 315/362] Revert "simplify to remove DiscreteProduct static
 function"

This reverts commit 453059bd61f4b08acdc43c37b9b098a5579d36a3.
---
 gtsam/discrete/DiscreteFactorGraph.cpp | 44 ++++++++++++++++----------
 1 file changed, 27 insertions(+), 17 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index b3029111a..0444f47ae 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -65,23 +65,11 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
-    // PRODUCT: multiply all factors
-    gttic(product);
-    DiscreteFactor::shared_ptr product = *this->begin();
+    DiscreteFactor::shared_ptr result = *this->begin();
     for (auto it = this->begin() + 1; it != this->end(); ++it) {
-      if (*it) product = product->multiply(*it);
+      if (*it) result = result->multiply(*it);
     }
-    gttoc(product);
-
-    DecisionTreeFactor = result->toDecisionTreeFactor();
-
-    // Max over all the potentials by pretending all keys are frontal:
-    auto denominator = product.max(product.size());
-
-    // Normalize the product factor to prevent underflow.
-    product = product / (*denominator);
-
-    return product;
+    return result->toDecisionTreeFactor();
   }
 
   /* ************************************************************************ */
@@ -123,12 +111,34 @@ namespace gtsam {
 //      }
 //  }
 
+  /**
+   * @brief Multiply all the `factors`.
+   *
+   * @param factors The factors to multiply as a DiscreteFactorGraph.
+   * @return DecisionTreeFactor
+   */
+  static DecisionTreeFactor DiscreteProduct(
+      const DiscreteFactorGraph& factors) {
+    // PRODUCT: multiply all factors
+    gttic(product);
+    DecisionTreeFactor product = factors.product();
+    gttoc(product);
+
+    // Max over all the potentials by pretending all keys are frontal:
+    auto denominator = product.max(product.size());
+
+    // Normalize the product factor to prevent underflow.
+    product = product / (*denominator);
+
+    return product;
+  }
+
   /* ************************************************************************ */
   // Alternate eliminate function for MPE
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DecisionTreeFactor product = factors.product();
+    DecisionTreeFactor product = DiscreteProduct(factors);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -206,7 +216,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DecisionTreeFactor product = factors.product();
+    DecisionTreeFactor product = DiscreteProduct(factors);
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);

From a7fc6e3763d4dc5ea5019906be37e05613d3f9d9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 15:08:58 -0500
Subject: [PATCH 316/362] convert everything to DecisionTreeFactor so we can
 use override operator* method

---
 gtsam_unstable/discrete/AllDiff.h       | 4 ++--
 gtsam_unstable/discrete/BinaryAllDiff.h | 4 ++--
 gtsam_unstable/discrete/Domain.h        | 4 ++--
 gtsam_unstable/discrete/SingleValue.h   | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index cfbd76e7c..032808dcd 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -56,8 +56,8 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
+    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
+                                                df->toDecisionTreeFactor());
   }
 
   /// Compute error for each assignment and return as a tree
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index a1a2bf0a6..0ebae4d77 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -72,8 +72,8 @@ class BinaryAllDiff : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
+    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
+                                                df->toDecisionTreeFactor());
   }
 
   /*
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index dea85934f..9a4a21847 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -93,8 +93,8 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
+    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
+                                                df->toDecisionTreeFactor());
   }
 
   /*
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 8675c929b..ebe23f7e4 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -66,8 +66,8 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
+    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
+                                                df->toDecisionTreeFactor());
   }
 
   /*

From 8390ffa2cbde062f7628a953bba6f43ba77cc7d1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 15:19:16 -0500
Subject: [PATCH 317/362] revert previous commit

---
 gtsam_unstable/discrete/AllDiff.h       | 4 ++--
 gtsam_unstable/discrete/BinaryAllDiff.h | 4 ++--
 gtsam_unstable/discrete/Domain.h        | 4 ++--
 gtsam_unstable/discrete/SingleValue.h   | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 032808dcd..cfbd76e7c 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -56,8 +56,8 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
-                                                df->toDecisionTreeFactor());
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
   }
 
   /// Compute error for each assignment and return as a tree
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 0ebae4d77..a1a2bf0a6 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -72,8 +72,8 @@ class BinaryAllDiff : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
-                                                df->toDecisionTreeFactor());
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
   }
 
   /*
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 9a4a21847..dea85934f 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -93,8 +93,8 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
-                                                df->toDecisionTreeFactor());
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
   }
 
   /*
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index ebe23f7e4..8675c929b 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -66,8 +66,8 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   /// Multiply factors, DiscreteFactor::shared_ptr edition
   DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(this->toDecisionTreeFactor() *
-                                                df->toDecisionTreeFactor());
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
   }
 
   /*

From bc63cc8cb88c7edebf86a4136d7231ab51a59e47 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 15:19:57 -0500
Subject: [PATCH 318/362] use double dispatch for else case

---
 gtsam/discrete/DecisionTreeFactor.cpp | 3 +++
 gtsam/discrete/TableFactor.cpp        | 4 ++++
 2 files changed, 7 insertions(+)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index c15fd4e2e..4b16dad8a 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -70,6 +70,9 @@ namespace gtsam {
       result = std::make_shared<TableFactor>((*tf) * TableFactor(*this));
     } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
       result = std::make_shared<DecisionTreeFactor>(this->operator*(*dtf));
+    } else {
+      // Simulate double dispatch in C++
+      result = std::make_shared<DecisionTreeFactor>(f->operator*(*this));
     }
     return result;
   }
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 3ca8fecda..6516a4a98 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -262,6 +262,10 @@ DiscreteFactor::shared_ptr TableFactor::multiply(
     result = std::make_shared<TableFactor>(this->operator*(*tf));
   } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
     result = std::make_shared<TableFactor>(this->operator*(TableFactor(*dtf)));
+  } else {
+    // Simulate double dispatch in C++
+    result = std::make_shared<DecisionTreeFactor>(
+        f->operator*(this->toDecisionTreeFactor()));
   }
   return result;
 }

From 713c49c9153f9f023621a6cd9a06997594ea52ab Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 15:20:09 -0500
Subject: [PATCH 319/362] more robust product

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 0444f47ae..a2b896286 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -65,9 +65,16 @@ namespace gtsam {
 
   /* ************************************************************************ */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
-    DiscreteFactor::shared_ptr result = *this->begin();
-    for (auto it = this->begin() + 1; it != this->end(); ++it) {
-      if (*it) result = result->multiply(*it);
+    DiscreteFactor::shared_ptr result;
+    for (auto it = this->begin(); it != this->end(); ++it) {
+      if (*it) {
+        if (result) {
+          result = result->multiply(*it);
+        } else {
+          // Assign to the first non-null factor
+          result = *it;
+        }
+      }
     }
     return result->toDecisionTreeFactor();
   }

From b83aadb20487f69c6ab932245f8524c8ec92fdde Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 15:37:37 -0500
Subject: [PATCH 320/362] remove accidental type change

---
 gtsam/discrete/DecisionTreeFactor.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index a5b82f277..eb6d9eaa2 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -181,12 +181,12 @@ namespace gtsam {
     }
 
     /// Create new factor by maximizing over all values with the same separator.
-    DiscreteFactor::shared_ptr max(size_t nrFrontals) const {
+    shared_ptr max(size_t nrFrontals) const {
       return combine(nrFrontals, Ring::max);
     }
 
     /// Create new factor by maximizing over all values with the same separator.
-    DiscreteFactor::shared_ptr max(const Ordering& keys) const {
+    shared_ptr max(const Ordering& keys) const {
       return combine(keys, Ring::max);
     }
 

From b5128b2c9fcf31d04e4760c3c4178d8449ad44c9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 19:40:37 -0500
Subject: [PATCH 321/362] use DecisionTreeFactor version of sum and max where
 not available

---
 gtsam_unstable/discrete/AllDiff.h       | 8 ++++----
 gtsam_unstable/discrete/BinaryAllDiff.h | 8 ++++----
 gtsam_unstable/discrete/Domain.h        | 8 ++++----
 gtsam_unstable/discrete/SingleValue.h   | 8 ++++----
 4 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index cf0e5e3cf..267ddb9fd 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -84,19 +84,19 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   uint64_t nrValues() const override { return 1; };
 
   DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(keys);
   }
 
   DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(keys);
   }
 };
 
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index c15ac8aec..3035d0620 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -108,19 +108,19 @@ class BinaryAllDiff : public Constraint {
   uint64_t nrValues() const override { return 1; };
 
   DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(keys);
   }
 
   DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(keys);
   }
 };
 
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index f64716028..4c2d3f9dd 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -123,19 +123,19 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   Constraint::shared_ptr partiallyApply(const Domains& domains) const override;
 
   DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(keys);
   }
 
   DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(keys);
   }
 };
 
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index c5824a96a..b6c91f912 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -89,19 +89,19 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   uint64_t nrValues() const override { return 1; };
 
   DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().sum(keys);
   }
 
   DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(nrFrontals);
   }
 
   DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    throw std::runtime_error("Not implemented");
+    return toDecisionTreeFactor().max(keys);
   }
 };
 

From 4ebca711461eb3fd914b74b4532242aedf38c048 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 20:44:10 -0500
Subject: [PATCH 322/362] divide operator for DiscreteFactor::shared_ptr

---
 gtsam/discrete/DecisionTreeFactor.cpp   | 13 +++++++++++++
 gtsam/discrete/DecisionTreeFactor.h     |  5 ++---
 gtsam/discrete/DiscreteFactor.h         |  4 ++++
 gtsam/discrete/TableFactor.cpp          | 14 ++++++++++++++
 gtsam/discrete/TableFactor.h            | 11 ++---------
 gtsam_unstable/discrete/AllDiff.cpp     |  6 ++++++
 gtsam_unstable/discrete/AllDiff.h       |  4 ++++
 gtsam_unstable/discrete/BinaryAllDiff.h |  6 ++++++
 gtsam_unstable/discrete/Domain.cpp      |  6 ++++++
 gtsam_unstable/discrete/Domain.h        |  4 ++++
 gtsam_unstable/discrete/SingleValue.cpp |  6 ++++++
 gtsam_unstable/discrete/SingleValue.h   |  4 ++++
 12 files changed, 71 insertions(+), 12 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 4b16dad8a..2f2c039a4 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -77,6 +77,19 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************ */
+  DiscreteFactor::shared_ptr DecisionTreeFactor::operator/(
+      const DiscreteFactor::shared_ptr& f) const {
+    if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+      return std::make_shared<TableFactor>(tf->operator/(TableFactor(*this)));
+    } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      return std::make_shared<DecisionTreeFactor>(this->operator/(*dtf));
+    } else {
+      return std::make_shared<DecisionTreeFactor>(
+          this->operator/(this->toDecisionTreeFactor()));
+    }
+  }
+
   /* ************************************************************************ */
   double DecisionTreeFactor::safe_div(const double& a, const double& b) {
     // The use for safe_div is when we divide the product factor by the sum
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index d3cb55fa5..a5327bdd0 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -165,9 +165,8 @@ namespace gtsam {
     }
 
     /// divide by DiscreteFactor::shared_ptr f (safely)
-    DecisionTreeFactor operator/(const DiscreteFactor::shared_ptr& f) const {
-      return apply(*std::dynamic_pointer_cast<DecisionTreeFactor>(f), safe_div);
-    }
+    DiscreteFactor::shared_ptr operator/(
+        const DiscreteFactor::shared_ptr& f) const override;
 
     /// Convert into a decision tree
     DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index adc79bbd5..6cbc00d09 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -140,6 +140,10 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   virtual DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& df) const = 0;
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  virtual DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const = 0;
+
   virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
 
   /// Create new factor by summing all values with the same separator values
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 6516a4a98..b692e9ba2 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -270,6 +270,20 @@ DiscreteFactor::shared_ptr TableFactor::multiply(
   return result;
 }
 
+/* ************************************************************************ */
+DiscreteFactor::shared_ptr TableFactor::operator/(
+    const DiscreteFactor::shared_ptr& f) const {
+  if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+    return std::make_shared<TableFactor>(this->operator/(*tf));
+  } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    return std::make_shared<TableFactor>(
+        this->operator/(TableFactor(f->discreteKeys(), *dtf)));
+  } else {
+    TableFactor divisor(f->toDecisionTreeFactor());
+    return std::make_shared<TableFactor>(this->operator/(divisor));
+  }
+}
+
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index f7d0f5215..a2f74758f 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -191,15 +191,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   }
 
   /// divide by DiscreteFactor::shared_ptr f (safely)
-  TableFactor operator/(const DiscreteFactor::shared_ptr& f) const {
-    if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
-      return apply(*tf, safe_div);
-    } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
-      return apply(TableFactor(f->discreteKeys(), *dtf), safe_div);
-    } else {
-      throw std::runtime_error("Unknown derived type for DiscreteFactor");
-    }
-  }
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& f) const override;
 
   /// Convert into a decisiontree
   DecisionTreeFactor toDecisionTreeFactor() const override;
diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp
index 585ca8103..01f50fa3d 100644
--- a/gtsam_unstable/discrete/AllDiff.cpp
+++ b/gtsam_unstable/discrete/AllDiff.cpp
@@ -56,6 +56,12 @@ DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
+/* ************************************************************************* */
+DiscreteFactor::shared_ptr AllDiff::operator/(
+    const DiscreteFactor::shared_ptr& df) const {
+  return this->toDecisionTreeFactor() / df;
+}
+
 /* ************************************************************************* */
 bool AllDiff::ensureArcConsistency(Key j, Domains* domains) const {
   Domain& Dj = domains->at(j);
diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 267ddb9fd..7a7b1cecc 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -60,6 +60,10 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
         this->operator*(df->toDecisionTreeFactor()));
   }
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const override;
+
   /// Compute error for each assignment and return as a tree
   AlgebraicDecisionTree<Key> errorTree() const override {
     throw std::runtime_error("AllDiff::error not implemented");
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 3035d0620..fbff8a01c 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -76,6 +76,12 @@ class BinaryAllDiff : public Constraint {
         this->operator*(df->toDecisionTreeFactor()));
   }
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return this->toDecisionTreeFactor() / df;
+  }
+
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index 74f621dc7..cecb7cc1a 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -49,6 +49,12 @@ DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
+/* ************************************************************************* */
+DiscreteFactor::shared_ptr Domain::operator/(
+    const DiscreteFactor::shared_ptr& df) const {
+  return this->toDecisionTreeFactor() / df;
+}
+
 /* ************************************************************************* */
 bool Domain::ensureArcConsistency(Key j, Domains* domains) const {
   if (j != key()) throw invalid_argument("Domain check on wrong domain");
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 4c2d3f9dd..7362e9caf 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -97,6 +97,10 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
         this->operator*(df->toDecisionTreeFactor()));
   }
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const override;
+
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp
index 220bc9c06..09a8314df 100644
--- a/gtsam_unstable/discrete/SingleValue.cpp
+++ b/gtsam_unstable/discrete/SingleValue.cpp
@@ -41,6 +41,12 @@ DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
+/* ************************************************************************* */
+DiscreteFactor::shared_ptr SingleValue::operator/(
+    const DiscreteFactor::shared_ptr& df) const {
+  return this->toDecisionTreeFactor() / df;
+}
+
 /* ************************************************************************* */
 bool SingleValue::ensureArcConsistency(Key j, Domains* domains) const {
   if (j != keys_[0])
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index b6c91f912..87c42fc80 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -70,6 +70,10 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
         this->operator*(df->toDecisionTreeFactor()));
   }
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const override;
+
   /*
    * Ensure Arc-consistency: just sets domain[j] to {value_}.
    * @param j domain to be checked

From fb1d52a18eec0e804276ef542edf4bdd34f69d3f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 20:49:24 -0500
Subject: [PATCH 323/362] fix constructor

---
 gtsam/discrete/DiscreteConditional.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 26f38e278..06a08eca5 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -45,7 +45,8 @@ template class GTSAM_EXPORT
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
                                          const DecisionTreeFactor& f)
-    : BaseFactor(f / f.sum(nrFrontals)), BaseConditional(nrFrontals) {}
+    : BaseFactor(f / f.sum(nrFrontals)->toDecisionTreeFactor()),
+      BaseConditional(nrFrontals) {}
 
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(size_t nrFrontals,

From e9822a70d2921fdbd433f55fa29d91ac9467f7f1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 20:50:24 -0500
Subject: [PATCH 324/362] update DiscreteFactorGraph to use
 DiscreteFactor::shared_ptr for elimination

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 37 +++++++++++++-------------
 gtsam/discrete/DiscreteFactorGraph.h   |  2 +-
 2 files changed, 19 insertions(+), 20 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index e05cf9e33..eb3221819 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -64,7 +64,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  DecisionTreeFactor DiscreteFactorGraph::product() const {
+  DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const {
     DiscreteFactor::shared_ptr result;
     for (auto it = this->begin(); it != this->end(); ++it) {
       if (*it) {
@@ -76,7 +76,7 @@ namespace gtsam {
         }
       }
     }
-    return result->toDecisionTreeFactor();
+    return result;
   }
 
   /* ************************************************************************ */
@@ -122,20 +122,20 @@ namespace gtsam {
    * @brief Multiply all the `factors`.
    *
    * @param factors The factors to multiply as a DiscreteFactorGraph.
-   * @return DecisionTreeFactor
+   * @return DiscreteFactor::shared_ptr
    */
-  static DecisionTreeFactor DiscreteProduct(
+  static DiscreteFactor::shared_ptr DiscreteProduct(
       const DiscreteFactorGraph& factors) {
     // PRODUCT: multiply all factors
     gttic(product);
-    DecisionTreeFactor product = factors.product();
+    DiscreteFactor::shared_ptr product = factors.product();
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:
-    auto denominator = product.max(product.size());
+    auto denominator = product->max(product->size());
 
     // Normalize the product factor to prevent underflow.
-    product = product / denominator;
+    product = product->operator/(denominator);
 
     return product;
   }
@@ -145,26 +145,25 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DecisionTreeFactor product = DiscreteProduct(factors);
+    DiscreteFactor::shared_ptr product = DiscreteProduct(factors);
 
     // max out frontals, this is the factor on the separator
     gttic(max);
-    DecisionTreeFactor::shared_ptr max =
-        std::dynamic_pointer_cast<DecisionTreeFactor>(product.max(frontalKeys));
+    DiscreteFactor::shared_ptr max = product->max(frontalKeys);
     gttoc(max);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     DiscreteKeys orderedKeys;
     for (auto&& key : frontalKeys)
-      orderedKeys.emplace_back(key, product.cardinality(key));
+      orderedKeys.emplace_back(key, product->cardinality(key));
     for (auto&& key : max->keys())
-      orderedKeys.emplace_back(key, product.cardinality(key));
+      orderedKeys.emplace_back(key, product->cardinality(key));
 
     // Make lookup with product
     gttic(lookup);
     size_t nrFrontals = frontalKeys.size();
-    auto lookup =
-        std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
+    auto lookup = std::make_shared<DiscreteLookupTable>(
+        nrFrontals, orderedKeys, product->toDecisionTreeFactor());
     gttoc(lookup);
 
     return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
@@ -224,12 +223,11 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DecisionTreeFactor product = DiscreteProduct(factors);
+    DiscreteFactor::shared_ptr product = DiscreteProduct(factors);
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);
-    DecisionTreeFactor::shared_ptr sum = std::dynamic_pointer_cast<DecisionTreeFactor>(
-        product.sum(frontalKeys));
+    DiscreteFactor::shared_ptr sum = product->sum(frontalKeys);
     gttoc(sum);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
@@ -241,8 +239,9 @@ namespace gtsam {
 
     // now divide product/sum to get conditional
     gttic(divide);
-    auto conditional =
-        std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
+    auto conditional = std::make_shared<DiscreteConditional>(
+        product->toDecisionTreeFactor(), sum->toDecisionTreeFactor(),
+        orderedKeys);
     gttoc(divide);
 
     return {conditional, sum};
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index b311cb78b..3d9e86cd1 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -148,7 +148,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
   DiscreteKeys discreteKeys() const;
 
   /** return product of all factors as a single factor */
-  DecisionTreeFactor product() const;
+  DiscreteFactor::shared_ptr product() const;
 
   /** 
    * Evaluates the factor graph given values, returns the joint probability of

From 2f8c8ddb75cb96b30b550bd03e0a659746938857 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 20:50:40 -0500
Subject: [PATCH 325/362] update tests

---
 gtsam/discrete/tests/testDiscreteConditional.cpp | 4 ++--
 gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 7 ++++---
 gtsam_unstable/discrete/tests/testCSP.cpp        | 2 +-
 gtsam_unstable/discrete/tests/testScheduler.cpp  | 2 +-
 4 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp
index d17c76837..b91e1bd8a 100644
--- a/gtsam/discrete/tests/testDiscreteConditional.cpp
+++ b/gtsam/discrete/tests/testDiscreteConditional.cpp
@@ -46,7 +46,7 @@ TEST(DiscreteConditional, constructors) {
   DecisionTreeFactor f2(
       X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor expected2 = f2 / f2.sum(1);
+  DecisionTreeFactor expected2 = f2 / f2.sum(1)->toDecisionTreeFactor();
   EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
 
   std::vector<double> probs{0.2, 0.5, 0.3, 0.6, 0.4, 0.7, 0.25, 0.55, 0.35, 0.65, 0.45, 0.75};
@@ -70,7 +70,7 @@ TEST(DiscreteConditional, constructors_alt_interface) {
   DecisionTreeFactor f2(
       X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor expected2 = f2 / f2.sum(1);
+  DecisionTreeFactor expected2 = f2 / f2.sum(1)->toDecisionTreeFactor();
   EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
 }
 
diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 4ee36f0ab..0c1dd7a2a 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -94,7 +94,7 @@ TEST_UNSAFE( DiscreteFactorGraph, DiscreteFactorGraphEvaluationTest) {
   EXPECT_DOUBLES_EQUAL( 1.944, graph(values), 1e-9);
 
   // Check if graph product works
-  DecisionTreeFactor product = graph.product();
+  DecisionTreeFactor product = graph.product()->toDecisionTreeFactor();
   EXPECT_DOUBLES_EQUAL( 1.944, product(values), 1e-9);
 }
 
@@ -117,7 +117,7 @@ TEST(DiscreteFactorGraph, test) {
       *std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
 
   // Normalize newFactor by max for comparison with expected
-  auto denominator = newFactor.max(newFactor.size());
+  auto denominator = newFactor.max(newFactor.size())->toDecisionTreeFactor();
 
   newFactor = newFactor / denominator;
 
@@ -131,7 +131,8 @@ TEST(DiscreteFactorGraph, test) {
   CHECK(&newFactor);
   DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
   // Normalize by max.
-  denominator = expectedFactor.max(expectedFactor.size());
+  denominator =
+      expectedFactor.max(expectedFactor.size())->toDecisionTreeFactor();
   // Ensure denominator is correct.
   expectedFactor = expectedFactor / denominator;
   EXPECT(assert_equal(expectedFactor, newFactor));
diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp
index 2b9a20ca6..6806bfe58 100644
--- a/gtsam_unstable/discrete/tests/testCSP.cpp
+++ b/gtsam_unstable/discrete/tests/testCSP.cpp
@@ -124,7 +124,7 @@ TEST(CSP, allInOne) {
   EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);
 
   // Just for fun, create the product and check it
-  DecisionTreeFactor product = csp.product();
+  DecisionTreeFactor product = csp.product()->toDecisionTreeFactor();
   // product.dot("product");
   DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
   EXPECT(assert_equal(expectedProduct, product));
diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp
index f868abb5e..5f9b7f287 100644
--- a/gtsam_unstable/discrete/tests/testScheduler.cpp
+++ b/gtsam_unstable/discrete/tests/testScheduler.cpp
@@ -113,7 +113,7 @@ TEST(schedulingExample, test) {
   EXPECT(assert_equal(expected, (DiscreteFactorGraph)s));
 
   // Do brute force product and output that to file
-  DecisionTreeFactor product = s.product();
+  DecisionTreeFactor product = s.product()->toDecisionTreeFactor();
   // product.dot("scheduling", false);
 
   // Do exact inference

From 2434e248e95d0efac75a18cddae2748ff8be4502 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 5 Jan 2025 20:54:56 -0500
Subject: [PATCH 326/362] undo print change in DiscreteLookupTable

---
 gtsam/discrete/DiscreteLookupDAG.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp
index c1c301525..d1108e7b7 100644
--- a/gtsam/discrete/DiscreteLookupDAG.cpp
+++ b/gtsam/discrete/DiscreteLookupDAG.cpp
@@ -48,7 +48,7 @@ void DiscreteLookupTable::print(const std::string& s,
     }
   }
   cout << "):\n";
-  BaseFactor::print("", formatter);
+  ADT::print("", formatter);
   cout << endl;
 }
 

From 43f755d9d8d0ede5868ac18ae33e9350c43a2e78 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 11:17:03 -0500
Subject: [PATCH 327/362] move multiply to Constraint.h

---
 gtsam_unstable/discrete/AllDiff.h       | 7 -------
 gtsam_unstable/discrete/BinaryAllDiff.h | 7 -------
 gtsam_unstable/discrete/Constraint.h    | 8 ++++++++
 gtsam_unstable/discrete/Domain.h        | 7 -------
 gtsam_unstable/discrete/SingleValue.h   | 7 -------
 5 files changed, 8 insertions(+), 28 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index cfbd76e7c..1180abad4 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -53,13 +53,6 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   /// Multiply into a decisiontree
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
-  /// Multiply factors, DiscreteFactor::shared_ptr edition
-  DiscreteFactor::shared_ptr multiply(
-      const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
-  }
-
   /// Compute error for each assignment and return as a tree
   AlgebraicDecisionTree<Key> errorTree() const override {
     throw std::runtime_error("AllDiff::error not implemented");
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index a1a2bf0a6..e96bfdfde 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -69,13 +69,6 @@ class BinaryAllDiff : public Constraint {
     return toDecisionTreeFactor() * f;
   }
 
-  /// Multiply factors, DiscreteFactor::shared_ptr edition
-  DiscreteFactor::shared_ptr multiply(
-      const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
-  }
-
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h
index 3526a282d..71ed7647a 100644
--- a/gtsam_unstable/discrete/Constraint.h
+++ b/gtsam_unstable/discrete/Constraint.h
@@ -78,6 +78,14 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor {
 
   /// Partially apply known values, domain version
   virtual shared_ptr partiallyApply(const Domains&) const = 0;
+
+  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return std::make_shared<DecisionTreeFactor>(
+        this->operator*(df->toDecisionTreeFactor()));
+  }
+
   /// @}
   /// @name Wrapper support
   /// @{
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index dea85934f..23a566d24 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -90,13 +90,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
   /// Multiply into a decisiontree
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
-  /// Multiply factors, DiscreteFactor::shared_ptr edition
-  DiscreteFactor::shared_ptr multiply(
-      const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
-  }
-
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 8675c929b..3df1209b8 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -63,13 +63,6 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   /// Multiply into a decisiontree
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
-  /// Multiply factors, DiscreteFactor::shared_ptr edition
-  DiscreteFactor::shared_ptr multiply(
-      const DiscreteFactor::shared_ptr& df) const override {
-    return std::make_shared<DecisionTreeFactor>(
-        this->operator*(df->toDecisionTreeFactor()));
-  }
-
   /*
    * Ensure Arc-consistency: just sets domain[j] to {value_}.
    * @param j domain to be checked

From 7561da4df2df5740a808d0e6ea4e957eb65cb4f2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 13:35:45 -0500
Subject: [PATCH 328/362] move operator/ to Constraint.h

---
 gtsam_unstable/discrete/AllDiff.cpp     | 6 ------
 gtsam_unstable/discrete/Constraint.h    | 6 ++++++
 gtsam_unstable/discrete/Domain.cpp      | 6 ------
 gtsam_unstable/discrete/SingleValue.cpp | 6 ------
 4 files changed, 6 insertions(+), 18 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp
index 01f50fa3d..585ca8103 100644
--- a/gtsam_unstable/discrete/AllDiff.cpp
+++ b/gtsam_unstable/discrete/AllDiff.cpp
@@ -56,12 +56,6 @@ DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
-/* ************************************************************************* */
-DiscreteFactor::shared_ptr AllDiff::operator/(
-    const DiscreteFactor::shared_ptr& df) const {
-  return this->toDecisionTreeFactor() / df;
-}
-
 /* ************************************************************************* */
 bool AllDiff::ensureArcConsistency(Key j, Domains* domains) const {
   Domain& Dj = domains->at(j);
diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h
index 71ed7647a..2d98ab40b 100644
--- a/gtsam_unstable/discrete/Constraint.h
+++ b/gtsam_unstable/discrete/Constraint.h
@@ -86,6 +86,12 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor {
         this->operator*(df->toDecisionTreeFactor()));
   }
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return this->toDecisionTreeFactor() / df;
+  }
+
   /// @}
   /// @name Wrapper support
   /// @{
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index cecb7cc1a..74f621dc7 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -49,12 +49,6 @@ DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
-/* ************************************************************************* */
-DiscreteFactor::shared_ptr Domain::operator/(
-    const DiscreteFactor::shared_ptr& df) const {
-  return this->toDecisionTreeFactor() / df;
-}
-
 /* ************************************************************************* */
 bool Domain::ensureArcConsistency(Key j, Domains* domains) const {
   if (j != key()) throw invalid_argument("Domain check on wrong domain");
diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp
index 09a8314df..220bc9c06 100644
--- a/gtsam_unstable/discrete/SingleValue.cpp
+++ b/gtsam_unstable/discrete/SingleValue.cpp
@@ -41,12 +41,6 @@ DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const {
   return toDecisionTreeFactor() * f;
 }
 
-/* ************************************************************************* */
-DiscreteFactor::shared_ptr SingleValue::operator/(
-    const DiscreteFactor::shared_ptr& df) const {
-  return this->toDecisionTreeFactor() / df;
-}
-
 /* ************************************************************************* */
 bool SingleValue::ensureArcConsistency(Key j, Domains* domains) const {
   if (j != keys_[0])

From ff5371fd4a42cb91fdf9d0a222dbef2d5036a294 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 13:38:45 -0500
Subject: [PATCH 329/362] move sum, max and nrValues to Constraint class as
 well

---
 gtsam_unstable/discrete/AllDiff.h       | 19 -------------------
 gtsam_unstable/discrete/BinaryAllDiff.h | 19 -------------------
 gtsam_unstable/discrete/Constraint.h    | 22 +++++++++++++++++++++-
 gtsam_unstable/discrete/Domain.h        | 16 ----------------
 gtsam_unstable/discrete/SingleValue.h   | 19 -------------------
 5 files changed, 21 insertions(+), 74 deletions(-)

diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 34e5c4700..1180abad4 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -72,25 +72,6 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
   /// Partially apply known values, domain version
   Constraint::shared_ptr partiallyApply(
       const Domains&) const override;
-
-  /// Get the number of non-zero values contained in this factor.
-  uint64_t nrValues() const override { return 1; };
-
-  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().sum(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    return toDecisionTreeFactor().sum(keys);
-  }
-
-  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().max(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    return toDecisionTreeFactor().max(keys);
-  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 0cd51ec61..e96bfdfde 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -96,25 +96,6 @@ class BinaryAllDiff : public Constraint {
   AlgebraicDecisionTree<Key> errorTree() const override {
     throw std::runtime_error("BinaryAllDiff::error not implemented");
   }
-
-  /// Get the number of non-zero values contained in this factor.
-  uint64_t nrValues() const override { return 1; };
-
-  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().sum(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    return toDecisionTreeFactor().sum(keys);
-  }
-
-  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().max(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    return toDecisionTreeFactor().max(keys);
-  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h
index 2d98ab40b..328fabbea 100644
--- a/gtsam_unstable/discrete/Constraint.h
+++ b/gtsam_unstable/discrete/Constraint.h
@@ -68,7 +68,8 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor {
   /*
    * Ensure Arc-consistency by checking every possible value of domain j.
    * @param j domain to be checked
-   * @param (in/out) domains all domains, but only domains->at(j) will be checked.
+   * @param (in/out) domains all domains, but only domains->at(j) will be
+   * checked.
    * @return true if domains->at(j) was changed, false otherwise.
    */
   virtual bool ensureArcConsistency(Key j, Domains* domains) const = 0;
@@ -92,6 +93,25 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor {
     return this->toDecisionTreeFactor() / df;
   }
 
+  /// Get the number of non-zero values contained in this factor.
+  uint64_t nrValues() const override { return 1; };
+
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
+    return toDecisionTreeFactor().sum(nrFrontals);
+  }
+
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
+    return toDecisionTreeFactor().sum(keys);
+  }
+
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
+    return toDecisionTreeFactor().max(nrFrontals);
+  }
+
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
+    return toDecisionTreeFactor().max(keys);
+  }
+
   /// @}
   /// @name Wrapper support
   /// @{
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 2372cf499..6ce846201 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -114,22 +114,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
 
   /// Partially apply known values, domain version
   Constraint::shared_ptr partiallyApply(const Domains& domains) const override;
-
-  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().sum(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    return toDecisionTreeFactor().sum(keys);
-  }
-
-  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().max(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    return toDecisionTreeFactor().max(keys);
-  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 5d4c2dca1..3df1209b8 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -77,25 +77,6 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
   /// Partially apply known values, domain version
   Constraint::shared_ptr partiallyApply(
       const Domains& domains) const override;
-
-  /// Get the number of non-zero values contained in this factor.
-  uint64_t nrValues() const override { return 1; };
-
-  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().sum(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override {
-    return toDecisionTreeFactor().sum(keys);
-  }
-
-  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
-    return toDecisionTreeFactor().max(nrFrontals);
-  }
-
-  DiscreteFactor::shared_ptr max(const Ordering& keys) const override {
-    return toDecisionTreeFactor().max(keys);
-  }
 };
 
 }  // namespace gtsam

From ab90e0b0f3d68f3f144b5fc4d7dd87a7e6425901 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 13:44:55 -0500
Subject: [PATCH 330/362] move include to .cpp

---
 gtsam/discrete/DecisionTreeFactor.cpp | 3 ++-
 gtsam/discrete/DecisionTreeFactor.h   | 1 -
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 4b16dad8a..e353fdebf 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -18,9 +18,10 @@
  */
 
 #include <gtsam/base/FastSet.h>
-#include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/TableFactor.h>
+#include <gtsam/hybrid/HybridValues.h>
 
 #include <utility>
 
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 3e70c0df9..ff9bf0df9 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -22,7 +22,6 @@
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/Ring.h>
-#include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Ordering.h>
 
 #include <algorithm>

From f043ac43a7c559b9707c693fbb4dd265b0483838 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 14:08:08 -0500
Subject: [PATCH 331/362] address review comments

---
 gtsam/discrete/DecisionTreeFactor.cpp |  9 +++++++++
 gtsam/discrete/DecisionTreeFactor.h   | 15 ++++++++++++++-
 gtsam/discrete/TableFactor.cpp        | 10 ++++++++++
 gtsam/discrete/TableFactor.h          | 15 ++++++++++++++-
 4 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index e353fdebf..ef7979d0a 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -68,11 +68,20 @@ namespace gtsam {
       const DiscreteFactor::shared_ptr& f) const {
     DiscreteFactor::shared_ptr result;
     if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+      // If f is a TableFactor, we convert `this` to a TableFactor since this
+      // conversion is cheaper than converting `f` to a DecisionTreeFactor. We
+      // then return a TableFactor.
       result = std::make_shared<TableFactor>((*tf) * TableFactor(*this));
+
     } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      // If `f` is a DecisionTreeFactor, simply call operator*.
       result = std::make_shared<DecisionTreeFactor>(this->operator*(*dtf));
+
     } else {
       // Simulate double dispatch in C++
+      // Useful for other classes which inherit from DiscreteFactor and have
+      // only `operator*(DecisionTreeFactor)` defined. Thus, other classes don't
+      // need to be updated.
       result = std::make_shared<DecisionTreeFactor>(f->operator*(*this));
     }
     return result;
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index ff9bf0df9..907f29a45 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -147,7 +147,20 @@ namespace gtsam {
     /// Calculate error for DiscreteValues `x`, is -log(probability).
     double error(const DiscreteValues& values) const override;
 
-    /// Multiply factors, DiscreteFactor::shared_ptr edition
+    /**
+     * @brief Multiply factors, DiscreteFactor::shared_ptr edition.
+     *
+     * This method accepts `DiscreteFactor::shared_ptr` and uses dynamic
+     * dispatch and specializations to perform the most efficient
+     * multiplication.
+     *
+     * While converting a DecisionTreeFactor to a TableFactor is efficient, the
+     * reverse is not. Hence we specialize the code to return a TableFactor if
+     * `f` is a TableFactor, and DecisionTreeFactor otherwise.
+     *
+     * @param f The factor to multiply with.
+     * @return DiscreteFactor::shared_ptr
+     */
     virtual DiscreteFactor::shared_ptr multiply(
         const DiscreteFactor::shared_ptr& f) const override;
 
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index 6516a4a98..fe901aac1 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -259,11 +259,21 @@ DiscreteFactor::shared_ptr TableFactor::multiply(
     const DiscreteFactor::shared_ptr& f) const {
   DiscreteFactor::shared_ptr result;
   if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+    // If `f` is a TableFactor, we can simply call `operator*`.
     result = std::make_shared<TableFactor>(this->operator*(*tf));
+
   } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+    // If `f` is a DecisionTreeFactor, we convert to a TableFactor which is
+    // cheaper than converting `this` to a DecisionTreeFactor.
     result = std::make_shared<TableFactor>(this->operator*(TableFactor(*dtf)));
+
   } else {
     // Simulate double dispatch in C++
+    // Useful for other classes which inherit from DiscreteFactor and have
+    // only `operator*(DecisionTreeFactor)` defined. Thus, other classes don't
+    // need to be updated to know about TableFactor.
+    // Those classes can be specialized to use TableFactor
+    // if efficiency is a problem.
     result = std::make_shared<DecisionTreeFactor>(
         f->operator*(this->toDecisionTreeFactor()));
   }
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 4b53d7e2b..a2e89b302 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -178,7 +178,20 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   /// multiply with DecisionTreeFactor
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
-  /// Multiply factors, DiscreteFactor::shared_ptr edition
+  /**
+   * @brief Multiply factors, DiscreteFactor::shared_ptr edition.
+   *
+   * This method accepts `DiscreteFactor::shared_ptr` and uses dynamic
+   * dispatch and specializations to perform the most efficient
+   * multiplication.
+   *
+   * While converting a DecisionTreeFactor to a TableFactor is efficient, the
+   * reverse is not.
+   * Hence we specialize the code to return a TableFactor always.
+   *
+   * @param f The factor to multiply with.
+   * @return DiscreteFactor::shared_ptr
+   */
   virtual DiscreteFactor::shared_ptr multiply(
       const DiscreteFactor::shared_ptr& f) const override;
 

From f932945652b6f60f266513904efd04fbb6eddef5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 18:30:34 -0500
Subject: [PATCH 332/362] check pointer casts

---
 gtsam/discrete/tests/testDecisionTreeFactor.cpp | 3 +++
 gtsam/discrete/tests/testTableFactor.cpp        | 3 +++
 2 files changed, 6 insertions(+)

diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index b4c5acc1b..88045ce3d 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -112,14 +112,17 @@ TEST(DecisionTreeFactor, sum_max) {
 
   DecisionTreeFactor expected(v1, "9 12");
   auto actual = std::dynamic_pointer_cast<DecisionTreeFactor>(f1.sum(1));
+  CHECK(actual);
   CHECK(assert_equal(expected, *actual, 1e-5));
 
   DecisionTreeFactor expected2(v1, "5 6");
   auto actual2 = std::dynamic_pointer_cast<DecisionTreeFactor>(f1.max(1));
+  CHECK(actual2);
   CHECK(assert_equal(expected2, *actual2));
 
   DecisionTreeFactor f2(v1 & v0, "1 2  3 4  5 6");
   auto actual22 = std::dynamic_pointer_cast<DecisionTreeFactor>(f2.sum(1));
+  CHECK(actual22);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index 4f6ec2f39..76a0f2b5c 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -306,14 +306,17 @@ TEST(TableFactor, sum_max) {
 
   TableFactor expected(v1, "9 12");
   auto actual = std::dynamic_pointer_cast<TableFactor>(f1.sum(1));
+  CHECK(actual);
   CHECK(assert_equal(expected, *actual, 1e-5));
 
   TableFactor expected2(v1, "5 6");
   auto actual2 = std::dynamic_pointer_cast<TableFactor>(f1.max(1));
+  CHECK(actual2);
   CHECK(assert_equal(expected2, *actual2));
 
   TableFactor f2(v1 & v0, "1 2  3 4  5 6");
   auto actual22 = std::dynamic_pointer_cast<TableFactor>(f2.sum(1));
+  CHECK(actual22);
 }
 
 /* ************************************************************************* */

From f8dedb503592f648c4d1d22a353dd201a760b697 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 18:37:40 -0500
Subject: [PATCH 333/362] use DiscreteFactor for DiscreteConditional
 constructor

---
 gtsam/discrete/DiscreteConditional.cpp | 4 ++--
 gtsam/discrete/DiscreteConditional.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 06a08eca5..19dcdc729 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -44,8 +44,8 @@ template class GTSAM_EXPORT
 
 /* ************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
-                                         const DecisionTreeFactor& f)
-    : BaseFactor(f / f.sum(nrFrontals)->toDecisionTreeFactor()),
+                                         const DiscreteFactor& f)
+    : BaseFactor((f / f.sum(nrFrontals))->toDecisionTreeFactor()),
       BaseConditional(nrFrontals) {}
 
 /* ************************************************************************** */
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 3ec9ae590..67f8a0056 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -54,7 +54,7 @@ class GTSAM_EXPORT DiscreteConditional
   DiscreteConditional() {}
 
   /// Construct from factor, taking the first `nFrontals` keys as frontals.
-  DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
+  DiscreteConditional(size_t nFrontals, const DiscreteFactor& f);
 
   /**
    * Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first

From c754f9bfdcb6c213003a511924a5c4df69f5b91c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 18:47:44 -0500
Subject: [PATCH 334/362] add comments

---
 gtsam/discrete/DecisionTreeFactor.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 2f2c039a4..6e25c6452 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -81,12 +81,18 @@ namespace gtsam {
   DiscreteFactor::shared_ptr DecisionTreeFactor::operator/(
       const DiscreteFactor::shared_ptr& f) const {
     if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
+      // Check if `f` is a TableFactor. If yes, then
+      // convert `this` to a TableFactor which is cheaper.
       return std::make_shared<TableFactor>(tf->operator/(TableFactor(*this)));
+
     } else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
+      // If `f` is a DecisionTreeFactor, divide normally.
       return std::make_shared<DecisionTreeFactor>(this->operator/(*dtf));
+
     } else {
+      // Else, convert `f` to a DecisionTreeFactor so we can divide
       return std::make_shared<DecisionTreeFactor>(
-          this->operator/(this->toDecisionTreeFactor()));
+          this->operator/(f->toDecisionTreeFactor()));
     }
   }
 

From 5913fd120d7b493ea02cc0563caf974d451c4c65 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 21:06:22 -0500
Subject: [PATCH 335/362] updates to get things working

---
 gtsam/discrete/DiscreteConditional.h       | 2 +-
 gtsam/discrete/TableDistribution.cpp       | 3 ++-
 gtsam/discrete/TableDistribution.h         | 2 +-
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 4 ++--
 4 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 5a26c45e0..19cc3a798 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -221,7 +221,7 @@ class GTSAM_EXPORT DiscreteConditional
    * @param keys The keys to sum over.
    * @return DiscreteFactor::shared_ptr
    */
-  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const;
+  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const override;
 
   /// @}
   /// @name Advanced Interface
diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index a7883571a..2a9c63d51 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -47,7 +47,8 @@ static Eigen::SparseVector<double> normalizeSparseTable(
 TableDistribution::TableDistribution(const TableFactor& f)
     : BaseConditional(f.keys().size(),
                       DecisionTreeFactor(f.discreteKeys(), ADT())),
-      table_(f / (*f.sum(f.keys().size()))) {}
+      table_(f / (*std::dynamic_pointer_cast<TableFactor>(
+                     f.sum(f.keys().size())))) {}
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const DiscreteKeys& keys,
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 39a1c481f..3aafdfda7 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -140,7 +140,7 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   }
 
   /// Get the number of non-zero values.
-  size_t nrValues() const { return table_.sparseTable().nonZeros(); }
+  uint64_t nrValues() const override { return table_.sparseTable().nonZeros(); }
 
   /// @}
 
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 594aa5c40..d7813f1e5 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -284,7 +284,7 @@ TableFactor TableProduct(const DiscreteFactorGraph &factors) {
   // Max over all the potentials by pretending all keys are frontal:
   auto denominator = product.max(product.size());
   // Normalize the product factor to prevent underflow.
-  product = product / (*denominator);
+  product = product / *std::dynamic_pointer_cast<TableFactor>(denominator);
 #if GTSAM_HYBRID_TIMING
   gttoc_(DiscreteNormalize);
 #endif
@@ -367,7 +367,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif
 
-    TableFactor::shared_ptr sum = product.sum(frontalKeys);
+    DiscreteFactor::shared_ptr sum = product.sum(frontalKeys);
 
     return {std::make_shared<HybridConditional>(conditional), sum};
 

From 90825b96af8aa5a7d56dc98222b522bcaaed41b0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 21:08:27 -0500
Subject: [PATCH 336/362] remove hybrid timing flag from DiscreteFactorGraph

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 9b1774f49..eb3221819 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -131,17 +131,11 @@ namespace gtsam {
     DiscreteFactor::shared_ptr product = factors.product();
     gttoc(product);
 
-#if GTSAM_HYBRID_TIMING
-    gttic_(DiscreteNormalize);
-#endif
     // Max over all the potentials by pretending all keys are frontal:
     auto denominator = product->max(product->size());
 
     // Normalize the product factor to prevent underflow.
     product = product->operator/(denominator);
-#if GTSAM_HYBRID_TIMING
-    gttoc_(DiscreteNormalize);
-#endif
 
     return product;
   }

From 82dba6322f4d4018a59c55f11d9a982a23d4ab7a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 22:14:59 -0500
Subject: [PATCH 337/362] new scaledProduct method instead of DiscreteProduct

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 9 ++++-----
 gtsam/discrete/DiscreteFactorGraph.h   | 5 +++++
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index eb3221819..cd31c52a0 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -124,11 +124,10 @@ namespace gtsam {
    * @param factors The factors to multiply as a DiscreteFactorGraph.
    * @return DiscreteFactor::shared_ptr
    */
-  static DiscreteFactor::shared_ptr DiscreteProduct(
-      const DiscreteFactorGraph& factors) {
+  DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const {
     // PRODUCT: multiply all factors
     gttic(product);
-    DiscreteFactor::shared_ptr product = factors.product();
+    DiscreteFactor::shared_ptr product = this->product();
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:
@@ -145,7 +144,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DiscreteFactor::shared_ptr product = DiscreteProduct(factors);
+    DiscreteFactor::shared_ptr product = factors.scaledProduct();
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -223,7 +222,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DiscreteFactor::shared_ptr product = DiscreteProduct(factors);
+    DiscreteFactor::shared_ptr product = factors.scaledProduct();
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index 3d9e86cd1..162d9b748 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -150,6 +150,11 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /** return product of all factors as a single factor */
   DiscreteFactor::shared_ptr product() const;
 
+  /** Return product of all factors as a single factor,
+   * which is scaled by the max to prevent underflow
+   */
+  DiscreteFactor::shared_ptr scaledProduct() const;
+
   /** 
    * Evaluates the factor graph given values, returns the joint probability of
    * the factor graph given specific instantiation of values

From 9960f2d8dcde7b4180cb815e717f251196762376 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 22:18:14 -0500
Subject: [PATCH 338/362] kill TableProduct in favor of
 DiscreteFactorGraph::scaledProduct

---
 gtsam/hybrid/HybridBayesTree.cpp           | 11 ++++-
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 50 +++++-----------------
 gtsam/hybrid/HybridGaussianFactorGraph.h   |  9 ----
 3 files changed, 19 insertions(+), 51 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 0df46f262..31d256d6f 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -45,9 +45,16 @@ bool HybridBayesTree::equals(const This& other, double tol) const {
 /* ************************************************************************* */
 DiscreteValues HybridBayesTree::discreteMaxProduct(
     const DiscreteFactorGraph& dfg) const {
-  TableFactor product = TableProduct(dfg);
+  DiscreteFactor::shared_ptr product = dfg.scaledProduct();
 
-  DiscreteValues assignment = TableDistribution(product).argmax();
+  // Check type of product, and get as TableFactor for efficiency.
+  TableFactor p;
+  if (auto tf = std::dynamic_pointer_cast<TableFactor>(product)) {
+    p = *tf;
+  } else {
+    p = TableFactor(product->toDecisionTreeFactor());
+  }
+  DiscreteValues assignment = TableDistribution(p).argmax();
   return assignment;
 }
 
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index d7813f1e5..581d027c8 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -255,43 +255,6 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors(
   return std::make_shared<TableFactor>(discreteKeys, potentials);
 }
 
-/* ************************************************************************ */
-TableFactor TableProduct(const DiscreteFactorGraph &factors) {
-  // PRODUCT: multiply all factors
-#if GTSAM_HYBRID_TIMING
-  gttic_(DiscreteProduct);
-#endif
-  TableFactor product;
-  for (auto &&factor : factors) {
-    if (factor) {
-      if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(factor)) {
-        product = product * dtc->table();
-      } else if (auto f = std::dynamic_pointer_cast<TableFactor>(factor)) {
-        product = product * (*f);
-      } else if (auto dtf =
-                     std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
-        product = product * TableFactor(*dtf);
-      }
-    }
-  }
-#if GTSAM_HYBRID_TIMING
-  gttoc_(DiscreteProduct);
-#endif
-
-#if GTSAM_HYBRID_TIMING
-  gttic_(DiscreteNormalize);
-#endif
-  // Max over all the potentials by pretending all keys are frontal:
-  auto denominator = product.max(product.size());
-  // Normalize the product factor to prevent underflow.
-  product = product / *std::dynamic_pointer_cast<TableFactor>(denominator);
-#if GTSAM_HYBRID_TIMING
-  gttoc_(DiscreteNormalize);
-#endif
-
-  return product;
-}
-
 /* ************************************************************************ */
 static DiscreteFactorGraph CollectDiscreteFactors(
     const HybridGaussianFactorGraph &factors) {
@@ -357,17 +320,24 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
   // so we can use the TableFactor for efficiency.
   if (frontalKeys.size() == dfg.keys().size()) {
     // Get product factor
-    TableFactor product = TableProduct(dfg);
+    DiscreteFactor::shared_ptr product = dfg.scaledProduct();
 
 #if GTSAM_HYBRID_TIMING
     gttic_(EliminateDiscreteFormDiscreteConditional);
 #endif
-    auto conditional = std::make_shared<TableDistribution>(product);
+    // Check type of product, and get as TableFactor for efficiency.
+    TableFactor p;
+    if (auto tf = std::dynamic_pointer_cast<TableFactor>(product)) {
+      p = *tf;
+    } else {
+      p = TableFactor(product->toDecisionTreeFactor());
+    }
+    auto conditional = std::make_shared<TableDistribution>(p);
 #if GTSAM_HYBRID_TIMING
     gttoc_(EliminateDiscreteFormDiscreteConditional);
 #endif
 
-    DiscreteFactor::shared_ptr sum = product.sum(frontalKeys);
+    DiscreteFactor::shared_ptr sum = product->sum(frontalKeys);
 
     return {std::make_shared<HybridConditional>(conditional), sum};
 
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index b7d815ec6..832ab56a6 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -271,13 +271,4 @@ template <>
 struct traits<HybridGaussianFactorGraph>
     : public Testable<HybridGaussianFactorGraph> {};
 
-/**
- * @brief Multiply all the `factors` and normalize the
- * product to prevent underflow.
- *
- * @param factors The factors to multiply as a DiscreteFactorGraph.
- * @return TableFactor
- */
-TableFactor TableProduct(const DiscreteFactorGraph& factors);
-
 }  // namespace gtsam

From 96a136b4e39e275f71cf046ae4b8c8696089599e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 23:01:47 -0500
Subject: [PATCH 339/362] override sum and max in TableDistribution

---
 gtsam/discrete/TableDistribution.cpp | 15 +++++++++++++++
 gtsam/discrete/TableDistribution.h   | 19 +++++++++++--------
 2 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 2a9c63d51..3c0605f27 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -91,6 +91,21 @@ bool TableDistribution::equals(const DiscreteFactor& other, double tol) const {
   }
 }
 
+/* ****************************************************************************/
+DiscreteFactor::shared_ptr TableDistribution::sum(size_t nrFrontals) const {
+  return table_.sum(nrFrontals);
+}
+
+/* ****************************************************************************/
+DiscreteFactor::shared_ptr TableDistribution::sum(const Ordering& keys) const {
+  return table_.sum(keys);
+}
+
+/* ****************************************************************************/
+DiscreteFactor::shared_ptr TableDistribution::max(size_t nrFrontals) const {
+  return table_.max(nrFrontals);
+}
+
 /* ****************************************************************************/
 DiscreteFactor::shared_ptr TableDistribution::max(const Ordering& keys) const {
   return table_.max(keys);
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 3aafdfda7..1c393bb1a 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -111,14 +111,17 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
     return table_.evaluate(values);
   }
 
-  /**
-   * @brief Create new factor by maximizing over all
-   * values with the same separator.
-   *
-   * @param keys The keys to sum over.
-   * @return DiscreteFactor::shared_ptr
-   */
-  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const override;
+  /// Create new factor by summing all values with the same separator values
+  DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override;
+
+  /// Create new factor by summing all values with the same separator values
+  DiscreteFactor::shared_ptr sum(const Ordering& keys) const override;
+
+  /// Create new factor by maximizing over all values with the same separator.
+  DiscreteFactor::shared_ptr max(size_t nrFrontals) const override;
+
+  /// Create new factor by maximizing over all values with the same separator.
+  DiscreteFactor::shared_ptr max(const Ordering& keys) const override;
 
   /**
    * @brief Return assignment that maximizes value.

From 3fb6f39b30021b5a5e4d2f551998bf14ab6f52cf Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jan 2025 23:13:06 -0500
Subject: [PATCH 340/362] override operator/ in TableDistribution

---
 gtsam/discrete/TableDistribution.cpp | 6 ++++++
 gtsam/discrete/TableDistribution.h   | 4 ++++
 2 files changed, 10 insertions(+)

diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 3c0605f27..621e6e394 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -111,6 +111,12 @@ DiscreteFactor::shared_ptr TableDistribution::max(const Ordering& keys) const {
   return table_.max(keys);
 }
 
+/* ****************************************************************************/
+DiscreteFactor::shared_ptr TableDistribution::operator/(
+    const DiscreteFactor::shared_ptr& f) const {
+  return table_ / f;
+}
+
 /* ************************************************************************ */
 DiscreteValues TableDistribution::argmax() const {
   uint64_t maxIdx = 0;
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 1c393bb1a..da349efe1 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -123,6 +123,10 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
   /// Create new factor by maximizing over all values with the same separator.
   DiscreteFactor::shared_ptr max(const Ordering& keys) const override;
 
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& f) const override;
+
   /**
    * @brief Return assignment that maximizes value.
    *

From 6bb70df05b393ea3d4cb7bc0fc1545287526dada Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 3 Jan 2025 07:58:05 -0500
Subject: [PATCH 341/362] Use newer binary output directory variables and don't
 specify output directory on each target

---
 cmake/GtsamBuildTypes.cmake                  | 6 +++---
 gtsam/3rdparty/cephes/CMakeLists.txt         | 3 +--
 gtsam/3rdparty/metis/libmetis/CMakeLists.txt | 1 -
 gtsam/CMakeLists.txt                         | 3 +--
 gtsam_unstable/CMakeLists.txt                | 3 +--
 5 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 2aad58abb..19314cd31 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -250,9 +250,9 @@ endif()
 
 # Make common binary output directory when on Windows
 if(WIN32)
-  set(RUNTIME_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
-  set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
-  set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib")
+  set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
+  set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/lib")
+  set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/lib")
 endif()
 
 # Set up build type list for cmake-gui
diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt
index aeac7b403..d510ddf81 100644
--- a/gtsam/3rdparty/cephes/CMakeLists.txt
+++ b/gtsam/3rdparty/cephes/CMakeLists.txt
@@ -106,8 +106,7 @@ if(WIN32)
   set_target_properties(
     cephes-gtsam
     PROPERTIES PREFIX ""
-               COMPILE_FLAGS /w
-               RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
+               COMPILE_FLAGS /w)
 endif()
 
 if(APPLE)
diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
index f3b8694f4..e0a178c5b 100644
--- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
@@ -13,7 +13,6 @@ if(WIN32)
 		set_target_properties(metis-gtsam PROPERTIES
 			PREFIX ""
 			COMPILE_FLAGS /w
-			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin"
 			WINDOWS_EXPORT_ALL_SYMBOLS ON)
 endif()
 
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 87440d19f..5d71bb7be 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -185,8 +185,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 	else()
 		set_target_properties(gtsam PROPERTIES
 			PREFIX ""
-			DEFINE_SYMBOL GTSAM_EXPORTS
-			RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
+			DEFINE_SYMBOL GTSAM_EXPORTS)
 	endif()
 endif()
 
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 1f7b4a7c7..730b6389a 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -108,8 +108,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 	else()
 		set_target_properties(gtsam_unstable PROPERTIES
 			PREFIX ""
-			DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS
-			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
+			DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS)
 	endif()
 endif()
 

From e08064d9e7edc40ea53ce72241e384489403e6d2 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 07:49:32 -0500
Subject: [PATCH 342/362] Use CMAKE_POSITION_INDEPENDENT_CODE instead of -fPIC

---
 CMakeLists.txt                               | 1 +
 cmake/GtsamBuildTypes.cmake                  | 1 -
 gtsam/3rdparty/metis/libmetis/CMakeLists.txt | 1 -
 3 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 90162fe29..85c3d1b44 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,6 +50,7 @@ if(MSVC)
   set(CMAKE_STATIC_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
 endif()
 
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 include(GtsamMakeConfigFile)
 include(GNUInstallDirs)
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 19314cd31..76c04a2f9 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -130,7 +130,6 @@ else()
 
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
     -Wall                                          # Enable common warnings
-    -fPIC                                          # ensure proper code generation for shared libraries
     $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
     $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
     $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables>  # TODO(dellaert): don't know how to resolve
diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
index e0a178c5b..96ec59f64 100644
--- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
@@ -3,7 +3,6 @@ include_directories(.)
 # Find sources.
 file(GLOB metis_sources *.c)
 # Build libmetis.
-add_definitions(-fPIC)
 add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
 if(UNIX)
   target_link_libraries(metis-gtsam m)

From 885959a36b0d1799f8ff0ce33b9421e4fa340906 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 07:51:36 -0500
Subject: [PATCH 343/362] Use new CMake policies up to 3.29 This silences most
 warnings about overriding /W3 with /w

---
 CMakeLists.txt | 17 +----------------
 1 file changed, 1 insertion(+), 16 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 85c3d1b44..453006d41 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,23 +1,8 @@
-cmake_minimum_required(VERSION 3.9)
-if (POLICY CMP0082)
-    cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately
-endif()
-if (POLICY CMP0102)
-cmake_policy(SET CMP0102 NEW) # set policy on advanced variables and cmake cache
-endif()
-if (POLICY CMP0156)
-cmake_policy(SET CMP0156 NEW) # new linker strategies
-endif()
+cmake_minimum_required(VERSION 3.9...3.29)
 if (POLICY CMP0167)
 cmake_policy(SET CMP0167 OLD) # Don't complain about boost
 endif()
 
-# allow parent project to override options
-if (POLICY CMP0077)
-  cmake_policy(SET CMP0077 NEW)
-endif(POLICY CMP0077)
-
-
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 3)

From 3d23152a75980b14e2ba5b84dbd84a286d07428d Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 07:52:59 -0500
Subject: [PATCH 344/362] Clean up version finding in HandleEigen

---
 cmake/HandleEigen.cmake | 23 +++++++----------------
 1 file changed, 7 insertions(+), 16 deletions(-)

diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake
index 707593889..becebafb9 100644
--- a/cmake/HandleEigen.cmake
+++ b/cmake/HandleEigen.cmake
@@ -13,12 +13,6 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it.
     find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED
 
-    # The actual include directory (for BUILD cmake target interface):
-    # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen
-    # versions.  So here I use the target itself to get the proper include
-    # directory (it is generated by cmake, thus has the correct path)
-    get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
-
     # check if MKL is also enabled - can have one or the other, but not both!
     # Note: Eigen >= v3.2.5 includes our patches
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
@@ -30,6 +24,8 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
         message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
     endif()
+    
+    set(GTSAM_EIGEN_VERSION "${EIGEN3_VERSION}")
 else()
     # Use bundled Eigen include path.
     # Clear any variables set by FindEigen3
@@ -46,7 +42,7 @@ else()
 
     add_library(gtsam_eigen3 INTERFACE)
 
-    target_include_directories(gtsam_eigen3 INTERFACE
+    target_include_directories(gtsam_eigen3 SYSTEM INTERFACE
       $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
       $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
     )
@@ -56,11 +52,8 @@ else()
 
     list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3)
     set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}")
-endif()
-
-# Detect Eigen version:
-set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
-if (EXISTS ${EIGEN_VER_H})
+    # Detect Eigen version:
+    set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
     file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
 
     # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION  XX", etc...
@@ -75,11 +68,9 @@ if (EXISTS ${EIGEN_VER_H})
     string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
 
     set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
+endif()
 
-    message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
-else()
-    message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
-endif ()
+message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
 
 if (MSVC)
     if (GTSAM_SHARED_LIB)

From f638035e104b6363e09d5b5cd25ae2daa7555ba5 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 07:53:19 -0500
Subject: [PATCH 345/362] Remove redundant MSVC optimization flag in GKLib

---
 gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake b/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake
index 7ea5bab94..229e39a90 100644
--- a/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake
+++ b/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake
@@ -15,7 +15,6 @@ option(GKRAND "enable GKRAND support" OFF)
 
 # Add compiler flags.
 if(MSVC)
-  set(GKlib_COPTS "/Ox")
   set(GKlib_COPTIONS "-DWIN32 -DMSC -D_CRT_SECURE_NO_DEPRECATE -DUSE_GKREGEX")
 elseif(MINGW)
   set(GKlib_COPTS "-DUSE_GKREGEX")

From 0435efdebcaf50bd28927a19140b134affebcf2a Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 08:27:19 -0500
Subject: [PATCH 346/362] Remove PREFIX ""

---
 gtsam/3rdparty/cephes/CMakeLists.txt         | 5 +----
 gtsam/3rdparty/metis/libmetis/CMakeLists.txt | 1 -
 gtsam/CMakeLists.txt                         | 4 +---
 gtsam_unstable/CMakeLists.txt                | 1 -
 4 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt
index d510ddf81..d1c8cfe0d 100644
--- a/gtsam/3rdparty/cephes/CMakeLists.txt
+++ b/gtsam/3rdparty/cephes/CMakeLists.txt
@@ -103,10 +103,7 @@ set_target_properties(
              C_STANDARD 99)
 
 if(WIN32)
-  set_target_properties(
-    cephes-gtsam
-    PROPERTIES PREFIX ""
-               COMPILE_FLAGS /w)
+  set_target_properties(cephes-gtsam PROPERTIES COMPILE_FLAGS /w)
 endif()
 
 if(APPLE)
diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
index 96ec59f64..8a35c2f6b 100644
--- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
@@ -10,7 +10,6 @@ endif()
 
 if(WIN32)
 		set_target_properties(metis-gtsam PROPERTIES
-			PREFIX ""
 			COMPILE_FLAGS /w
 			WINDOWS_EXPORT_ALL_SYMBOLS ON)
 endif()
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 5d71bb7be..3c2073547 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -183,9 +183,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 			PREFIX "lib")
 		target_compile_definitions(gtsam PRIVATE GTSAM_IMPORT_STATIC)
 	else()
-		set_target_properties(gtsam PROPERTIES
-			PREFIX ""
-			DEFINE_SYMBOL GTSAM_EXPORTS)
+		set_target_properties(gtsam PROPERTIES DEFINE_SYMBOL GTSAM_EXPORTS)
 	endif()
 endif()
 
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 730b6389a..aaffe54a8 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -107,7 +107,6 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 			COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC)
 	else()
 		set_target_properties(gtsam_unstable PROPERTIES
-			PREFIX ""
 			DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS)
 	endif()
 endif()

From 7742f025fcda64a3b5ac68d64d149858897630cf Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 08:45:47 -0500
Subject: [PATCH 347/362] Clean up MSVC flags to allow use of MSVC variables
 instead Generator expressions were used to prevent the addition of /MD and
 /Zi if the CMake version is new enough to have specific MSVC variables for
 controlling those flags

---
 cmake/GtsamBuildTypes.cmake | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 76c04a2f9..0d249ad44 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -108,13 +108,15 @@ endif()
 
 # Other (non-preprocessor macros) compiler flags:
 if(MSVC)
+  set(CMAKE_3_15 $<VERSION_LESS:${CMAKE_VERSION},3.15>)
+  set(CMAKE_3_25 $<VERSION_LESS:${CMAKE_VERSION},3.25>)
   # Common to all configurations, next for each configuration:
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          /W3 /GR /EHsc /MP  CACHE STRING "(User editable) Private compiler flags for all configurations.")
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           /MDd /Zi /Ob0 /Od /RTC1  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  /MD /O2 /D /Zi /d2Zi+  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         /MD /O2  CACHE STRING "(User editable) Private compiler flags for Release configuration.")
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING       /MD /O2  /Zi  CACHE STRING "(User editable) Private compiler flags for Profiling configuration.")
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          /MD /O2  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           $<${CMAKE_3_15}:/MDd> $<${CMAKE_3_25}:/Zi> /Ob0 /Od /RTC1  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  $<${CMAKE_3_15}:/MD> /O2  $<${CMAKE_3_25}:/Zi>  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         $<${CMAKE_3_15}:/MD> /O2  CACHE STRING "(User editable) Private compiler flags for Release configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING       $<${CMAKE_3_15}:/MD> /O2  $<${CMAKE_3_25}:/Zi>  CACHE STRING "(User editable) Private compiler flags for Profiling configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          $<${CMAKE_3_15}:/MD> /O2  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
 else()
   # Common to all configurations, next for each configuration:
 

From 1d5d8f89d8b1b37d65aef220ea40593c10a4aa89 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 08:48:21 -0500
Subject: [PATCH 348/362] Clean up the enabling of C++17 MSVC flag is not
 needed and the minimum CMake version is higher now

---
 cmake/GtsamBuildTypes.cmake | 17 +++--------------
 1 file changed, 3 insertions(+), 14 deletions(-)

diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 0d249ad44..8c86c6f3d 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -156,20 +156,9 @@ mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING)
 mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING)
 
 # Enable C++17:
-if (NOT CMAKE_VERSION VERSION_LESS 3.8)
-    set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.")
-    # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html
-    set(CMAKE_CXX_EXTENSIONS OFF)
-    if (MSVC)
-      # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above?
-      list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++17)
-    endif()
-else()
-  # Old cmake versions:
-  if (NOT MSVC)
-    list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-std=c++17>)
-  endif()
-endif()
+set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.")
+# See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html
+set(CMAKE_CXX_EXTENSIONS OFF)
 
 # Merge all user-defined flags into the variables that are to be actually used by CMake:
 foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES})

From bf4f6f5554c3b190539da5631e6246f146ef8fc6 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Tue, 7 Jan 2025 08:54:41 -0500
Subject: [PATCH 349/362] Remove /FORCE:MULTIPLE from CMAKE_STATIC_LINKER_FLAGS
 It's not a valid flag for static linking

---
 CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 453006d41..262f38121 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,7 +32,6 @@ if(MSVC)
   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_POSITION_INDEPENDENT_CODE ON)

From 3d2dd7c619978f7f3dbd358ce396a701e78ea541 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 10:52:05 -0500
Subject: [PATCH 350/362] update scaledProduct docs

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

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index cd31c52a0..7e059c5e5 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -118,12 +118,7 @@ namespace gtsam {
 //      }
 //  }
 
-  /**
-   * @brief Multiply all the `factors`.
-   *
-   * @param factors The factors to multiply as a DiscreteFactorGraph.
-   * @return DiscreteFactor::shared_ptr
-   */
+  /* ************************************************************************ */
   DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const {
     // PRODUCT: multiply all factors
     gttic(product);
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index 162d9b748..f4d1a1833 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -150,8 +150,12 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /** return product of all factors as a single factor */
   DiscreteFactor::shared_ptr product() const;
 
-  /** Return product of all factors as a single factor,
-   * which is scaled by the max to prevent underflow
+  /**
+   * @brief Return product of all `factors` as a single factor,
+   * which is scaled by the max value to prevent underflow
+   *
+   * @param factors The factors to multiply as a DiscreteFactorGraph.
+   * @return DiscreteFactor::shared_ptr
    */
   DiscreteFactor::shared_ptr scaledProduct() const;
 

From 9228f0f7716a6fa1ca070aad894de0c21efe84e8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 11:19:21 -0500
Subject: [PATCH 351/362] fix headers

---
 gtsam/discrete/TableDistribution.h                    | 1 -
 gtsam/hybrid/HybridGaussianFactorGraph.cpp            | 1 +
 gtsam/hybrid/HybridGaussianFactorGraph.h              | 1 -
 gtsam/hybrid/tests/testHybridBayesNet.cpp             | 1 +
 gtsam/hybrid/tests/testHybridEstimation.cpp           | 1 +
 gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp  | 1 +
 gtsam/hybrid/tests/testHybridGaussianISAM.cpp         | 4 ++--
 gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 9 +++++----
 gtsam/hybrid/tests/testHybridNonlinearISAM.cpp        | 1 +
 gtsam/hybrid/tests/testSerializationHybrid.cpp        | 4 ++--
 10 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index da349efe1..15ec9959c 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -18,7 +18,6 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/discrete/Signature.h>
 #include <gtsam/discrete/TableFactor.h>
 #include <gtsam/inference/Conditional-inst.h>
 
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 581d027c8..cf56b52ed 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -25,6 +25,7 @@
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/discrete/TableFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 832ab56a6..e3c1e2d55 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -20,7 +20,6 @@
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 5c788446c..63a1393c5 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -20,6 +20,7 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/DiscreteFactor.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 425b29742..ef2ae9c41 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/discrete/DiscreteBayesNet.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index fb09bb618..c8735c40a 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -25,6 +25,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
index 5edb5ea20..54964f6f7 100644
--- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridGaussianISAM.h>
@@ -141,8 +142,7 @@ TEST(HybridGaussianISAM, IncrementalInference) {
       expectedRemainingGraph->eliminateMultifrontal(discreteOrdering);
 
   // Test the probability values with regression tests.
-  auto discrete =
-      isam[M(1)]->conditional()->asDiscrete<TableDistribution>();
+  auto discrete = isam[M(1)]->conditional()->asDiscrete<TableDistribution>();
   EXPECT(assert_equal(0.095292, (*discrete)({{M(0), 0}, {M(1), 0}}), 1e-5));
   EXPECT(assert_equal(0.282758, (*discrete)({{M(0), 1}, {M(1), 0}}), 1e-5));
   EXPECT(assert_equal(0.314175, (*discrete)({{M(0), 0}, {M(1), 1}}), 1e-5));
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index e020e851f..5bf97d093 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
@@ -512,10 +513,10 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
   // P(m1)
   EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)});
   EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
-  TableDistribution dtc = *hybridBayesNet->at(4)->asDiscrete<TableDistribution>();
-  EXPECT(
-      DiscreteConditional(dtc.nrFrontals(), dtc.toDecisionTreeFactor())
-          .equals(*discreteBayesNet.at(1)));
+  TableDistribution dtc =
+      *hybridBayesNet->at(4)->asDiscrete<TableDistribution>();
+  EXPECT(DiscreteConditional(dtc.nrFrontals(), dtc.toDecisionTreeFactor())
+             .equals(*discreteBayesNet.at(1)));
 }
 
 /****************************************************************************
diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index e6249f4ac..b32860cff 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridNonlinearISAM.h>
diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp
index af4a81fdf..9aabe309b 100644
--- a/gtsam/hybrid/tests/testSerializationHybrid.cpp
+++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp
@@ -18,6 +18,7 @@
 
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -44,8 +45,7 @@ BOOST_CLASS_EXPORT_GUID(HybridFactor, "gtsam_HybridFactor");
 BOOST_CLASS_EXPORT_GUID(JacobianFactor, "gtsam_JacobianFactor");
 BOOST_CLASS_EXPORT_GUID(GaussianConditional, "gtsam_GaussianConditional");
 BOOST_CLASS_EXPORT_GUID(DiscreteConditional, "gtsam_DiscreteConditional");
-BOOST_CLASS_EXPORT_GUID(TableDistribution,
-                        "gtsam_TableDistribution");
+BOOST_CLASS_EXPORT_GUID(TableDistribution, "gtsam_TableDistribution");
 
 BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor");
 using ADT = AlgebraicDecisionTree<Key>;

From b81ab86b6960452f948b0af43e64ba1f81722e37 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 14:52:48 -0500
Subject: [PATCH 352/362] make ADT with nullptr in TableDistribution

---
 gtsam/discrete/AlgebraicDecisionTree.h | 3 +++
 gtsam/discrete/TableDistribution.cpp   | 7 +++----
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index 383346ab1..a8ec66f73 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -57,6 +57,9 @@ namespace gtsam {
 
     AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {}
 
+    /// Constructor which accepts root pointer
+    AlgebraicDecisionTree(const typename Base::NodePtr root) : Base(root) {}
+
     // Explicitly non-explicit constructor
     AlgebraicDecisionTree(const Base& add) : Base(add) {}
 
diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 621e6e394..4b9979d3a 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -45,15 +45,14 @@ static Eigen::SparseVector<double> normalizeSparseTable(
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const TableFactor& f)
-    : BaseConditional(f.keys().size(),
-                      DecisionTreeFactor(f.discreteKeys(), ADT())),
+    : BaseConditional(f.keys().size(), f.discreteKeys(), ADT(nullptr)),
       table_(f / (*std::dynamic_pointer_cast<TableFactor>(
                      f.sum(f.keys().size())))) {}
 
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const DiscreteKeys& keys,
                                      const std::vector<double>& potentials)
-    : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
+    : BaseConditional(keys.size(), keys, ADT(nullptr)),
       table_(TableFactor(
           keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) {
 }
@@ -61,7 +60,7 @@ TableDistribution::TableDistribution(const DiscreteKeys& keys,
 /* ************************************************************************** */
 TableDistribution::TableDistribution(const DiscreteKeys& keys,
                                      const std::string& potentials)
-    : BaseConditional(keys.size(), keys, DecisionTreeFactor(keys, ADT())),
+    : BaseConditional(keys.size(), keys, ADT(nullptr)),
       table_(TableFactor(
           keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) {
 }

From 3629c33ecd06261797d752a8fcac972b72f35317 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 14:53:28 -0500
Subject: [PATCH 353/362] override sample in TableDistribution

---
 gtsam/discrete/DiscreteConditional.h |  2 +-
 gtsam/discrete/TableDistribution.cpp | 33 ++++++++++++++++++++++++++++
 gtsam/discrete/TableDistribution.h   |  7 ++++++
 gtsam/discrete/TableFactor.h         |  2 +-
 4 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 19cc3a798..1bca0b09f 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -199,7 +199,7 @@ class GTSAM_EXPORT DiscreteConditional
    * @param parentsValues Known values of the parents
    * @return sample from conditional
    */
-  size_t sample(const DiscreteValues& parentsValues) const;
+  virtual size_t sample(const DiscreteValues& parentsValues) const;
 
   /// Single parent version.
   size_t sample(size_t parent_value) const;
diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp
index 4b9979d3a..e8696c5b1 100644
--- a/gtsam/discrete/TableDistribution.cpp
+++ b/gtsam/discrete/TableDistribution.cpp
@@ -138,4 +138,37 @@ void TableDistribution::prune(size_t maxNrAssignments) {
   table_ = table_.prune(maxNrAssignments);
 }
 
+/* ****************************************************************************/
+size_t TableDistribution::sample(const DiscreteValues& parentsValues) const {
+  static mt19937 rng(2);  // random number generator
+
+  DiscreteKeys parentsKeys;
+  for (auto&& [key, _] : parentsValues) {
+    parentsKeys.push_back({key, table_.cardinality(key)});
+  }
+
+  // Get the correct conditional distribution: P(F|S=parentsValues)
+  TableFactor pFS = table_.choose(parentsValues, parentsKeys);
+
+  // TODO(Duy): only works for one key now, seems horribly slow this way
+  if (nrFrontals() != 1) {
+    throw std::invalid_argument(
+        "TableDistribution::sample can only be called on single variable "
+        "conditionals");
+  }
+  Key key = firstFrontalKey();
+  size_t nj = cardinality(key);
+  vector<double> p(nj);
+  DiscreteValues frontals;
+  for (size_t value = 0; value < nj; value++) {
+    frontals[key] = value;
+    p[value] = pFS(frontals);  // P(F=value|S=parentsValues)
+    if (p[value] == 1.0) {
+      return value;  // shortcut exit
+    }
+  }
+  std::discrete_distribution<size_t> distribution(p.begin(), p.end());
+  return distribution(rng);
+}
+
 }  // namespace gtsam
diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h
index 15ec9959c..72786a515 100644
--- a/gtsam/discrete/TableDistribution.h
+++ b/gtsam/discrete/TableDistribution.h
@@ -133,6 +133,13 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional {
    */
   DiscreteValues argmax() const;
 
+  /**
+   * sample
+   * @param parentsValues Known values of the parents
+   * @return sample from conditional
+   */
+  virtual size_t sample(const DiscreteValues& parentsValues) const override;
+
   /// @}
   /// @name Advanced Interface
   /// @{
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index 43f84f874..1cb9eda8b 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -211,7 +211,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   DecisionTreeFactor toDecisionTreeFactor() const override;
 
   /// Create a TableFactor that is a subset of this TableFactor
-  TableFactor choose(const DiscreteValues assignments,
+  TableFactor choose(const DiscreteValues parentAssignments,
                      DiscreteKeys parent_keys) const;
 
   /// Create new factor by summing all values with the same separator values

From 9dfdf552e1ba58e659acc62283e9fdf5e6f709e5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 14:54:49 -0500
Subject: [PATCH 354/362] add hack to multiply DiscreteConditional with
 TableDistribution

---
 gtsam/discrete/DiscreteConditional.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 606f4c13c..f5ad2b98a 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -77,6 +77,13 @@ DiscreteConditional::DiscreteConditional(const Signature& signature)
 /* ************************************************************************** */
 DiscreteConditional DiscreteConditional::operator*(
     const DiscreteConditional& other) const {
+  // If the root is a nullptr, we have a TableDistribution
+  // TODO(Varun) Revisit this hack after RSS2025 submission
+  if (!other.root_) {
+    DiscreteConditional dc(other.nrFrontals(), other.toDecisionTreeFactor());
+    return dc * (*this);
+  }
+
   // Take union of frontal keys
   std::set<Key> newFrontals;
   for (auto&& key : this->frontals()) newFrontals.insert(key);

From 9c2ecc3c15ea4db5ccfbda855090c58dc3b7380f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 14:55:30 -0500
Subject: [PATCH 355/362] simplify multiplication

---
 gtsam/hybrid/HybridBayesNet.cpp           | 9 ++-------
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 8 +-------
 2 files changed, 3 insertions(+), 14 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index d6fd7e6bd..8668bedd6 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridValues.h>
 
@@ -52,13 +53,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   // Multiply into one big conditional. NOTE: possibly quite expensive.
   DiscreteConditional joint;
   for (auto &&conditional : marginal) {
-    // The last discrete conditional may be a TableDistribution
-    if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(conditional)) {
-      DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
-      joint = joint * dc;
-    } else {
-      joint = joint * (*conditional);
-    }
+    joint = joint * (*conditional);
   }
 
   // Create the result starting with the pruned joint.
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 63a1393c5..989694b26 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -451,13 +451,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
 
   DiscreteConditional joint;
   for (auto&& conditional : posterior->discreteMarginal()) {
-    // The last discrete conditional may be a TableDistribution
-    if (auto dtc = std::dynamic_pointer_cast<TableDistribution>(conditional)) {
-      DiscreteConditional dc(dtc->nrFrontals(), dtc->toDecisionTreeFactor());
-      joint = joint * dc;
-    } else {
-      joint = joint * (*conditional);
-    }
+    joint = joint * (*conditional);
   }
 
   size_t maxNrLeaves = 3;

From 4fc2387a6307e7395b50e0a07f82b28853aa51f4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 15:15:16 -0500
Subject: [PATCH 356/362] fix relinearization in HybridNonlinearISAM

---
 gtsam/hybrid/HybridNonlinearISAM.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp
index 29e467d86..3b4856dfb 100644
--- a/gtsam/hybrid/HybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/HybridNonlinearISAM.cpp
@@ -15,6 +15,7 @@
  * @author Varun Agrawal
  */
 
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 #include <gtsam/hybrid/HybridNonlinearISAM.h>
@@ -65,7 +66,14 @@ void HybridNonlinearISAM::reorderRelinearize() {
     // Obtain the new linearization point
     const Values newLinPoint = estimate();
 
-    auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete());
+    DiscreteConditional::shared_ptr discreteProbabilities;
+
+    auto discreteRoot = isam_.roots().at(0)->conditional();
+    if (discreteRoot->asDiscrete<TableDistribution>()) {
+      discreteProbabilities = discreteRoot->asDiscrete<TableDistribution>();
+    } else {
+      discreteProbabilities = discreteRoot->asDiscrete();
+    }
 
     isam_.clear();
 
@@ -73,7 +81,7 @@ void HybridNonlinearISAM::reorderRelinearize() {
     HybridNonlinearFactorGraph pruned_factors;
     for (auto&& factor : factors_) {
       if (auto nf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
-        pruned_factors.push_back(nf->prune(discreteProbs));
+        pruned_factors.push_back(nf->prune(*discreteProbabilities));
       } else {
         pruned_factors.push_back(factor);
       }

From 3ecc232c0ac3a45bdb3f8ab647e14d415dfc850f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jan 2025 15:21:24 -0500
Subject: [PATCH 357/362] fix tests

---
 gtsam/hybrid/tests/testHybridMotionModel.cpp  | 35 +++++++++----------
 .../tests/testHybridNonlinearFactorGraph.cpp  |  4 +--
 2 files changed, 19 insertions(+), 20 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp
index 5d307e81f..a4de6a17b 100644
--- a/gtsam/hybrid/tests/testHybridMotionModel.cpp
+++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp
@@ -21,8 +21,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/discrete/TableDistribution.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
@@ -145,8 +145,8 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Importance sampling run with 100k samples gives 50.051/49.949
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     TableDistribution expected(m1, "50 50");
-    EXPECT(assert_equal(expected,
-                        *(bn->at(2)->asDiscrete<TableDistribution>())));
+    EXPECT(
+        assert_equal(expected, *(bn->at(2)->asDiscrete<TableDistribution>())));
   }
 
   {
@@ -163,8 +163,8 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) {
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     TableDistribution expected(m1, "44.3854 55.6146");
-    EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete<TableDistribution>()),
+                        0.02));
   }
 }
 
@@ -253,8 +253,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     TableDistribution expected(m1, "48.3158 51.6842");
     EXPECT(assert_equal(
-        expected, *(eliminated->at(2)->asDiscrete<TableDistribution>()),
-        0.02));
+        expected, *(eliminated->at(2)->asDiscrete<TableDistribution>()), 0.02));
   }
 
   {
@@ -269,8 +268,8 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) {
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     TableDistribution expected(m1, "55.396 44.604");
-    EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete<TableDistribution>()),
+                        0.02));
   }
 }
 
@@ -347,8 +346,8 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     TableDistribution expected(m1, "51.7762 48.2238");
-    EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.02));
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete<TableDistribution>()),
+                        0.02));
   }
 
   {
@@ -363,8 +362,8 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) {
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     TableDistribution expected(m1, "49.0762 50.9238");
-    EXPECT(assert_equal(
-        expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.05));
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete<TableDistribution>()),
+                        0.05));
   }
 }
 
@@ -390,8 +389,8 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) {
   // Values taken from an importance sampling run with 100k samples:
   // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
   TableDistribution expected(m1, "8.91527 91.0847");
-  EXPECT(assert_equal(
-      expected, *(bn->at(2)->asDiscrete<TableDistribution>()), 0.01));
+  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete<TableDistribution>()),
+                      0.01));
 }
 
 /* ************************************************************************* */
@@ -496,7 +495,7 @@ TEST(HybridGaussianFactorGraph, DifferentMeans) {
         VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
         DiscreteValues{{M(1), 1}});
 
-    // EXPECT(assert_equal(expected, actual));
+    EXPECT(assert_equal(expected, actual));
 
     {
       DiscreteValues dv{{M(1), 0}};
@@ -546,8 +545,8 @@ TEST(HybridGaussianFactorGraph, DifferentCovariances) {
   DiscreteValues dv0{{M(1), 0}};
   DiscreteValues dv1{{M(1), 1}};
 
-  DiscreteConditional expected_m1(m1, "0.5/0.5");
-  DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
+  TableDistribution expected_m1(m1, "0.5 0.5");
+  TableDistribution actual_m1 = *(hbn->at(2)->asDiscrete<TableDistribution>());
 
   EXPECT(assert_equal(expected_m1, actual_m1));
 }
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 5bf97d093..3df03021b 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -1063,8 +1063,8 @@ TEST(HybridNonlinearFactorGraph, DifferentCovariances) {
   DiscreteValues dv0{{M(1), 0}};
   DiscreteValues dv1{{M(1), 1}};
 
-  DiscreteConditional expected_m1(m1, "0.5/0.5");
-  DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
+  TableDistribution expected_m1(m1, "0.5 0.5");
+  TableDistribution actual_m1 = *(hbn->at(2)->asDiscrete<TableDistribution>());
 
   EXPECT(assert_equal(expected_m1, actual_m1));
 }

From 3760b02d8ecc946db9283db90176c339a47397e7 Mon Sep 17 00:00:00 2001
From: MagnusAagaard <37117207+MagnusAagaard@users.noreply.github.com>
Date: Wed, 8 Jan 2025 10:28:28 +0100
Subject: [PATCH 358/362] Use std::abs to avoid implicit conversion of double
 input to int

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

diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h
index 20bf005c1..fe06fbaae 100644
--- a/gtsam/sfm/TranslationFactor.h
+++ b/gtsam/sfm/TranslationFactor.h
@@ -134,7 +134,7 @@ class BilinearAngleTranslationFactor
                        OptionalMatrixType H1, OptionalMatrixType H2,
                        OptionalMatrixType H3) const override {
     // Ideally we should use a positive real valued scalar datatype for scale.
-    const double abs_scale = abs(scale[0]);
+    const double abs_scale = std::abs(scale[0]);
     const Point3 predicted = (Tb - Ta) * abs_scale;
     if (H1) {
       *H1 = -Matrix3::Identity() * abs_scale;

From f5774f0443f71a623f84f8f8fb688cf01a3360f6 Mon Sep 17 00:00:00 2001
From: Matt Morley <matthew.morley.ca@gmail.com>
Date: Wed, 8 Jan 2025 21:54:44 -0700
Subject: [PATCH 359/362] Enable extra warnings

---
 cmake/GtsamBuildTypes.cmake | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 12d4b3a39..55ddf61bc 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -111,7 +111,7 @@ if(MSVC)
   set(CMAKE_3_15 $<VERSION_LESS:${CMAKE_VERSION},3.15>)
   set(CMAKE_3_25 $<VERSION_LESS:${CMAKE_VERSION},3.25>)
   # Common to all configurations, next for each configuration:
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          /W3 /GR /EHsc /MP  CACHE STRING "(User editable) Private compiler flags for all configurations.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          /W3 /WX /GR /EHsc /MP  CACHE STRING "(User editable) Private compiler flags for all configurations.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           $<${CMAKE_3_15}:/MDd> $<${CMAKE_3_25}:/Zi> /Ob0 /Od /RTC1  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  $<${CMAKE_3_15}:/MD> /O2  $<${CMAKE_3_25}:/Zi>  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         $<${CMAKE_3_15}:/MD> /O2  CACHE STRING "(User editable) Private compiler flags for Release configuration.")
@@ -131,12 +131,15 @@ else()
   endif()
 
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
+    -Werror                                        # Enable warnings as errors
     -Wall                                          # Enable common warnings
-    $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
-    $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
-    $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables>  # TODO(dellaert): don't know how to resolve
+    -Wpedantic                                     # Enable pedantic warnings
+    $<$<COMPILE_LANGUAGE:CXX>:-Wextra -Wno-unused-parameter> # Enable extra warnings, but ignore no-unused-parameter (as we ifdef out chunks of code)
+    $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr>            # Error: return local address
+    $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address>       # Error: return local address
+    $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables>   # TODO(dellaert): don't know how to resolve
     $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-vtables>  # TODO(dellaert): don't know how to resolve
-    -Wreturn-type  -Werror=return-type             # Error on missing return()
+    -Wreturn-type                                  # Error on missing return()
     -Wformat -Werror=format-security               # Error on wrong printf() arguments
     $<$<COMPILE_LANGUAGE:CXX>:${flag_override_}>   # Enforce the use of the override keyword
     #

From 3ed3c3b2e839a27433161c690550ae7e3da60ae3 Mon Sep 17 00:00:00 2001
From: Matt Morley <matthew.morley.ca@gmail.com>
Date: Wed, 8 Jan 2025 21:54:50 -0700
Subject: [PATCH 360/362] Bump CI GCC 11->12

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

diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml
index da398ad23..3c1275a55 100644
--- a/.github/workflows/build-linux.yml
+++ b/.github/workflows/build-linux.yml
@@ -27,7 +27,7 @@ jobs:
         name: [
           ubuntu-20.04-gcc-9,
           ubuntu-20.04-clang-9,
-          ubuntu-22.04-gcc-11,
+          ubuntu-22.04-gcc-12,
           ubuntu-22.04-clang-14,
         ]
 
@@ -44,7 +44,7 @@ jobs:
             compiler: clang
             version: "9"
 
-          - name: ubuntu-22.04-gcc-11
+          - name: ubuntu-22.04-gcc-12
             os: ubuntu-22.04
             compiler: gcc
             version: "11"

From fd2f39e271a13a8755329023e36d88c920635d18 Mon Sep 17 00:00:00 2001
From: Matt Morley <matthew.morley.ca@gmail.com>
Date: Wed, 8 Jan 2025 22:00:09 -0700
Subject: [PATCH 361/362] Add misc explicit operators

---
 gtsam/base/TestableAssertions.h              | 3 ++-
 gtsam/geometry/Line3.cpp                     | 1 -
 gtsam/geometry/Pose3.h                       | 5 ++---
 gtsam/geometry/Rot2.h                        | 3 ++-
 gtsam/geometry/SO4.cpp                       | 1 -
 gtsam/geometry/Similarity3.cpp               | 1 -
 gtsam/hybrid/HybridGaussianProductFactor.cpp | 2 +-
 gtsam/linear/VectorValues.h                  | 3 ++-
 gtsam_unstable/base/Dummy.h                  | 2 ++
 gtsam_unstable/base/FixedVector.h            | 2 ++
 gtsam_unstable/slam/Mechanization_bRn2.h     | 2 ++
 11 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h
index b5068ad95..f0ea79a2e 100644
--- a/gtsam/base/TestableAssertions.h
+++ b/gtsam/base/TestableAssertions.h
@@ -32,7 +32,8 @@ namespace gtsam {
 /**
  * Equals testing for basic types
  */
-inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) {
+inline bool assert_equal(const Key& expected, const Key& actual) {
+  // TODO - why isn't tol used?
   if(expected != actual) {
     std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
     return false;
diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp
index f5cf344f5..5a4b22075 100644
--- a/gtsam/geometry/Line3.cpp
+++ b/gtsam/geometry/Line3.cpp
@@ -97,7 +97,6 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
   Rot3 cRw = wRc.inverse();
   Rot3 cRl = cRw * wL.R_;
 
-  Vector2 w_ab;
   Vector3 t = ((wL.R_).transpose() * wTc.translation());
   Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
 
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 54bdd7f4c..e4896ae26 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -56,9 +56,8 @@ public:
 
   /** Copy constructor */
   Pose3(const Pose3& pose) = default;
-  //  :
-  //     R_(pose.R_), t_(pose.t_) {
-  // }
+
+  Pose3& operator=(const Pose3& other) = default;
 
   /** Construct from R,t */
   Pose3(const Rot3& R, const Point3& t) :
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 7a6f25c4d..faa0e79a2 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -53,7 +53,8 @@ namespace gtsam {
     
     /** copy constructor */
     Rot2(const Rot2& r) = default;
-    //  : Rot2(r.c_, r.s_) {}
+
+    Rot2& operator=(const Rot2& other) = default;
 
     /// Constructor from angle in radians == exponential map at identity
     Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp
index 1c4920af8..e2d6c2a69 100644
--- a/gtsam/geometry/SO4.cpp
+++ b/gtsam/geometry/SO4.cpp
@@ -107,7 +107,6 @@ SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
   }
 
   // Build expX = exp(xi^)
-  Matrix4 expX;
   using std::cos;
   using std::sin;
   const auto X2 = X * X;
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index cf2360b08..e569755ef 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -45,7 +45,6 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
 static double calculateScale(const Point3Pairs &d_abPointPairs,
                              const Rot3 &aRb) {
   double x = 0, y = 0;
-  Point3 da, db;
   for (const auto& [da, db] : d_abPointPairs) {
     const Vector3 da_prime = aRb * db;
     y += da.transpose() * da_prime;
diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp
index 280059f54..9a34ea1e9 100644
--- a/gtsam/hybrid/HybridGaussianProductFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp
@@ -33,7 +33,7 @@ static Y add(const Y& y1, const Y& y2) {
   GaussianFactorGraph result = y1.first;
   result.push_back(y2.first);
   return {result, y1.second + y2.second};
-};
+}
 
 /* *******************************************************************************/
 HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a,
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 4728706c5..35a381745 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -106,7 +106,7 @@ namespace gtsam {
     template<class CONTAINER>
     explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
 
-    /** Implicit copy constructor to specialize the explicit constructor from any container. */
+    /** Copy constructor to specialize the explicit constructor from any container. */
     VectorValues(const VectorValues& c) : values_(c.values_) {}
 
     /** Create from a pair of iterators over pair<Key,Vector>. */
@@ -119,6 +119,7 @@ namespace gtsam {
     /// Constructor from Vector, with Scatter
     VectorValues(const Vector& c, const Scatter& scatter);
 
+    // We override the copy constructor; expicitly declare operator=
     VectorValues& operator=(const VectorValues& other) = default;
 
     /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h
index 548bce344..f26585ac1 100644
--- a/gtsam_unstable/base/Dummy.h
+++ b/gtsam_unstable/base/Dummy.h
@@ -29,6 +29,8 @@ namespace gtsam {
     size_t id;
     Dummy();
     ~Dummy();
+    Dummy(const Dummy& other) = default;
+    Dummy& operator=(const Dummy& other) = default;
     void print(const std::string& s="") const ;
     unsigned char dummyTwoVar(unsigned char a) const ;
   };
diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h
index b1fff90ef..ad6e4322d 100644
--- a/gtsam_unstable/base/FixedVector.h
+++ b/gtsam_unstable/base/FixedVector.h
@@ -37,6 +37,8 @@ public:
   /** copy constructors */
   FixedVector(const FixedVector& v) : Base(v) {}
 
+  FixedVector& operator=(const FixedVector& other) = default;
+
   /** Convert from a variable-size vector to a fixed size vector */
   FixedVector(const Vector& v) : Base(v) {}
 
diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h
index 3d91f6b0e..b23a4c3ee 100644
--- a/gtsam_unstable/slam/Mechanization_bRn2.h
+++ b/gtsam_unstable/slam/Mechanization_bRn2.h
@@ -37,6 +37,8 @@ public:
   /// Copy constructor
   Mechanization_bRn2(const Mechanization_bRn2& other) = default;
 
+  Mechanization_bRn2& operator=(const Mechanization_bRn2& other) = default;
+
   /// gravity in the body frame
   Vector3 b_g(double g_e) const {
     Vector3 n_g(0, 0, g_e);

From 93f463ddbf8e990e6dccc622fcb8ecb67f21549a Mon Sep 17 00:00:00 2001
From: JaiXJM-BB <jmoraes@blackberry.com>
Date: Fri, 10 Jan 2025 10:20:50 -0500
Subject: [PATCH 362/362] QNX for develop branch

* GitLab version (branch QNX_7.1_v4.1.1)

* ADDED: Build tested with target `install` or `all` if cross compiling for QNX

* ADDED: Test Installation when building with QNX (Review this for upstreaming)

* UPDATED: Build tests, fixed some unit tests. Floating points still off.

* UPDATED: Tests Fix

* UPDATED: all non-serialization tests working.

* QNX 8.0: Working version.

* REMOVED: Removal of test prints

* UPDATED: formatting to match, removed commented out testing lines
---
 cmake/GtsamBuildTypes.cmake                        |  3 ++-
 cmake/GtsamTesting.cmake                           | 12 ++++++++----
 cmake/HandlePrintConfiguration.cmake               |  2 +-
 gtsam/3rdparty/ceres/CMakeLists.txt                |  7 ++++++-
 gtsam/3rdparty/metis/include/CMakeLists.txt        |  7 ++++++-
 gtsam/CMakeLists.txt                               |  4 ++++
 gtsam/base/CMakeLists.txt                          | 14 +++++++++++---
 gtsam/base/std_optional_serialization.h            |  4 ++++
 gtsam/basis/CMakeLists.txt                         |  6 +++++-
 gtsam/config.h.in                                  |  5 +++++
 gtsam/discrete/CMakeLists.txt                      |  7 ++++++-
 gtsam/geometry/CMakeLists.txt                      |  6 +++++-
 gtsam/inference/CMakeLists.txt                     |  6 +++++-
 gtsam/inference/tests/testOrdering.cpp             |  2 +-
 gtsam/linear/CMakeLists.txt                        |  6 +++++-
 gtsam/navigation/CMakeLists.txt                    |  6 +++++-
 gtsam/nonlinear/CMakeLists.txt                     | 13 ++++++++++---
 .../nonlinear/tests/testSerializationNonlinear.cpp |  7 ++++++-
 gtsam/sam/CMakeLists.txt                           |  6 +++++-
 gtsam/sfm/CMakeLists.txt                           |  6 +++++-
 gtsam/slam/CMakeLists.txt                          |  6 +++++-
 gtsam/symbolic/CMakeLists.txt                      |  6 +++++-
 gtsam/symbolic/tests/testSymbolicBayesTree.cpp     |  4 ++--
 gtsam_unstable/base/CMakeLists.txt                 |  6 +++++-
 gtsam_unstable/discrete/CMakeLists.txt             |  6 +++++-
 gtsam_unstable/discrete/tests/testScheduler.cpp    |  4 ++++
 gtsam_unstable/dynamics/CMakeLists.txt             |  6 +++++-
 gtsam_unstable/geometry/CMakeLists.txt             |  6 +++++-
 gtsam_unstable/linear/CMakeLists.txt               |  6 +++++-
 gtsam_unstable/nonlinear/CMakeLists.txt            |  6 +++++-
 gtsam_unstable/partition/CMakeLists.txt            |  6 +++++-
 .../partition/tests/testFindSeparator.cpp          |  6 ++++++
 gtsam_unstable/slam/CMakeLists.txt                 |  6 +++++-
 tests/testSerializationSlam.cpp                    |  1 -
 34 files changed, 167 insertions(+), 37 deletions(-)

diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 55ddf61bc..cf0dfdfc7 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -207,7 +207,8 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
   endif()
 endif()
 
-if (NOT MSVC)
+if ((NOT MSVC) AND (NOT QNX))
+  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
   if(GTSAM_BUILD_WITH_MARCH_NATIVE)
     # Check if Apple OS and compiler is [Apple]Clang
     if(APPLE AND (${CMAKE_CXX_COMPILER_ID} MATCHES "^(Apple)?Clang$"))
diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake
index 47b059213..3135ffc18 100644
--- a/cmake/GtsamTesting.cmake
+++ b/cmake/GtsamTesting.cmake
@@ -197,9 +197,11 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 
 				# Add TOPSRCDIR
 				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
-
+				
 				# Exclude from 'make all' and 'make install'
-				set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+				if(NOT QNX OR NOT DEFINED ENV{QNX_BUILD_TESTS})
+					set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+				endif()
 
 				# Configure target folder (for MSVC and Xcode)
 				set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
@@ -240,8 +242,10 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
 
 			# Exclude from 'make all' and 'make install'
-			set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
-
+			if(NOT QNX OR NOT DEFINED ENV{QNX_BUILD_TESTS})
+				set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+			endif()
+		
 			# Configure target folder (for MSVC and Xcode)
 			set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests")
 		endif()
diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake
index ac68be20f..17693e46c 100644
--- a/cmake/HandlePrintConfiguration.cmake
+++ b/cmake/HandlePrintConfiguration.cmake
@@ -22,7 +22,7 @@ if(GTSAM_UNSTABLE_AVAILABLE)
     print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable")
 endif()
 
-if(NOT MSVC AND NOT XCODE_VERSION)
+if(NOT MSVC AND NOT XCODE_VERSION AND NOT QNX)
     print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE}     "Build for native architecture  ")
     print_config("Build type" "${CMAKE_BUILD_TYPE}")
     print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
diff --git a/gtsam/3rdparty/ceres/CMakeLists.txt b/gtsam/3rdparty/ceres/CMakeLists.txt
index 98b2cffce..e8d39da67 100644
--- a/gtsam/3rdparty/ceres/CMakeLists.txt
+++ b/gtsam/3rdparty/ceres/CMakeLists.txt
@@ -1,2 +1,7 @@
 file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h")
-install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres)
\ No newline at end of file
+if(NOT QNX)
+    install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres)
+else()
+    #Install in the install include directory rather than at the install prefix.
+    install(FILES ${ceres_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/ceres)
+endif()
\ No newline at end of file
diff --git a/gtsam/3rdparty/metis/include/CMakeLists.txt b/gtsam/3rdparty/metis/include/CMakeLists.txt
index 73754eebf..0d5af2e83 100644
--- a/gtsam/3rdparty/metis/include/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/include/CMakeLists.txt
@@ -1 +1,6 @@
-install(FILES metis.h DESTINATION include/gtsam/3rdparty/metis)
\ No newline at end of file
+if(NOT QNX)
+    install(FILES metis.h DESTINATION include/gtsam/3rdparty/metis)
+else()
+    #Install in the install include directory rather than at the install prefix.
+    install(FILES metis.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/metis)
+endif()
\ No newline at end of file
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 3c2073547..db4a982a0 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -99,6 +99,10 @@ IF(MSVC)
 ENDIF(MSVC)
 
 # Generate and install config and dllexport files
+#For config.in searches
+if(QNX)
+set(QNX_TARGET_DATASET_DIR "$ENV{QNX_TARGET_DATASET_DIR}")
+endif()
 configure_file(config.h.in config.h)
 set(library_name GTSAM) # For substitution in dllexport.h.in
 configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt
index 66d3ec721..2989bbaab 100644
--- a/gtsam/base/CMakeLists.txt
+++ b/gtsam/base/CMakeLists.txt
@@ -1,9 +1,17 @@
 # Install headers
+# Header groups
 file(GLOB base_headers "*.h")
-install(FILES ${base_headers} DESTINATION include/gtsam/base)
-
 file(GLOB base_headers_tree "treeTraversal/*.h")
-install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
+
+# Installation
+if(NOT QNX)
+    install(FILES ${base_headers} DESTINATION include/gtsam/base)
+    install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
+else()
+    # For QNX, Install in the Installation's Include Directory
+    install(FILES ${base_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/base)
+    install(FILES ${base_headers_tree} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/base/treeTraversal)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h
index 93a5c8dba..e11a167ff 100644
--- a/gtsam/base/std_optional_serialization.h
+++ b/gtsam/base/std_optional_serialization.h
@@ -52,6 +52,10 @@ namespace boost { namespace serialization { struct U; } }
 namespace std { template<> struct is_trivially_default_constructible<boost::serialization::U> : std::false_type {}; }
 namespace std { template<> struct is_trivially_copy_constructible<boost::serialization::U> : std::false_type {}; }
 namespace std { template<> struct is_trivially_move_constructible<boost::serialization::U> : std::false_type {}; }
+// QCC (The QNX GCC-based Compiler) also has this issue, but it also extends to trivial destructor.
+#if defined(__QNX__)
+namespace std { template<> struct is_trivially_destructible<boost::serialization::U> : std::false_type {}; }
+#endif
 #endif
 #endif
 
diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt
index 203fd96a2..98e6076e9 100644
--- a/gtsam/basis/CMakeLists.txt
+++ b/gtsam/basis/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB basis_headers "*.h")
-install(FILES ${basis_headers} DESTINATION include/gtsam/basis)
+if(NOT QNX)
+    install(FILES ${basis_headers} DESTINATION include/gtsam/basis) 
+else()
+    install(FILES ${basis_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/basis)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 58b93ee1c..9ba562cc4 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -26,7 +26,12 @@
 
 // Paths to example datasets distributed with GTSAM
 #define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data"
+#if !defined(__QNX__) 
 #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data"
+#else
+//Set toolbox path to the path on the target.
+#define GTSAM_INSTALLED_DATASET_DIR "@QNX_TARGET_DATASET_DIR@/gtsam_examples/Data"
+#endif
 
 // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices)
 #cmakedefine GTSAM_USE_QUATERNIONS
diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt
index 1c6aa9747..a4b5cc681 100644
--- a/gtsam/discrete/CMakeLists.txt
+++ b/gtsam/discrete/CMakeLists.txt
@@ -1,7 +1,12 @@
 # Install headers
 set(subdir discrete)
 file(GLOB discrete_headers "*.h")
-install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete)
+# FIXME: exclude headers
+if(QNX)
+    install(FILES ${discrete_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/discrete)
+else()
+    install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt
index dabdde45c..fb96d23de 100644
--- a/gtsam/geometry/CMakeLists.txt
+++ b/gtsam/geometry/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB geometry_headers "*.h")
-install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
+if(QNX)
+    install(FILES ${geometry_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/geometry)
+else()
+    install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt
index c3df3a989..2344221bc 100644
--- a/gtsam/inference/CMakeLists.txt
+++ b/gtsam/inference/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB inference_headers "*.h")
-install(FILES ${inference_headers} DESTINATION include/gtsam/inference)
+if(QNX)
+    install(FILES ${inference_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/inference)
+else()
+    install(FILES ${inference_headers} DESTINATION include/gtsam/inference)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp
index b6cfcb6ed..7b7628f5b 100644
--- a/gtsam/inference/tests/testOrdering.cpp
+++ b/gtsam/inference/tests/testOrdering.cpp
@@ -303,7 +303,7 @@ TEST(Ordering, MetisLoop) {
   symbolicGraph.push_factor(0, 5);
 
   // METIS
-#if defined(__APPLE__)
+#if defined(__APPLE__) || defined(__QNX__)
   {
     Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
     //  - P( 1 0 3)
diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt
index 084c27057..e3947851c 100644
--- a/gtsam/linear/CMakeLists.txt
+++ b/gtsam/linear/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB linear_headers "*.h")
-install(FILES ${linear_headers} DESTINATION include/gtsam/linear)
+if(QNX)
+    install(FILES ${linear_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/linear)
+else()
+    install(FILES ${linear_headers} DESTINATION include/gtsam/linear)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/navigation/CMakeLists.txt b/gtsam/navigation/CMakeLists.txt
index e2b2fdce6..c266b353a 100644
--- a/gtsam/navigation/CMakeLists.txt
+++ b/gtsam/navigation/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB navigation_headers "*.h")
-install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation)
+if(QNX)
+    install(FILES ${navigation_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/navigation)
+else()
+    install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt
index 29ff2efa5..98facc59e 100644
--- a/gtsam/nonlinear/CMakeLists.txt
+++ b/gtsam/nonlinear/CMakeLists.txt
@@ -1,9 +1,16 @@
 # Install headers
 file(GLOB nonlinear_headers "*.h")
-install(FILES ${nonlinear_headers} DESTINATION "include/gtsam/nonlinear")
+if(QNX)
+    install(FILES ${nonlinear_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/nonlinear)
+else()
+    install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear)
+endif()
 
 file(GLOB nonlinear_headers_internal "internal/*.h")
-install(FILES ${nonlinear_headers_internal} DESTINATION "include/gtsam/nonlinear/internal")
-
+if(QNX)
+    install(FILES ${nonlinear_headers_internal} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/nonlinear/internal)
+else()
+    install(FILES ${nonlinear_headers_internal} DESTINATION include/gtsam/nonlinear/internal)
+endif()
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
index d6f693a23..a96ede430 100644
--- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
+++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
@@ -32,7 +32,6 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
-
 /* ************************************************************************* */
 // Create GUIDs for Noisemodels
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
@@ -153,9 +152,15 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) {
 
   // Deserialize XML
   PriorFactor<Pose3> factor_deserialized_xml = PriorFactor<Pose3>();
+  #if !defined(__QNX__)
   deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR
                          "/../../gtsam/nonlinear/tests/priorFactor.xml",
                          factor_deserialized_xml);
+  #else
+  bool c = deserializeFromXMLFile(
+                        "priorFactor.xml",
+                        factor_deserialized_xml);
+  #endif
   EXPECT(assert_equal(factor, factor_deserialized_xml));
 #endif
 }
diff --git a/gtsam/sam/CMakeLists.txt b/gtsam/sam/CMakeLists.txt
index bf20b751c..0ba13f837 100644
--- a/gtsam/sam/CMakeLists.txt
+++ b/gtsam/sam/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB sam_headers "*.h")
-install(FILES ${sam_headers} DESTINATION include/gtsam/sam)
+if(QNX)
+    install(FILES ${sam_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/sam)
+else()
+    install(FILES ${sam_headers} DESTINATION include/gtsam/sam)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/sfm/CMakeLists.txt b/gtsam/sfm/CMakeLists.txt
index fde997840..c3286d81a 100644
--- a/gtsam/sfm/CMakeLists.txt
+++ b/gtsam/sfm/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB sfm_headers "*.h")
-install(FILES ${sfm_headers} DESTINATION include/gtsam/sfm)
+if(QNX)
+    install(FILES ${sfm_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/sfm)
+else()
+    install(FILES ${sfm_headers} DESTINATION include/gtsam/sfm)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt
index 22645973d..b35293912 100644
--- a/gtsam/slam/CMakeLists.txt
+++ b/gtsam/slam/CMakeLists.txt
@@ -4,7 +4,11 @@ set (slam_excluded_headers #"")
 )
 
 file(GLOB slam_headers "*.h")
-install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
+if(QNX)
+    install(FILES ${slam_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/slam)
+else()
+    install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/symbolic/CMakeLists.txt b/gtsam/symbolic/CMakeLists.txt
index feb073f69..2c25a0970 100644
--- a/gtsam/symbolic/CMakeLists.txt
+++ b/gtsam/symbolic/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB symbolic_headers "*.h")
-install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
+if(QNX)
+    install(FILES ${symbolic_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/symbolic)
+else()
+    install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
+endif()
 
 # Build tests
 add_subdirectory(tests)
diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp
index e4a47bdfb..04fe0434b 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp
@@ -687,7 +687,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
   {
     Ordering ordering = Ordering::Create(Ordering::METIS, sfg);
 // Linux and Mac split differently when using Metis
-#if defined(__APPLE__)
+#if defined(__APPLE__) || defined(__QNX__)
     EXPECT(assert_equal(Ordering{5, 4, 2, 1, 0, 3}, ordering));
 #elif defined(_WIN32)
     EXPECT(assert_equal(Ordering{4, 3, 1, 0, 5, 2}, ordering));
@@ -700,7 +700,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
     //  | | - P( 5 | 0 4)
     //  | - P( 2 | 1 3)
     SymbolicBayesTree expected;
-#if defined(__APPLE__)
+#if defined(__APPLE__) || defined(__QNX__)
     expected.insertRoot(
         NodeClique(Keys(1)(0)(3), 3,
                    Children(                         //
diff --git a/gtsam_unstable/base/CMakeLists.txt b/gtsam_unstable/base/CMakeLists.txt
index 2cb96be36..b638302d8 100644
--- a/gtsam_unstable/base/CMakeLists.txt
+++ b/gtsam_unstable/base/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB base_headers "*.h")
-install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base)
+if(QNX)
+    install(FILES ${base_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/base)
+else()
+    install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam_unstable/discrete/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt
index 18346a45a..45e073fab 100644
--- a/gtsam_unstable/discrete/CMakeLists.txt
+++ b/gtsam_unstable/discrete/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB discrete_headers "*.h")
-install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete)
+if(QNX)
+    install(FILES ${discrete_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/discrete)
+else()
+    install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp
index 5f9b7f287..ab3a0df05 100644
--- a/gtsam_unstable/discrete/tests/testScheduler.cpp
+++ b/gtsam_unstable/discrete/tests/testScheduler.cpp
@@ -140,7 +140,11 @@ TEST(schedulingExample, test) {
 
 /* ************************************************************************* */
 TEST(schedulingExample, smallFromFile) {
+  #if !defined(__QNX__)
   string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/");
+  #else
+  string path(""); //Same Directory
+  #endif
   Scheduler s(2, path + "small.csv");
 
   // add areas
diff --git a/gtsam_unstable/dynamics/CMakeLists.txt b/gtsam_unstable/dynamics/CMakeLists.txt
index 66aef9455..bd5469f72 100644
--- a/gtsam_unstable/dynamics/CMakeLists.txt
+++ b/gtsam_unstable/dynamics/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB dynamics_headers "*.h")
-install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics)
+if(QNX)
+    install(FILES ${dynamics_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/dynamics)
+else()
+    install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam_unstable/geometry/CMakeLists.txt b/gtsam_unstable/geometry/CMakeLists.txt
index c8b7e250f..3432bab19 100644
--- a/gtsam_unstable/geometry/CMakeLists.txt
+++ b/gtsam_unstable/geometry/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB geometry_headers "*.h")
-install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
+if(QNX)
+    install(FILES ${geometry_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/geometry)
+else()
+    install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt
index 99a4b814e..50f7fb5fd 100644
--- a/gtsam_unstable/linear/CMakeLists.txt
+++ b/gtsam_unstable/linear/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB linear_headers "*.h")
-install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear)
+if(QNX)
+    install(FILES ${linear_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/linear)
+else()
+    install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt
index 9e0cb68e1..b8483ecab 100644
--- a/gtsam_unstable/nonlinear/CMakeLists.txt
+++ b/gtsam_unstable/nonlinear/CMakeLists.txt
@@ -1,6 +1,10 @@
 # Install headers
 file(GLOB nonlinear_headers "*.h")
-install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
+if(QNX)
+    install(FILES ${nonlinear_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/nonlinear)
+else()
+    install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/gtsam_unstable/partition/CMakeLists.txt b/gtsam_unstable/partition/CMakeLists.txt
index 74951bf93..f9e36a768 100644
--- a/gtsam_unstable/partition/CMakeLists.txt
+++ b/gtsam_unstable/partition/CMakeLists.txt
@@ -1,5 +1,9 @@
 # Install headers
 file(GLOB partition_headers "*.h")
-install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/partition)
+if(QNX)
+    install(FILES ${partition_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/partition)
+else()
+    install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/partition)
+endif()
 
 add_subdirectory(tests)
diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp
index a659c3c22..f425be8c7 100644
--- a/gtsam_unstable/partition/tests/testFindSeparator.cpp
+++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp
@@ -104,7 +104,13 @@ TEST ( Partition, edgePartitionByMetis2 )
   graph.push_back(std::make_shared<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1));
   graph.push_back(std::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20));
   graph.push_back(std::make_shared<GenericFactor3D>(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1));
+  //QNX Testing: fix tiebreaker to match 
+  #if !defined(__QNX__)
   std::vector<size_t> keys{0, 1, 2, 3, 4};
+  #else
+  //Anything where 2 is before 0 will work.
+  std::vector<size_t> keys{2, 0, 3, 1, 4};
+  #endif
 
   WorkSpace workspace(6);
   std::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt
index a86beac63..780de9051 100644
--- a/gtsam_unstable/slam/CMakeLists.txt
+++ b/gtsam_unstable/slam/CMakeLists.txt
@@ -5,7 +5,11 @@ set (slam_excluded_headers #"")
 
 file(GLOB slam_headers "*.h")
 list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
-install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
+if(QNX)
+    install(FILES ${slam_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/slam)
+else()
+    install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
+endif()
 
 # Add all tests
 add_subdirectory(tests)
diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp
index c4170b108..a29485720 100644
--- a/tests/testSerializationSlam.cpp
+++ b/tests/testSerializationSlam.cpp
@@ -634,7 +634,6 @@ TEST(SubgraphSolver, Solves) {
     KeyInfo keyInfo(Ab);
     std::map<Key, Vector> lambda;
     system.build(Ab, keyInfo, lambda);
-
     // Create a perturbed (non-zero) RHS
     const auto xbar = system.Rc1().optimize();  // merely for use in zero below
     auto values_y = VectorValues::Zero(xbar);