diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 9df0012c7..b4441f15a 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -195,41 +195,13 @@ HybridValues HybridBayesNet::sample() const { } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::logDiscretePosteriorPrime( +AlgebraicDecisionTree HybridBayesNet::errorTree( const VectorValues &continuousValues) const { AlgebraicDecisionTree result(0.0); - // Get logProbability function for a conditional or arbitrarily small - // logProbability if the conditional was pruned out. - auto probFunc = [continuousValues]( - const GaussianConditional::shared_ptr &conditional) { - return conditional ? conditional->logProbability(continuousValues) : -1e20; - }; - // Iterate over each conditional. for (auto &&conditional : *this) { - if (auto gm = conditional->asHybrid()) { - // If conditional is hybrid, select based on assignment and compute - // logProbability. - result = result + DecisionTree(gm->conditionals(), probFunc); - } else if (auto gc = conditional->asGaussian()) { - // If continuous, get the logProbability and add it to the result - double logProbability = gc->logProbability(continuousValues); - // Add the computed logProbability to every leaf of the tree. - result = result.apply([logProbability](double leaf_value) { - return leaf_value + logProbability; - }); - } else if (auto dc = conditional->asDiscrete()) { - // If discrete, add the discrete logProbability in the right branch - if (result.nrLeaves() == 1) { - result = dc->errorTree().apply([](double error) { return -error; }); - } else { - result = result.apply([dc](const Assignment &assignment, - double leaf_value) { - return leaf_value + dc->logProbability(DiscreteValues(assignment)); - }); - } - } + result = result + conditional->errorTree(continuousValues); } return result; @@ -238,10 +210,9 @@ AlgebraicDecisionTree HybridBayesNet::logDiscretePosteriorPrime( /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::discretePosterior( const VectorValues &continuousValues) const { - AlgebraicDecisionTree log_p = - this->logDiscretePosteriorPrime(continuousValues); + AlgebraicDecisionTree errors = this->errorTree(continuousValues); AlgebraicDecisionTree p = - log_p.apply([](double log) { return exp(log); }); + errors.apply([](double error) { return exp(-error); }); return p / p.sum(); } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 94a0762de..a997174ec 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -217,8 +217,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using Base::error; /** - * @brief Compute the log posterior log P'(M|x) of all assignments up to a - * constant, returning the result as an algebraic decision tree. + * @brief Compute the negative log posterior log P'(M|x) of all assignments up + * to a constant, returning the result as an algebraic decision tree. * * @note The joint P(X,M) is p(X|M) P(M) * Then the posterior on M given X=x is is P(M|x) = p(x|M) P(M) / p(x). @@ -229,7 +229,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values x at which to compute log P'(M|x) * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree logDiscretePosteriorPrime( + AlgebraicDecisionTree errorTree( const VectorValues &continuousValues) const; using BayesNet::logProbability; // expose HybridValues version diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index ea9ca8285..521bca4a7 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -95,18 +95,16 @@ TEST(HybridBayesNet, EvaluatePureDiscrete) { EXPECT_DOUBLES_EQUAL(-log(0.4), bayesNet.error(zero), 1e-9); EXPECT_DOUBLES_EQUAL(-log(0.6), bayesNet.error(one), 1e-9); - // logDiscretePosteriorPrime, TODO: useless as -errorTree? - AlgebraicDecisionTree expected({Asia}, - std::vector{log(0.4), log(0.6)}); - EXPECT(assert_equal(expected, bayesNet.logDiscretePosteriorPrime({}))); + // errorTree + AlgebraicDecisionTree expected(asiaKey, -log(0.4), -log(0.6)); + EXPECT(assert_equal(expected, bayesNet.errorTree({}))); // logProbability EXPECT_DOUBLES_EQUAL(log(0.4), bayesNet.logProbability(zero), 1e-9); EXPECT_DOUBLES_EQUAL(log(0.6), bayesNet.logProbability(one), 1e-9); // discretePosterior - AlgebraicDecisionTree expectedPosterior({Asia}, - std::vector{0.4, 0.6}); + AlgebraicDecisionTree expectedPosterior(asiaKey, 0.4, 0.6); EXPECT(assert_equal(expectedPosterior, bayesNet.discretePosterior({}))); // toFactorGraph @@ -169,15 +167,21 @@ TEST(HybridBayesNet, Tiny) { px->negLogConstant() - log(0.4); const double error1 = chosen1.error(vv) + gc1->negLogConstant() - px->negLogConstant() - log(0.6); + // print errors: + std::cout << "error0 = " << error0 << std::endl; + std::cout << "error1 = " << error1 << std::endl; EXPECT_DOUBLES_EQUAL(error0, bayesNet.error(zero), 1e-9); EXPECT_DOUBLES_EQUAL(error1, bayesNet.error(one), 1e-9); EXPECT_DOUBLES_EQUAL(error0 + logP0, error1 + logP1, 1e-9); - // logDiscretePosteriorPrime, TODO: useless as -errorTree? - AlgebraicDecisionTree expected(M(0), logP0, logP1); - EXPECT(assert_equal(expected, bayesNet.logDiscretePosteriorPrime(vv))); + // errorTree + AlgebraicDecisionTree expected(M(0), error0, error1); + EXPECT(assert_equal(expected, bayesNet.errorTree(vv))); // discretePosterior + // We have: P(z|x,mode)P(x)P(mode). When we condition on z and x, we get + // P(mode|z,x) \propto P(z|x,mode)P(x)P(mode) + // Normalizing this yields posterior P(mode|z,x) = {0.8, 0.2} double q0 = std::exp(logP0), q1 = std::exp(logP1), sum = q0 + q1; AlgebraicDecisionTree expectedPosterior(M(0), q0 / sum, q1 / sum); EXPECT(assert_equal(expectedPosterior, bayesNet.discretePosterior(vv))); @@ -191,6 +195,19 @@ TEST(HybridBayesNet, Tiny) { ratio[0] = std::exp(-fg.error(zero)) / bayesNet.evaluate(zero); ratio[1] = std::exp(-fg.error(one)) / bayesNet.evaluate(one); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); + + // TODO(Frank): Better test: check if discretePosteriors agree ! + // Since ϕ(M, x) \propto P(M,x|z) + // q0 = std::exp(-fg.error(zero)); + // q1 = std::exp(-fg.error(one)); + // sum = q0 + q1; + // AlgebraicDecisionTree fgPosterior(M(0), q0 / sum, q1 / sum); + VectorValues xv{{X(0), Vector1(5.0)}}; + fg.printErrors(zero); + fg.printErrors(one); + GTSAM_PRINT(fg.errorTree(xv)); + auto fgPosterior = fg.discretePosterior(xv); + EXPECT(assert_equal(expectedPosterior, fgPosterior)); } /* ****************************************************************************/ @@ -556,8 +573,8 @@ TEST(HybridBayesNet, ErrorTreeWithConditional) { AlgebraicDecisionTree errorTree = gfg.errorTree(vv); // regression - AlgebraicDecisionTree expected(m1, 59.335390372, 5050.125); - EXPECT(assert_equal(expected, errorTree, 1e-9)); + AlgebraicDecisionTree expected(m1, 60.028538, 5050.8181); + EXPECT(assert_equal(expected, errorTree, 1e-4)); } /* ************************************************************************* */