Improved interface for Sampler to allow alternate usage with noiseModels, removed references to SharedDiagonal

gtsam/linear/Sampler.cpp

@@ -22,23 +22,29 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Sampler::Sampler(const SharedDiagonal& model, int32_t seed)
-	: sigmas_(model->sigmas()), generator_(static_cast<unsigned>(seed))
+Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed)
+	: model_(model), generator_(static_cast<unsigned>(seed))
 {
 }
 
 /* ************************************************************************* */
 Sampler::Sampler(const Vector& sigmas, int32_t seed)
-	: sigmas_(sigmas), generator_(static_cast<unsigned>(seed))
+: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed))
 {
 }
 
 /* ************************************************************************* */
-Vector Sampler::sample() {
-	size_t d = dim();
+Sampler::Sampler(int32_t seed)
+: generator_(static_cast<unsigned>(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

gtsam/linear/Sampler.h

@@ -18,7 +18,7 @@
 
 #pragma once
 
-#include <gtsam/linear/SharedDiagonal.h>
+#include <gtsam/linear/NoiseModel.h>
 
 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);
+
 };
 
 }

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()));