diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 4404ccfdc8..e01fcbdcf2 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 dcdf3a8e5c..de0a38271c 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 021ca13614..6c2f4a65bd 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 ba04263f87..a427d20429 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -33,46 +33,34 @@ const DiscreteKey mode{M(0), 2}; /** * Create a tiny two variable hybrid model which represents * the generative probability P(z,x,mode) = P(z|x,mode)P(x)P(mode). + * num_measurements is the number of measurements of the continuous variable x0. + * If manyModes is true, then we introduce one mode per measurement. */ -inline HybridBayesNet createHybridBayesNet(int num_measurements = 1) { +inline HybridBayesNet createHybridBayesNet(int num_measurements = 1, + bool manyModes = false) { HybridBayesNet bayesNet; // Create Gaussian mixture z_i = x0 + noise for each measurement. for (int i = 0; i < num_measurements; i++) { - const auto conditional0 = boost::make_shared( - GaussianConditional::FromMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5)); - const auto conditional1 = boost::make_shared( - GaussianConditional::FromMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)); - GaussianMixture gm({Z(i)}, {X(0)}, {mode}, {conditional0, conditional1}); + const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; + GaussianMixture gm({Z(i)}, {X(0)}, {mode_i}, + {GaussianConditional::sharedMeanAndStddev( + Z(i), I_1x1, X(0), Z_1x1, 0.5), + GaussianConditional::sharedMeanAndStddev( + Z(i), I_1x1, X(0), Z_1x1, 3)}); bayesNet.emplaceMixture(gm); // copy :-( } // Create prior on X(0). - const auto prior_on_x0 = - GaussianConditional::FromMeanAndStddev(X(0), Vector1(5.0), 0.5); - bayesNet.emplaceGaussian(prior_on_x0); // copy :-( + bayesNet.addGaussian( + GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5)); // Add prior on mode. - bayesNet.emplaceDiscrete(mode, "4/6"); - - return bayesNet; -} - -/** - * Convert a hybrid Bayes net to a hybrid Gaussian factor graph. - */ -inline HybridGaussianFactorGraph convertBayesNet( - const HybridBayesNet& bayesNet, const VectorValues& measurements) { - HybridGaussianFactorGraph fg; - int num_measurements = bayesNet.size() - 2; - for (int i = 0; i < num_measurements; i++) { - auto conditional = bayesNet.atMixture(i); - auto factor = conditional->likelihood({{Z(i), measurements.at(Z(i))}}); - fg.push_back(factor); + const size_t nrModes = manyModes ? num_measurements : 1; + for (int i = 0; i < nrModes; i++) { + bayesNet.emplaceDiscrete(DiscreteKey{M(i), 2}, "4/6"); } - fg.push_back(bayesNet.atGaussian(num_measurements)); - fg.push_back(bayesNet.atDiscrete(num_measurements + 1)); - return fg; + return bayesNet; } /** @@ -83,12 +71,15 @@ inline HybridGaussianFactorGraph convertBayesNet( */ inline HybridGaussianFactorGraph createHybridGaussianFactorGraph( int num_measurements = 1, - boost::optional measurements = boost::none) { - auto bayesNet = createHybridBayesNet(num_measurements); + boost::optional measurements = boost::none, + bool manyModes = false) { + auto bayesNet = createHybridBayesNet(num_measurements, manyModes); if (measurements) { - return convertBayesNet(bayesNet, *measurements); + // Use the measurements to create a hybrid factor graph. + return bayesNet.toFactorGraph(*measurements); } else { - return convertBayesNet(bayesNet, bayesNet.sample().continuous()); + // Sample from the generative model to create a hybrid factor graph. + return bayesNet.toFactorGraph(bayesNet.sample().continuous()); } } diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index fa371cf163..cc45718751 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -54,8 +54,10 @@ using namespace gtsam; using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::M; +using gtsam::symbol_shorthand::N; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; +using gtsam::symbol_shorthand::Z; /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, Creation) { @@ -624,30 +626,36 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { num_measurements, VectorValues{{Z(0), Vector1(5.0)}}); EXPECT_LONGS_EQUAL(3, fg.size()); - auto sum = fg.assembleGraphTree(); + // Assemble graph tree: + auto actual = fg.assembleGraphTree(); + + // Create expected decision tree with two factor graphs: // Get mixture factor: auto mixture = boost::dynamic_pointer_cast(fg.at(0)); - using GF = GaussianFactor::shared_ptr; + CHECK(mixture); // Get prior factor: - const GF prior = - boost::dynamic_pointer_cast(fg.at(1))->inner(); + const auto gf = boost::dynamic_pointer_cast(fg.at(1)); + CHECK(gf); + using GF = GaussianFactor::shared_ptr; + const GF prior = gf->asGaussian(); + CHECK(prior); // Create DiscreteValues for both 0 and 1: DiscreteValues d0{{M(0), 0}}, d1{{M(0), 1}}; // Expected decision tree with two factor graphs: // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) - GaussianFactorGraphTree expectedSum{ + GaussianFactorGraphTree expected{ M(0), {GaussianFactorGraph(std::vector{mixture->factor(d0), prior}), mixture->constant(d0)}, {GaussianFactorGraph(std::vector{mixture->factor(d1), prior}), mixture->constant(d1)}}; - EXPECT(assert_equal(expectedSum(d0), sum(d0), 1e-5)); - EXPECT(assert_equal(expectedSum(d1), sum(d1), 1e-5)); + EXPECT(assert_equal(expected(d0), actual(d0), 1e-5)); + EXPECT(assert_equal(expected(d1), actual(d1), 1e-5)); } /* ****************************************************************************/ @@ -657,6 +665,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { const int num_measurements = 1; auto fg = tiny::createHybridGaussianFactorGraph( num_measurements, VectorValues{{Z(0), Vector1(5.0)}}); + EXPECT_LONGS_EQUAL(3, fg.size()); // Create expected Bayes Net: HybridBayesNet expectedBayesNet; @@ -691,6 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { auto fg = tiny::createHybridGaussianFactorGraph( num_measurements, VectorValues{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}}); + EXPECT_LONGS_EQUAL(4, fg.size()); // Create expected Bayes Net: HybridBayesNet expectedBayesNet; @@ -716,6 +726,152 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01)); } +/* ****************************************************************************/ +// Test eliminating tiny net with 1 mode per measurement. +TEST(HybridGaussianFactorGraph, EliminateTiny22) { + // Create factor graph with 2 measurements such that posterior mean = 5.0. + using symbol_shorthand::Z; + const int num_measurements = 2; + const bool manyModes = true; + + // Create Bayes net and convert to factor graph. + auto bn = tiny::createHybridBayesNet(num_measurements, manyModes); + const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}}; + auto fg = bn.toFactorGraph(measurements); + EXPECT_LONGS_EQUAL(5, fg.size()); + + // Test elimination + Ordering ordering; + ordering.push_back(X(0)); + ordering.push_back(M(0)); + ordering.push_back(M(1)); + const auto posterior = fg.eliminateSequential(ordering); + + // Compute the log-ratio between the Bayes net and the factor graph. + auto compute_ratio = [&](HybridValues *sample) -> double { + // update sample with given measurements: + sample->update(measurements); + return bn.evaluate(*sample) / posterior->evaluate(*sample); + }; + + // Set up sampling + std::mt19937_64 rng(42); + + // The error evaluated by the factor graph and the Bayes net should differ by + // the normalizing term computed via the Bayes net determinant. + HybridValues sample = bn.sample(&rng); + double expected_ratio = compute_ratio(&sample); + // regression + EXPECT_DOUBLES_EQUAL(0.018253037966018862, expected_ratio, 1e-6); + + // 3. Do sampling + constexpr int num_samples = 100; + for (size_t i = 0; i < num_samples; i++) { + // Sample from the bayes net + HybridValues sample = bn.sample(&rng); + + // Check that the ratio is constant. + EXPECT_DOUBLES_EQUAL(expected_ratio, compute_ratio(&sample), 1e-6); + } +} + +/* ****************************************************************************/ +// Test elimination of a switching network with one mode per measurement. +TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { + // Create a switching network with one mode per measurement. + HybridBayesNet bn; + + // NOTE: we add reverse topological so we can sample from the Bayes net.: + + // Add measurements: + for (size_t t : {0, 1, 2}) { + // Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t): + const auto noise_mode_t = DiscreteKey{N(t), 2}; + GaussianMixture gm({Z(t)}, {X(t)}, {noise_mode_t}, + {GaussianConditional::sharedMeanAndStddev( + Z(t), I_1x1, X(t), Z_1x1, 0.5), + GaussianConditional::sharedMeanAndStddev( + Z(t), I_1x1, X(t), Z_1x1, 3.0)}); + bn.emplaceMixture(gm); // copy :-( + + // Create prior on discrete mode M(t): + bn.emplaceDiscrete(noise_mode_t, "20/80"); + } + + // Add motion models: + for (size_t t : {2, 1}) { + // Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1): + const auto motion_model_t = DiscreteKey{M(t), 2}; + GaussianMixture gm({X(t)}, {X(t - 1)}, {motion_model_t}, + {GaussianConditional::sharedMeanAndStddev( + X(t), I_1x1, X(t - 1), Z_1x1, 0.2), + GaussianConditional::sharedMeanAndStddev( + X(t), I_1x1, X(t - 1), I_1x1, 0.2)}); + bn.emplaceMixture(gm); // copy :-( + + // Create prior on motion model M(t): + bn.emplaceDiscrete(motion_model_t, "40/60"); + } + + // Create Gaussian prior on continuous X(0) using sharedMeanAndStddev: + bn.addGaussian(GaussianConditional::sharedMeanAndStddev(X(0), Z_1x1, 0.1)); + + // Make sure we an sample from the Bayes net: + EXPECT_LONGS_EQUAL(6, bn.sample().continuous().size()); + + // Create measurements consistent with moving right every time: + const VectorValues measurements{ + {Z(0), Vector1(0.0)}, {Z(1), Vector1(1.0)}, {Z(2), Vector1(2.0)}}; + const auto fg = bn.toFactorGraph(measurements); + + // Create ordering that eliminates in time order, then discrete modes: + Ordering ordering; + ordering.push_back(X(2)); + ordering.push_back(X(1)); + ordering.push_back(X(0)); + ordering.push_back(N(0)); + ordering.push_back(N(1)); + ordering.push_back(N(2)); + ordering.push_back(M(1)); + ordering.push_back(M(2)); + + // Test elimination result has correct size: + const auto posterior = fg.eliminateSequential(ordering); + // GTSAM_PRINT(*posterior); + + // Test elimination result has correct size: + EXPECT_LONGS_EQUAL(8, posterior->size()); + + // TODO(dellaert): below is copy/pasta from above, refactor + + // Compute the log-ratio between the Bayes net and the factor graph. + auto compute_ratio = [&](HybridValues *sample) -> double { + // update sample with given measurements: + sample->update(measurements); + return bn.evaluate(*sample) / posterior->evaluate(*sample); + }; + + // Set up sampling + std::mt19937_64 rng(42); + + // The error evaluated by the factor graph and the Bayes net should differ by + // the normalizing term computed via the Bayes net determinant. + HybridValues sample = bn.sample(&rng); + double expected_ratio = compute_ratio(&sample); + // regression + EXPECT_DOUBLES_EQUAL(0.0094526745785019472, expected_ratio, 1e-6); + + // 3. Do sampling + constexpr int num_samples = 100; + for (size_t i = 0; i < num_samples; i++) { + // Sample from the bayes net + HybridValues sample = bn.sample(&rng); + + // Check that the ratio is constant. + EXPECT_DOUBLES_EQUAL(expected_ratio, compute_ratio(&sample), 1e-6); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 39a21a6172..9fd217abf8 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++) diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d25efb2e1a..05b8b86b88 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -100,6 +100,12 @@ namespace gtsam { const Matrix& A2, Key parent2, const Vector& b, double sigma); + /// Create shared pointer by forwarding arguments to fromMeanAndStddev. + template + static shared_ptr sharedMeanAndStddev(Args&&... args) { + return boost::make_shared(FromMeanAndStddev(std::forward(args)...)); + } + /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by * \c first and \c last must be in increasing order, meaning that the parents of any * conditional may not include a conditional coming before it.