Evaluate for hybrid Bayes nets

gtsam/hybrid/HybridBayesNet.cpp

@@ -235,30 +235,23 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
 
 /* ************************************************************************* */
 double HybridBayesNet::evaluate(const HybridValues &values) const {
-  const DiscreteValues& discreteValues = values.discrete();
+  const DiscreteValues &discreteValues = values.discrete();
-  const VectorValues& continuosValues = values.continuous();
+  const VectorValues &continuousValues = values.continuous();
 
   double probability = 1.0;
 
   // Iterate over each conditional.
   for (auto &&conditional : *this) {
     if (conditional->isHybrid()) {
-      // If conditional is hybrid, select based on assignment and compute error.
+      // If conditional is hybrid, select based on assignment and evaluate.
-      // GaussianMixture::shared_ptr gm = conditional->asMixture();
+      const GaussianMixture::shared_ptr gm = conditional->asMixture();
-      // AlgebraicDecisionTree<Key> conditional_error =
+      const auto conditional = (*gm)(discreteValues);
-      //     gm->error(continuousValues);
+      probability *= conditional->evaluate(continuousValues);
-
-      // error_tree = error_tree + conditional_error;
-
     } else if (conditional->isContinuous()) {
-      // If continuous only, get the (double) error
+      // If continuous only, evaluate the probability and multiply.
-      // and add it to the error_tree
+      probability *= conditional->asGaussian()->evaluate(continuousValues);
-      // double error = conditional->asGaussian()->error(continuousValues);
-      // // Add the computed error to every leaf of the error tree.
-      // error_tree = error_tree.apply(
-      //     [error](double leaf_value) { return leaf_value + error; });
     } else if (conditional->isDiscrete()) {
-      // Conditional is discrete-only, we skip.
+      // Conditional is discrete-only, so return its probability.
       probability *=
           conditional->asDiscreteConditional()->operator()(discreteValues);
     }

gtsam/hybrid/HybridBayesNet.h

@@ -95,10 +95,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
    */
   GaussianBayesNet choose(const DiscreteValues &assignment) const;
 
-  //** evaluate for given HybridValues */
+  /// Evaluate hybrid probability density for given HybridValues.
   double evaluate(const HybridValues &values) const;
 
-  //** (Preferred) sugar for the above for given DiscreteValues */
+  /// Evaluate hybrid probability density for given HybridValues, sugar.
   double operator()(const HybridValues &values) const {
     return evaluate(values);
   }

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -72,51 +72,44 @@ TEST(HybridBayesNet, evaluatePureDiscrete) {
 }
 
 /* ****************************************************************************/
-// Test evaluate for a hybrid Bayes net P(X1|Asia) P(Asia).
+// Test evaluate for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia).
 TEST(HybridBayesNet, evaluateHybrid) {
-  HybridBayesNet bayesNet;
+  const auto continuousConditional = GaussianConditional::FromMeanAndStddev(
+      X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0);
 
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
+  const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)),
+                       model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0));
 
-  const Vector2 d0(1, 2);
+  const auto conditional0 = boost::make_shared<GaussianConditional>(
-  Matrix22 R0 = Matrix22::Ones();
+                 X(1), Vector1::Constant(5), I_1x1, model0),
-  const auto conditional0 =
+             conditional1 = boost::make_shared<GaussianConditional>(
-      boost::make_shared<GaussianConditional>(X(1), d0, R0, model);
+                 X(1), Vector1::Constant(2), I_1x1, model1);
-
-  const Vector2 d1(2, 1);
-  Matrix22 R1 = Matrix22::Ones();
-  const auto conditional1 =
-      boost::make_shared<GaussianConditional>(X(1), d1, R1, model);
 
   // TODO(dellaert): creating and adding mixture is clumsy.
-  std::vector<GaussianConditional::shared_ptr> conditionals{conditional0,
+  const auto mixture = GaussianMixture::FromConditionals(
-                                                            conditional1};
+      {X(1)}, {}, {Asia}, {conditional0, conditional1});
-  const auto mixture =
+
-      GaussianMixture::FromConditionals({X(1)}, {}, {Asia}, conditionals);  
+  // Create hybrid Bayes net.
+  HybridBayesNet bayesNet;
+  bayesNet.push_back(HybridConditional(
+      boost::make_shared<GaussianConditional>(continuousConditional)));
   bayesNet.push_back(
       HybridConditional(boost::make_shared<GaussianMixture>(mixture)));
-
-  // Add component probabilities.
   bayesNet.add(Asia, "99/1");
 
   // Create values at which to evaluate.
   HybridValues values;
   values.insert(asiaKey, 0);
-  values.insert(X(1), Vector2(1, 2));
+  values.insert(X(0), Vector1(-6));
+  values.insert(X(1), Vector1(1));
 
-  // TODO(dellaert): we need real probabilities!
   const double conditionalProbability =
-      conditional0->error(values.continuous());
+      continuousConditional.evaluate(values.continuous());
-  EXPECT_DOUBLES_EQUAL(conditionalProbability * 0.99, bayesNet.evaluate(values), 1e-9);
+  const double mixtureProbability = conditional0->evaluate(values.continuous());
+  EXPECT_DOUBLES_EQUAL(conditionalProbability * mixtureProbability * 0.99,
+                       bayesNet.evaluate(values), 1e-9);
 }
 
-  // const Vector2 d1(2, 1);
-  // Matrix22 R1 = Matrix22::Ones();
-  // Matrix22 S1 = Matrix22::Identity() * 2;
-  // const auto conditional1 =
-  //     boost::make_shared<GaussianConditional>(X(1), d1, R1, X(2), S1, model);
-
-
 /* ****************************************************************************/
 // Test choosing an assignment of conditionals
 TEST(HybridBayesNet, Choose) {