Merge pull request #1356 from borglab/hybrid/elimination

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>
 
@@ -36,20 +37,17 @@ GaussianMixture::GaussianMixture(
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() const {
   return conditionals_;
 }
 
 /* *******************************************************************************/
-GaussianMixture GaussianMixture::FromConditionals(
+GaussianMixture::GaussianMixture(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
-    const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
+    const std::vector<GaussianConditional::shared_ptr> &conditionalsList)
-  Conditionals dt(discreteParents, conditionalsList);
+    : GaussianMixture(continuousFrontals, continuousParents, discreteParents,
-
+                      Conditionals(discreteParents, conditionalsList)) {}
-  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
-                         dt);
-}
 
 /* *******************************************************************************/
 GaussianMixture::Sum GaussianMixture::add(
@@ -128,6 +126,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;

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
@@ -112,21 +114,11 @@ 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);
 
-  /// @}
-  /// @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,9 +132,25 @@ 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 Conditionals &conditionals() const;
 
   /**
    * @brief Compute error of the GaussianMixture as a tree.
@@ -181,6 +189,7 @@ class GTSAM_EXPORT GaussianMixture
    * @return Sum
    */
   Sum add(const Sum &sum) const;
+  /// @}
 };
 
 /// Return the DiscreteKey vector as a set.

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -35,16 +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,
-GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
+  // but not if both are empty or both are not empty:
-    const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+  if (factors_.empty() ^ e->factors_.empty()) return false;
-    const std::vector<GaussianFactor::shared_ptr> &factors) {
-  Factors dt(discreteKeys, factors);
 
-  return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
+  // 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);
+                         });
 }
 
 /* *******************************************************************************/
@@ -52,18 +55,22 @@ void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   std::cout << "{\n";
-  factors_.print(
+  if (factors_.empty()) {
-      "", [&](Key k) { return formatter(k); },
+    std::cout << "  empty" << std::endl;
-      [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+  } else {
-        RedirectCout rd;
+    factors_.print(
-        std::cout << ":\n";
+        "", [&](Key k) { return formatter(k); },
-        if (gf && !gf->empty()) {
+        [&](const GaussianFactor::shared_ptr &gf) -> std::string {
-          gf->print("", formatter);
+          RedirectCout rd;
-          return rd.str();
+          std::cout << ":\n";
-        } else {
+          if (gf && !gf->empty()) {
-          return "nullptr";
+            gf->print("", formatter);
-        }
+            return rd.str();
-      });
+          } else {
+            return "nullptr";
+          }
+        });
+  }
   std::cout << "}" << std::endl;
 }
 

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
   /// @{

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)...)));
   }

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 =
+  AlgebraicDecisionTree<Key> prob_tree = error_tree.apply([](double error) {
-      error_tree.apply([](double error) { return exp(-error); });
+    // NOTE: The 0.5 term is handled by each factor
+    return exp(-error);
+  });
   return prob_tree;
 }
 

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.

gtsam/hybrid/hybrid.i

@@ -55,7 +55,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);
@@ -67,12 +67,13 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
 
 #include <gtsam/hybrid/GaussianMixture.h>
 class GaussianMixture : gtsam::HybridFactor {
-  static GaussianMixture FromConditionals(
+  GaussianMixture(const gtsam::KeyVector& continuousFrontals,
-      const gtsam::KeyVector& continuousFrontals,
+                  const gtsam::KeyVector& continuousParents,
-      const gtsam::KeyVector& continuousParents,
+                  const gtsam::DiscreteKeys& discreteParents,
-      const gtsam::DiscreteKeys& discreteParents,
+                  const std::vector<gtsam::GaussianConditional::shared_ptr>&
-      const std::vector<gtsam::GaussianConditional::shared_ptr>&
+                      conditionalsList);
-          conditionalsList);
+
+  gtsam::GaussianMixtureFactor* likelihood(const gtsam::VectorValues &frontals) const;
 
   void print(string s = "GaussianMixture\n",
              const gtsam::KeyFormatter& keyFormatter =
@@ -105,18 +106,32 @@ class HybridBayesTree {
                  gtsam::DefaultKeyFormatter) const;
 };
 
-#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
 class HybridBayesNet {
   HybridBayesNet();
   void add(const gtsam::HybridConditional& s);
-  void addMixture(const gtsam::GaussianMixture& s);
+  void addMixture(const gtsam::GaussianMixture* s);
-  void addGaussian(const gtsam::GaussianConditional& s);
+  void addGaussian(const gtsam::GaussianConditional* s);
-  void addDiscrete(const gtsam::DiscreteConditional& s);
+  void addDiscrete(const gtsam::DiscreteConditional* s);
-  void addDiscrete(const gtsam::DiscreteKey& key, string spec);
+
-  void addDiscrete(const gtsam::DiscreteKey& key,
+  void emplaceMixture(const gtsam::GaussianMixture& s);
-                   const gtsam::DiscreteKeys& parents, string spec);
+  void emplaceMixture(const gtsam::KeyVector& continuousFrontals,
-  void addDiscrete(const gtsam::DiscreteKey& key,
+                      const gtsam::KeyVector& continuousParents,
-                   const std::vector<gtsam::DiscreteKey>& parents, string spec);
+                      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);
+  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;
@@ -154,9 +169,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 push_back(gtsam::DecisionTreeFactor* factor);
-  void add(gtsam::DecisionTreeFactor* factor);
+  void push_back(gtsam::JacobianFactor* factor);
-  void add(gtsam::JacobianFactor* factor);
 
   bool empty() const;
   void remove(size_t i);
@@ -167,6 +181,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);

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),

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,59 @@ 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 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();
+  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));
+  return GaussianMixture({Z(0)}, {X(0)}, {mode}, {conditional0, conditional1});
+}
+
+/* ************************************************************************* */
+// Create a test for continuousParents.
+TEST(GaussianMixture, ContinuousParents) {
+  const GaussianMixture gm = createSimpleGaussianMixture();
+  const KeyVector continuousParentKeys = gm.continuousParents();
+  // Check that the continuous parent keys are correct:
+  EXPECT(continuousParentKeys.size() == 1);
+  EXPECT(continuousParentKeys[0] == X(0));
+}
+
+/* ************************************************************************* */
+/// 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));
+  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:
+  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));
 }
 
 /* ************************************************************************* */

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.emplaceGaussian(continuousConditional);
-  bayesNet.addMixture(GaussianMixture::FromConditionals(
+  GaussianMixture gm({X(1)}, {}, {Asia}, {conditional0, conditional1});
-      {X(1)}, {}, {Asia}, {conditional0, conditional1}));
+  bayesNet.emplaceMixture(gm);  // copy :-(
-  bayesNet.addDiscrete(Asia, "99/1");
+  bayesNet.emplaceDiscrete(Asia, "99/1");
 
   // Create values at which to evaluate.
   HybridValues values;

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())}));

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));
   }

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,

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,

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,
+        gm = GaussianMixture([X(1)], [], dkeys, [conditional0, conditional1]) 
-                                              [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()

python/gtsam/tests/test_HybridFactorGraph.py

@@ -11,75 +11,167 @@ 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, 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)
+        model = noiseModel.Unit.Create(3)
-        dk = gtsam.DiscreteKeys()
+        dk = DiscreteKeys()
         dk.push_back((C(0), 2))
 
-        jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)),
+        jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
-                                   noiseModel)
+        jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
-        jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)),
-                                   noiseModel)
 
-        gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
+        gmf = GaussianMixtureFactor([X(0)], dk, [jf1, jf2])
 
-        hfg = gtsam.HybridGaussianFactorGraph()
+        hfg = HybridGaussianFactorGraph()
-        hfg.add(jf1)
+        hfg.push_back(jf1)
-        hfg.add(jf2)
+        hfg.push_back(jf2)
         hfg.push_back(gmf)
 
         hbn = hfg.eliminateSequential(
-            gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
+            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
-                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)
+        model = noiseModel.Unit.Create(3)
-        dk = gtsam.DiscreteKeys()
+        dk = DiscreteKeys()
         dk.push_back((C(0), 2))
 
-        jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)),
+        jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
-                                   noiseModel)
+        jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
-        jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)),
-                                   noiseModel)
 
-        gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
+        gmf = GaussianMixtureFactor([X(0)], dk, [jf1, jf2])
 
-        hfg = gtsam.HybridGaussianFactorGraph()
+        hfg = HybridGaussianFactorGraph()
-        hfg.add(jf1)
+        hfg.push_back(jf1)
-        hfg.add(jf2)
+        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(
+            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
-                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)
+        # 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__":
     unittest.main()