From 4fc02a6aa296b3214b850f76cb07e5a977faf023 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Dec 2022 23:51:20 +0530 Subject: [PATCH 01/13] Add optional model parameter to sample methods --- gtsam/linear/GaussianBayesNet.cpp | 15 ++++++++------ gtsam/linear/GaussianBayesNet.h | 11 ++++++---- gtsam/linear/GaussianConditional.cpp | 31 ++++++++++++++++++---------- gtsam/linear/GaussianConditional.h | 11 ++++++---- 4 files changed, 43 insertions(+), 25 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 41a734b34b..d42fbe7722 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -59,27 +59,30 @@ 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; } /* ************************************************************************ */ - VectorValues GaussianBayesNet::sample() const { + VectorValues GaussianBayesNet::sample(const SharedDiagonal& model) const { return sample(&kRandomNumberGenerator); } - VectorValues GaussianBayesNet::sample(VectorValues given) const { + VectorValues GaussianBayesNet::sample(VectorValues given, + const SharedDiagonal& model) const { return sample(given, &kRandomNumberGenerator); } diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 83328576f2..570bfef58d 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,13 +111,15 @@ 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; + VectorValues sample(const SharedDiagonal& model = nullptr) const; /// Sample from an incomplete BayesNet, use default rng - VectorValues sample(VectorValues given) const; + VectorValues sample(VectorValues given, + const SharedDiagonal& model = nullptr) const; /** * Return ordering corresponding to a topological sort. diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 60ddb1b7d0..363d25d112 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -293,39 +293,48 @@ double GaussianConditional::logDeterminant() const { /* ************************************************************************ */ 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); } /* ************************************************************************ */ - VectorValues GaussianConditional::sample() const { - return sample(&kRandomNumberGenerator); + VectorValues GaussianConditional::sample(const SharedDiagonal& model) const { + return sample(&kRandomNumberGenerator, model); } - VectorValues GaussianConditional::sample(const VectorValues& given) const { - return sample(given, &kRandomNumberGenerator); + VectorValues GaussianConditional::sample(const VectorValues& given, + const SharedDiagonal& model) const { + return sample(given, &kRandomNumberGenerator, model); } /* ************************************************************************ */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 8af7f66029..1ca9b7d531 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -188,7 +188,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 @@ -198,13 +199,15 @@ namespace gtsam { * auto sample = gbn.sample(given, &rng); */ VectorValues sample(const VectorValues& parentsValues, - std::mt19937_64* rng) const; + std::mt19937_64* rng, + const SharedDiagonal& model = nullptr) const; /// Sample, use default rng - VectorValues sample() const; + VectorValues sample(const SharedDiagonal& model = nullptr) const; /// Sample with given values, use default rng - VectorValues sample(const VectorValues& parentsValues) const; + VectorValues sample(const VectorValues& parentsValues, + const SharedDiagonal& model = nullptr) const; /// @} From cdf1c4ec5da3a9e8306f3727da186e03dcfca519 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Dec 2022 23:58:56 +0530 Subject: [PATCH 02/13] hybrid bayes net sample method --- gtsam/hybrid/HybridBayesNet.cpp | 40 +++++++++++++++++++++++++++ gtsam/hybrid/HybridBayesNet.h | 48 ++++++++++++++++++++++++++++++++- 2 files changed, 87 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 48c4b6d508..0e2bfd740e 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -20,6 +20,9 @@ #include #include +// In Wrappers we have no access to this so have a default ready +static std::mt19937_64 kRandomNumberGenerator(42); + namespace gtsam { /* ************************************************************************* */ @@ -232,6 +235,43 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { return gbn.optimize(); } +/* ************************************************************************* */ +HybridValues HybridBayesNet::sample(VectorValues given, std::mt19937_64 *rng, + SharedDiagonal model) const { + DiscreteBayesNet dbn; + for (size_t idx = 0; idx < size(); idx++) { + if (factors_.at(idx)->isDiscrete()) { + // If factor at `idx` is discrete-only, we add to the discrete bayes net. + dbn.push_back(this->atDiscrete(idx)); + } + } + // Sample a discrete assignment. + DiscreteValues assignment = dbn.sample(); + // Select the continuous bayes net corresponding to the assignment. + GaussianBayesNet gbn = this->choose(assignment); + // Sample from the gaussian bayes net. + VectorValues sample = gbn.sample(given, rng, model); + return HybridValues(assignment, sample); +} + +/* ************************************************************************* */ +HybridValues HybridBayesNet::sample(std::mt19937_64 *rng, + SharedDiagonal model) const { + VectorValues given; + return sample(given, rng, model); +} + +/* ************************************************************************* */ +HybridValues HybridBayesNet::sample(VectorValues given, + SharedDiagonal model) const { + return sample(given, &kRandomNumberGenerator, model); +} + +/* ************************************************************************* */ +HybridValues HybridBayesNet::sample(SharedDiagonal model) const { + return sample(&kRandomNumberGenerator, model); +} + /* ************************************************************************* */ double HybridBayesNet::error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f8ec609119..d6809e0360 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -120,7 +120,53 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ DecisionTreeFactor::shared_ptr discreteConditionals() const; - public: + /** + * @brief Sample from an incomplete BayesNet, given missing variables. + * + * Example: + * std::mt19937_64 rng(42); + * VectorValues given = ...; + * auto sample = bn.sample(given, &rng); + * + * @param given Values of missing variables. + * @param rng The pseudo-random number generator. + * @param model Optional diagonal noise model to use in sampling. + * @return HybridValues + */ + HybridValues sample(VectorValues given, std::mt19937_64 *rng, + SharedDiagonal model = nullptr) const; + + /** + * @brief Sample using ancestral sampling. + * + * Example: + * std::mt19937_64 rng(42); + * auto sample = bn.sample(&rng); + * + * @param rng The pseudo-random number generator. + * @param model Optional diagonal noise model to use in sampling. + * @return HybridValues + */ + HybridValues sample(std::mt19937_64 *rng, + SharedDiagonal model = nullptr) const; + + /** + * @brief Sample from an incomplete BayesNet, use default rng. + * + * @param given Values of missing variables. + * @param model Optional diagonal noise model to use in sampling. + * @return HybridValues + */ + HybridValues sample(VectorValues given, SharedDiagonal model = nullptr) const; + + /** + * @brief Sample using ancestral sampling, use default rng. + * + * @param model Optional diagonal noise model to use in sampling. + * @return HybridValues + */ + HybridValues sample(SharedDiagonal model = nullptr) const; + /// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves. HybridBayesNet prune(size_t maxNrLeaves); From e9978284c857837fd453ebe9fa09df23445381ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 00:54:26 +0530 Subject: [PATCH 03/13] add unit test for sampling --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 64 +++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 050c01aeda..84d4e545e6 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -316,6 +316,70 @@ TEST(HybridBayesNet, Serialization) { EXPECT(equalsBinary(hbn)); } +/* ****************************************************************************/ +// Test HybridBayesNet sampling. +TEST(HybridBayesNet, Sampling) { + 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); + + DiscreteKey mode(M(0), 2); + auto discrete_prior = boost::make_shared(mode, "1/1"); + nfg.push_discrete(discrete_prior); + + Values initial; + double z0 = 0.0, z1 = 1.0; + initial.insert(X(0), z0); + initial.insert(X(1), z1); + + // Create the factor graph from the nonlinear factor graph. + HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial); + // Eliminate into BN + Ordering ordering = fg->getHybridOrdering(); + HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering); + + // Set up sampling + std::mt19937_64 gen(11); + + // Initialize containers for computing the mean values. + vector discrete_samples; + VectorValues average_continuous; + + size_t num_samples = 1000; + for (size_t i = 0; i < num_samples; i++) { + // Sample + HybridValues sample = bn->sample(&gen, noise_model); + + discrete_samples.push_back(sample.discrete()[M(0)]); + + if (i == 0) { + average_continuous.insert(sample.continuous()); + } else { + average_continuous += sample.continuous(); + } + } + double discrete_sum = + std::accumulate(discrete_samples.begin(), discrete_samples.end(), + decltype(discrete_samples)::value_type(0)); + + // regression for specific RNG seed + EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9); + + VectorValues expected; + expected.insert({X(0), Vector1(-0.0131207162712)}); + expected.insert({X(1), Vector1(-0.499026377568)}); + // regression for specific RNG seed + EXPECT(assert_equal(expected, average_continuous.scale(1.0 / num_samples))); +} + /* ************************************************************************* */ int main() { TestResult tr; From 789b5d2eb681e700867294c8e75006671f77c77a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 06:58:49 +0530 Subject: [PATCH 04/13] Revert "Add optional model parameter to sample methods" This reverts commit 4fc02a6aa296b3214b850f76cb07e5a977faf023. --- gtsam/linear/GaussianBayesNet.cpp | 15 ++++++-------- gtsam/linear/GaussianBayesNet.h | 11 ++++------ gtsam/linear/GaussianConditional.cpp | 31 ++++++++++------------------ gtsam/linear/GaussianConditional.h | 11 ++++------ 4 files changed, 25 insertions(+), 43 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index d42fbe7722..41a734b34b 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -59,30 +59,27 @@ 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; } /* ************************************************************************ */ - VectorValues GaussianBayesNet::sample(const SharedDiagonal& model) const { + VectorValues GaussianBayesNet::sample() const { return sample(&kRandomNumberGenerator); } - VectorValues GaussianBayesNet::sample(VectorValues given, - const SharedDiagonal& model) const { + VectorValues GaussianBayesNet::sample(VectorValues given) const { return sample(given, &kRandomNumberGenerator); } diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 570bfef58d..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,15 +110,13 @@ 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 SharedDiagonal& model = nullptr) const; + VectorValues sample() const; /// Sample from an incomplete BayesNet, use default rng - VectorValues sample(VectorValues given, - const SharedDiagonal& model = nullptr) const; + VectorValues sample(VectorValues given) const; /** * Return ordering corresponding to a topological sort. diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 363d25d112..60ddb1b7d0 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -293,48 +293,39 @@ double GaussianConditional::logDeterminant() const { /* ************************************************************************ */ 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); } /* ************************************************************************ */ - VectorValues GaussianConditional::sample(const SharedDiagonal& model) const { - return sample(&kRandomNumberGenerator, model); + VectorValues GaussianConditional::sample() const { + return sample(&kRandomNumberGenerator); } - VectorValues GaussianConditional::sample(const VectorValues& given, - const SharedDiagonal& model) const { - return sample(given, &kRandomNumberGenerator, model); + VectorValues GaussianConditional::sample(const VectorValues& given) const { + return sample(given, &kRandomNumberGenerator); } /* ************************************************************************ */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 1ca9b7d531..8af7f66029 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -188,8 +188,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 @@ -199,15 +198,13 @@ namespace gtsam { * auto sample = gbn.sample(given, &rng); */ VectorValues sample(const VectorValues& parentsValues, - std::mt19937_64* rng, - const SharedDiagonal& model = nullptr) const; + std::mt19937_64* rng) const; /// Sample, use default rng - VectorValues sample(const SharedDiagonal& model = nullptr) const; + VectorValues sample() const; /// Sample with given values, use default rng - VectorValues sample(const VectorValues& parentsValues, - const SharedDiagonal& model = nullptr) const; + VectorValues sample(const VectorValues& parentsValues) const; /// @} From c2452643888f5f456eb7430e8e07d2a4fbd2eb9d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 07:13:40 +0530 Subject: [PATCH 05/13] remove model --- gtsam/hybrid/HybridBayesNet.cpp | 19 ++++++++----------- gtsam/hybrid/HybridBayesNet.h | 14 ++++---------- 2 files changed, 12 insertions(+), 21 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 0e2bfd740e..5e0d185e87 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -236,8 +236,7 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(VectorValues given, std::mt19937_64 *rng, - SharedDiagonal model) const { +HybridValues HybridBayesNet::sample(VectorValues given, std::mt19937_64 *rng) const { DiscreteBayesNet dbn; for (size_t idx = 0; idx < size(); idx++) { if (factors_.at(idx)->isDiscrete()) { @@ -250,26 +249,24 @@ HybridValues HybridBayesNet::sample(VectorValues given, std::mt19937_64 *rng, // Select the continuous bayes net corresponding to the assignment. GaussianBayesNet gbn = this->choose(assignment); // Sample from the gaussian bayes net. - VectorValues sample = gbn.sample(given, rng, model); + VectorValues sample = gbn.sample(given, rng); return HybridValues(assignment, sample); } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(std::mt19937_64 *rng, - SharedDiagonal model) const { +HybridValues HybridBayesNet::sample(std::mt19937_64 *rng) const { VectorValues given; - return sample(given, rng, model); + return sample(given, rng); } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(VectorValues given, - SharedDiagonal model) const { - return sample(given, &kRandomNumberGenerator, model); +HybridValues HybridBayesNet::sample(VectorValues given) const { + return sample(given, &kRandomNumberGenerator); } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(SharedDiagonal model) const { - return sample(&kRandomNumberGenerator, model); +HybridValues HybridBayesNet::sample() const { + return sample(&kRandomNumberGenerator); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index d6809e0360..4b39ace256 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -130,11 +130,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * * @param given Values of missing variables. * @param rng The pseudo-random number generator. - * @param model Optional diagonal noise model to use in sampling. * @return HybridValues */ - HybridValues sample(VectorValues given, std::mt19937_64 *rng, - SharedDiagonal model = nullptr) const; + HybridValues sample(VectorValues given, std::mt19937_64 *rng) const; /** * @brief Sample using ancestral sampling. @@ -144,28 +142,24 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * auto sample = bn.sample(&rng); * * @param rng The pseudo-random number generator. - * @param model Optional diagonal noise model to use in sampling. * @return HybridValues */ - HybridValues sample(std::mt19937_64 *rng, - SharedDiagonal model = nullptr) const; + HybridValues sample(std::mt19937_64 *rng) const; /** * @brief Sample from an incomplete BayesNet, use default rng. * * @param given Values of missing variables. - * @param model Optional diagonal noise model to use in sampling. * @return HybridValues */ - HybridValues sample(VectorValues given, SharedDiagonal model = nullptr) const; + HybridValues sample(VectorValues given) const; /** * @brief Sample using ancestral sampling, use default rng. * - * @param model Optional diagonal noise model to use in sampling. * @return HybridValues */ - HybridValues sample(SharedDiagonal model = nullptr) const; + HybridValues sample() const; /// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves. HybridBayesNet prune(size_t maxNrLeaves); From fe394cc07499cbfaa734a6914a50961611fa4721 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 07:35:18 +0530 Subject: [PATCH 06/13] remove `factors_` from Bayes net implementation --- gtsam/hybrid/HybridBayesNet.cpp | 71 +++++++++++++++------------------ 1 file changed, 33 insertions(+), 38 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 5e0d185e87..de940ec698 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -29,8 +29,8 @@ namespace gtsam { DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { AlgebraicDecisionTree decisionTree; - // The canonical decision tree factor which will get the discrete conditionals - // added to it. + // The canonical decision tree factor which will get + // the discrete conditionals added to it. DecisionTreeFactor dtFactor; for (size_t i = 0; i < this->size(); i++) { @@ -176,35 +176,35 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { /* ************************************************************************* */ GaussianMixture::shared_ptr HybridBayesNet::atMixture(size_t i) const { - return factors_.at(i)->asMixture(); + return at(i)->asMixture(); } /* ************************************************************************* */ GaussianConditional::shared_ptr HybridBayesNet::atGaussian(size_t i) const { - return factors_.at(i)->asGaussian(); + return at(i)->asGaussian(); } /* ************************************************************************* */ DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const { - return factors_.at(i)->asDiscreteConditional(); + return at(i)->asDiscreteConditional(); } /* ************************************************************************* */ GaussianBayesNet HybridBayesNet::choose( const DiscreteValues &assignment) const { GaussianBayesNet gbn; - for (size_t idx = 0; idx < size(); idx++) { - if (factors_.at(idx)->isHybrid()) { - // If factor is hybrid, select based on assignment. - GaussianMixture gm = *this->atMixture(idx); + for (const sharedConditional &conditional : *this) { + if (conditional->isHybrid()) { + // If conditional is hybrid, select based on assignment. + GaussianMixture gm = *conditional->asMixture(); gbn.push_back(gm(assignment)); - } else if (factors_.at(idx)->isContinuous()) { + } else if (conditional->isContinuous()) { // If continuous only, add gaussian conditional. - gbn.push_back((this->atGaussian(idx))); + gbn.push_back((conditional->asGaussian())); - } else if (factors_.at(idx)->isDiscrete()) { - // If factor at `idx` is discrete-only, we simply continue. + } else if (conditional->isDiscrete()) { + // If conditional is discrete-only, we simply continue. continue; } } @@ -216,7 +216,7 @@ GaussianBayesNet HybridBayesNet::choose( HybridValues HybridBayesNet::optimize() const { // Solve for the MPE DiscreteBayesNet discrete_bn; - for (auto &conditional : factors_) { + for (auto &conditional : *this) { if (conditional->isDiscrete()) { discrete_bn.push_back(conditional->asDiscreteConditional()); } @@ -236,12 +236,13 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(VectorValues given, std::mt19937_64 *rng) const { +HybridValues HybridBayesNet::sample(VectorValues given, + std::mt19937_64 *rng) const { DiscreteBayesNet dbn; - for (size_t idx = 0; idx < size(); idx++) { - if (factors_.at(idx)->isDiscrete()) { - // If factor at `idx` is discrete-only, we add to the discrete bayes net. - dbn.push_back(this->atDiscrete(idx)); + for (const sharedConditional &conditional : *this) { + if (conditional->isDiscrete()) { + // If conditional is discrete-only, we add to the discrete bayes net. + dbn.push_back(conditional->asDiscrete()); } } // Sample a discrete assignment. @@ -279,34 +280,28 @@ double HybridBayesNet::error(const VectorValues &continuousValues, /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::error( const VectorValues &continuousValues) const { - AlgebraicDecisionTree error_tree; - - // Iterate over each factor. - for (size_t idx = 0; idx < size(); idx++) { - AlgebraicDecisionTree conditional_error; + AlgebraicDecisionTree error_tree(0.0); - if (factors_.at(idx)->isHybrid()) { - // If factor is hybrid, select based on assignment and compute error. - GaussianMixture::shared_ptr gm = this->atMixture(idx); - conditional_error = gm->error(continuousValues); + // Iterate over each conditional. + for (const sharedConditional &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); - // Assign for the first index, add error for subsequent ones. - if (idx == 0) { - error_tree = conditional_error; - } else { - error_tree = error_tree + conditional_error; - } + error_tree = error_tree + conditional_error; - } else if (factors_.at(idx)->isContinuous()) { + } else if (conditional->isContinuous()) { // If continuous only, get the (double) error // and add it to the error_tree - double error = this->atGaussian(idx)->error(continuousValues); + 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; }); - } else if (factors_.at(idx)->isDiscrete()) { - // If factor at `idx` is discrete-only, we skip. + } else if (conditional->isDiscrete()) { + // Conditional is discrete-only, we skip. continue; } } From 417a7cebf3b9d26a11169cfa4d3210c0c4ca9e1b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 09:04:12 +0530 Subject: [PATCH 07/13] if noise model not initialized in GaussianConditional, init it to Unit --- gtsam/linear/GaussianConditional.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 60ddb1b7d0..85b17373ce 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -299,14 +299,22 @@ double GaussianConditional::logDeterminant() const { "GaussianConditional::sample can only be called on single variable " "conditionals"); } + + // Noise model to sample from. + noiseModel::Diagonal::shared_ptr _model; if (!model_) { - throw std::invalid_argument( - "GaussianConditional::sample can only be called if a diagonal noise " - "model was specified at construction."); + // Initialize model_ to Unit noiseModel if not initialized. + // Unit noise model specifies sigmas as 1, since + // the noise information should already be in information matrix A. + // Dim should be number of rows since + // noise model dimension should match (Ax - b) + _model = noiseModel::Unit::Create(rows()); + } else { + _model = model_; } VectorValues solution = solve(parentsValues); Key key = firstFrontalKey(); - const Vector& sigmas = model_->sigmas(); + const Vector& sigmas = _model->sigmas(); solution[key] += Sampler::sampleDiagonal(sigmas, rng); return solution; } From ff8a58671df46a5001f0be76c073ac25311db77a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 09:07:09 +0530 Subject: [PATCH 08/13] address review comments --- gtsam/hybrid/HybridBayesNet.cpp | 32 ++++++++++++++++---------------- gtsam/hybrid/HybridBayesNet.h | 8 ++++---- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index de940ec698..54129775fa 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -8,7 +8,7 @@ /** * @file HybridBayesNet.cpp - * @brief A bayes net of Gaussian Conditionals indexed by discrete keys. + * @brief A Bayes net of Gaussian Conditionals indexed by discrete keys. * @author Fan Jiang * @author Varun Agrawal * @author Shangjie Xue @@ -56,7 +56,7 @@ std::function &, double)> prunerFunc( const DecisionTreeFactor &decisionTree, const HybridConditional &conditional) { // Get the discrete keys as sets for the decision tree - // and the gaussian mixture. + // and the Gaussian mixture. auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys()); auto conditionalKeySet = DiscreteKeysAsSet(conditional.discreteKeys()); @@ -65,7 +65,7 @@ std::function &, double)> prunerFunc( double probability) -> double { // typecast so we can use this to get probability value DiscreteValues values(choices); - // Case where the gaussian mixture has the same + // Case where the Gaussian mixture has the same // discrete keys as the decision tree. if (conditionalKeySet == decisionTreeKeySet) { if (decisionTree(values) == 0) { @@ -156,7 +156,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { if (conditional->isHybrid()) { GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture(); - // Make a copy of the gaussian mixture and prune it! + // Make a copy of the Gaussian mixture and prune it! auto prunedGaussianMixture = boost::make_shared(*gaussianMixture); prunedGaussianMixture->prune(*decisionTree); @@ -200,7 +200,7 @@ GaussianBayesNet HybridBayesNet::choose( gbn.push_back(gm(assignment)); } else if (conditional->isContinuous()) { - // If continuous only, add gaussian conditional. + // If continuous only, add Gaussian conditional. gbn.push_back((conditional->asGaussian())); } else if (conditional->isDiscrete()) { @@ -236,32 +236,32 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(VectorValues given, +HybridValues HybridBayesNet::sample(HybridValues& given, std::mt19937_64 *rng) const { DiscreteBayesNet dbn; for (const sharedConditional &conditional : *this) { if (conditional->isDiscrete()) { - // If conditional is discrete-only, we add to the discrete bayes net. - dbn.push_back(conditional->asDiscrete()); + // If conditional is discrete-only, we add to the discrete Bayes net. + dbn.push_back(conditional->asDiscreteConditional()); } } // Sample a discrete assignment. - DiscreteValues assignment = dbn.sample(); - // Select the continuous bayes net corresponding to the assignment. - GaussianBayesNet gbn = this->choose(assignment); - // Sample from the gaussian bayes net. - VectorValues sample = gbn.sample(given, rng); - return HybridValues(assignment, sample); + const DiscreteValues assignment = dbn.sample(given.discrete()); + // Select the continuous Bayes net corresponding to the assignment. + GaussianBayesNet gbn = choose(assignment); + // Sample from the Gaussian Bayes net. + VectorValues sample = gbn.sample(given.continuous(), rng); + return {assignment, sample}; } /* ************************************************************************* */ HybridValues HybridBayesNet::sample(std::mt19937_64 *rng) const { - VectorValues given; + HybridValues given; return sample(given, rng); } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(VectorValues given) const { +HybridValues HybridBayesNet::sample(HybridValues& given) const { return sample(given, &kRandomNumberGenerator); } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 4b39ace256..3412aaf78c 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -8,7 +8,7 @@ /** * @file HybridBayesNet.h - * @brief A bayes net of Gaussian Conditionals indexed by discrete keys. + * @brief A Bayes net of Gaussian Conditionals indexed by discrete keys. * @author Varun Agrawal * @author Fan Jiang * @author Frank Dellaert @@ -43,7 +43,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// @name Standard Constructors /// @{ - /** Construct empty bayes net */ + /** Construct empty Bayes net */ HybridBayesNet() = default; /// @} @@ -132,7 +132,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param rng The pseudo-random number generator. * @return HybridValues */ - HybridValues sample(VectorValues given, std::mt19937_64 *rng) const; + HybridValues sample(HybridValues& given, std::mt19937_64 *rng) const; /** * @brief Sample using ancestral sampling. @@ -152,7 +152,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param given Values of missing variables. * @return HybridValues */ - HybridValues sample(VectorValues given) const; + HybridValues sample(HybridValues& given) const; /** * @brief Sample using ancestral sampling, use default rng. From 0be2a679c02035a5b2d8ad3e9e273c701c47d855 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 09:08:47 +0530 Subject: [PATCH 09/13] update test --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 31 +++++++++++++---------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 84d4e545e6..3e3fab3760 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -356,7 +356,7 @@ TEST(HybridBayesNet, Sampling) { size_t num_samples = 1000; for (size_t i = 0; i < num_samples; i++) { // Sample - HybridValues sample = bn->sample(&gen, noise_model); + HybridValues sample = bn->sample(&gen); discrete_samples.push_back(sample.discrete()[M(0)]); @@ -366,18 +366,23 @@ TEST(HybridBayesNet, Sampling) { average_continuous += sample.continuous(); } } - double discrete_sum = - std::accumulate(discrete_samples.begin(), discrete_samples.end(), - decltype(discrete_samples)::value_type(0)); - - // regression for specific RNG seed - EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9); - - VectorValues expected; - expected.insert({X(0), Vector1(-0.0131207162712)}); - expected.insert({X(1), Vector1(-0.499026377568)}); - // regression for specific RNG seed - EXPECT(assert_equal(expected, average_continuous.scale(1.0 / num_samples))); + + EXPECT_LONGS_EQUAL(2, average_continuous.size()); + EXPECT_LONGS_EQUAL(num_samples, discrete_samples.size()); + + // Regressions don't work across platforms :-( + // // regression for specific RNG seed + // double discrete_sum = + // std::accumulate(discrete_samples.begin(), discrete_samples.end(), + // decltype(discrete_samples)::value_type(0)); + // EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9); + + // VectorValues expected; + // expected.insert({X(0), Vector1(-0.0131207162712)}); + // expected.insert({X(1), Vector1(-0.499026377568)}); + // // regression for specific RNG seed + // EXPECT(assert_equal(expected, average_continuous.scale(1.0 / + // num_samples))); } /* ************************************************************************* */ From b0235239cee76bc3e4fb5ef5ce915d7cd3bb77ee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 10:17:01 +0530 Subject: [PATCH 10/13] use auto&& in for loops --- gtsam/hybrid/HybridBayesNet.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 54129775fa..e72e16c451 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -33,8 +33,7 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { // the discrete conditionals added to it. DecisionTreeFactor dtFactor; - for (size_t i = 0; i < this->size(); i++) { - HybridConditional::shared_ptr conditional = this->at(i); + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { // Convert to a DecisionTreeFactor and add it to the main factor. DecisionTreeFactor f(*conditional->asDiscreteConditional()); @@ -104,6 +103,7 @@ void HybridBayesNet::updateDiscreteConditionals( const DecisionTreeFactor::shared_ptr &prunedDecisionTree) { KeyVector prunedTreeKeys = prunedDecisionTree->keys(); + // Loop with index since we need it later. for (size_t i = 0; i < this->size(); i++) { HybridConditional::shared_ptr conditional = this->at(i); if (conditional->isDiscrete()) { @@ -193,7 +193,7 @@ DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const { GaussianBayesNet HybridBayesNet::choose( const DiscreteValues &assignment) const { GaussianBayesNet gbn; - for (const sharedConditional &conditional : *this) { + for (auto &&conditional : *this) { if (conditional->isHybrid()) { // If conditional is hybrid, select based on assignment. GaussianMixture gm = *conditional->asMixture(); @@ -216,7 +216,7 @@ GaussianBayesNet HybridBayesNet::choose( HybridValues HybridBayesNet::optimize() const { // Solve for the MPE DiscreteBayesNet discrete_bn; - for (auto &conditional : *this) { + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_bn.push_back(conditional->asDiscreteConditional()); } @@ -236,10 +236,10 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(HybridValues& given, +HybridValues HybridBayesNet::sample(HybridValues &given, std::mt19937_64 *rng) const { DiscreteBayesNet dbn; - for (const sharedConditional &conditional : *this) { + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { // If conditional is discrete-only, we add to the discrete Bayes net. dbn.push_back(conditional->asDiscreteConditional()); @@ -261,7 +261,7 @@ HybridValues HybridBayesNet::sample(std::mt19937_64 *rng) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(HybridValues& given) const { +HybridValues HybridBayesNet::sample(HybridValues &given) const { return sample(given, &kRandomNumberGenerator); } @@ -283,7 +283,7 @@ AlgebraicDecisionTree HybridBayesNet::error( AlgebraicDecisionTree error_tree(0.0); // Iterate over each conditional. - for (const sharedConditional &conditional : *this) { + for (auto &&conditional : *this) { if (conditional->isHybrid()) { // If conditional is hybrid, select based on assignment and compute error. GaussianMixture::shared_ptr gm = conditional->asMixture(); From f6a2e7cf464418eb1213a3173008c5468c95b262 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 10:20:55 +0530 Subject: [PATCH 11/13] mark parameters for sample as const --- gtsam/hybrid/HybridBayesNet.cpp | 4 ++-- gtsam/hybrid/HybridBayesNet.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e72e16c451..7a46d7832d 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -236,7 +236,7 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(HybridValues &given, +HybridValues HybridBayesNet::sample(const HybridValues &given, std::mt19937_64 *rng) const { DiscreteBayesNet dbn; for (auto &&conditional : *this) { @@ -261,7 +261,7 @@ HybridValues HybridBayesNet::sample(std::mt19937_64 *rng) const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::sample(HybridValues &given) const { +HybridValues HybridBayesNet::sample(const HybridValues &given) const { return sample(given, &kRandomNumberGenerator); } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 3412aaf78c..1e6bebf1a8 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -132,7 +132,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param rng The pseudo-random number generator. * @return HybridValues */ - HybridValues sample(HybridValues& given, std::mt19937_64 *rng) const; + HybridValues sample(const HybridValues &given, std::mt19937_64 *rng) const; /** * @brief Sample using ancestral sampling. @@ -152,7 +152,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param given Values of missing variables. * @return HybridValues */ - HybridValues sample(HybridValues& given) const; + HybridValues sample(const HybridValues &given) const; /** * @brief Sample using ancestral sampling, use default rng. From 77b4028e47aa53437a94f361dfe5aa0e154733f1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 10:21:15 +0530 Subject: [PATCH 12/13] make GaussianBayesNet::sample functional --- gtsam/linear/GaussianBayesNet.cpp | 5 +++-- gtsam/linear/GaussianBayesNet.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 41a734b34b..229d4a9327 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -64,8 +64,9 @@ namespace gtsam { return sample(result, rng); } - VectorValues GaussianBayesNet::sample(VectorValues result, + VectorValues GaussianBayesNet::sample(const VectorValues& given, std::mt19937_64* rng) const { + VectorValues result(given); // sample each node in reverse topological sort order (parents first) for (auto cg : boost::adaptors::reverse(*this)) { const VectorValues sampled = cg->sample(result, rng); @@ -79,7 +80,7 @@ namespace gtsam { return sample(&kRandomNumberGenerator); } - VectorValues GaussianBayesNet::sample(VectorValues given) const { + VectorValues GaussianBayesNet::sample(const VectorValues& given) const { return sample(given, &kRandomNumberGenerator); } diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 83328576f2..e81d6af332 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -110,13 +110,13 @@ namespace gtsam { * VectorValues given = ...; * auto sample = gbn.sample(given, &rng); */ - VectorValues sample(VectorValues given, std::mt19937_64* rng) const; + VectorValues sample(const VectorValues& given, std::mt19937_64* rng) const; /// Sample using ancestral sampling, use default rng VectorValues sample() const; /// Sample from an incomplete BayesNet, use default rng - VectorValues sample(VectorValues given) const; + VectorValues sample(const VectorValues& given) const; /** * Return ordering corresponding to a topological sort. From da86e06efcdccd733dde18a5c11cbd2ee98800d0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Dec 2022 10:31:56 +0530 Subject: [PATCH 13/13] make sigmas initialization cleaner --- gtsam/linear/GaussianConditional.cpp | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 85b17373ce..4597156bc1 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -300,21 +300,11 @@ double GaussianConditional::logDeterminant() const { "conditionals"); } - // Noise model to sample from. - noiseModel::Diagonal::shared_ptr _model; - if (!model_) { - // Initialize model_ to Unit noiseModel if not initialized. - // Unit noise model specifies sigmas as 1, since - // the noise information should already be in information matrix A. - // Dim should be number of rows since - // noise model dimension should match (Ax - b) - _model = noiseModel::Unit::Create(rows()); - } else { - _model = model_; - } VectorValues solution = solve(parentsValues); Key key = firstFrontalKey(); - const Vector& sigmas = _model->sigmas(); + // The vector of sigma values for sampling. + // If no model, initialize sigmas to 1, else to model sigmas + const Vector& sigmas = (!model_) ? Vector::Ones(rows()) : model_->sigmas(); solution[key] += Sampler::sampleDiagonal(sigmas, rng); return solution; }