diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 12b56ece8..6024add07 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/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 VectorValues& continuosValues = values.continuous(); + const DiscreteValues &discreteValues = values.discrete(); + 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. - // GaussianMixture::shared_ptr gm = conditional->asMixture(); - // AlgebraicDecisionTree conditional_error = - // gm->error(continuousValues); - - // error_tree = error_tree + conditional_error; - + // If conditional is hybrid, select based on assignment and evaluate. + const GaussianMixture::shared_ptr gm = conditional->asMixture(); + const auto conditional = (*gm)(discreteValues); + probability *= conditional->evaluate(continuousValues); } else if (conditional->isContinuous()) { - // If continuous only, get the (double) error - // and add it to the error_tree - // 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; }); + // If continuous only, evaluate the probability and multiply. + probability *= conditional->asGaussian()->evaluate(continuousValues); } else if (conditional->isDiscrete()) { - // Conditional is discrete-only, we skip. + // Conditional is discrete-only, so return its probability. probability *= conditional->asDiscreteConditional()->operator()(discreteValues); } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index ff13ca1b7..4e41cb11d 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -95,10 +95,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ 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); } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 8f9632ba3..f90152abe 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/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); - Matrix22 R0 = Matrix22::Ones(); - const auto conditional0 = - boost::make_shared(X(1), d0, R0, model); - - const Vector2 d1(2, 1); - Matrix22 R1 = Matrix22::Ones(); - const auto conditional1 = - boost::make_shared(X(1), d1, R1, model); + const auto conditional0 = boost::make_shared( + X(1), Vector1::Constant(5), I_1x1, model0), + conditional1 = boost::make_shared( + X(1), Vector1::Constant(2), I_1x1, model1); // TODO(dellaert): creating and adding mixture is clumsy. - std::vector conditionals{conditional0, - conditional1}; - const auto mixture = - GaussianMixture::FromConditionals({X(1)}, {}, {Asia}, conditionals); + const auto mixture = GaussianMixture::FromConditionals( + {X(1)}, {}, {Asia}, {conditional0, conditional1}); + + // Create hybrid Bayes net. + HybridBayesNet bayesNet; + bayesNet.push_back(HybridConditional( + boost::make_shared(continuousConditional))); bayesNet.push_back( HybridConditional(boost::make_shared(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()); - EXPECT_DOUBLES_EQUAL(conditionalProbability * 0.99, bayesNet.evaluate(values), 1e-9); + continuousConditional.evaluate(values.continuous()); + 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(X(1), d1, R1, X(2), S1, model); - - /* ****************************************************************************/ // Test choosing an assignment of conditionals TEST(HybridBayesNet, Choose) {