diff --git a/gtsam.h b/gtsam.h index 8ee778f4c..413a1bc7c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1563,17 +1563,15 @@ virtual class Robust : gtsam::noiseModel::Base { #include class Sampler { - //Constructors + // Constructors Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(Vector sigmas, int seed); - Sampler(int seed); - //Standard Interface + // Standard Interface size_t dim() const; Vector sigmas() const; gtsam::noiseModel::Diagonal* model() const; Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; #include diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index b324ea784..df99191b2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 9f1527bf9..16c7e73e0 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,6 +11,8 @@ /** * @file Sampler.cpp + * @brief sampling from a diagonal NoiseModel + * @author Frank Dellaert * @author Alex Cunningham */ @@ -18,25 +20,16 @@ namespace gtsam { /* ************************************************************************* */ -Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) - : model_(model), generator_(static_cast(seed)) -{ -} +Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, + uint_fast64_t seed) + : model_(model), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(const Vector& sigmas, int32_t seed) -: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast(seed)) -{ -} +Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) + : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(int32_t seed) -: generator_(static_cast(seed)) -{ -} - -/* ************************************************************************* */ -Vector Sampler::sampleDiagonal(const Vector& sigmas) { +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { @@ -55,18 +48,23 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) { } /* ************************************************************************* */ -Vector Sampler::sample() { +Vector Sampler::sample() const { assert(model_.get()); const Vector& sigmas = model_->sigmas(); return sampleDiagonal(sigmas); } /* ************************************************************************* */ -Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} + +Vector Sampler::sampleNewModel( + const noiseModel::Diagonal::shared_ptr& model) const { assert(model.get()); const Vector& sigmas = model->sigmas(); return sampleDiagonal(sigmas); } +#endif /* ************************************************************************* */ -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 93145c31a..54c240a2b 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @brief sampling that can be parameterized using a NoiseModel to generate samples from * @file Sampler.h - * the given distribution + * @brief sampling from a NoiseModel + * @author Frank Dellaert * @author Alex Cunningham */ @@ -27,67 +27,74 @@ namespace gtsam { /** * Sampling structure that keeps internal random number generators for * diagonal distributions specified by NoiseModel - * - * This is primarily to allow for variable seeds, and does roughly the same - * thing as sample() in NoiseModel. */ class GTSAM_EXPORT Sampler { -protected: + protected: /** noiseModel created at generation */ noiseModel::Diagonal::shared_ptr model_; /** generator */ - std::mt19937_64 generator_; + mutable std::mt19937_64 generator_; -public: + public: typedef boost::shared_ptr shared_ptr; + /// @name constructors + /// @{ + /** * Create a sampler for the distribution specified by a diagonal NoiseModel * with a manually specified seed * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u); + explicit Sampler(const noiseModel::Diagonal::shared_ptr& model, + uint_fast64_t seed = 42u); /** - * Create a sampler for a distribution specified by a vector of sigmas directly + * Create a sampler for a distribution specified by a vector of sigmas + * directly * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const Vector& sigmas, int32_t seed = 42u); + explicit Sampler(const Vector& sigmas, uint_fast64_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); + /// @} + /// @name access functions + /// @{ + + size_t dim() const { + assert(model_.get()); + return model_->dim(); + } + + Vector sigmas() const { + assert(model_.get()); + return model_->sigmas(); + } - /** access functions */ - 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 - * NOTE: not const due to need to update the underlying generator - */ - Vector sample(); + /// @} + /// @name basic functionality + /// @{ - /** - * 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 sampleNewModel(const noiseModel::Diagonal::shared_ptr& model); + /// sample from distribution + Vector sample() const; -protected: + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + explicit Sampler(uint_fast64_t seed = 42u); + Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const; + /// @} +#endif + + protected: /** given sigmas for a diagonal model, returns a sample */ - Vector sampleDiagonal(const Vector& sigmas); - + Vector sampleDiagonal(const Vector& sigmas) const; }; -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index bd718500e..5831d9048 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -10,8 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file testSampler + * @file testSampler.cpp + * @brief unit tests for Sampler class * @author Alex Cunningham + * @author Frank Dellaert */ #include @@ -22,14 +24,15 @@ using namespace gtsam; const double tol = 1e-5; +static const Vector3 kSigmas(1.0, 0.1, 0.0); + /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector3(1.0, 0.1, 0.0); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); + auto model = noiseModel::Diagonal::Sigmas(kSigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); - EXPECT(assert_equal(sigmas, sampler1.sigmas())); - EXPECT(assert_equal(sigmas, sampler2.sigmas())); + EXPECT(assert_equal(kSigmas, sampler1.sigmas())); + EXPECT(assert_equal(kSigmas, sampler2.sigmas())); EXPECT_LONGS_EQUAL(3, sampler1.dim()); EXPECT_LONGS_EQUAL(3, sampler2.dim()); Vector actual1 = sampler1.sample(); @@ -38,5 +41,8 @@ TEST(testSampler, basic) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 5fb9f78d7..649d0fe12 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner { const Bias estimatedBias_; // Create two samplers for acceleration and omega noise - mutable Sampler gyroSampler_, accSampler_; + Sampler gyroSampler_, accSampler_; public: ScenarioRunner(const Scenario& scenario, const SharedParams& p, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 45d009b1c..052bb3343 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -252,7 +252,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, is.seekg(0, ios::beg); // If asked, create a sampler with random number generator - Sampler sampler; + std::unique_ptr sampler; if (addNoise) { noiseModel::Diagonal::shared_ptr noise; if (model) @@ -261,7 +261,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, throw invalid_argument( "gtsam::load2D: invalid noise model for adding noise" "(current version assumes diagonal noise model)!"); - sampler = Sampler(noise); + sampler.reset(new Sampler(noise)); } // Parse the pose constraints @@ -289,7 +289,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, model = modelInFile; if (addNoise) - l1Xl2 = l1Xl2.retract(sampler.sample()); + l1Xl2 = l1Xl2.retract(sampler->sample()); // Insert vertices if pure odometry file if (!initial->exists(id1))