diff --git a/gtsam.h b/gtsam.h index 45c13ec72..566d18803 100644 --- a/gtsam.h +++ b/gtsam.h @@ -45,10 +45,13 @@ class Pose2 { class SharedGaussian { SharedGaussian(Matrix covariance); + void print(string s) const; }; class SharedDiagonal { SharedDiagonal(Vector sigmas); + void print(string s) const; + Vector sample() const; }; class VectorValues { diff --git a/gtsam/linear/SharedDiagonal.h b/gtsam/linear/SharedDiagonal.h index 13707393a..b839cca1a 100644 --- a/gtsam/linear/SharedDiagonal.h +++ b/gtsam/linear/SharedDiagonal.h @@ -44,6 +44,12 @@ namespace gtsam { // note, deliberately not in noiseModel namespace noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { } + /// Print + inline void print(const std::string &s) const { (*this)->print(s); } + + /// Generate a sample + inline Vector sample() const { return (*this)->sample(); } + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index e74a7a647..bb9c3546c 100644 --- a/gtsam/linear/SharedGaussian.h +++ b/gtsam/linear/SharedGaussian.h @@ -55,6 +55,9 @@ namespace gtsam { // note, deliberately not in noiseModel namespace Base(noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s)) {} #endif + /// Print + inline void print(const std::string &s) const { (*this)->print(s); } + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 38e309ab5..1e519b6dd 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -134,6 +134,14 @@ TEST(NoiseModel, equals) EXPECT(assert_inequal(*i1,*i2)); } +/* ************************************************************************* */ +TEST(NoiseModel, sample) { + Vector s = Vector_(3,1.0,2.0,3.0); + SharedDiagonal model = sharedSigmas(s); + Vector v = model->sample(); + // no check as no way yet to set random seed +} + /* ************************************************************************* */ TEST(NoiseModel, ConstrainedMixed ) {