diff --git a/.cproject b/.cproject
index 8f288d0da..6af662ee8 100644
--- a/.cproject
+++ b/.cproject
@@ -8,6 +8,7 @@
+
@@ -18,7 +19,7 @@
-
+
diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp
index 58b06d857..ad6ca3e36 100644
--- a/cpp/NoiseModel.cpp
+++ b/cpp/NoiseModel.cpp
@@ -10,9 +10,14 @@
#include
#include
#include
+
#include
#include
#include
+#include
+#include
+#include
+
#include "NoiseModel.h"
namespace ublas = boost::numeric::ublas;
@@ -146,6 +151,17 @@ namespace gtsam {
H = vector_scale(invsigmas_, H);
}
+ Vector Diagonal::sample() const {
+ Vector result(dim_);
+ for (int i = 0; i < dim_; i++) {
+ typedef boost::normal_distribution Normal;
+ Normal dist(0.0, this->sigmas_(i));
+ boost::variate_generator norm(generator, dist);
+ result(i) = norm();
+ }
+ return result;
+ }
+
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
@@ -266,6 +282,17 @@ namespace gtsam {
H *= invsigma_;
}
+ // faster version
+ Vector Isotropic::sample() const {
+ typedef boost::normal_distribution Normal;
+ Normal dist(0.0, this->sigma_);
+ boost::variate_generator norm(generator, dist);
+ Vector result(dim_);
+ for (int i = 0; i < dim_; i++)
+ result(i) = norm();
+ return result;
+ }
+
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
cout << "Unit (" << dim_ << ") " << name << endl;
diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h
index d000a8c73..7e6a0302b 100644
--- a/cpp/NoiseModel.h
+++ b/cpp/NoiseModel.h
@@ -218,6 +218,11 @@ namespace gtsam {
inline const Vector& sigmas() const { return sigmas_; }
inline double sigma(size_t i) const { return sigmas_(i); }
+ /**
+ * generate random variate
+ */
+ virtual Vector sample() const;
+
}; // Diagonal
@@ -331,6 +336,12 @@ namespace gtsam {
* Return standard deviation
*/
inline double sigma() const { return sigma_; }
+
+ /**
+ * generate random variate
+ */
+ virtual Vector sample() const;
+
};
/**