diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 4404ccfdc..e01fcbdcf 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -377,4 +377,27 @@ AlgebraicDecisionTree HybridBayesNet::probPrime( return error_tree.apply([](double error) { return exp(-error); }); } +/* ************************************************************************* */ +HybridGaussianFactorGraph HybridBayesNet::toFactorGraph( + const VectorValues &measurements) const { + HybridGaussianFactorGraph fg; + + // For all nodes in the Bayes net, if its frontal variable is in measurements, + // replace it by a likelihood factor: + for (auto &&conditional : *this) { + if (conditional->frontalsIn(measurements)) { + if (auto gc = conditional->asGaussian()) + fg.push_back(gc->likelihood(measurements)); + else if (auto gm = conditional->asMixture()) + fg.push_back(gm->likelihood(measurements)); + else { + throw std::runtime_error("Unknown conditional type"); + } + } else { + fg.push_back(conditional); + } + } + return fg; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index dcdf3a8e5..de0a38271 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -229,6 +229,12 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { AlgebraicDecisionTree probPrime( const VectorValues &continuousValues) const; + /** + * Convert a hybrid Bayes net to a hybrid Gaussian factor graph by converting + * all conditionals with instantiated measurements into likelihood factors. + */ + HybridGaussianFactorGraph toFactorGraph( + const VectorValues &measurements) const; /// @} private: diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 021ca1361..6c2f4a65b 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -178,6 +178,16 @@ class GTSAM_EXPORT HybridConditional /// Return the error of the underlying conditional. double error(const HybridValues& values) const override; + /// Check if VectorValues `measurements` contains all frontal keys. + bool frontalsIn(const VectorValues& measurements) const { + for (Key key : frontals()) { + if (!measurements.exists(key)) { + return false; + } + } + return true; + } + /// @} private: diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index 3ff9bec85..2e1a66a8e 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -37,7 +37,7 @@ const DiscreteKey mode{M(0), 2}; * If manyModes is true, then we introduce one mode per measurement. */ inline HybridBayesNet createHybridBayesNet(int numMeasurements = 1, - bool manyModes = false) { + bool manyModes = false) { HybridBayesNet bayesNet; // Create Gaussian mixture z_i = x0 + noise for each measurement. @@ -64,30 +64,6 @@ inline HybridBayesNet createHybridBayesNet(int numMeasurements = 1, return bayesNet; } -/** - * Convert a hybrid Bayes net to a hybrid Gaussian factor graph. - */ -inline HybridGaussianFactorGraph convertBayesNet( - const HybridBayesNet& bayesNet, const VectorValues& measurements) { - HybridGaussianFactorGraph fg; - // For all nodes in the Bayes net, if its frontal variable is in measurements, - // replace it by a likelihood factor: - for (const HybridConditional::shared_ptr& conditional : bayesNet) { - if (measurements.exists(conditional->firstFrontalKey())) { - if (auto gc = conditional->asGaussian()) - fg.push_back(gc->likelihood(measurements)); - else if (auto gm = conditional->asMixture()) - fg.push_back(gm->likelihood(measurements)); - else { - throw std::runtime_error("Unknown conditional type"); - } - } else { - fg.push_back(conditional); - } - } - return fg; -} - /** * Create a tiny two variable hybrid factor graph which represents a discrete * mode and a continuous variable x0, given a number of measurements of the @@ -101,10 +77,10 @@ inline HybridGaussianFactorGraph createHybridGaussianFactorGraph( auto bayesNet = createHybridBayesNet(numMeasurements, manyModes); if (measurements) { // Use the measurements to create a hybrid factor graph. - return convertBayesNet(bayesNet, *measurements); + return bayesNet.toFactorGraph(*measurements); } else { // Sample from the generative model to create a hybrid factor graph. - return convertBayesNet(bayesNet, bayesNet.sample().continuous()); + return bayesNet.toFactorGraph(bayesNet.sample().continuous()); } } diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 6697e5084..bfaf6d264 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -735,7 +735,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny22) { // Create Bayes net and convert to factor graph. auto bn = tiny::createHybridBayesNet(numMeasurements, manyModes); const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}}; - auto fg = tiny::convertBayesNet(bn, measurements); + auto fg = bn.toFactorGraph(measurements); EXPECT_LONGS_EQUAL(5, fg.size()); // Test elimination diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 39a21a617..9fd217abf 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -263,7 +263,7 @@ double GaussianConditional::evaluate(const VectorValues& x) const { Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); frontalVec = R().transpose().triangularView().solve(frontalVec); - // Check for indeterminant solution + // Check for indeterminate solution if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++)