From a6b90023f38f0f8b5650c404401333a7d0c8eb42 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 11:54:59 -0500
Subject: [PATCH 01/10] Added zero parents FromMeanAndStddev

---
 gtsam/linear/GaussianConditional.cpp | 15 +++++++++++++--
 gtsam/linear/GaussianConditional.h   |  9 +++++++--
 gtsam/linear/linear.i                |  4 ++++
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 5e8a193cf..7cdff914f 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -63,13 +63,24 @@ namespace gtsam {
       : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
         BaseConditional(1) {}
 
+  /* ************************************************************************ */
+  GaussianConditional GaussianConditional::FromMeanAndStddev(Key key,
+                                                             const Vector& mu,
+                                                             double sigma) {
+    // |Rx - d| = |x-(Ay + b)|/sigma
+    const Matrix R = Matrix::Identity(mu.size(), mu.size());
+    const Vector& d = mu;
+    return GaussianConditional(key, d, R,
+                               noiseModel::Isotropic::Sigma(mu.size(), sigma));
+  }
+
   /* ************************************************************************ */
   GaussianConditional GaussianConditional::FromMeanAndStddev(
       Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
     // |Rx + Sy - d| = |x-(Ay + b)|/sigma
     const Matrix R = Matrix::Identity(b.size(), b.size());
     const Matrix S = -A;
-    const Vector d = b;
+    const Vector& d = b;
     return GaussianConditional(key, d, R, parent, S,
                                noiseModel::Isotropic::Sigma(b.size(), sigma));
   }
@@ -82,7 +93,7 @@ namespace gtsam {
     const Matrix R = Matrix::Identity(b.size(), b.size());
     const Matrix S = -A1;
     const Matrix T = -A2;
-    const Vector d = b;
+    const Vector& d = b;
     return GaussianConditional(key, d, R, parent1, S, parent2, T,
                                noiseModel::Isotropic::Sigma(b.size(), sigma));
   }
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index a72a73973..af1c5d80e 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -84,12 +84,17 @@ namespace gtsam {
       const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
       const SharedDiagonal& sigmas = SharedDiagonal());
 
-    /// Construct from mean A1 p1 + b and standard deviation.
+    /// Construct from mean `mu` and standard deviation `sigma`.
+    static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu,
+                                                 double sigma);
+
+    /// Construct from conditional mean `A1 p1 + b` and standard deviation.
     static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
                                                  Key parent, const Vector& b,
                                                  double sigma);
 
-    /// Construct from mean A1 p1 + A2 p2 + b and standard deviation.
+    /// Construct from conditional mean `A1 p1 + A2 p2 + b` and standard
+    /// deviation `sigma`.
     static GaussianConditional FromMeanAndStddev(Key key,  //
                                                  const Matrix& A1, Key parent1,
                                                  const Matrix& A2, Key parent2,
diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index f5857b0c5..6f241da55 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -470,6 +470,10 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
                       size_t name2, Matrix T);
 
   // Named constructors
+  static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 
+                                                      const Vector& mu,
+                                                      double sigma);
+
   static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 
                                                       const Matrix& A,
                                                       gtsam::Key parent,

From 7d58207dae808697d9812b88f8c101f62e1184af Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 11:55:19 -0500
Subject: [PATCH 02/10] Renamed add to emplace, add is for shared pointers

---
 gtsam/hybrid/HybridBayesNet.h             | 21 ++++++++++++++++---
 gtsam/hybrid/hybrid.i                     | 25 +++++++++++++++--------
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 12 +++++------
 3 files changed, 41 insertions(+), 17 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 488ee0d14..a64b3bb4f 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -69,23 +69,38 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// Add HybridConditional to Bayes Net
   using Base::add;
 
+  /// Add a Gaussian Mixture to the Bayes Net.
+  void addMixture(const GaussianMixture::shared_ptr &ptr) {
+    push_back(HybridConditional(ptr));
+  }
+
+  /// Add a Gaussian conditional to the Bayes Net.
+  void addGaussian(const GaussianConditional::shared_ptr &ptr) {
+    push_back(HybridConditional(ptr));
+  }
+
+  /// Add a discrete conditional to the Bayes Net.
+  void addDiscrete(const DiscreteConditional::shared_ptr &ptr) {
+    push_back(HybridConditional(ptr));
+  }
+
   /// Add a Gaussian Mixture to the Bayes Net.
   template <typename... T>
-  void addMixture(T &&...args) {
+  void emplaceMixture(T &&...args) {
     push_back(HybridConditional(
         boost::make_shared<GaussianMixture>(std::forward<T>(args)...)));
   }
 
   /// Add a Gaussian conditional to the Bayes Net.
   template <typename... T>
-  void addGaussian(T &&...args) {
+  void emplaceGaussian(T &&...args) {
     push_back(HybridConditional(
         boost::make_shared<GaussianConditional>(std::forward<T>(args)...)));
   }
 
   /// Add a discrete conditional to the Bayes Net.
   template <typename... T>
-  void addDiscrete(T &&...args) {
+  void emplaceDiscrete(T &&...args) {
     push_back(HybridConditional(
         boost::make_shared<DiscreteConditional>(std::forward<T>(args)...)));
   }
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index acda94638..ab070c68f 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -108,14 +108,23 @@ class HybridBayesTree {
 class HybridBayesNet {
   HybridBayesNet();
   void add(const gtsam::HybridConditional& s);
-  void addMixture(const gtsam::GaussianMixture& s);
-  void addGaussian(const gtsam::GaussianConditional& s);
-  void addDiscrete(const gtsam::DiscreteConditional& s);
-  void addDiscrete(const gtsam::DiscreteKey& key, string spec);
-  void addDiscrete(const gtsam::DiscreteKey& key,
-                   const gtsam::DiscreteKeys& parents, string spec);
-  void addDiscrete(const gtsam::DiscreteKey& key,
-                   const std::vector<gtsam::DiscreteKey>& parents, string spec);
+  void addMixture(const gtsam::GaussianMixture* s);
+  void addGaussian(const gtsam::GaussianConditional* s);
+  void addDiscrete(const gtsam::DiscreteConditional* s);
+
+  void emplaceMixture(const gtsam::GaussianMixture& s);
+  void emplaceGaussian(const gtsam::GaussianConditional& s);
+  void emplaceDiscrete(const gtsam::DiscreteConditional& s);
+  void emplaceDiscrete(const gtsam::DiscreteKey& key, string spec);
+  void emplaceDiscrete(const gtsam::DiscreteKey& key,
+                       const gtsam::DiscreteKeys& parents, string spec);
+  void emplaceDiscrete(const gtsam::DiscreteKey& key,
+                       const std::vector<gtsam::DiscreteKey>& parents,
+                       string spec);
+
+  gtsam::GaussianMixture* atMixture(size_t i) const;
+  gtsam::GaussianConditional* atGaussian(size_t i) const;
+  gtsam::DiscreteConditional* atDiscrete(size_t i) const;
 
   bool empty() const;
   size_t size() const;
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 8c887a2aa..9a2ca1208 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -43,7 +43,7 @@ static const DiscreteKey Asia(asiaKey, 2);
 // Test creation of a pure discrete Bayes net.
 TEST(HybridBayesNet, Creation) {
   HybridBayesNet bayesNet;
-  bayesNet.addDiscrete(Asia, "99/1");
+  bayesNet.emplaceDiscrete(Asia, "99/1");
 
   DiscreteConditional expected(Asia, "99/1");
   CHECK(bayesNet.atDiscrete(0));
@@ -54,7 +54,7 @@ TEST(HybridBayesNet, Creation) {
 // Test adding a Bayes net to another one.
 TEST(HybridBayesNet, Add) {
   HybridBayesNet bayesNet;
-  bayesNet.addDiscrete(Asia, "99/1");
+  bayesNet.emplaceDiscrete(Asia, "99/1");
 
   HybridBayesNet other;
   other.push_back(bayesNet);
@@ -65,7 +65,7 @@ TEST(HybridBayesNet, Add) {
 // Test evaluate for a pure discrete Bayes net P(Asia).
 TEST(HybridBayesNet, evaluatePureDiscrete) {
   HybridBayesNet bayesNet;
-  bayesNet.addDiscrete(Asia, "99/1");
+  bayesNet.emplaceDiscrete(Asia, "99/1");
   HybridValues values;
   values.insert(asiaKey, 0);
   EXPECT_DOUBLES_EQUAL(0.99, bayesNet.evaluate(values), 1e-9);
@@ -87,10 +87,10 @@ TEST(HybridBayesNet, evaluateHybrid) {
 
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
-  bayesNet.addGaussian(continuousConditional);
-  bayesNet.addMixture(GaussianMixture::FromConditionals(
+  bayesNet.emplaceGaussian(continuousConditional);
+  bayesNet.emplaceMixture(GaussianMixture::FromConditionals(
       {X(1)}, {}, {Asia}, {conditional0, conditional1}));
-  bayesNet.addDiscrete(Asia, "99/1");
+  bayesNet.emplaceDiscrete(Asia, "99/1");
 
   // Create values at which to evaluate.
   HybridValues values;

From e42805eba3dcab0864cb48164c339b3b512e850d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 12:22:56 -0500
Subject: [PATCH 03/10] Get rid of (redundant and undocumented) FromFactors
 named constructor.

---
 gtsam/hybrid/GaussianMixtureFactor.cpp        |  9 ----
 gtsam/hybrid/GaussianMixtureFactor.h          | 11 ++---
 gtsam/hybrid/hybrid.i                         |  2 +-
 gtsam/hybrid/tests/Switching.h                |  2 +-
 gtsam/hybrid/tests/testGaussianMixture.cpp    | 41 ++++++++++++++++++-
 .../tests/testHybridGaussianFactorGraph.cpp   |  4 +-
 6 files changed, 48 insertions(+), 21 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index fd437f52c..881a97a1b 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -38,15 +38,6 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   return e != nullptr && Base::equals(*e, tol);
 }
 
-/* *******************************************************************************/
-GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
-    const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
-    const std::vector<GaussianFactor::shared_ptr> &factors) {
-  Factors dt(discreteKeys, factors);
-
-  return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
-}
-
 /* *******************************************************************************/
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 0b65b5aa9..b8f475de3 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -93,19 +93,16 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * @brief Construct a new GaussianMixtureFactor object using a vector of
    * GaussianFactor shared pointers.
    *
-   * @param keys Vector of keys for continuous factors.
+   * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
    * @param factors Vector of gaussian factor shared pointers.
    */
-  GaussianMixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys,
+  GaussianMixtureFactor(const KeyVector &continuousKeys,
+                        const DiscreteKeys &discreteKeys,
                         const std::vector<GaussianFactor::shared_ptr> &factors)
-      : GaussianMixtureFactor(keys, discreteKeys,
+      : GaussianMixtureFactor(continuousKeys, discreteKeys,
                               Factors(discreteKeys, factors)) {}
 
-  static This FromFactors(
-      const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
-      const std::vector<GaussianFactor::shared_ptr> &factors);
-
   /// @}
   /// @name Testable
   /// @{
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index ab070c68f..2721612f9 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -54,7 +54,7 @@ virtual class HybridDiscreteFactor {
 
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 class GaussianMixtureFactor : gtsam::HybridFactor {
-  static GaussianMixtureFactor FromFactors(
+  GaussianMixtureFactor(
       const gtsam::KeyVector& continuousKeys,
       const gtsam::DiscreteKeys& discreteKeys,
       const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index f9e1916d0..59c57f8a0 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -57,7 +57,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
 
   // keyFunc(1) to keyFunc(n+1)
   for (size_t t = 1; t < n; t++) {
-    hfg.add(GaussianMixtureFactor::FromFactors(
+    hfg.add(GaussianMixtureFactor(
         {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}},
         {boost::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
                                             I_3x3, Z_3x1),
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 310081f02..fe6a57dee 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -20,6 +20,8 @@
 
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianConditional.h>
 
@@ -33,6 +35,7 @@ using namespace gtsam;
 using noiseModel::Isotropic;
 using symbol_shorthand::M;
 using symbol_shorthand::X;
+using symbol_shorthand::Z;
 
 /* ************************************************************************* */
 /* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a
@@ -127,7 +130,43 @@ TEST(GaussianMixture, Error) {
   assignment[M(1)] = 0;
   EXPECT_DOUBLES_EQUAL(0.5, mixture.error(values, assignment), 1e-8);
   assignment[M(1)] = 1;
-  EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), 1e-8);
+  EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment),
+                       1e-8);
+}
+
+/* ************************************************************************* */
+// Create a likelihood factor for a Gaussian mixture, return a Mixture factor on
+// the parents.
+GaussianMixtureFactor::shared_ptr likelihood(const HybridValues& values) {
+  GaussianMixtureFactor::shared_ptr factor;
+  return factor;
+}
+
+/// Check that likelihood returns a mixture factor on the parents.
+TEST(GaussianMixture, Likelihood) {
+  // Create mode key: 0 is low-noise, 1 is high-noise.
+  Key modeKey = M(0);
+  DiscreteKey mode(modeKey, 2);
+
+  // Create Gaussian mixture Z(0) = X(0) + noise.
+  // TODO(dellaert): making copies below is not ideal !
+  Matrix1 I = Matrix1::Identity();
+  const auto conditional0 = boost::make_shared<GaussianConditional>(
+      GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5));
+  const auto conditional1 = boost::make_shared<GaussianConditional>(
+      GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
+  const auto gm = GaussianMixture::FromConditionals(
+      {Z(0)}, {X(0)}, {mode}, {conditional0, conditional1});
+
+  // Call the likelihood function:
+  VectorValues measurements;
+  measurements.insert(Z(0), Vector1(0));
+  HybridValues values(DiscreteValues(), measurements);
+  const auto factor = likelihood(values);
+
+  // Check that the factor is a mixture factor on the parents.
+  const GaussianMixtureFactor expected = GaussianMixtureFactor();
+  EXPECT(assert_equal(*factor, expected));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 55e4c28ad..f774e8ef1 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -176,7 +176,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  hfg.add(GaussianMixtureFactor::FromFactors(
+  hfg.add(GaussianMixtureFactor(
       {X(1)}, {{M(1), 2}},
       {boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
        boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
@@ -235,7 +235,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
   hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
 
   {
-    hfg.add(GaussianMixtureFactor::FromFactors(
+    hfg.add(GaussianMixtureFactor(
         {X(0)}, {{M(0), 2}},
         {boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
          boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));

From 1e87a81d010dec84bfc6c1608a044149e7dd6a1a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:12:03 -0500
Subject: [PATCH 04/10] Made method const

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

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index c0815b2d7..82d16226a 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -36,7 +36,7 @@ GaussianMixture::GaussianMixture(
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() const {
   return conditionals_;
 }
 
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 88d5a02c0..a3393dbb0 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -142,7 +142,7 @@ class GTSAM_EXPORT GaussianMixture
   /// @}
 
   /// Getter for the underlying Conditionals DecisionTree
-  const Conditionals &conditionals();
+  const Conditionals &conditionals() const;
 
   /**
    * @brief Compute error of the GaussianMixture as a tree.

From 364417e4aaf90a3727bbf9def41402581ed1c4e9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:12:12 -0500
Subject: [PATCH 05/10] Fixed equals and print

---
 gtsam/hybrid/GaussianMixtureFactor.cpp | 42 ++++++++++++++++++--------
 1 file changed, 29 insertions(+), 13 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 881a97a1b..32ca1432c 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -35,7 +35,19 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
-  return e != nullptr && Base::equals(*e, tol);
+  if (e == nullptr) return false;
+
+  // This will return false if either factors_ is empty or e->factors_ is empty,
+  // but not if both are empty or both are not empty:
+  if (factors_.empty() ^ e->factors_.empty()) return false;
+
+  // Check the base and the factors:
+  return Base::equals(*e, tol) &&
+         factors_.equals(e->factors_,
+                         [tol](const GaussianFactor::shared_ptr &f1,
+                               const GaussianFactor::shared_ptr &f2) {
+                           return f1->equals(*f2, tol);
+                         });
 }
 
 /* *******************************************************************************/
@@ -43,18 +55,22 @@ void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   std::cout << "{\n";
-  factors_.print(
-      "", [&](Key k) { return formatter(k); },
-      [&](const GaussianFactor::shared_ptr &gf) -> std::string {
-        RedirectCout rd;
-        std::cout << ":\n";
-        if (gf && !gf->empty()) {
-          gf->print("", formatter);
-          return rd.str();
-        } else {
-          return "nullptr";
-        }
-      });
+  if (factors_.empty()) {
+    std::cout << "  empty" << std::endl;
+  } else {
+    factors_.print(
+        "", [&](Key k) { return formatter(k); },
+        [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+          RedirectCout rd;
+          std::cout << ":\n";
+          if (gf && !gf->empty()) {
+            gf->print("", formatter);
+            return rd.str();
+          } else {
+            return "nullptr";
+          }
+        });
+  }
   std::cout << "}" << std::endl;
 }
 

From 611f61c7f4189c668326073f66185e47cd9b2fc3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:21:20 -0500
Subject: [PATCH 06/10] proto code for likelihood

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

diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index fe6a57dee..5542d86a9 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -135,19 +135,12 @@ TEST(GaussianMixture, Error) {
 }
 
 /* ************************************************************************* */
-// Create a likelihood factor for a Gaussian mixture, return a Mixture factor on
-// the parents.
-GaussianMixtureFactor::shared_ptr likelihood(const HybridValues& values) {
-  GaussianMixtureFactor::shared_ptr factor;
-  return factor;
-}
-
-/// Check that likelihood returns a mixture factor on the parents.
-TEST(GaussianMixture, Likelihood) {
-  // Create mode key: 0 is low-noise, 1 is high-noise.
-  Key modeKey = M(0);
-  DiscreteKey mode(modeKey, 2);
+// Create mode key: 0 is low-noise, 1 is high-noise.
+static const Key modeKey = M(0);
+static const DiscreteKey mode(modeKey, 2);
 
+// Create a simple GaussianMixture
+static GaussianMixture createSimpleGaussianMixture() {
   // Create Gaussian mixture Z(0) = X(0) + noise.
   // TODO(dellaert): making copies below is not ideal !
   Matrix1 I = Matrix1::Identity();
@@ -157,15 +150,77 @@ TEST(GaussianMixture, Likelihood) {
       GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
   const auto gm = GaussianMixture::FromConditionals(
       {Z(0)}, {X(0)}, {mode}, {conditional0, conditional1});
+  return gm;
+}
+
+/* ************************************************************************* */
+std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys& dkeys) {
+  std::set<DiscreteKey> s;
+  s.insert(dkeys.begin(), dkeys.end());
+  return s;
+}
+
+// Get only the continuous parent keys as a KeyVector:
+KeyVector continuousParents(const GaussianMixture& gm) {
+  // Get all parent keys:
+  const auto range = gm.parents();
+  KeyVector continuousParentKeys(range.begin(), range.end());
+  // Loop over all discrete keys:
+  for (const auto& discreteKey : gm.discreteKeys()) {
+    const Key key = discreteKey.first;
+    // remove that key from continuousParentKeys:
+    continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
+                                           continuousParentKeys.end(), key),
+                               continuousParentKeys.end());
+  }
+  return continuousParentKeys;
+}
+
+// Create a test for continuousParents.
+TEST(GaussianMixture, ContinuousParents) {
+  const GaussianMixture gm = createSimpleGaussianMixture();
+  const KeyVector continuousParentKeys = continuousParents(gm);
+  // Check that the continuous parent keys are correct:
+  EXPECT(continuousParentKeys.size() == 1);
+  EXPECT(continuousParentKeys[0] == X(0));
+}
+
+/* ************************************************************************* */
+// Create a likelihood factor for a Gaussian mixture, return a Mixture factor.
+GaussianMixtureFactor::shared_ptr likelihood(const GaussianMixture& gm,
+                                             const VectorValues& frontals) {
+  // TODO(dellaert): check that values has all frontals
+  const DiscreteKeys discreteParentKeys = gm.discreteKeys();
+  const KeyVector continuousParentKeys = continuousParents(gm);
+  const GaussianMixtureFactor::Factors likelihoods(
+      gm.conditionals(),
+      [&](const GaussianConditional::shared_ptr& conditional) {
+        return conditional->likelihood(frontals);
+      });
+  return boost::make_shared<GaussianMixtureFactor>(
+      continuousParentKeys, discreteParentKeys, likelihoods);
+}
+
+/// Check that likelihood returns a mixture factor on the parents.
+TEST(GaussianMixture, Likelihood) {
+  const GaussianMixture gm = createSimpleGaussianMixture();
 
   // Call the likelihood function:
   VectorValues measurements;
   measurements.insert(Z(0), Vector1(0));
-  HybridValues values(DiscreteValues(), measurements);
-  const auto factor = likelihood(values);
+  const auto factor = likelihood(gm, measurements);
 
   // Check that the factor is a mixture factor on the parents.
-  const GaussianMixtureFactor expected = GaussianMixtureFactor();
+  // Loop over all discrete assignments over the discrete parents:
+  const DiscreteKeys discreteParentKeys = gm.discreteKeys();
+
+  // Apply the likelihood function to all conditionals:
+  const GaussianMixtureFactor::Factors factors(
+      gm.conditionals(),
+      [measurements](const GaussianConditional::shared_ptr& conditional) {
+        return conditional->likelihood(measurements);
+      });
+  const GaussianMixtureFactor expected({X(0)}, {mode}, factors);
   EXPECT(assert_equal(*factor, expected));
 }
 

From 7ba53925253ca877ce81d385075e7c1c63b501f3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:28:20 -0500
Subject: [PATCH 07/10] likelihood method (as well as continuousParents)

---
 gtsam/hybrid/GaussianMixture.cpp           | 31 ++++++++++++++++
 gtsam/hybrid/GaussianMixture.h             | 29 +++++++++------
 gtsam/hybrid/tests/testGaussianMixture.cpp | 41 ++--------------------
 3 files changed, 52 insertions(+), 49 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 82d16226a..a5d06f04d 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
@@ -128,6 +129,36 @@ void GaussianMixture::print(const std::string &s,
       });
 }
 
+/* ************************************************************************* */
+KeyVector GaussianMixture::continuousParents() const {
+  // Get all parent keys:
+  const auto range = parents();
+  KeyVector continuousParentKeys(range.begin(), range.end());
+  // Loop over all discrete keys:
+  for (const auto &discreteKey : discreteKeys()) {
+    const Key key = discreteKey.first;
+    // remove that key from continuousParentKeys:
+    continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
+                                           continuousParentKeys.end(), key),
+                               continuousParentKeys.end());
+  }
+  return continuousParentKeys;
+}
+
+/* ************************************************************************* */
+boost::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
+    const VectorValues &frontals) const {
+  // TODO(dellaert): check that values has all frontals
+  const DiscreteKeys discreteParentKeys = discreteKeys();
+  const KeyVector continuousParentKeys = continuousParents();
+  const GaussianMixtureFactor::Factors likelihoods(
+      conditionals(), [&](const GaussianConditional::shared_ptr &conditional) {
+        return conditional->likelihood(frontals);
+      });
+  return boost::make_shared<GaussianMixtureFactor>(
+      continuousParentKeys, discreteParentKeys, likelihoods);
+}
+
 /* ************************************************************************* */
 std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
   std::set<DiscreteKey> s;
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index a3393dbb0..672a886ad 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -29,6 +29,8 @@
 
 namespace gtsam {
 
+class GaussianMixtureFactor;
+
 /**
  * @brief A conditional of gaussian mixtures indexed by discrete variables, as
  * part of a Bayes Network. This is the result of the elimination of a
@@ -117,16 +119,6 @@ class GTSAM_EXPORT GaussianMixture
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
-  /// @}
-  /// @name Standard API
-  /// @{
-
-  GaussianConditional::shared_ptr operator()(
-      const DiscreteValues &discreteValues) const;
-
-  /// Returns the total number of continuous components
-  size_t nrComponents() const;
-
   /// @}
   /// @name Testable
   /// @{
@@ -140,6 +132,22 @@ class GTSAM_EXPORT GaussianMixture
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
   /// @}
+  /// @name Standard API
+  /// @{
+
+  GaussianConditional::shared_ptr operator()(
+      const DiscreteValues &discreteValues) const;
+
+  /// Returns the total number of continuous components
+  size_t nrComponents() const;
+
+  /// Returns the continuous keys among the parents.
+  KeyVector continuousParents() const;
+
+  // Create a likelihood factor for a Gaussian mixture, return a Mixture factor
+  // on the parents.
+  boost::shared_ptr<GaussianMixtureFactor> likelihood(
+      const VectorValues &frontals) const;
 
   /// Getter for the underlying Conditionals DecisionTree
   const Conditionals &conditionals() const;
@@ -181,6 +189,7 @@ class GTSAM_EXPORT GaussianMixture
    * @return Sum
    */
   Sum add(const Sum &sum) const;
+  /// @}
 };
 
 /// Return the DiscreteKey vector as a set.
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 5542d86a9..ed5771770 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -154,53 +154,16 @@ static GaussianMixture createSimpleGaussianMixture() {
 }
 
 /* ************************************************************************* */
-std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys& dkeys) {
-  std::set<DiscreteKey> s;
-  s.insert(dkeys.begin(), dkeys.end());
-  return s;
-}
-
-// Get only the continuous parent keys as a KeyVector:
-KeyVector continuousParents(const GaussianMixture& gm) {
-  // Get all parent keys:
-  const auto range = gm.parents();
-  KeyVector continuousParentKeys(range.begin(), range.end());
-  // Loop over all discrete keys:
-  for (const auto& discreteKey : gm.discreteKeys()) {
-    const Key key = discreteKey.first;
-    // remove that key from continuousParentKeys:
-    continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
-                                           continuousParentKeys.end(), key),
-                               continuousParentKeys.end());
-  }
-  return continuousParentKeys;
-}
-
 // Create a test for continuousParents.
 TEST(GaussianMixture, ContinuousParents) {
   const GaussianMixture gm = createSimpleGaussianMixture();
-  const KeyVector continuousParentKeys = continuousParents(gm);
+  const KeyVector continuousParentKeys = gm.continuousParents();
   // Check that the continuous parent keys are correct:
   EXPECT(continuousParentKeys.size() == 1);
   EXPECT(continuousParentKeys[0] == X(0));
 }
 
 /* ************************************************************************* */
-// Create a likelihood factor for a Gaussian mixture, return a Mixture factor.
-GaussianMixtureFactor::shared_ptr likelihood(const GaussianMixture& gm,
-                                             const VectorValues& frontals) {
-  // TODO(dellaert): check that values has all frontals
-  const DiscreteKeys discreteParentKeys = gm.discreteKeys();
-  const KeyVector continuousParentKeys = continuousParents(gm);
-  const GaussianMixtureFactor::Factors likelihoods(
-      gm.conditionals(),
-      [&](const GaussianConditional::shared_ptr& conditional) {
-        return conditional->likelihood(frontals);
-      });
-  return boost::make_shared<GaussianMixtureFactor>(
-      continuousParentKeys, discreteParentKeys, likelihoods);
-}
-
 /// Check that likelihood returns a mixture factor on the parents.
 TEST(GaussianMixture, Likelihood) {
   const GaussianMixture gm = createSimpleGaussianMixture();
@@ -208,7 +171,7 @@ TEST(GaussianMixture, Likelihood) {
   // Call the likelihood function:
   VectorValues measurements;
   measurements.insert(Z(0), Vector1(0));
-  const auto factor = likelihood(gm, measurements);
+  const auto factor = gm.likelihood(measurements);
 
   // Check that the factor is a mixture factor on the parents.
   // Loop over all discrete assignments over the discrete parents:

From d4ee6997f7e032b9720c554ecd2b6fe6ab96b545 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:54:44 -0500
Subject: [PATCH 08/10] Remove FromConditionals

---
 gtsam/hybrid/GaussianMixture.cpp           | 11 ++++------
 gtsam/hybrid/GaussianMixture.h             |  2 +-
 gtsam/hybrid/hybrid.i                      | 25 +++++++++++++---------
 gtsam/hybrid/tests/testGaussianMixture.cpp |  4 +---
 gtsam/hybrid/tests/testHybridBayesNet.cpp  |  4 ++--
 python/gtsam/tests/test_HybridBayesNet.py  |  5 ++---
 6 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index a5d06f04d..e34467527 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -42,15 +42,12 @@ const GaussianMixture::Conditionals &GaussianMixture::conditionals() const {
 }
 
 /* *******************************************************************************/
-GaussianMixture GaussianMixture::FromConditionals(
+GaussianMixture::GaussianMixture(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
-    const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
-  Conditionals dt(discreteParents, conditionalsList);
-
-  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
-                         dt);
-}
+    const std::vector<GaussianConditional::shared_ptr> &conditionalsList)
+    : GaussianMixture(continuousFrontals, continuousParents, discreteParents,
+                      Conditionals(discreteParents, conditionalsList)) {}
 
 /* *******************************************************************************/
 GaussianMixture::Sum GaussianMixture::add(
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 672a886ad..2cdc23b46 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -114,7 +114,7 @@ class GTSAM_EXPORT GaussianMixture
    * @param discreteParents Discrete parents variables
    * @param conditionals List of conditionals
    */
-  static This FromConditionals(
+  GaussianMixture(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 2721612f9..29247cdc3 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -66,12 +66,13 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
 
 #include <gtsam/hybrid/GaussianMixture.h>
 class GaussianMixture : gtsam::HybridFactor {
-  static GaussianMixture FromConditionals(
-      const gtsam::KeyVector& continuousFrontals,
-      const gtsam::KeyVector& continuousParents,
-      const gtsam::DiscreteKeys& discreteParents,
-      const std::vector<gtsam::GaussianConditional::shared_ptr>&
-          conditionalsList);
+  GaussianMixture(const gtsam::KeyVector& continuousFrontals,
+                  const gtsam::KeyVector& continuousParents,
+                  const gtsam::DiscreteKeys& discreteParents,
+                  const std::vector<gtsam::GaussianConditional::shared_ptr>&
+                      conditionalsList);
+
+  gtsam::GaussianMixtureFactor* likelihood(const gtsam::VectorValues &frontals) const;
 
   void print(string s = "GaussianMixture\n",
              const gtsam::KeyFormatter& keyFormatter =
@@ -104,7 +105,7 @@ class HybridBayesTree {
                  gtsam::DefaultKeyFormatter) const;
 };
 
-#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
 class HybridBayesNet {
   HybridBayesNet();
   void add(const gtsam::HybridConditional& s);
@@ -113,6 +114,11 @@ class HybridBayesNet {
   void addDiscrete(const gtsam::DiscreteConditional* s);
 
   void emplaceMixture(const gtsam::GaussianMixture& s);
+  void emplaceMixture(const gtsam::KeyVector& continuousFrontals,
+                      const gtsam::KeyVector& continuousParents,
+                      const gtsam::DiscreteKeys& discreteParents,
+                      const std::vector<gtsam::GaussianConditional::shared_ptr>&
+                          conditionalsList);
   void emplaceGaussian(const gtsam::GaussianConditional& s);
   void emplaceDiscrete(const gtsam::DiscreteConditional& s);
   void emplaceDiscrete(const gtsam::DiscreteKey& key, string spec);
@@ -162,9 +168,8 @@ class HybridGaussianFactorGraph {
   void push_back(const gtsam::HybridBayesNet& bayesNet);
   void push_back(const gtsam::HybridBayesTree& bayesTree);
   void push_back(const gtsam::GaussianMixtureFactor* gmm);
-
-  void add(gtsam::DecisionTreeFactor* factor);
-  void add(gtsam::JacobianFactor* factor);
+  void push_back(gtsam::DecisionTreeFactor* factor);
+  void push_back(gtsam::JacobianFactor* factor);
 
   bool empty() const;
   void remove(size_t i);
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index ed5771770..242c9ba41 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -148,9 +148,7 @@ static GaussianMixture createSimpleGaussianMixture() {
       GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5));
   const auto conditional1 = boost::make_shared<GaussianConditional>(
       GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
-  const auto gm = GaussianMixture::FromConditionals(
-      {Z(0)}, {X(0)}, {mode}, {conditional0, conditional1});
-  return gm;
+  return GaussianMixture({Z(0)}, {X(0)}, {mode}, {conditional0, conditional1});
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 9a2ca1208..627d82694 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -88,8 +88,8 @@ TEST(HybridBayesNet, evaluateHybrid) {
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.emplaceGaussian(continuousConditional);
-  bayesNet.emplaceMixture(GaussianMixture::FromConditionals(
-      {X(1)}, {}, {Asia}, {conditional0, conditional1}));
+  GaussianMixture gm({X(1)}, {}, {Asia}, {conditional0, conditional1});
+  bayesNet.emplaceMixture(gm);  // copy :-(
   bayesNet.emplaceDiscrete(Asia, "99/1");
 
   // Create values at which to evaluate.
diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py
index 66cddf05e..af89a4ba7 100644
--- a/python/gtsam/tests/test_HybridBayesNet.py
+++ b/python/gtsam/tests/test_HybridBayesNet.py
@@ -42,14 +42,13 @@ class TestHybridBayesNet(GtsamTestCase):
         conditional1 = GaussianConditional(X(1), [2], I_1x1, model1)
         dkeys = DiscreteKeys()
         dkeys.push_back(Asia)
-        gm = GaussianMixture.FromConditionals([X(1)], [], dkeys,
-                                              [conditional0, conditional1])  #
+        gm = GaussianMixture([X(1)], [], dkeys, [conditional0, conditional1]) 
 
         # Create hybrid Bayes net.
         bayesNet = HybridBayesNet()
         bayesNet.addGaussian(gc)
         bayesNet.addMixture(gm)
-        bayesNet.addDiscrete(Asia, "99/1")
+        bayesNet.emplaceDiscrete(Asia, "99/1")
 
         # Create values at which to evaluate.
         values = HybridValues()

From 2d688a1986de6f48e2ac0a0fa8a9e3bcddeae2e0 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:55:06 -0500
Subject: [PATCH 09/10] Added tests to convert Hybrid BN to corresponding
 "likelihood" FG

---
 python/gtsam/tests/test_HybridFactorGraph.py | 135 +++++++++++++++----
 1 file changed, 107 insertions(+), 28 deletions(-)

diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 37bb5b93c..37243b937 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -13,73 +13,152 @@ Author: Fan Jiang
 import unittest
 
 import numpy as np
-from gtsam.symbol_shorthand import C, X
+from gtsam.symbol_shorthand import C, M, X, Z
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
+from gtsam import (
+    DecisionTreeFactor,
+    DiscreteConditional,
+    DiscreteKeys,
+    GaussianConditional,
+    GaussianMixture,
+    GaussianMixtureFactor,
+    HybridGaussianFactorGraph,
+    JacobianFactor,
+    Ordering,
+    noiseModel,
+)
 
 
 class TestHybridGaussianFactorGraph(GtsamTestCase):
     """Unit tests for HybridGaussianFactorGraph."""
+
     def test_create(self):
         """Test construction of hybrid factor graph."""
-        noiseModel = gtsam.noiseModel.Unit.Create(3)
-        dk = gtsam.DiscreteKeys()
+        model = noiseModel.Unit.Create(3)
+        dk = DiscreteKeys()
         dk.push_back((C(0), 2))
 
-        jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)),
-                                   noiseModel)
-        jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)),
-                                   noiseModel)
+        jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
+        jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
 
-        gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
+        gmf = GaussianMixtureFactor([X(0)], dk, [jf1, jf2])
 
-        hfg = gtsam.HybridGaussianFactorGraph()
-        hfg.add(jf1)
-        hfg.add(jf2)
+        hfg = HybridGaussianFactorGraph()
+        hfg.push_back(jf1)
+        hfg.push_back(jf2)
         hfg.push_back(gmf)
 
         hbn = hfg.eliminateSequential(
-            gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
-                hfg, [C(0)]))
+            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
+        )
 
         self.assertEqual(hbn.size(), 2)
 
         mixture = hbn.at(0).inner()
-        self.assertIsInstance(mixture, gtsam.GaussianMixture)
+        self.assertIsInstance(mixture, GaussianMixture)
         self.assertEqual(len(mixture.keys()), 2)
 
         discrete_conditional = hbn.at(hbn.size() - 1).inner()
-        self.assertIsInstance(discrete_conditional, gtsam.DiscreteConditional)
+        self.assertIsInstance(discrete_conditional, DiscreteConditional)
 
     def test_optimize(self):
         """Test construction of hybrid factor graph."""
-        noiseModel = gtsam.noiseModel.Unit.Create(3)
-        dk = gtsam.DiscreteKeys()
+        model = noiseModel.Unit.Create(3)
+        dk = DiscreteKeys()
         dk.push_back((C(0), 2))
 
-        jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)),
-                                   noiseModel)
-        jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)),
-                                   noiseModel)
+        jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
+        jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
 
-        gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
+        gmf = GaussianMixtureFactor([X(0)], dk, [jf1, jf2])
 
-        hfg = gtsam.HybridGaussianFactorGraph()
-        hfg.add(jf1)
-        hfg.add(jf2)
+        hfg = HybridGaussianFactorGraph()
+        hfg.push_back(jf1)
+        hfg.push_back(jf2)
         hfg.push_back(gmf)
 
         dtf = gtsam.DecisionTreeFactor([(C(0), 2)], "0 1")
-        hfg.add(dtf)
+        hfg.push_back(dtf)
 
         hbn = hfg.eliminateSequential(
-            gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
-                hfg, [C(0)]))
+            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
+        )
 
         hv = hbn.optimize()
         self.assertEqual(hv.atDiscrete(C(0)), 1)
 
+    @staticmethod
+    def tiny(num_measurements: int = 1):
+        """Create a tiny two variable hybrid model."""
+        # Create hybrid Bayes net.
+        bayesNet = gtsam.HybridBayesNet()
+
+        # Create mode key: 0 is low-noise, 1 is high-noise.
+        modeKey = M(0)
+        mode = (modeKey, 2)
+
+        # Create Gaussian mixture Z(0) = X(0) + noise for each measurement.
+        I = np.eye(1)
+        keys = DiscreteKeys()
+        keys.push_back(mode)
+        for i in range(num_measurements):
+            conditional0 = GaussianConditional.FromMeanAndStddev(
+                Z(i), I, X(0), [0], sigma=0.5
+            )
+            conditional1 = GaussianConditional.FromMeanAndStddev(
+                Z(i), I, X(0), [0], sigma=3
+            )
+            bayesNet.emplaceMixture([Z(i)], [X(0)], keys, [conditional0, conditional1])
+
+        # Create prior on X(0).
+        prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0)
+        bayesNet.addGaussian(prior_on_x0)
+
+        # Add prior on mode.
+        bayesNet.emplaceDiscrete(mode, "1/1")
+
+        return bayesNet
+
+    def test_tiny(self):
+        """Test a tiny two variable hybrid model."""
+        bayesNet = self.tiny()
+        sample = bayesNet.sample()
+        # print(sample)
+
+        # Create a factor graph from the Bayes net with sampled measurements.
+        fg = HybridGaussianFactorGraph()
+        conditional = bayesNet.atMixture(0)
+        measurement = gtsam.VectorValues()
+        measurement.insert(Z(0), sample.at(Z(0)))
+        factor = conditional.likelihood(measurement)
+        fg.push_back(factor)
+        fg.push_back(bayesNet.atGaussian(1))
+        fg.push_back(bayesNet.atDiscrete(2))
+        
+        self.assertEqual(fg.size(), 3)
+
+    def test_tiny2(self):
+        """Test a tiny two variable hybrid model, with 2 measurements."""
+        # Create the Bayes net and sample from it.
+        bayesNet = self.tiny(num_measurements=2)
+        sample = bayesNet.sample()
+        # print(sample)
+
+        # Create a factor graph from the Bayes net with sampled measurements.
+        fg = HybridGaussianFactorGraph()
+        for i in range(2):
+            conditional = bayesNet.atMixture(i)
+            measurement = gtsam.VectorValues()
+            measurement.insert(Z(i), sample.at(Z(i)))
+            factor = conditional.likelihood(measurement)
+            fg.push_back(factor)
+        fg.push_back(bayesNet.atGaussian(2))
+        fg.push_back(bayesNet.atDiscrete(3))
+        
+        self.assertEqual(fg.size(), 4)
+
 
 if __name__ == "__main__":
     unittest.main()

From a4659f01c75d810c1a360dfaa58c77d26731dbce Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 14:13:35 -0500
Subject: [PATCH 10/10] Add error and probPrime variants

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp   | 43 +++++++++++++++++++-
 gtsam/hybrid/HybridGaussianFactorGraph.h     | 25 ++++++++++++
 gtsam/hybrid/hybrid.i                        |  6 +++
 python/gtsam/tests/test_HybridFactorGraph.py | 19 +++++++--
 4 files changed, 88 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 32653bdec..b110f8586 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -468,12 +468,51 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
   return error_tree;
 }
 
+/* ************************************************************************ */
+double HybridGaussianFactorGraph::error(
+    const VectorValues &continuousValues,
+    const DiscreteValues &discreteValues) const {
+  double error = 0.0;
+  for (size_t idx = 0; idx < size(); idx++) {
+    auto factor = factors_.at(idx);
+
+    if (factor->isHybrid()) {
+      if (auto c = boost::dynamic_pointer_cast<HybridConditional>(factor)) {
+        error += c->asMixture()->error(continuousValues, discreteValues);
+      }
+      if (auto f = boost::dynamic_pointer_cast<GaussianMixtureFactor>(factor)) {
+        error += f->error(continuousValues, discreteValues);
+      }
+
+    } else if (factor->isContinuous()) {
+      if (auto f = boost::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
+        error += f->inner()->error(continuousValues);
+      }
+      if (auto cg = boost::dynamic_pointer_cast<HybridConditional>(factor)) {
+        error += cg->asGaussian()->error(continuousValues);
+      }
+    }
+  }
+  return error;
+}
+
+/* ************************************************************************ */
+double HybridGaussianFactorGraph::probPrime(
+    const VectorValues &continuousValues,
+    const DiscreteValues &discreteValues) const {
+  double error = this->error(continuousValues, discreteValues);
+  // NOTE: The 0.5 term is handled by each factor
+  return std::exp(-error);
+}
+
 /* ************************************************************************ */
 AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime(
     const VectorValues &continuousValues) const {
   AlgebraicDecisionTree<Key> error_tree = this->error(continuousValues);
-  AlgebraicDecisionTree<Key> prob_tree =
-      error_tree.apply([](double error) { return exp(-error); });
+  AlgebraicDecisionTree<Key> prob_tree = error_tree.apply([](double error) {
+    // NOTE: The 0.5 term is handled by each factor
+    return exp(-error);
+  });
   return prob_tree;
 }
 
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index ac9ae1a46..9de18b6af 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -182,6 +182,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
    */
   AlgebraicDecisionTree<Key> error(const VectorValues& continuousValues) const;
 
+  /**
+   * @brief Compute error given a continuous vector values
+   * and a discrete assignment.
+   *
+   * @param continuousValues The continuous VectorValues
+   * for computing the error.
+   * @param discreteValues The specific discrete assignment
+   * whose error we wish to compute.
+   * @return double
+   */
+  double error(const VectorValues& continuousValues,
+               const DiscreteValues& discreteValues) const;
+
   /**
    * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$
    * for each discrete assignment, and return as a tree.
@@ -193,6 +206,18 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   AlgebraicDecisionTree<Key> probPrime(
       const VectorValues& continuousValues) const;
 
+  /**
+   * @brief Compute the unnormalized posterior probability for a continuous
+   * vector values given a specific assignment.
+   *
+   * @param continuousValues The vector values for which to compute the
+   * posterior probability.
+   * @param discreteValues The specific assignment to use for the computation.
+   * @return double
+   */
+  double probPrime(const VectorValues& continuousValues,
+                   const DiscreteValues& discreteValues) const;
+
   /**
    * @brief Return a Colamd constrained ordering where the discrete keys are
    * eliminated after the continuous keys.
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 29247cdc3..3dbf5d542 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -180,6 +180,12 @@ class HybridGaussianFactorGraph {
   void print(string s = "") const;
   bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const;
 
+  // evaluation
+  double error(const gtsam::VectorValues& continuousValues,
+               const gtsam::DiscreteValues& discreteValues) const;
+  double probPrime(const gtsam::VectorValues& continuousValues,
+                   const gtsam::DiscreteValues& discreteValues) const;
+
   gtsam::HybridBayesNet* eliminateSequential();
   gtsam::HybridBayesNet* eliminateSequential(
       gtsam::Ordering::OrderingType type);
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 37243b937..2ebc87971 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -11,6 +11,7 @@ Author: Fan Jiang
 # pylint: disable=invalid-name, no-name-in-module, no-member
 
 import unittest
+import math
 
 import numpy as np
 from gtsam.symbol_shorthand import C, M, X, Z
@@ -110,7 +111,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
             conditional1 = GaussianConditional.FromMeanAndStddev(
                 Z(i), I, X(0), [0], sigma=3
             )
-            bayesNet.emplaceMixture([Z(i)], [X(0)], keys, [conditional0, conditional1])
+            bayesNet.emplaceMixture(
+                [Z(i)], [X(0)], keys, [conditional0, conditional1]
+            )
 
         # Create prior on X(0).
         prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0)
@@ -136,7 +139,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         fg.push_back(factor)
         fg.push_back(bayesNet.atGaussian(1))
         fg.push_back(bayesNet.atDiscrete(2))
-        
+
         self.assertEqual(fg.size(), 3)
 
     def test_tiny2(self):
@@ -156,8 +159,18 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
             fg.push_back(factor)
         fg.push_back(bayesNet.atGaussian(2))
         fg.push_back(bayesNet.atDiscrete(3))
-        
+
         self.assertEqual(fg.size(), 4)
+        # Calculate ratio  between Bayes net probability and the factor graph:
+        continuousValues = gtsam.VectorValues()
+        continuousValues.insert(X(0), sample.at(X(0)))
+        discreteValues = sample.discrete()
+        expected_ratio = bayesNet.evaluate(sample) / fg.probPrime(
+            continuousValues, discreteValues
+        )
+        print(expected_ratio)
+
+        # TODO(dellaert): Change the mode to 0 and calculate the ratio again.
 
 
 if __name__ == "__main__":