From ffd1802ceaafd574517335bd34ddd525c8e1227b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Dec 2022 20:19:23 +0530 Subject: [PATCH 1/6] add optional model parameter to sample method --- gtsam/linear/GaussianBayesNet.cpp | 10 ++++++---- gtsam/linear/GaussianBayesNet.h | 6 ++++-- gtsam/linear/GaussianConditional.cpp | 22 +++++++++++++++------- gtsam/linear/GaussianConditional.h | 7 ++++--- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 6dcf662a90..8db301aa56 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -59,16 +59,18 @@ namespace gtsam { } /* ************************************************************************ */ - VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const { + VectorValues GaussianBayesNet::sample(std::mt19937_64* rng, + const SharedDiagonal& model) const { VectorValues result; // no missing variables -> create an empty vector - return sample(result, rng); + return sample(result, rng, model); } VectorValues GaussianBayesNet::sample(VectorValues result, - std::mt19937_64* rng) const { + std::mt19937_64* rng, + const SharedDiagonal& model) const { // sample each node in reverse topological sort order (parents first) for (auto cg : boost::adaptors::reverse(*this)) { - const VectorValues sampled = cg->sample(result, rng); + const VectorValues sampled = cg->sample(result, rng, model); result.insert(sampled); } return result; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 83328576f2..e6dae61260 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -101,7 +101,8 @@ namespace gtsam { * std::mt19937_64 rng(42); * auto sample = gbn.sample(&rng); */ - VectorValues sample(std::mt19937_64* rng) const; + VectorValues sample(std::mt19937_64* rng, + const SharedDiagonal& model = nullptr) const; /** * Sample from an incomplete BayesNet, given missing variables @@ -110,7 +111,8 @@ namespace gtsam { * VectorValues given = ...; * auto sample = gbn.sample(given, &rng); */ - VectorValues sample(VectorValues given, std::mt19937_64* rng) const; + VectorValues sample(VectorValues given, std::mt19937_64* rng, + const SharedDiagonal& model = nullptr) const; /// Sample using ancestral sampling, use default rng VectorValues sample() const; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 9515776416..1a6620d62a 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -279,30 +279,38 @@ namespace gtsam { /* ************************************************************************ */ VectorValues GaussianConditional::sample(const VectorValues& parentsValues, - std::mt19937_64* rng) const { + std::mt19937_64* rng, + const SharedDiagonal& model) const { if (nrFrontals() != 1) { throw std::invalid_argument( "GaussianConditional::sample can only be called on single variable " "conditionals"); } - if (!model_) { + + VectorValues solution = solve(parentsValues); + Key key = firstFrontalKey(); + + Vector sigmas; + if (model_) { + sigmas = model_->sigmas(); + } else if (model) { + sigmas = model->sigmas(); + } else { throw std::invalid_argument( "GaussianConditional::sample can only be called if a diagonal noise " "model was specified at construction."); } - VectorValues solution = solve(parentsValues); - Key key = firstFrontalKey(); - const Vector& sigmas = model_->sigmas(); solution[key] += Sampler::sampleDiagonal(sigmas, rng); return solution; } - VectorValues GaussianConditional::sample(std::mt19937_64* rng) const { + VectorValues GaussianConditional::sample(std::mt19937_64* rng, + const SharedDiagonal& model) const { if (nrParents() != 0) throw std::invalid_argument( "sample() can only be invoked on no-parent prior"); VectorValues values; - return sample(values); + return sample(values, rng, model); } /* ************************************************************************ */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 4822e3eaf7..ccf916cd7f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -166,7 +166,8 @@ namespace gtsam { * std::mt19937_64 rng(42); * auto sample = gbn.sample(&rng); */ - VectorValues sample(std::mt19937_64* rng) const; + VectorValues sample(std::mt19937_64* rng, + const SharedDiagonal& model = nullptr) const; /** * Sample from conditional, given missing variables @@ -175,8 +176,8 @@ namespace gtsam { * VectorValues given = ...; * auto sample = gbn.sample(given, &rng); */ - VectorValues sample(const VectorValues& parentsValues, - std::mt19937_64* rng) const; + VectorValues sample(const VectorValues& parentsValues, std::mt19937_64* rng, + const SharedDiagonal& model = nullptr) const; /// Sample, use default rng VectorValues sample() const; From bdb7836d0e496cacd942405323e55dc0711f51b1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Dec 2022 20:19:41 +0530 Subject: [PATCH 2/6] sampling test --- gtsam/hybrid/HybridBayesNet.cpp | 1 + gtsam/hybrid/tests/testHybridEstimation.cpp | 68 +++++++++++++++++++++ 2 files changed, 69 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 7338873bc0..fea87d3e5a 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -279,6 +279,7 @@ AlgebraicDecisionTree HybridBayesNet::error( return error_tree; } +/* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::probPrime( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree = this->error(continuousValues); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 39c5eea157..ef5e882bbf 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -432,6 +432,74 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); } +/****************************************************************************/ +/** + * Test for correctness via sampling. + * + * Given the conditional P(x0, m0, x1| z0, z1) + * with meaasurements z0, z1, we: + * 1. Start with the corresponding Factor Graph `FG`. + * 2. Eliminate the factor graph into a Bayes Net `BN`. + * 3. Sample from the Bayes Net. + * 4. Check that the ratio `BN(x)/FG(x) = constant` for all samples `x`. + */ +TEST(HybridEstimation, CorrectnessViaSampling) { + HybridNonlinearFactorGraph nfg; + + auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); + auto zero_motion = + boost::make_shared>(X(0), X(1), 0, noise_model); + auto one_motion = + boost::make_shared>(X(0), X(1), 1, noise_model); + std::vector factors = {zero_motion, one_motion}; + nfg.emplace_nonlinear>(X(0), 0.0, noise_model); + nfg.emplace_hybrid( + KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); + + Values initial; + double z0 = 0.0, z1 = 1.0; + initial.insert(X(0), z0); + initial.insert(X(1), z1); + + // 1. Create the factor graph from the nonlinear factor graph. + HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial); + // 2. Eliminate into BN + Ordering ordering = fg->getHybridOrdering(); + HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering); + + // Set up sampling + std::random_device rd; + std::mt19937_64 gen(rd()); + // Discrete distribution with 50/50 weightage on both discrete variables. + std::discrete_distribution<> ddist({50, 50}); + + // 3. Do sampling + std::vector ratios; + int num_samples = 1000; + + for (size_t i = 0; i < num_samples; i++) { + // Sample a discrete value + DiscreteValues assignment; + assignment[M(0)] = ddist(gen); + + // Using the discrete sample, get the corresponding bayes net. + GaussianBayesNet gbn = bn->choose(assignment); + // Sample from the bayes net + VectorValues sample = gbn.sample(&gen, noise_model); + // Compute the ratio in log form and canonical form + double log_ratio = bn->error(sample, assignment) - fg->error(sample, assignment); + double ratio = exp(-log_ratio); + + // Store the ratio for post-processing + ratios.push_back(ratio); + } + + // 4. Check that all samples == 1.0 (constant) + double ratio_sum = std::accumulate(ratios.begin(), ratios.end(), + decltype(ratios)::value_type(0)); + EXPECT_DOUBLES_EQUAL(1.0, ratio_sum / num_samples, 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; From aa86af2d778953a1bbf4d0123aa208f6b0c3ea00 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 20:11:19 +0530 Subject: [PATCH 3/6] Revert "add optional model parameter to sample method" This reverts commit ffd1802ceaafd574517335bd34ddd525c8e1227b. --- gtsam/linear/GaussianBayesNet.cpp | 10 ++++------ gtsam/linear/GaussianBayesNet.h | 6 ++---- gtsam/linear/GaussianConditional.cpp | 22 +++++++--------------- gtsam/linear/GaussianConditional.h | 7 +++---- 4 files changed, 16 insertions(+), 29 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8db301aa56..6dcf662a90 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -59,18 +59,16 @@ namespace gtsam { } /* ************************************************************************ */ - VectorValues GaussianBayesNet::sample(std::mt19937_64* rng, - const SharedDiagonal& model) const { + VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const { VectorValues result; // no missing variables -> create an empty vector - return sample(result, rng, model); + return sample(result, rng); } VectorValues GaussianBayesNet::sample(VectorValues result, - std::mt19937_64* rng, - const SharedDiagonal& model) const { + std::mt19937_64* rng) const { // sample each node in reverse topological sort order (parents first) for (auto cg : boost::adaptors::reverse(*this)) { - const VectorValues sampled = cg->sample(result, rng, model); + const VectorValues sampled = cg->sample(result, rng); result.insert(sampled); } return result; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index e6dae61260..83328576f2 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -101,8 +101,7 @@ namespace gtsam { * std::mt19937_64 rng(42); * auto sample = gbn.sample(&rng); */ - VectorValues sample(std::mt19937_64* rng, - const SharedDiagonal& model = nullptr) const; + VectorValues sample(std::mt19937_64* rng) const; /** * Sample from an incomplete BayesNet, given missing variables @@ -111,8 +110,7 @@ namespace gtsam { * VectorValues given = ...; * auto sample = gbn.sample(given, &rng); */ - VectorValues sample(VectorValues given, std::mt19937_64* rng, - const SharedDiagonal& model = nullptr) const; + VectorValues sample(VectorValues given, std::mt19937_64* rng) const; /// Sample using ancestral sampling, use default rng VectorValues sample() const; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 1a6620d62a..9515776416 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -279,38 +279,30 @@ namespace gtsam { /* ************************************************************************ */ VectorValues GaussianConditional::sample(const VectorValues& parentsValues, - std::mt19937_64* rng, - const SharedDiagonal& model) const { + std::mt19937_64* rng) const { if (nrFrontals() != 1) { throw std::invalid_argument( "GaussianConditional::sample can only be called on single variable " "conditionals"); } - - VectorValues solution = solve(parentsValues); - Key key = firstFrontalKey(); - - Vector sigmas; - if (model_) { - sigmas = model_->sigmas(); - } else if (model) { - sigmas = model->sigmas(); - } else { + if (!model_) { throw std::invalid_argument( "GaussianConditional::sample can only be called if a diagonal noise " "model was specified at construction."); } + VectorValues solution = solve(parentsValues); + Key key = firstFrontalKey(); + const Vector& sigmas = model_->sigmas(); solution[key] += Sampler::sampleDiagonal(sigmas, rng); return solution; } - VectorValues GaussianConditional::sample(std::mt19937_64* rng, - const SharedDiagonal& model) const { + VectorValues GaussianConditional::sample(std::mt19937_64* rng) const { if (nrParents() != 0) throw std::invalid_argument( "sample() can only be invoked on no-parent prior"); VectorValues values; - return sample(values, rng, model); + return sample(values); } /* ************************************************************************ */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index ccf916cd7f..4822e3eaf7 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -166,8 +166,7 @@ namespace gtsam { * std::mt19937_64 rng(42); * auto sample = gbn.sample(&rng); */ - VectorValues sample(std::mt19937_64* rng, - const SharedDiagonal& model = nullptr) const; + VectorValues sample(std::mt19937_64* rng) const; /** * Sample from conditional, given missing variables @@ -176,8 +175,8 @@ namespace gtsam { * VectorValues given = ...; * auto sample = gbn.sample(given, &rng); */ - VectorValues sample(const VectorValues& parentsValues, std::mt19937_64* rng, - const SharedDiagonal& model = nullptr) const; + VectorValues sample(const VectorValues& parentsValues, + std::mt19937_64* rng) const; /// Sample, use default rng VectorValues sample() const; From 798c51aec99ecf313a27304c82d2de0b2a522e9a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 20:32:08 +0530 Subject: [PATCH 4/6] update sampling test to use new sample method --- gtsam/hybrid/tests/testHybridEstimation.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index ef5e882bbf..be32a97c7d 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -469,25 +469,20 @@ TEST(HybridEstimation, CorrectnessViaSampling) { // Set up sampling std::random_device rd; - std::mt19937_64 gen(rd()); - // Discrete distribution with 50/50 weightage on both discrete variables. - std::discrete_distribution<> ddist({50, 50}); + std::mt19937_64 gen(11); // 3. Do sampling std::vector ratios; int num_samples = 1000; for (size_t i = 0; i < num_samples; i++) { - // Sample a discrete value - DiscreteValues assignment; - assignment[M(0)] = ddist(gen); - - // Using the discrete sample, get the corresponding bayes net. - GaussianBayesNet gbn = bn->choose(assignment); // Sample from the bayes net - VectorValues sample = gbn.sample(&gen, noise_model); + HybridValues sample = bn->sample(&gen); + // Compute the ratio in log form and canonical form - double log_ratio = bn->error(sample, assignment) - fg->error(sample, assignment); + DiscreteValues assignment = sample.discrete(); + double log_ratio = bn->error(sample.continuous(), assignment) - + fg->error(sample.continuous(), assignment); double ratio = exp(-log_ratio); // Store the ratio for post-processing From 13d22b123a1eae92551fa196561040dd5eb11279 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 21:10:18 +0530 Subject: [PATCH 5/6] address review comments --- gtsam/hybrid/tests/testHybridEstimation.cpp | 32 +++++++++++++-------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index be32a97c7d..11d298ef3b 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -436,8 +436,8 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { /** * Test for correctness via sampling. * - * Given the conditional P(x0, m0, x1| z0, z1) - * with meaasurements z0, z1, we: + * Compute the conditional P(x0, m0, x1| z0, z1) + * with measurements z0, z1. To do so, we: * 1. Start with the corresponding Factor Graph `FG`. * 2. Eliminate the factor graph into a Bayes Net `BN`. * 3. Sample from the Bayes Net. @@ -446,15 +446,20 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { TEST(HybridEstimation, CorrectnessViaSampling) { HybridNonlinearFactorGraph nfg; - auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); - auto zero_motion = + // First we create a hybrid nonlinear factor graph + // which represents f(x0, x1, m0; z0, z1). + // We linearize and eliminate this to get + // the required Factor Graph FG and Bayes Net BN. + const auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); + const auto zero_motion = boost::make_shared>(X(0), X(1), 0, noise_model); - auto one_motion = + const auto one_motion = boost::make_shared>(X(0), X(1), 1, noise_model); - std::vector factors = {zero_motion, one_motion}; + nfg.emplace_nonlinear>(X(0), 0.0, noise_model); nfg.emplace_hybrid( - KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); + KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, + std::vector{zero_motion, one_motion}); Values initial; double z0 = 0.0, z1 = 1.0; @@ -463,13 +468,13 @@ TEST(HybridEstimation, CorrectnessViaSampling) { // 1. Create the factor graph from the nonlinear factor graph. HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial); + // 2. Eliminate into BN - Ordering ordering = fg->getHybridOrdering(); + const Ordering ordering = fg->getHybridOrdering(); HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering); // Set up sampling - std::random_device rd; - std::mt19937_64 gen(11); + std::mt19937_64 rng(11); // 3. Do sampling std::vector ratios; @@ -477,10 +482,10 @@ TEST(HybridEstimation, CorrectnessViaSampling) { for (size_t i = 0; i < num_samples; i++) { // Sample from the bayes net - HybridValues sample = bn->sample(&gen); + const HybridValues sample = bn->sample(&rng); // Compute the ratio in log form and canonical form - DiscreteValues assignment = sample.discrete(); + const DiscreteValues assignment = sample.discrete(); double log_ratio = bn->error(sample.continuous(), assignment) - fg->error(sample.continuous(), assignment); double ratio = exp(-log_ratio); @@ -490,6 +495,9 @@ TEST(HybridEstimation, CorrectnessViaSampling) { } // 4. Check that all samples == 1.0 (constant) + // The error evaluated by the factor graph and the bayes net should be the + // same since the FG represents the unnormalized joint distribution and the BN + // is the unnormalized conditional, hence giving the ratio value as 1. double ratio_sum = std::accumulate(ratios.begin(), ratios.end(), decltype(ratios)::value_type(0)); EXPECT_DOUBLES_EQUAL(1.0, ratio_sum / num_samples, 1e-9); From 1e17dd3655695f078c04757b55d57c310f50c542 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Dec 2022 01:05:32 +0530 Subject: [PATCH 6/6] compute sampling ratio for one sample and then for multiple samples --- gtsam/hybrid/tests/testHybridEstimation.cpp | 42 ++++++++++++--------- 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 11d298ef3b..e4a45bf4dd 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -477,30 +477,36 @@ TEST(HybridEstimation, CorrectnessViaSampling) { std::mt19937_64 rng(11); // 3. Do sampling - std::vector ratios; - int num_samples = 1000; + int num_samples = 10; + + // Functor to compute the ratio between the + // Bayes net and the factor graph. + auto compute_ratio = + [](const HybridBayesNet::shared_ptr& bayesNet, + const HybridGaussianFactorGraph::shared_ptr& factorGraph, + const HybridValues& sample) -> double { + const DiscreteValues assignment = sample.discrete(); + // Compute in log form for numerical stability + double log_ratio = bayesNet->error(sample.continuous(), assignment) - + factorGraph->error(sample.continuous(), assignment); + double ratio = exp(-log_ratio); + return ratio; + }; + // The error evaluated by the factor graph and the Bayes net should differ by + // the normalizing term computed via the Bayes net determinant. + const HybridValues sample = bn->sample(&rng); + double ratio = compute_ratio(bn, fg, sample); + // regression + EXPECT_DOUBLES_EQUAL(1.0, ratio, 1e-9); + + // 4. Check that all samples == constant for (size_t i = 0; i < num_samples; i++) { // Sample from the bayes net const HybridValues sample = bn->sample(&rng); - // Compute the ratio in log form and canonical form - const DiscreteValues assignment = sample.discrete(); - double log_ratio = bn->error(sample.continuous(), assignment) - - fg->error(sample.continuous(), assignment); - double ratio = exp(-log_ratio); - - // Store the ratio for post-processing - ratios.push_back(ratio); + EXPECT_DOUBLES_EQUAL(ratio, compute_ratio(bn, fg, sample), 1e-9); } - - // 4. Check that all samples == 1.0 (constant) - // The error evaluated by the factor graph and the bayes net should be the - // same since the FG represents the unnormalized joint distribution and the BN - // is the unnormalized conditional, hence giving the ratio value as 1. - double ratio_sum = std::accumulate(ratios.begin(), ratios.end(), - decltype(ratios)::value_type(0)); - EXPECT_DOUBLES_EQUAL(1.0, ratio_sum / num_samples, 1e-9); } /* ************************************************************************* */