diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 058533104..f69cfdae6 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -22,23 +22,29 @@ namespace gtsam { /* ************************************************************************* */ -Sampler::Sampler(const SharedDiagonal& model, int32_t seed) - : sigmas_(model->sigmas()), generator_(static_cast(seed)) +Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) + : model_(model), generator_(static_cast(seed)) { } /* ************************************************************************* */ Sampler::Sampler(const Vector& sigmas, int32_t seed) - : sigmas_(sigmas), generator_(static_cast(seed)) +: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast(seed)) { } /* ************************************************************************* */ -Vector Sampler::sample() { - size_t d = dim(); +Sampler::Sampler(int32_t seed) +: generator_(static_cast(seed)) +{ +} + +/* ************************************************************************* */ +Vector Sampler::sampleDiagonal(const Vector& sigmas) { + size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { - double sigma = sigmas_(i); + double sigma = sigmas(i); // handle constrained case separately if (sigma == 0.0) { @@ -53,4 +59,19 @@ Vector Sampler::sample() { return result; } +/* ************************************************************************* */ +Vector Sampler::sample() { + assert(model_.get()); + const Vector& sigmas = model_->sigmas(); + return sampleDiagonal(sigmas); +} + +/* ************************************************************************* */ +Vector Sampler::sample(const noiseModel::Diagonal::shared_ptr& model) { + assert(model.get()); + const Vector& sigmas = model->sigmas(); + return sampleDiagonal(sigmas); +} +/* ************************************************************************* */ + } // \namespace gtsam diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 53b59a9ea..1150f5d55 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -31,8 +31,8 @@ namespace gtsam { */ class Sampler { protected: - /** sigmas from the noise model */ - Vector sigmas_; + /** noiseModel created at generation */ + noiseModel::Diagonal::shared_ptr model_; /** generator */ boost::minstd_rand generator_; @@ -46,7 +46,7 @@ public: * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const SharedDiagonal& model, int32_t seed = 42u); + Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u); /** * Create a sampler for a distribution specified by a vector of sigmas directly @@ -55,9 +55,17 @@ public: */ Sampler(const Vector& sigmas, int32_t seed = 42u); + /** + * Create a sampler without a given noisemodel - pass in to sample + * + * NOTE: do not use zero as a seed, it will break the generator + */ + Sampler(int32_t seed = 42u); + /** access functions */ - size_t dim() const { return sigmas_.size(); } - Vector sigmas() const { return sigmas_; } + size_t dim() const { assert(model_.get()); return model_->dim(); } + Vector sigmas() const { assert(model_.get()); return model_->sigmas(); } + const noiseModel::Diagonal::shared_ptr& model() const { return model_; } /** * sample from distribution @@ -65,6 +73,19 @@ public: */ Vector sample(); + /** + * Sample from noisemodel passed in as an argument, + * can be used without having initialized a model for the system. + * + * NOTE: not const due to need to update the underlying generator + */ + Vector sample(const noiseModel::Diagonal::shared_ptr& model); + +protected: + + /** given sigmas for a diagonal model, returns a sample */ + Vector sampleDiagonal(const Vector& sigmas); + }; } diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index e01788b65..ffb546e40 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -25,7 +25,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { Vector sigmas = Vector_(3, 1.0, 0.1, 0.0); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(sigmas); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); EXPECT(assert_equal(sigmas, sampler1.sigmas()));