sampling from NoiseModel

.cproject

@@ -8,6 +8,7 @@
 <externalSettings/>
 <extensions>
 <extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
+<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
 <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
@@ -18,7 +19,7 @@
 <configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" name="MacOSX GCC" parent="org.eclipse.cdt.build.core.emptycfg">
 <folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
 <toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
-<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
+<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
 <builder arguments="" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
 <tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
 <tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">

cpp/NoiseModel.cpp

@@ -10,9 +10,14 @@
 #include <iostream>
 #include <typeinfo>
 #include <stdexcept>
+
 #include <boost/numeric/ublas/lu.hpp>
 #include <boost/numeric/ublas/io.hpp>
 #include <boost/foreach.hpp>
+#include <boost/random/linear_congruential.hpp>
+#include <boost/random/normal_distribution.hpp>
+#include <boost/random/variate_generator.hpp>
+
 #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<double> Normal;
+					Normal dist(0.0, this->sigmas_(i));
+					boost::variate_generator<boost::minstd_rand&, Normal> 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<double> Normal;
+			Normal dist(0.0, this->sigma_);
+			boost::variate_generator<boost::minstd_rand&, Normal> 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;

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;
+
   };
 
   /**