From 0dc4f417e9387a93f93c4632b785e6e83a6ff1a3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 21 Jan 2010 08:31:18 +0000
Subject: [PATCH] sampling from NoiseModel

---
 .cproject          |  3 ++-
 cpp/NoiseModel.cpp | 27 +++++++++++++++++++++++++++
 cpp/NoiseModel.h   | 11 +++++++++++
 3 files changed, 40 insertions(+), 1 deletion(-)

diff --git a/.cproject b/.cproject
index 8f288d0da..6af662ee8 100644
--- a/.cproject
+++ b/.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">
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 <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;
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;
+
   };
 
   /**