From 0f23eedbd063fe982f913392267b02b3e3617d30 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 19 Aug 2024 11:39:02 -0400
Subject: [PATCH 01/10] fix issue in geometry.i file

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

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 3d816fc25..095a350dd 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -856,7 +856,7 @@ class Cal3_S2Stereo {
   gtsam::Matrix K() const;
   gtsam::Point2 principalPoint() const;
   double baseline() const;
-  Vector6 vector() const;
+  gtsam::Vector6 vector() const;
   gtsam::Matrix inverse() const;
 };
 

From 6b1d89d0a7ebb84db3c35f578820d1d3ae063838 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 14:19:35 -0400
Subject: [PATCH 02/10] fix testMixtureFactor test

---
 gtsam/hybrid/tests/testMixtureFactor.cpp | 26 ++++++------------------
 1 file changed, 6 insertions(+), 20 deletions(-)

diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp
index c03df4d61..58a12b3d8 100644
--- a/gtsam/hybrid/tests/testMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testMixtureFactor.cpp
@@ -252,30 +252,16 @@ TEST(MixtureFactor, DifferentCovariances) {
 
   // Check that we get different error values at the MLE point μ.
   AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
-  auto cond0 = hbn->at(0)->asMixture();
-  auto cond1 = hbn->at(1)->asMixture();
-  auto discrete_cond = hbn->at(2)->asDiscrete();
 
   HybridValues hv0(cv, DiscreteValues{{M(1), 0}});
   HybridValues hv1(cv, DiscreteValues{{M(1), 1}});
-  AlgebraicDecisionTree<Key> expectedErrorTree(
-      m1,
-      cond0->error(hv0)  // cond0(0)->logNormalizationConstant()
-                         // - cond0(1)->logNormalizationConstant
-          + cond1->error(hv0) + discrete_cond->error(DiscreteValues{{M(1), 0}}),
-      cond0->error(hv1)  // cond1(0)->logNormalizationConstant()
-                         // - cond1(1)->logNormalizationConstant
-          + cond1->error(hv1) +
-          discrete_cond->error(DiscreteValues{{M(1), 0}}));
+
+  auto cond0 = hbn->at(0)->asMixture();
+  auto cond1 = hbn->at(1)->asMixture();
+  auto discrete_cond = hbn->at(2)->asDiscrete();
+  AlgebraicDecisionTree<Key> expectedErrorTree(m1, 9.90348755254,
+                                               0.69314718056);
   EXPECT(assert_equal(expectedErrorTree, errorTree));
-
-  DiscreteValues dv;
-  dv.insert({M(1), 1});
-  HybridValues expected_values(cv, dv);
-
-  HybridValues actual_values = hbn->optimize();
-
-  EXPECT(assert_equal(expected_values, actual_values));
 }
 
 /* ************************************************************************* */

From 6d9fc8e5f21b020e5dc8f250912d973d243ee665 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 15:26:04 -0400
Subject: [PATCH 03/10] undo change in GaussianMixture

---
 gtsam/hybrid/GaussianMixture.cpp | 44 ++++++++++++++++----------------
 1 file changed, 22 insertions(+), 22 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 8278549d0..36a34226b 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -71,23 +71,26 @@ GaussianMixture::GaussianMixture(
                       Conditionals(discreteParents, conditionals)) {}
 
 /* *******************************************************************************/
-GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const {
-  auto wrap = [](const GaussianConditional::shared_ptr &gc) {
-    if (gc) {
-      return GaussianBayesNet{gc};
-    } else {
-      return GaussianBayesNet();
-    }
+// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from
+// GaussianMixtureFactor, no?
+GaussianFactorGraphTree GaussianMixture::add(
+    const GaussianFactorGraphTree &sum) const {
+  using Y = GaussianFactorGraph;
+  auto add = [](const Y &graph1, const Y &graph2) {
+    auto result = graph1;
+    result.push_back(graph2);
+    return result;
   };
-  return {conditionals_, wrap};
+  const auto tree = asGaussianFactorGraphTree();
+  return sum.empty() ? tree : sum.apply(tree, add);
 }
 
 /* *******************************************************************************/
 GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
-  auto wrap = [](const GaussianBayesNet &gbn) {
-    return GaussianFactorGraph(gbn);
+  auto wrap = [](const GaussianConditional::shared_ptr &gc) {
+    return GaussianFactorGraph{gc};
   };
-  return {this->asGaussianBayesNetTree(), wrap};
+  return {conditionals_, wrap};
 }
 
 /*
@@ -108,18 +111,15 @@ GaussianBayesNetTree GaussianMixture::add(
 }
 
 /* *******************************************************************************/
-// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from
-// GaussianMixtureFactor, no?
-GaussianFactorGraphTree GaussianMixture::add(
-    const GaussianFactorGraphTree &sum) const {
-  using Y = GaussianFactorGraph;
-  auto add = [](const Y &graph1, const Y &graph2) {
-    auto result = graph1;
-    result.push_back(graph2);
-    return result;
+GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const {
+  auto wrap = [](const GaussianConditional::shared_ptr &gc) {
+    if (gc) {
+      return GaussianBayesNet{gc};
+    } else {
+      return GaussianBayesNet();
+    }
   };
-  const auto tree = asGaussianFactorGraphTree();
-  return sum.empty() ? tree : sum.apply(tree, add);
+  return {conditionals_, wrap};
 }
 
 /* *******************************************************************************/

From fd2062b2079a319a5a70c058aacc0adb3f252065 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 15:48:07 -0400
Subject: [PATCH 04/10] remove changes so we can break up PR into smaller ones

---
 gtsam/hybrid/GaussianMixture.cpp              |  67 +----
 gtsam/hybrid/GaussianMixture.h                |  18 +-
 gtsam/hybrid/GaussianMixtureFactor.cpp        |  83 +-----
 gtsam/hybrid/GaussianMixtureFactor.h          |  18 +-
 gtsam/hybrid/HybridBayesNet.cpp               |   6 +-
 gtsam/hybrid/HybridFactor.h                   |   3 -
 gtsam/hybrid/HybridGaussianFactorGraph.cpp    |  40 +--
 .../tests/testGaussianMixtureFactor.cpp       | 247 +-----------------
 .../tests/testHybridNonlinearFactorGraph.cpp  |  15 --
 gtsam/hybrid/tests/testMixtureFactor.cpp      | 149 -----------
 gtsam/linear/GaussianBayesNet.cpp             |  20 --
 gtsam/linear/GaussianBayesNet.h               |  14 -
 gtsam/linear/GaussianConditional.cpp          |  10 +-
 gtsam/linear/VectorValues.h                   |   5 -
 gtsam/linear/linear.i                         |   5 -
 .../linear/tests/testGaussianConditional.cpp  |   3 -
 16 files changed, 37 insertions(+), 666 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 36a34226b..531a9f2e5 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -24,7 +24,6 @@
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/inference/Conditional-inst.h>
-#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
@@ -93,35 +92,6 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
   return {conditionals_, wrap};
 }
 
-/*
-*******************************************************************************/
-GaussianBayesNetTree GaussianMixture::add(
-    const GaussianBayesNetTree &sum) const {
-  using Y = GaussianBayesNet;
-  auto add = [](const Y &graph1, const Y &graph2) {
-    auto result = graph1;
-    if (graph2.size() == 0) {
-      return GaussianBayesNet();
-    }
-    result.push_back(graph2);
-    return result;
-  };
-  const auto tree = asGaussianBayesNetTree();
-  return sum.empty() ? tree : sum.apply(tree, add);
-}
-
-/* *******************************************************************************/
-GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const {
-  auto wrap = [](const GaussianConditional::shared_ptr &gc) {
-    if (gc) {
-      return GaussianBayesNet{gc};
-    } else {
-      return GaussianBayesNet();
-    }
-  };
-  return {conditionals_, wrap};
-}
-
 /* *******************************************************************************/
 size_t GaussianMixture::nrComponents() const {
   size_t total = 0;
@@ -348,15 +318,8 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
 AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
     const VectorValues &continuousValues) const {
   auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
-    // Check if valid pointer
-    if (conditional) {
-      return conditional->error(continuousValues) +  //
-             logConstant_ - conditional->logNormalizationConstant();
-    } else {
-      // If not valid, pointer, it means this conditional was pruned,
-      // so we return maximum error.
-      return std::numeric_limits<double>::max();
-    }
+    return conditional->error(continuousValues) +  //
+           logConstant_ - conditional->logNormalizationConstant();
   };
   DecisionTree<Key, double> error_tree(conditionals_, errorFunc);
   return error_tree;
@@ -364,32 +327,10 @@ AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
 
 /* *******************************************************************************/
 double GaussianMixture::error(const HybridValues &values) const {
-  // Check if discrete keys in discrete assignment are
-  // present in the GaussianMixture
-  KeyVector dKeys = this->discreteKeys_.indices();
-  bool valid_assignment = false;
-  for (auto &&kv : values.discrete()) {
-    if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) {
-      valid_assignment = true;
-      break;
-    }
-  }
-
-  // The discrete assignment is not valid so we return 0.0 erorr.
-  if (!valid_assignment) {
-    return 0.0;
-  }
-
   // Directly index to get the conditional, no need to build the whole tree.
   auto conditional = conditionals_(values.discrete());
-  if (conditional) {
-    return conditional->error(values.continuous()) +  //
-           logConstant_ - conditional->logNormalizationConstant();
-  } else {
-    // If not valid, pointer, it means this conditional was pruned,
-    // so we return maximum error.
-    return std::numeric_limits<double>::max();
-  }
+  return conditional->error(values.continuous()) +  //
+         logConstant_ - conditional->logNormalizationConstant();
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index cefc14241..c1ef504f8 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -72,12 +72,6 @@ class GTSAM_EXPORT GaussianMixture
    */
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;
 
-  /**
-   * @brief Convert a DecisionTree of conditionals into
-   * a DecisionTree of Gaussian Bayes nets.
-   */
-  GaussianBayesNetTree asGaussianBayesNetTree() const;
-
   /**
    * @brief Helper function to get the pruner functor.
    *
@@ -221,7 +215,8 @@ class GTSAM_EXPORT GaussianMixture
    * @return AlgebraicDecisionTree<Key> A decision tree on the discrete keys
    * only, with the leaf values as the error for each assignment.
    */
-  AlgebraicDecisionTree<Key> errorTree(const VectorValues &continuousValues) const;
+  AlgebraicDecisionTree<Key> errorTree(
+      const VectorValues &continuousValues) const;
 
   /**
    * @brief Compute the logProbability of this Gaussian Mixture.
@@ -255,15 +250,6 @@ class GTSAM_EXPORT GaussianMixture
    * @return GaussianFactorGraphTree
    */
   GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
-
-  /**
-   * @brief Merge the Gaussian Bayes Nets in `this` and `sum` while
-   * maintaining the decision tree structure.
-   *
-   * @param sum Decision Tree of Gaussian Bayes Nets
-   * @return GaussianBayesNetTree
-   */
-  GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const;
   /// @}
 
  private:
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index e519cefe6..a3db16d04 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -28,86 +28,11 @@
 
 namespace gtsam {
 
-/**
- * @brief Helper function to correct the [A|b] matrices in the factor components
- * with the normalizer values.
- * This is done by storing the normalizer value in
- * the `b` vector as an additional row.
- *
- * @param factors DecisionTree of GaussianFactor shared pointers.
- * @param varyingNormalizers Flag indicating the normalizers are different for
- * each component.
- * @return GaussianMixtureFactor::Factors
- */
-GaussianMixtureFactor::Factors correct(
-    const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) {
-  if (!varyingNormalizers) {
-    return factors;
-  }
-
-  // First compute all the sqrt(|2 pi Sigma|) terms
-  auto computeNormalizers = [](const GaussianMixtureFactor::sharedFactor &gf) {
-    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
-    // If we have, say, a Hessian factor, then no need to do anything
-    if (!jf) return 0.0;
-
-    auto model = jf->get_model();
-    // If there is no noise model, there is nothing to do.
-    if (!model) {
-      return 0.0;
-    }
-    // Since noise models are Gaussian, we can get the logDeterminant using the
-    // same trick as in GaussianConditional
-    double logDetR =
-        model->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
-    double logDeterminantSigma = -2.0 * logDetR;
-
-    size_t n = model->dim();
-    constexpr double log2pi = 1.8378770664093454835606594728112;
-    return n * log2pi + logDeterminantSigma;
-  };
-
-  AlgebraicDecisionTree<Key> log_normalizers =
-      DecisionTree<Key, double>(factors, computeNormalizers);
-
-  // Find the minimum value so we can "proselytize" to positive values.
-  // Done because we can't have sqrt of negative numbers.
-  double min_log_normalizer = log_normalizers.min();
-  log_normalizers = log_normalizers.apply(
-      [&min_log_normalizer](double n) { return n - min_log_normalizer; });
-
-  // Finally, update the [A|b] matrices.
-  auto update = [&log_normalizers](
-                    const Assignment<Key> &assignment,
-                    const GaussianMixtureFactor::sharedFactor &gf) {
-    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
-    if (!jf) return gf;
-    // If there is no noise model, there is nothing to do.
-    if (!jf->get_model()) return gf;
-    // If the log_normalizer is 0, do nothing
-    if (log_normalizers(assignment) == 0.0) return gf;
-
-    GaussianFactorGraph gfg;
-    gfg.push_back(jf);
-
-    Vector c(1);
-    c << std::sqrt(log_normalizers(assignment));
-    auto constantFactor = std::make_shared<JacobianFactor>(c);
-
-    gfg.push_back(constantFactor);
-    return std::dynamic_pointer_cast<GaussianFactor>(
-        std::make_shared<JacobianFactor>(gfg));
-  };
-  return factors.apply(update);
-}
-
 /* *******************************************************************************/
 GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
                                              const DiscreteKeys &discreteKeys,
-                                             const Factors &factors,
-                                             bool varyingNormalizers)
-    : Base(continuousKeys, discreteKeys),
-      factors_(correct(factors, varyingNormalizers)) {}
+                                             const Factors &factors)
+    : Base(continuousKeys, discreteKeys), factors_(factors) {}
 
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
@@ -129,9 +54,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
 /* *******************************************************************************/
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
-  std::cout << (s.empty() ? "" : s + "\n");
-  std::cout << "GaussianMixtureFactor" << std::endl;
-  HybridFactor::print("", formatter);
+  HybridFactor::print(s, formatter);
   std::cout << "{\n";
   if (factors_.empty()) {
     std::cout << "  empty" << std::endl;
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 588501bbe..67d12ddb0 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -82,13 +82,10 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * their cardinalities.
    * @param factors The decision tree of Gaussian factors stored as the mixture
    * density.
-   * @param varyingNormalizers Flag indicating factor components have varying
-   * normalizer values.
    */
   GaussianMixtureFactor(const KeyVector &continuousKeys,
                         const DiscreteKeys &discreteKeys,
-                        const Factors &factors,
-                        bool varyingNormalizers = false);
+                        const Factors &factors);
 
   /**
    * @brief Construct a new GaussianMixtureFactor object using a vector of
@@ -97,16 +94,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
    * @param factors Vector of gaussian factor shared pointers.
-   * @param varyingNormalizers Flag indicating factor components have varying
-   * normalizer values.
    */
   GaussianMixtureFactor(const KeyVector &continuousKeys,
                         const DiscreteKeys &discreteKeys,
-                        const std::vector<sharedFactor> &factors,
-                        bool varyingNormalizers = false)
+                        const std::vector<sharedFactor> &factors)
       : GaussianMixtureFactor(continuousKeys, discreteKeys,
-                              Factors(discreteKeys, factors),
-                              varyingNormalizers) {}
+                              Factors(discreteKeys, factors)) {}
 
   /// @}
   /// @name Testable
@@ -114,8 +107,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
 
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
-  void print(const std::string &s = "", const KeyFormatter &formatter =
-                                            DefaultKeyFormatter) const override;
+  void print(
+      const std::string &s = "GaussianMixtureFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
   /// @}
   /// @name Standard API
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 1d01baed2..bb0f23686 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -220,16 +220,16 @@ GaussianBayesNet HybridBayesNet::choose(
 /* ************************************************************************* */
 HybridValues HybridBayesNet::optimize() const {
   // Collect all the discrete factors to compute MPE
-  DiscreteFactorGraph discrete_fg;
+  DiscreteBayesNet discrete_bn;
 
   for (auto &&conditional : *this) {
     if (conditional->isDiscrete()) {
-      discrete_fg.push_back(conditional->asDiscrete());
+      discrete_bn.push_back(conditional->asDiscrete());
     }
   }
 
   // Solve for the MPE
-  DiscreteValues mpe = discrete_fg.optimize();
+  DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
 
   // Given the MPE, compute the optimal continuous values.
   return HybridValues(optimize(mpe), mpe);
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 418489d66..afd1c8032 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -13,7 +13,6 @@
  *  @file HybridFactor.h
  *  @date Mar 11, 2022
  *  @author Fan Jiang
- *  @author Varun Agrawal
  */
 
 #pragma once
@@ -34,8 +33,6 @@ class HybridValues;
 
 /// Alias for DecisionTree of GaussianFactorGraphs
 using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
-/// Alias for DecisionTree of GaussianBayesNets
-using GaussianBayesNetTree = DecisionTree<Key, GaussianBayesNet>;
 
 KeyVector CollectKeys(const KeyVector &continuousKeys,
                       const DiscreteKeys &discreteKeys);
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index f05dfd423..b764dc9e0 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -279,37 +279,21 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
 using Result = std::pair<std::shared_ptr<GaussianConditional>,
                          GaussianMixtureFactor::sharedFactor>;
 
-/**
- * Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
- * from the residual error at the mean μ.
- * The residual error contains no keys, and only
- * depends on the discrete separator if present.
- */
+// Integrate the probability mass in the last continuous conditional using
+// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean.
+//   discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m))
 static std::shared_ptr<Factor> createDiscreteFactor(
     const DecisionTree<Key, Result> &eliminationResults,
     const DiscreteKeys &discreteSeparator) {
-  auto logProbability = [&](const Result &pair) -> double {
+  auto probability = [&](const Result &pair) -> double {
     const auto &[conditional, factor] = pair;
     static const VectorValues kEmpty;
     // If the factor is not null, it has no keys, just contains the residual.
     if (!factor) return 1.0;  // TODO(dellaert): not loving this.
-
-    // Logspace version of:
-    // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
-    // We take negative of the logNormalizationConstant `log(1/k)`
-    // to get `log(k)`.
-    return -factor->error(kEmpty) + (-conditional->logNormalizationConstant());
+    return exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
   };
 
-  AlgebraicDecisionTree<Key> logProbabilities(
-      DecisionTree<Key, double>(eliminationResults, logProbability));
-
-  // Perform normalization
-  double max_log = logProbabilities.max();
-  AlgebraicDecisionTree probabilities = DecisionTree<Key, double>(
-      logProbabilities,
-      [&max_log](const double x) { return exp(x - max_log); });
-  probabilities = probabilities.normalize(probabilities.sum());
+  DecisionTree<Key, double> probabilities(eliminationResults, probability);
 
   return std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities);
 }
@@ -370,12 +354,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   // Perform elimination!
   DecisionTree<Key, Result> eliminationResults(factorGraphTree, eliminate);
 
-  // Create the GaussianMixture from the conditionals
-  GaussianMixture::Conditionals conditionals(
-      eliminationResults, [](const Result &pair) { return pair.first; });
-  auto gaussianMixture = std::make_shared<GaussianMixture>(
-      frontalKeys, continuousSeparator, discreteSeparator, conditionals);
-
   // If there are no more continuous parents we create a DiscreteFactor with the
   // error for each discrete choice. Otherwise, create a GaussianMixtureFactor
   // on the separator, taking care to correct for conditional constants.
@@ -385,6 +363,12 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
           : createGaussianMixtureFactor(eliminationResults, continuousSeparator,
                                         discreteSeparator);
 
+  // Create the GaussianMixture from the conditionals
+  GaussianMixture::Conditionals conditionals(
+      eliminationResults, [](const Result &pair) { return pair.first; });
+  auto gaussianMixture = std::make_shared<GaussianMixture>(
+      frontalKeys, continuousSeparator, discreteSeparator, conditionals);
+
   return {std::make_shared<HybridConditional>(gaussianMixture), newFactor};
 }
 
diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
index 47b9ddc99..9cc7e6bfd 100644
--- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
@@ -22,13 +22,9 @@
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
-#include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/nonlinear/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -60,6 +56,7 @@ TEST(GaussianMixtureFactor, Sum) {
   auto b = Matrix::Zero(2, 1);
   Vector2 sigmas;
   sigmas << 1, 2;
+  auto model = noiseModel::Diagonal::Sigmas(sigmas, true);
 
   auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
   auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
@@ -109,8 +106,7 @@ TEST(GaussianMixtureFactor, Printing) {
   GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
 
   std::string expected =
-      R"(GaussianMixtureFactor
-Hybrid [x1 x2; 1]{
+      R"(Hybrid [x1 x2; 1]{
  Choice(1) 
  0 Leaf :
   A[x1] = [
@@ -182,8 +178,7 @@ TEST(GaussianMixtureFactor, Error) {
   continuousValues.insert(X(2), Vector2(1, 1));
 
   // error should return a tree of errors, with nodes for each discrete value.
-  AlgebraicDecisionTree<Key> error_tree =
-      mixtureFactor.errorTree(continuousValues);
+  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.errorTree(continuousValues);
 
   std::vector<DiscreteKey> discrete_keys = {m1};
   // Error values for regression test
@@ -196,240 +191,8 @@ TEST(GaussianMixtureFactor, Error) {
   DiscreteValues discreteValues;
   discreteValues[m1.first] = 1;
   EXPECT_DOUBLES_EQUAL(
-      4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9);
-}
-
-/* ************************************************************************* */
-// Test components with differing means
-TEST(GaussianMixtureFactor, DifferentMeans) {
-  DiscreteKey m1(M(1), 2), m2(M(2), 2);
-
-  Values values;
-  double x1 = 0.0, x2 = 1.75, x3 = 2.60;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-  values.insert(X(3), x3);
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
-
-  auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0)
-                ->linearize(values);
-  auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1)
-                ->linearize(values);
-  std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
-
-  GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors, true);
-  HybridGaussianFactorGraph hfg;
-  hfg.push_back(mixtureFactor);
-
-  f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0)
-           ->linearize(values);
-  f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1)
-           ->linearize(values);
-  std::vector<GaussianFactor::shared_ptr> factors23{f0, f1};
-  hfg.push_back(GaussianMixtureFactor({X(2), X(3)}, {m2}, factors23, true));
-
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  hfg.push_back(prior);
-
-  hfg.push_back(PriorFactor<double>(X(2), 2.0, prior_noise).linearize(values));
-
-  auto bn = hfg.eliminateSequential();
-  HybridValues actual = bn->optimize();
-
-  HybridValues expected(
-      VectorValues{
-          {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
-      DiscreteValues{{M(1), 1}, {M(2), 0}});
-
-  EXPECT(assert_equal(expected, actual));
-
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-}
-
-/* ************************************************************************* */
-/**
- * @brief Test components with differing covariances.
- * The factor graph is
- *     *-X1-*-X2
- *          |
- *          M1
- */
-TEST(GaussianMixtureFactor, DifferentCovariances) {
-  DiscreteKey m1(M(1), 2);
-
-  Values values;
-  double x1 = 1.0, x2 = 1.0;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-
-  double between = 0.0;
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  // Create via toFactorGraph
-  using symbol_shorthand::Z;
-  Matrix H0_1, H0_2, H1_1, H1_2;
-  Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
-  std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H0_1 /*Sp1*/},
-                                                {X(2), H0_2 /*Tp2*/}};
-
-  Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
-  std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H1_1 /*Sp1*/},
-                                                {X(2), H1_2 /*Tp2*/}};
-  gtsam::GaussianMixtureFactor gmf(
-      {X(1), X(2)}, {m1},
-      {std::make_shared<JacobianFactor>(X(1), H0_1, X(2), H0_2, -d0, model0),
-       std::make_shared<JacobianFactor>(X(1), H1_1, X(2), H1_2, -d1, model1)},
-      true);
-
-  // Create FG with single GaussianMixtureFactor
-  HybridGaussianFactorGraph mixture_fg;
-  mixture_fg.add(gmf);
-
-  // Linearized prior factor on X1
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  mixture_fg.push_back(prior);
-
-  auto hbn = mixture_fg.eliminateSequential();
-  // hbn->print();
-
-  VectorValues cv;
-  cv.insert(X(1), Vector1(0.0));
-  cv.insert(X(2), Vector1(0.0));
-
-  // Check that the error values at the MLE point μ.
-  AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
-
-  DiscreteValues dv0{{M(1), 0}};
-  DiscreteValues dv1{{M(1), 1}};
-
-  // regression
-  EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9);
-  EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9);
-
-  DiscreteConditional expected_m1(m1, "0.5/0.5");
-  DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
-
-  EXPECT(assert_equal(expected_m1, actual_m1));
-}
-
-/* ************************************************************************* */
-/**
- * @brief Test components with differing covariances
- * but with a Bayes net P(Z|X, M) converted to a FG.
- */
-TEST(GaussianMixtureFactor, DifferentCovariances2) {
-  DiscreteKey m1(M(1), 2);
-
-  Values values;
-  double x1 = 1.0, x2 = 1.0;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-
-  double between = 0.0;
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  // Create via toFactorGraph
-  using symbol_shorthand::Z;
-  Matrix H0_1, H0_2, H1_1, H1_2;
-  Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
-  std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H0_1 /*Sp1*/},
-                                                {X(2), H0_2 /*Tp2*/}};
-
-  Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
-  std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H1_1 /*Sp1*/},
-                                                {X(2), H1_2 /*Tp2*/}};
-  auto gm = new gtsam::GaussianMixture(
-      {Z(1)}, {X(1), X(2)}, {m1},
-      {std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
-       std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
-  gtsam::HybridBayesNet bn;
-  bn.emplace_back(gm);
-
-  gtsam::VectorValues measurements;
-  measurements.insert(Z(1), gtsam::Z_1x1);
-  // Create FG with single GaussianMixtureFactor
-  HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
-
-  // Linearized prior factor on X1
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  mixture_fg.push_back(prior);
-
-  auto hbn = mixture_fg.eliminateSequential();
-
-  VectorValues cv;
-  cv.insert(X(1), Vector1(0.0));
-  cv.insert(X(2), Vector1(0.0));
-
-  // Check that the error values at the MLE point μ.
-  AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
-
-  DiscreteValues dv0{{M(1), 0}};
-  DiscreteValues dv1{{M(1), 1}};
-
-  // regression
-  EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9);
-  EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9);
-
-  DiscreteConditional expected_m1(m1, "0.5/0.5");
-  DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
-
-  EXPECT(assert_equal(expected_m1, actual_m1));
+      4.0, mixtureFactor.error({continuousValues, discreteValues}),
+      1e-9);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 2d851b0ff..cf56e35d6 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -510,7 +510,6 @@ factor 0:
   b = [ -10 ]
   No noise model
 factor 1: 
-GaussianMixtureFactor
 Hybrid [x0 x1; m0]{
  Choice(m0) 
  0 Leaf :
@@ -535,7 +534,6 @@ Hybrid [x0 x1; m0]{
 
 }
 factor 2: 
-GaussianMixtureFactor
 Hybrid [x1 x2; m1]{
  Choice(m1) 
  0 Leaf :
@@ -677,26 +675,22 @@ factor 6:  P( m1 | m0 ):
 size: 3
 conditional 0: Hybrid  P( x0 | x1 m0)
  Discrete Keys = (m0, 2), 
- logNormalizationConstant: 1.38862
 
  Choice(m0) 
  0 Leaf p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.85087 ]
-  logNormalizationConstant: 1.38862
   No noise model
 
  1 Leaf p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.95037 ]
-  logNormalizationConstant: 1.38862
   No noise model
 
 conditional 1: Hybrid  P( x1 | x2 m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
- logNormalizationConstant: 1.3935
 
  Choice(m1) 
  0 Choice(m0) 
@@ -704,14 +698,12 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.99901 ]
-  logNormalizationConstant: 1.3935
   No noise model
 
  0 1 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.90098 ]
-  logNormalizationConstant: 1.3935
   No noise model
 
  1 Choice(m0) 
@@ -719,19 +711,16 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10.098 ]
-  logNormalizationConstant: 1.3935
   No noise model
 
  1 1 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10 ]
-  logNormalizationConstant: 1.3935
   No noise model
 
 conditional 2: Hybrid  P( x2 | m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
- logNormalizationConstant: 1.38857
 
  Choice(m1) 
  0 Choice(m0) 
@@ -740,7 +729,6 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.1489 ]
   mean: 1 elements
   x2: -1.0099
-  logNormalizationConstant: 1.38857
   No noise model
 
  0 1 Leaf p(x2)
@@ -748,7 +736,6 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.1479 ]
   mean: 1 elements
   x2: -1.0098
-  logNormalizationConstant: 1.38857
   No noise model
 
  1 Choice(m0) 
@@ -757,7 +744,6 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.0504 ]
   mean: 1 elements
   x2: -1.0001
-  logNormalizationConstant: 1.38857
   No noise model
 
  1 1 Leaf p(x2)
@@ -765,7 +751,6 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.0494 ]
   mean: 1 elements
   x2: -1
-  logNormalizationConstant: 1.38857
   No noise model
 
 )";
diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp
index 58a12b3d8..0b2564403 100644
--- a/gtsam/hybrid/tests/testMixtureFactor.cpp
+++ b/gtsam/hybrid/tests/testMixtureFactor.cpp
@@ -18,9 +18,6 @@
 
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/discrete/DiscreteValues.h>
-#include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
-#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
 #include <gtsam/hybrid/MixtureFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/BetweenFactor.h>
@@ -118,152 +115,6 @@ TEST(MixtureFactor, Dim) {
   EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
 }
 
-/* ************************************************************************* */
-// Test components with differing means
-TEST(MixtureFactor, DifferentMeans) {
-  DiscreteKey m1(M(1), 2), m2(M(2), 2);
-
-  Values values;
-  double x1 = 0.0, x2 = 1.75, x3 = 2.60;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-  values.insert(X(3), x3);
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
-
-  auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0);
-  auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
-  HybridNonlinearFactorGraph hnfg;
-  hnfg.push_back(mixtureFactor);
-
-  f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0);
-  f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors23{f0, f1};
-  hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23));
-
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise);
-  hnfg.push_back(prior);
-
-  hnfg.emplace_shared<PriorFactor<double>>(X(2), 2.0, prior_noise);
-
-  auto hgfg = hnfg.linearize(values);
-  auto bn = hgfg->eliminateSequential();
-  HybridValues actual = bn->optimize();
-
-  HybridValues expected(
-      VectorValues{
-          {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
-      DiscreteValues{{M(1), 1}, {M(2), 0}});
-
-  EXPECT(assert_equal(expected, actual));
-
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-}
-
-/* ************************************************************************* */
-// Test components with differing covariances
-TEST(MixtureFactor, DifferentCovariances) {
-  DiscreteKey m1(M(1), 2);
-
-  Values values;
-  double x1 = 1.0, x2 = 1.0;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-
-  double between = 0.0;
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  // Create via toFactorGraph
-  using symbol_shorthand::Z;
-  Matrix H0_1, H0_2, H1_1, H1_2;
-  Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
-  std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H0_1 /*Sp1*/},
-                                                {X(2), H0_2 /*Tp2*/}};
-
-  Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
-  std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H1_1 /*Sp1*/},
-                                                {X(2), H1_2 /*Tp2*/}};
-  auto gm = new gtsam::GaussianMixture(
-      {Z(1)}, {X(1), X(2)}, {m1},
-      {std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
-       std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
-  gtsam::HybridBayesNet bn;
-  bn.emplace_back(gm);
-
-  gtsam::VectorValues measurements;
-  measurements.insert(Z(1), gtsam::Z_1x1);
-  // Create FG with single GaussianMixtureFactor
-  HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
-
-  // Linearized prior factor on X1
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  mixture_fg.push_back(prior);
-
-  auto hbn = mixture_fg.eliminateSequential();
-
-  VectorValues cv;
-  cv.insert(X(1), Vector1(0.0));
-  cv.insert(X(2), Vector1(0.0));
-
-  // Check that we get different error values at the MLE point μ.
-  AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
-
-  HybridValues hv0(cv, DiscreteValues{{M(1), 0}});
-  HybridValues hv1(cv, DiscreteValues{{M(1), 1}});
-
-  auto cond0 = hbn->at(0)->asMixture();
-  auto cond1 = hbn->at(1)->asMixture();
-  auto discrete_cond = hbn->at(2)->asDiscrete();
-  AlgebraicDecisionTree<Key> expectedErrorTree(m1, 9.90348755254,
-                                               0.69314718056);
-  EXPECT(assert_equal(expectedErrorTree, errorTree));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index 861e19cc9..b04878ac5 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -243,25 +243,5 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  double GaussianBayesNet::logNormalizationConstant() const {
-    /*
-    normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
-    logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
-    
-    log det(Sigma)) = -2.0 * logDeterminant()
-    thus, logConstant = -0.5*n*log(2*pi) + logDeterminant()
-
-    BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i())
-    = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i())
-    = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant()
-    */
-    double logNormConst = 0.0;
-    for (const sharedConditional& cg : *this) {
-      logNormConst += cg->logNormalizationConstant();
-    }
-    return logNormConst;
-  }
-
-  /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index ea1cb8603..19781d1e6 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -82,12 +82,6 @@ namespace gtsam {
     /** Check equality */
     bool equals(const This& bn, double tol = 1e-9) const;
 
-    /// Check exact equality.
-    friend bool operator==(const GaussianBayesNet& lhs,
-                           const GaussianBayesNet& rhs) {
-      return lhs.isEqual(rhs);
-    }
-
     /// print graph
     void print(
         const std::string& s = "",
@@ -234,14 +228,6 @@ namespace gtsam {
      * @return The determinant */
     double logDeterminant() const;
 
-    /**
-     * @brief Get the log of the normalization constant corresponding to the
-     * joint Gaussian density represented by this Bayes net.
-     *
-     * @return double
-     */
-    double logNormalizationConstant() const;
-
     /**
      * Backsubstitute with a different RHS vector than the one stored in this BayesNet.
      * gy=inv(R*inv(Sigma))*gx
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index f986eed02..0112835aa 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -121,7 +121,6 @@ namespace gtsam {
       const auto mean = solve({});  // solve for mean.
       mean.print("  mean", formatter);
     }
-    cout << "  logNormalizationConstant: " << logNormalizationConstant() << std::endl;
     if (model_)
       model_->print("  Noise model: ");
     else
@@ -185,13 +184,8 @@ namespace gtsam {
   double GaussianConditional::logNormalizationConstant() const {
     constexpr double log2pi = 1.8378770664093454835606594728112;
     size_t n = d().size();
-    // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
-    // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
-    // Hence, log det(Sigma)) = -2.0 * logDeterminant()
-    // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant())
-    //     = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant())
-    //     = -0.5*n*log(2*pi) + logDeterminant()
-    return -0.5 * n * log2pi + logDeterminant();
+    // log det(Sigma)) = - 2.0 * logDeterminant()
+    return - 0.5 * n * log2pi + logDeterminant();
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 99ee4eb83..7fbd43ffc 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -263,11 +263,6 @@ namespace gtsam {
     /** equals required by Testable for unit testing */
     bool equals(const VectorValues& x, double tol = 1e-9) const;
 
-    /// Check exact equality.
-    friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) {
-      return lhs.equals(rhs);
-    }
-
     /// @{
     /// @name Advanced Interface
     /// @{
diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index 3a629f349..4b0963b8d 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -510,17 +510,12 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
                       size_t name2, gtsam::Matrix T,
                       const gtsam::noiseModel::Diagonal* sigmas);
-  GaussianConditional(const vector<std::pair<gtsam::Key, gtsam::Matrix>> terms,
-                      size_t nrFrontals, gtsam::Vector d,
-                      const gtsam::noiseModel::Diagonal* sigmas);
 
   // Constructors with no noise model
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R);
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S);
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
                       size_t name2, gtsam::Matrix T);
-  GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals,
-                      const gtsam::VerticalBlockMatrix& augmentedMatrix);
 
   // Named constructors
   static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index dcd821889..a4a722012 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -516,7 +516,6 @@ TEST(GaussianConditional, Print) {
     "  d = [ 20 40 ]\n"
     "  mean: 1 elements\n"
     "  x0: 20 40\n"
-    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected, conditional, "GaussianConditional"));
 
@@ -531,7 +530,6 @@ TEST(GaussianConditional, Print) {
     "  S[x1] = [ -1 -2 ]\n"
     "          [ -3 -4 ]\n"
     "  d = [ 20 40 ]\n"
-    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional"));
 
@@ -547,7 +545,6 @@ TEST(GaussianConditional, Print) {
     "  S[y1] = [ -5 -6 ]\n"
     "          [ -7 -8 ]\n"
     "  d = [ 20 40 ]\n"
-    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional"));
 }

From cea84b8c8945249f57905b3162d9e7cd7369d83f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 15:50:39 -0400
Subject: [PATCH 05/10] reduce the diff even more

---
 gtsam/hybrid/GaussianMixture.cpp                      | 2 --
 gtsam/hybrid/HybridBayesNet.cpp                       | 1 -
 gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 3 ---
 3 files changed, 6 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 531a9f2e5..c105a329e 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -145,8 +145,6 @@ void GaussianMixture::print(const std::string &s,
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }
   std::cout << "\n";
-  std::cout << " logNormalizationConstant: " << logConstant_ << "\n"
-            << std::endl;
   conditionals_.print(
       "", [&](Key k) { return formatter(k); },
       [&](const GaussianConditional::shared_ptr &gf) -> std::string {
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index bb0f23686..b02967555 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -221,7 +221,6 @@ GaussianBayesNet HybridBayesNet::choose(
 HybridValues HybridBayesNet::optimize() const {
   // Collect all the discrete factors to compute MPE
   DiscreteBayesNet discrete_bn;
-
   for (auto &&conditional : *this) {
     if (conditional->isDiscrete()) {
       discrete_bn.push_back(conditional->asDiscrete());
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index cf56e35d6..93081d309 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -675,7 +675,6 @@ factor 6:  P( m1 | m0 ):
 size: 3
 conditional 0: Hybrid  P( x0 | x1 m0)
  Discrete Keys = (m0, 2), 
-
  Choice(m0) 
  0 Leaf p(x0 | x1)
   R = [ 10.0499 ]
@@ -691,7 +690,6 @@ conditional 0: Hybrid  P( x0 | x1 m0)
 
 conditional 1: Hybrid  P( x1 | x2 m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
-
  Choice(m1) 
  0 Choice(m0) 
  0 0 Leaf p(x1 | x2)
@@ -721,7 +719,6 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
 
 conditional 2: Hybrid  P( x2 | m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
-
  Choice(m1) 
  0 Choice(m0) 
  0 0 Leaf p(x2)

From 7269d80b1c1077921944754487783a29e8fc354e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 15:57:36 -0400
Subject: [PATCH 06/10] improvements to GaussianConditional and
 GaussianBayesNet

---
 gtsam/linear/GaussianBayesNet.cpp             | 20 +++++++++++++++++++
 gtsam/linear/GaussianBayesNet.h               | 14 +++++++++++++
 gtsam/linear/GaussianConditional.cpp          | 10 ++++++++--
 gtsam/linear/VectorValues.h                   |  5 +++++
 gtsam/linear/linear.i                         |  5 +++++
 .../linear/tests/testGaussianConditional.cpp  |  3 +++
 6 files changed, 55 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index b04878ac5..861e19cc9 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -243,5 +243,25 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+  double GaussianBayesNet::logNormalizationConstant() const {
+    /*
+    normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
+    logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
+    
+    log det(Sigma)) = -2.0 * logDeterminant()
+    thus, logConstant = -0.5*n*log(2*pi) + logDeterminant()
+
+    BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i())
+    = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i())
+    = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant()
+    */
+    double logNormConst = 0.0;
+    for (const sharedConditional& cg : *this) {
+      logNormConst += cg->logNormalizationConstant();
+    }
+    return logNormConst;
+  }
+
+  /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index 19781d1e6..ea1cb8603 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -82,6 +82,12 @@ namespace gtsam {
     /** Check equality */
     bool equals(const This& bn, double tol = 1e-9) const;
 
+    /// Check exact equality.
+    friend bool operator==(const GaussianBayesNet& lhs,
+                           const GaussianBayesNet& rhs) {
+      return lhs.isEqual(rhs);
+    }
+
     /// print graph
     void print(
         const std::string& s = "",
@@ -228,6 +234,14 @@ namespace gtsam {
      * @return The determinant */
     double logDeterminant() const;
 
+    /**
+     * @brief Get the log of the normalization constant corresponding to the
+     * joint Gaussian density represented by this Bayes net.
+     *
+     * @return double
+     */
+    double logNormalizationConstant() const;
+
     /**
      * Backsubstitute with a different RHS vector than the one stored in this BayesNet.
      * gy=inv(R*inv(Sigma))*gx
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 0112835aa..f986eed02 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -121,6 +121,7 @@ namespace gtsam {
       const auto mean = solve({});  // solve for mean.
       mean.print("  mean", formatter);
     }
+    cout << "  logNormalizationConstant: " << logNormalizationConstant() << std::endl;
     if (model_)
       model_->print("  Noise model: ");
     else
@@ -184,8 +185,13 @@ namespace gtsam {
   double GaussianConditional::logNormalizationConstant() const {
     constexpr double log2pi = 1.8378770664093454835606594728112;
     size_t n = d().size();
-    // log det(Sigma)) = - 2.0 * logDeterminant()
-    return - 0.5 * n * log2pi + logDeterminant();
+    // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
+    // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
+    // Hence, log det(Sigma)) = -2.0 * logDeterminant()
+    // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant())
+    //     = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant())
+    //     = -0.5*n*log(2*pi) + logDeterminant()
+    return -0.5 * n * log2pi + logDeterminant();
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 7fbd43ffc..99ee4eb83 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -263,6 +263,11 @@ namespace gtsam {
     /** equals required by Testable for unit testing */
     bool equals(const VectorValues& x, double tol = 1e-9) const;
 
+    /// Check exact equality.
+    friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) {
+      return lhs.equals(rhs);
+    }
+
     /// @{
     /// @name Advanced Interface
     /// @{
diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index 4b0963b8d..3a629f349 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -510,12 +510,17 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
                       size_t name2, gtsam::Matrix T,
                       const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianConditional(const vector<std::pair<gtsam::Key, gtsam::Matrix>> terms,
+                      size_t nrFrontals, gtsam::Vector d,
+                      const gtsam::noiseModel::Diagonal* sigmas);
 
   // Constructors with no noise model
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R);
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S);
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
                       size_t name2, gtsam::Matrix T);
+  GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals,
+                      const gtsam::VerticalBlockMatrix& augmentedMatrix);
 
   // Named constructors
   static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index a4a722012..dcd821889 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -516,6 +516,7 @@ TEST(GaussianConditional, Print) {
     "  d = [ 20 40 ]\n"
     "  mean: 1 elements\n"
     "  x0: 20 40\n"
+    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected, conditional, "GaussianConditional"));
 
@@ -530,6 +531,7 @@ TEST(GaussianConditional, Print) {
     "  S[x1] = [ -1 -2 ]\n"
     "          [ -3 -4 ]\n"
     "  d = [ 20 40 ]\n"
+    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional"));
 
@@ -545,6 +547,7 @@ TEST(GaussianConditional, Print) {
     "  S[y1] = [ -5 -6 ]\n"
     "          [ -7 -8 ]\n"
     "  d = [ 20 40 ]\n"
+    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional"));
 }

From 8d54c4abe0fea13334fb2f6927639e53b250fae9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 16:40:13 -0400
Subject: [PATCH 07/10] update VectorValues::== docstring

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

diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 99ee4eb83..2fa50b7f6 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -263,7 +263,7 @@ namespace gtsam {
     /** equals required by Testable for unit testing */
     bool equals(const VectorValues& x, double tol = 1e-9) const;
 
-    /// Check exact equality.
+    /// Check equality.
     friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) {
       return lhs.equals(rhs);
     }

From a78ffe19e81d6ae41b385535046d1bd5cbfcf938 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 16:45:19 -0400
Subject: [PATCH 08/10] update unit test to also check for
 GaussianBayesNet::logNormalizationConstant

---
 gtsam/linear/tests/testGaussianBayesNet.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 966b70915..99453ee4e 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -80,6 +80,8 @@ TEST(GaussianBayesNet, Evaluate1) {
                        smallBayesNet.at(0)->logNormalizationConstant() +
                            smallBayesNet.at(1)->logNormalizationConstant(),
                        1e-9);
+  EXPECT_DOUBLES_EQUAL(log(constant), smallBayesNet.logNormalizationConstant(),
+                       1e-9);
   const double actual = smallBayesNet.evaluate(mean);
   EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
 }

From 910300b810cb32dcabf02e6286918f2442de6fec Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 20 Aug 2024 17:43:48 -0400
Subject: [PATCH 09/10] fix test

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

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 93081d309..751e84d91 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -680,12 +680,14 @@ conditional 0: Hybrid  P( x0 | x1 m0)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.85087 ]
+  logNormalizationConstant: 1.38862
   No noise model
 
  1 Leaf p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.95037 ]
+  logNormalizationConstant: 1.38862
   No noise model
 
 conditional 1: Hybrid  P( x1 | x2 m0 m1)
@@ -696,12 +698,14 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.99901 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
  0 1 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.90098 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
  1 Choice(m0) 
@@ -709,12 +713,14 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10.098 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
  1 1 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
 conditional 2: Hybrid  P( x2 | m0 m1)
@@ -726,6 +732,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.1489 ]
   mean: 1 elements
   x2: -1.0099
+  logNormalizationConstant: 1.38857
   No noise model
 
  0 1 Leaf p(x2)
@@ -733,6 +740,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.1479 ]
   mean: 1 elements
   x2: -1.0098
+  logNormalizationConstant: 1.38857
   No noise model
 
  1 Choice(m0) 
@@ -741,6 +749,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.0504 ]
   mean: 1 elements
   x2: -1.0001
+  logNormalizationConstant: 1.38857
   No noise model
 
  1 1 Leaf p(x2)
@@ -748,6 +757,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.0494 ]
   mean: 1 elements
   x2: -1
+  logNormalizationConstant: 1.38857
   No noise model
 
 )";

From 73d971a3c6aef8985e947d824ba96ba2ebaae0a6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 21 Aug 2024 04:12:21 -0400
Subject: [PATCH 10/10] unit tests for AlgebraicDecisionTree helper methods

---
 .../tests/testAlgebraicDecisionTree.cpp       | 49 +++++++++++++++++++
 1 file changed, 49 insertions(+)

diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
index 19f4686c2..d65a9c82b 100644
--- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
+++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
@@ -593,6 +593,55 @@ TEST(ADT, zero) {
   EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9);
 }
 
+/// Example ADT from 0 to 11.
+ADT exampleADT() {
+  DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+  ADT f(A & B & C, "0 6  2 8  4 10    1 7  3 9  5 11");
+  return f;
+}
+/* ************************************************************************** */
+// Test sum
+TEST(ADT, Sum) {
+  ADT a = exampleADT();
+  double expected_sum = 0;
+  for (double i = 0; i < 12; i++) {
+    expected_sum += i;
+  }
+  EXPECT_DOUBLES_EQUAL(expected_sum, a.sum(), 1e-9);
+}
+
+/* ************************************************************************** */
+// Test normalize
+TEST(ADT, Normalize) {
+  ADT a = exampleADT();
+  double sum = a.sum();
+  auto actual = a.normalize(sum);
+
+  DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+  DiscreteKeys keys = DiscreteKeys{A, B, C};
+  std::vector<double> cpt{0 / sum, 6 / sum,  2 / sum, 8 / sum,
+                          4 / sum, 10 / sum, 1 / sum, 7 / sum,
+                          3 / sum, 9 / sum,  5 / sum, 11 / sum};
+  ADT expected(keys, cpt);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************** */
+// Test min
+TEST(ADT, Min) {
+  ADT a = exampleADT();
+  double min = a.min();
+  EXPECT_DOUBLES_EQUAL(0.0, min, 1e-9);
+}
+
+/* ************************************************************************** */
+// Test max
+TEST(ADT, Max) {
+  ADT a = exampleADT();
+  double max = a.max();
+  EXPECT_DOUBLES_EQUAL(11.0, max, 1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;