case change: SharedGaussian and SharedDiagonal are now classes with their own header file. Needed for MATLAB TORO hail Mary

cpp/BetweenFactor.h

@@ -33,7 +33,7 @@ namespace gtsam {
 
 		/** Constructor */
 		BetweenFactor(const Key& key1, const Key& key2, const T& measured,
-				const sharedGaussian& model) :
+				const SharedGaussian& model) :
 			Base(model, key1, key2), measured_(measured) {
 		}
 

cpp/GaussianFactor.cpp

@@ -323,7 +323,7 @@ GaussianFactor::eliminate(const Symbol& key) const
 	Matrix Ab = matrix_augmented(ordering,false);
 
 	// Use in-place QR on dense Ab appropriate to NoiseModel
-	sharedDiagonal noiseModel = model_->QR(Ab);
+	SharedDiagonal noiseModel = model_->QR(Ab);
 
 	// get dimensions of the eliminated variable
 	// TODO: this is another map find that should be avoided !

cpp/GaussianFactor.h

@@ -18,7 +18,7 @@
 #include "Factor.h"
 #include "Matrix.h"
 #include "VectorConfig.h"
-#include "NoiseModel.h"
+#include "SharedDiagonal.h"
 #include "SymbolMap.h"
 
 namespace gtsam {
@@ -40,7 +40,7 @@ public:
 
 protected:
 
-	sharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
+	SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
 	SymbolMap<Matrix> As_; // linear matrices
 	Vector b_; // right-hand-side
 
@@ -57,7 +57,7 @@ public:
 
 	/** Construct unary factor */
 	GaussianFactor(const Symbol& key1, const Matrix& A1,
-			const Vector& b, const sharedDiagonal& model) :
+			const Vector& b, const SharedDiagonal& model) :
 		b_(b), model_(model) {
 		As_.insert(make_pair(key1, A1));
 	}
@@ -65,7 +65,7 @@ public:
 	/** Construct binary factor */
 	GaussianFactor(const Symbol& key1, const Matrix& A1,
 			const Symbol& key2, const Matrix& A2,
-			const Vector& b, const sharedDiagonal& model) :
+			const Vector& b, const SharedDiagonal& model) :
 		b_(b), model_(model)  {
 		As_.insert(make_pair(key1, A1));
 		As_.insert(make_pair(key2, A2));
@@ -75,7 +75,7 @@ public:
 	GaussianFactor(const Symbol& key1, const Matrix& A1,
 			const Symbol& key2, const Matrix& A2,
 			const Symbol& key3, const Matrix& A3,
-			const Vector& b, const sharedDiagonal& model) :
+			const Vector& b, const SharedDiagonal& model) :
 		b_(b), model_(model)  {
 		As_.insert(make_pair(key1, A1));
 		As_.insert(make_pair(key2, A2));
@@ -84,7 +84,7 @@ public:
 
 	/** Construct an n-ary factor */
 	GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
-	    const Vector &b, const sharedDiagonal& model) :
+	    const Vector &b, const SharedDiagonal& model) :
 	    b_(b), model_(model)  {
 	  for(unsigned int i=0; i<terms.size(); i++)
 	    As_.insert(terms[i]);
@@ -127,7 +127,7 @@ public:
 	const Vector& get_sigmas() const {	return model_->sigmas();	}
 
 	/** get a copy of model */
-	const sharedDiagonal& get_model() const { return model_;  }
+	const SharedDiagonal& get_model() const { return model_;  }
 
 	/**
 	 * get a copy of the A matrix from a specific node

cpp/GaussianFactorGraph.cpp

@@ -181,7 +181,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
 	FOREACH_PAIR(key,dim,vs) {
 		Matrix A = eye(dim);
 		Vector b = zero(dim);
-		sharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
+		SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
 		sharedFactor prior(new GaussianFactor(key,A,b, model));
 		result.push_back(prior);
 	}

cpp/GaussianFactorGraph.h

@@ -48,14 +48,14 @@ namespace gtsam {
 
   	/** Add a unary factor */
     inline void add(const Symbol& key1, const Matrix& A1,
-  			const Vector& b, const sharedDiagonal& model) {
+  			const Vector& b, const SharedDiagonal& model) {
     	push_back(sharedFactor(new GaussianFactor(key1,A1,b,model)));
   	}
 
   	/** Add a binary factor */
     inline void add(const Symbol& key1, const Matrix& A1,
   			const Symbol& key2, const Matrix& A2,
-  			const Vector& b, const sharedDiagonal& model) {
+  			const Vector& b, const SharedDiagonal& model) {
     	push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model)));
   	}
 
@@ -63,13 +63,13 @@ namespace gtsam {
     inline void add(const Symbol& key1, const Matrix& A1,
   			const Symbol& key2, const Matrix& A2,
   			const Symbol& key3, const Matrix& A3,
-  			const Vector& b, const sharedDiagonal& model) {
+  			const Vector& b, const SharedDiagonal& model) {
     	push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model)));
   	}
 
   	/** Add an n-ary factor */
     inline void add(const std::vector<std::pair<Symbol, Matrix> > &terms,
-  	    const Vector &b, const sharedDiagonal& model) {
+  	    const Vector &b, const SharedDiagonal& model) {
     	push_back(sharedFactor(new GaussianFactor(terms,b,model)));
   	}
 

cpp/Makefile.am

@@ -88,7 +88,7 @@ testBinaryBayesNet_SOURCES    = testBinaryBayesNet.cpp
 testBinaryBayesNet_LDADD      = libgtsam.la 
 
 # Gaussian inference 
-headers += GaussianFactorSet.h 
+headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h
 sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
 check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
 testVectorConfig_SOURCES        = testVectorConfig.cpp

cpp/NoiseModel.cpp

@@ -24,6 +24,7 @@
 #include <boost/random/variate_generator.hpp>
 
 #include "NoiseModel.h"
+#include "SharedDiagonal.h"
 
 namespace ublas = boost::numeric::ublas;
 typedef ublas::matrix_column<Matrix> column;
@@ -121,7 +122,7 @@ namespace gtsam {
 		}
 
 		// General QR, see also special version in Constrained
-		sharedDiagonal Gaussian::QR(Matrix& Ab) const {
+		SharedDiagonal Gaussian::QR(Matrix& Ab) const {
 			// get size(A) and maxRank
 			// TODO: really no rank problems ?
 			size_t m = Ab.size1(), n = Ab.size2()-1;
@@ -207,7 +208,7 @@ namespace gtsam {
 		// Special version of QR for Constrained calls slower but smarter code
 		// that deals with possibly zero sigmas
 		// It is Gram-Schmidt orthogonalization rather than Householder
-		sharedDiagonal Diagonal::QR(Matrix& Ab) const {
+		SharedDiagonal Diagonal::QR(Matrix& Ab) const {
 			// get size(A) and maxRank
 			size_t m = Ab.size1(), n = Ab.size2()-1;
 			size_t maxRank = min(m,n);

cpp/NoiseModel.h

@@ -15,7 +15,7 @@
 
 namespace gtsam {
 
-	class sharedDiagonal; // forward declare, defined at end
+	class SharedDiagonal; // forward declare, defined at end
 
 	namespace noiseModel {
 
@@ -146,7 +146,7 @@ namespace gtsam {
      * @param Ab is the m*(n+1) augmented system matrix [A b]
      * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
      */
-    virtual sharedDiagonal QR(Matrix& Ab) const;
+    virtual SharedDiagonal QR(Matrix& Ab) const;
 
 		/**
 		 * Return R itself, but note that Whiten(H) is cheaper than R*H
@@ -210,7 +210,7 @@ namespace gtsam {
     /**
      * Apply QR factorization to the system [A b], taking into account constraints
      */
-		virtual sharedDiagonal QR(Matrix& Ab) const;
+		virtual SharedDiagonal QR(Matrix& Ab) const;
 
 		/**
      * Return standard deviations (sqrt of diagonal)
@@ -373,42 +373,5 @@ namespace gtsam {
 
 	} // namespace noiseModel
 
-	using namespace noiseModel;
-
-	// TODO: very ugly, just keep shared pointers and use Sigma/Sigmas everywhere
-
-	// A useful convenience class to refer to a shared Gaussian model
-	// Define GTSAM_MAGIC_GAUSSIAN to desired dimension to have access to slightly
-	// dangerous and non-shared (inefficient, wasteful) noise models. Only in tests!
-	struct sharedGaussian : public Gaussian::shared_ptr {
-		sharedGaussian() {}
-		sharedGaussian(const    Gaussian::shared_ptr& p):Gaussian::shared_ptr(p) {}
-		sharedGaussian(const    Diagonal::shared_ptr& p):Gaussian::shared_ptr(p) {}
-		sharedGaussian(const Constrained::shared_ptr& p):Gaussian::shared_ptr(p) {}
-		sharedGaussian(const   Isotropic::shared_ptr& p):Gaussian::shared_ptr(p) {}
-		sharedGaussian(const        Unit::shared_ptr& p):Gaussian::shared_ptr(p) {}
-#ifdef GTSAM_MAGIC_GAUSSIAN
-		sharedGaussian(const Matrix& covariance):Gaussian::shared_ptr(Gaussian::Covariance(covariance)) {}
-		sharedGaussian(const Vector& sigmas):Gaussian::shared_ptr(Diagonal::Sigmas(sigmas)) {}
-		sharedGaussian(const double& s):Gaussian::shared_ptr(Isotropic::Sigma(GTSAM_MAGIC_GAUSSIAN,s)) {}
-#endif
-		};
-
-	// A useful convenience class to refer to a shared Diagonal model
-	// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
-	// that call Sigmas and Sigma, respectively.
-	struct sharedDiagonal : public Diagonal::shared_ptr {
-		sharedDiagonal() {}
-		sharedDiagonal(const    Diagonal::shared_ptr& p):Diagonal::shared_ptr(p) {}
-		sharedDiagonal(const Constrained::shared_ptr& p):Diagonal::shared_ptr(p) {}
-		sharedDiagonal(const   Isotropic::shared_ptr& p):Diagonal::shared_ptr(p) {}
-		sharedDiagonal(const        Unit::shared_ptr& p):Diagonal::shared_ptr(p) {}
-		sharedDiagonal(const Vector& sigmas):Diagonal::shared_ptr(Diagonal::Sigmas(sigmas)) {}
-		};
-
-	// TODO: make these the ones really used in unit tests
-	inline sharedDiagonal sharedSigmas(const Vector& sigmas) { return Diagonal::Sigmas(sigmas);}
-	inline sharedDiagonal sharedSigma(int dim, double sigma) { return Isotropic::Sigma(dim,sigma);}
-
-
 } // namespace gtsam
+

cpp/NonlinearConstraint-inl.h

@@ -122,12 +122,12 @@ NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const Vect
 
 	// construct probabilistic factor
 	Matrix A1 = vector_scale(lambda, grad);
-	sharedDiagonal probModel = sharedSigma(this->p_,1.0);
+	SharedDiagonal probModel = sharedSigma(this->p_,1.0);
 	GaussianFactor::shared_ptr factor(new
 			GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
 
 	// construct the constraint
-	sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
+	SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
 	GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel));
 
 	return std::make_pair(factor, constraint);
@@ -222,12 +222,12 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
 	// construct probabilistic factor
 	Matrix A1 = vector_scale(lambda, grad1);
 	Matrix A2 = vector_scale(lambda, grad2);
-	sharedDiagonal probModel = sharedSigma(this->p_,1.0);
+	SharedDiagonal probModel = sharedSigma(this->p_,1.0);
 	GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2,
 			this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
 
 	// construct the constraint
-	sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
+	SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
 	GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1,
 			key2_, grad2, -1.0 * g, constraintModel));
 

cpp/NonlinearEquality.h

@@ -83,7 +83,7 @@ namespace gtsam {
 			Matrix A;
 			Vector b = - evaluateError(xj, A);
 			// TODO pass unwhitened + noise model to Gaussian factor
-			sharedDiagonal model = noiseModel::Constrained::All(b.size());
+			SharedDiagonal model = noiseModel::Constrained::All(b.size());
 			return	GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, model));
 		}
 

cpp/NonlinearFactor.h

@@ -18,7 +18,7 @@
 #include "Factor.h"
 #include "Vector.h"
 #include "Matrix.h"
-#include "NoiseModel.h"
+#include "SharedGaussian.h"
 #include "GaussianFactor.h"
 
 #define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \
@@ -43,7 +43,7 @@ namespace gtsam {
 
 		typedef NonlinearFactor<Config> This;
 
-		sharedGaussian noiseModel_; /** Noise model */
+		SharedGaussian noiseModel_; /** Noise model */
 		std::list<Symbol> keys_; /** cached keys */
 
 	public:
@@ -56,7 +56,7 @@ namespace gtsam {
 		 *  Constructor
 		 *  @param noiseModel shared pointer to a noise model
 		 */
-		NonlinearFactor(const sharedGaussian& noiseModel) :
+		NonlinearFactor(const SharedGaussian& noiseModel) :
 			noiseModel_(noiseModel) {
 		}
 
@@ -148,7 +148,7 @@ namespace gtsam {
 		 *  @param z measurement
 		 *  @param key by which to look up X value in Config
 		 */
-		NonlinearFactor1(const sharedGaussian& noiseModel,
+		NonlinearFactor1(const SharedGaussian& noiseModel,
 				const Key& key1) :
 			Base(noiseModel), key_(key1) {
 			this->keys_.push_back(key_);
@@ -242,7 +242,7 @@ namespace gtsam {
 		 * @param j1 key of the first variable
 		 * @param j2 key of the second variable
 		 */
-		NonlinearFactor2(const sharedGaussian& noiseModel, Key1 j1,
+		NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1,
 				Key2 j2) :
 			Base(noiseModel), key1_(j1), key2_(j2) {
 			this->keys_.push_back(key1_);

cpp/PriorFactor.h

@@ -31,7 +31,7 @@ namespace gtsam {
 
 		/** Constructor */
 		PriorFactor(const Key& key, const T& prior,
-				const sharedGaussian& model) :
+				const SharedGaussian& model) :
 			Base(model, key), prior_(prior) {
 		}
 

cpp/SharedDiagonal.h

@@ -0,0 +1,45 @@
+/*
+ * SharedDiagonal.h
+ * @brief Class that wraps a shared noise model with diagonal covariance
+ * @Author: Frank Dellaert
+ * Created on: Jan 22, 2010
+ */
+
+#pragma once
+
+#include "NoiseModel.h"
+
+namespace gtsam { // note, deliberately not in noiseModel namespace
+
+	// A useful convenience class to refer to a shared Diagonal model
+	// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
+	// that call Sigmas and Sigma, respectively.
+	struct SharedDiagonal: public noiseModel::Diagonal::shared_ptr {
+		SharedDiagonal() {
+		}
+		SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) :
+			noiseModel::Diagonal::shared_ptr(p) {
+		}
+		SharedDiagonal(const noiseModel::Constrained::shared_ptr& p) :
+			noiseModel::Diagonal::shared_ptr(p) {
+		}
+		SharedDiagonal(const noiseModel::Isotropic::shared_ptr& p) :
+			noiseModel::Diagonal::shared_ptr(p) {
+		}
+		SharedDiagonal(const noiseModel::Unit::shared_ptr& p) :
+			noiseModel::Diagonal::shared_ptr(p) {
+		}
+		SharedDiagonal(const Vector& sigmas) :
+			noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
+		}
+	};
+
+	// TODO: make these the ones really used in unit tests
+	inline SharedDiagonal sharedSigmas(const Vector& sigmas) {
+		return noiseModel::Diagonal::Sigmas(sigmas);
+	}
+	inline SharedDiagonal sharedSigma(int dim, double sigma) {
+		return noiseModel::Isotropic::Sigma(dim, sigma);
+	}
+
+}

cpp/SharedGaussian.h

@@ -0,0 +1,59 @@
+/*
+ * SharedGaussian.h
+ * @brief Class that wraps a shared noise model with diagonal covariance
+ * @Author: Frank Dellaert
+ * Created on: Jan 22, 2010
+ */
+
+#pragma once
+
+#include "NoiseModel.h"
+
+namespace gtsam { // note, deliberately not in noiseModel namespace
+
+	/**
+	 * A useful convenience class to refer to a shared Gaussian model
+	 * Also needed to make noise models in matlab
+	 */
+	struct SharedGaussian: public noiseModel::Gaussian::shared_ptr {
+		SharedGaussian() {
+		}
+		// TODO: better way ?
+		SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) :
+			noiseModel::Gaussian::shared_ptr(p) {
+		}
+		SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) :
+			noiseModel::Gaussian::shared_ptr(p) {
+		}
+		SharedGaussian(const noiseModel::Constrained::shared_ptr& p) :
+			noiseModel::Gaussian::shared_ptr(p) {
+		}
+		SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) :
+			noiseModel::Gaussian::shared_ptr(p) {
+		}
+		SharedGaussian(const noiseModel::Unit::shared_ptr& p) :
+			noiseModel::Gaussian::shared_ptr(p) {
+		}
+
+		// Define GTSAM_MAGIC_GAUSSIAN to have access to slightly
+		// dangerous and non-shared (inefficient, wasteful) noise models.
+		// Intended to be used only in tests (if you must) and the MATLAB wrapper
+#ifdef GTSAM_MAGIC_GAUSSIAN
+		SharedGaussian(const Matrix& covariance) :
+			noiseModel::Gaussian::shared_ptr(noiseModel::Gaussian::Covariance(covariance)) {
+		}
+		SharedGaussian(const Vector& sigmas) :
+			noiseModel::Gaussian::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
+		}
+#endif
+// Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians
+// Not intended for human use, only for backwards compatibility of old unit tests
+#ifdef GTSAM_DANGEROUS_GAUSSIAN
+		SharedGaussian(const double& s) :
+			noiseModel::Gaussian::shared_ptr(noiseModel::Isotropic::Sigma(
+					GTSAM_DANGEROUS_GAUSSIAN, s)) {
+		}
+#endif
+	};
+
+}

cpp/Simulated3D.h

@@ -49,7 +49,7 @@ namespace simulated3D {
 		Vector z_;
 
 		Point2Prior3D(const Vector& z,
-					const sharedGaussian& model, const PoseKey& j) :
+					const SharedGaussian& model, const PoseKey& j) :
 				NonlinearFactor1<VectorConfig, PoseKey, Vector> (model, j), z_(z) {
 			}
 
@@ -66,7 +66,7 @@ namespace simulated3D {
 		Vector z_;
 
 		Simulated3DMeasurement(const Vector& z,
-					const sharedGaussian& model, PoseKey& j1, PointKey j2) :
+					const SharedGaussian& model, PoseKey& j1, PointKey j2) :
 				z_(z),
 						NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, Vector> (
 								model, j1, j2) {

cpp/gtsam-broken.h

@@ -0,0 +1,60 @@
+// These are currently broken
+// Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class
+// We also have to solve the shared pointer mess to avoid duplicate methods
+
+class Point2Prior {
+  Point2Prior(Vector mu, double sigma, string key);
+  Vector error_vector(const VectorConfig& c) const;
+  GaussianFactor* linearize(const VectorConfig& c) const;
+  double sigma();
+  Vector measurement();
+  double error(const VectorConfig& c) const;
+  void print(string s) const;
+};
+
+class Simulated2DOdometry {
+  Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
+  Vector error_vector(const VectorConfig& c) const;
+  GaussianFactor* linearize(const VectorConfig& c) const;
+  double sigma();
+  Vector measurement();
+  double error(const VectorConfig& c) const;
+  void print(string s) const;
+};
+
+class Simulated2DMeasurement {
+  Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
+  Vector error_vector(const VectorConfig& c) const;
+  GaussianFactor* linearize(const VectorConfig& c) const;
+  double sigma();
+  Vector measurement();
+  double error(const VectorConfig& c) const;
+  void print(string s) const;
+};
+
+class Pose2Config{
+	Pose2Config();
+	Pose2 get(string key) const;
+	void insert(string name, const Pose2& val);
+    void print(string s) const;
+    void clear();
+    int size();
+};
+
+class Pose2Factor {
+	Pose2Factor(string key1, string key2,
+			const Pose2& measured, Matrix measurement_covariance);
+	void print(string name) const;
+	double error(const Pose2Config& c) const;
+	size_t size() const;
+	GaussianFactor* linearize(const Pose2Config& config) const;
+};
+
+class Pose2Graph{
+	Pose2Graph();
+	void print(string s) const;
+	GaussianFactorGraph* linearize_(const Pose2Config& config) const;
+	void push_back(Pose2Factor* factor);
+};
+
+

cpp/gtsam.h

@@ -1,3 +1,12 @@
+class SharedGaussian {
+		SharedGaussian(Matrix covariance);
+		SharedGaussian(Vector sigmas);
+};
+
+class SharedDiagonal {
+	SharedDiagonal(Vector sigmas);
+};
+
 class Ordering {
   Ordering();
   Ordering(string key);
@@ -25,22 +34,17 @@ class VectorConfig {
   void clear();
 };
 
-class GaussianFactorSet {
-  GaussianFactorSet();
-  void push_back(GaussianFactor* factor);
-};
-
 class GaussianFactor {
   GaussianFactor(string key1,
 	       Matrix A1,
 	       Vector b_in,
-	       double sigma);
+	       const SharedDiagonal& model);
   GaussianFactor(string key1,
 	       Matrix A1,
 	       string key2,
 	       Matrix A2,
 	       Vector b_in,
-	       double sigma);
+	       const SharedDiagonal& model);
   GaussianFactor(string key1,
 	       Matrix A1,
 	       string key2,
@@ -48,7 +52,7 @@ class GaussianFactor {
 	       string key3,
 	       Matrix A3,
 	       Vector b_in,
-	       double sigma);
+	       const SharedDiagonal& model);
   bool empty() const;
   Vector get_b() const;
   Matrix get_A(string key) const;
@@ -59,6 +63,11 @@ class GaussianFactor {
   pair<Matrix,Vector> matrix(const Ordering& ordering) const;
 };
 
+class GaussianFactorSet {
+  GaussianFactorSet();
+  void push_back(GaussianFactor* factor);
+};
+
 class GaussianConditional {
   GaussianConditional();
   GaussianConditional(string key,
@@ -132,36 +141,6 @@ class Point3 {
   void print(string s) const;
 }; 
 
-class Point2Prior {
-  Point2Prior(Vector mu, double sigma, string key);
-  Vector error_vector(const VectorConfig& c) const;
-  GaussianFactor* linearize(const VectorConfig& c) const;
-  double sigma();
-  Vector measurement();
-  double error(const VectorConfig& c) const;
-  void print(string s) const;
-};
-
-class Simulated2DOdometry {
-  Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
-  Vector error_vector(const VectorConfig& c) const;
-  GaussianFactor* linearize(const VectorConfig& c) const;
-  double sigma();
-  Vector measurement();
-  double error(const VectorConfig& c) const;
-  void print(string s) const;
-};
-
-class Simulated2DMeasurement {
-  Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
-  Vector error_vector(const VectorConfig& c) const;
-  GaussianFactor* linearize(const VectorConfig& c) const;
-  double sigma();
-  Vector measurement();
-  double error(const VectorConfig& c) const;
-  void print(string s) const;
-};
-
 class Pose2 {
   Pose2();
   Pose2(const Pose2& pose);
@@ -179,30 +158,3 @@ class Pose2 {
   Point2 t() const;
   Rot2 r() const;
 };
-
-class Pose2Config{
-	Pose2Config();
-	Pose2 get(string key) const;
-	void insert(string name, const Pose2& val);
-    void print(string s) const;
-    void clear();
-    int size();
-};
-
-class Pose2Factor {
-	Pose2Factor(string key1, string key2,
-			const Pose2& measured, Matrix measurement_covariance);
-	void print(string name) const;
-	double error(const Pose2Config& c) const;
-	size_t size() const;
-	GaussianFactor* linearize(const Pose2Config& config) const;
-};
-
-class Pose2Graph{
-	Pose2Graph();
-	void print(string s) const;
-	GaussianFactorGraph* linearize_(const Pose2Config& config) const;
-	void push_back(Pose2Factor* factor);
-};
-
-

cpp/planarSLAM.cpp

@@ -26,19 +26,19 @@ namespace gtsam {
 		}
 
 		void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Odometry(i, j, z, model));
 			push_back(factor);
 		}
 
 		void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Bearing(i, j, z, model));
 			push_back(factor);
 		}
 
 		void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Range(i, j, z, model));
 			push_back(factor);
 		}

cpp/planarSLAM.h

@@ -35,7 +35,7 @@ namespace gtsam {
 
 		BearingFactor(); /* Default constructor */
 		BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
-				const sharedGaussian& model) :
+				const SharedGaussian& model) :
 			Base(model, i, j), z_(z) {
 		}
 
@@ -64,7 +64,7 @@ namespace gtsam {
 		RangeFactor(); /* Default constructor */
 
 		RangeFactor(const PoseKey& i, const PointKey& j, double z,
-				const sharedGaussian& model) :
+				const SharedGaussian& model) :
 			Base(model, i, j), z_(z) {
 		}
 
@@ -94,11 +94,11 @@ namespace gtsam {
 		struct Graph: public NonlinearFactorGraph<Config> {
 			void addPoseConstraint(const PoseKey& i, const Pose2& p);
 			void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
-					const sharedGaussian& model);
+					const SharedGaussian& model);
 			void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
-					const sharedGaussian& model);
+					const SharedGaussian& model);
 			void addRange(const PoseKey& i, const PointKey& j, double z,
-					const sharedGaussian& model);
+					const SharedGaussian& model);
 		};
 
 		// Optimizer

cpp/pose2SLAM.cpp

@@ -30,13 +30,13 @@ namespace gtsam {
 
 		/* ************************************************************************* */
 		void Graph::addPrior(const Key& i, const Pose2& p,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Prior(i, p, model));
 			push_back(factor);
 		}
 
 		void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Constraint(i, j, z, model));
 			push_back(factor);
 		}

cpp/pose2SLAM.h

@@ -42,8 +42,8 @@ namespace gtsam {
 		struct Graph: public NonlinearFactorGraph<Config> {
 			typedef BetweenFactor<Config, Key, Pose2> Constraint;
 			typedef Pose2 Pose;
-			void addPrior(const Key& i, const Pose2& p, const sharedGaussian& model);
+			void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model);
-			void addConstraint(const Key& i, const Key& j, const Pose2& z, const sharedGaussian& model);
+			void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model);
 			void addHardConstraint(const Key& i, const Pose2& p);
 		};
 

cpp/pose3SLAM.cpp

@@ -33,13 +33,13 @@ namespace gtsam {
 
 		/* ************************************************************************* */
 		void Graph::addPrior(const Key& i, const Pose3& p,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Prior(i, p, model));
 			push_back(factor);
 		}
 
 		void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z,
-				const sharedGaussian& model) {
+				const SharedGaussian& model) {
 			sharedFactor factor(new Constraint(i, j, z, model));
 			push_back(factor);
 		}

cpp/pose3SLAM.h

@@ -41,9 +41,9 @@ namespace gtsam {
 		// Graph
 		struct Graph: public NonlinearFactorGraph<Config> {
 			void addPrior(const Key& i, const Pose3& p,
-					const sharedGaussian& model);
+					const SharedGaussian& model);
 			void addConstraint(const Key& i, const Key& j, const Pose3& z,
-					const sharedGaussian& model);
+					const SharedGaussian& model);
 			void addHardConstraint(const Key& i, const Pose3& p);
 		};
 

cpp/simulated2D.h

@@ -56,7 +56,7 @@ namespace gtsam {
 
 			Point2 z_;
 
-			Prior(const Point2& z, const sharedGaussian& model, const PoseKey& key) :
+			Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) :
 				NonlinearFactor1<Config, PoseKey, Point2> (model, key), z_(z) {
 			}
 
@@ -74,7 +74,7 @@ namespace gtsam {
 				Point2> {
 			Point2 z_;
 
-			Odometry(const Point2& z, const sharedGaussian& model, const PoseKey& j1,
+			Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1,
 					const PoseKey& j2) :
 				z_(z), NonlinearFactor2<Config, PoseKey, Point2, PoseKey, Point2> (
 						model, j1, j2) {
@@ -95,7 +95,7 @@ namespace gtsam {
 
 			Point2 z_;
 
-			Measurement(const Point2& z, const sharedGaussian& model,
+			Measurement(const Point2& z, const SharedGaussian& model,
 					const PoseKey& j1, const PointKey& j2) :
 				z_(z), NonlinearFactor2<Config, PoseKey, Point2, PointKey, Point2> (
 						model, j1, j2) {

cpp/smallExample.cpp

@@ -12,8 +12,6 @@
 
 using namespace std;
 
-// TODO: DANGEROUS, create shared pointers
-#define GTSAM_MAGIC_GAUSSIAN 2
 #define GTSAM_MAGIC_KEY
 
 #include "Ordering.h"
@@ -30,9 +28,10 @@ namespace example {
 
 	typedef boost::shared_ptr<NonlinearFactor<Config> > shared;
 
-	static sharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
+	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
-	static sharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
+	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
-	static sharedDiagonal constraintModel = noiseModel::Constrained::All(2);
+	static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
+	static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
 
 	/* ************************************************************************* */
 	boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
@@ -41,27 +40,23 @@ namespace example {
 				new Graph);
 
 		// prior on x1
-		double sigma1 = 0.1;
 		Point2 mu;
-		shared f1(new simulated2D::Prior(mu, sigma1, 1));
+		shared f1(new simulated2D::Prior(mu, sigma0_1, 1));
 		nlfg->push_back(f1);
 
 		// odometry between x1 and x2
-		double sigma2 = 0.1;
 		Point2 z2(1.5, 0);
-		shared f2(new simulated2D::Odometry(z2, sigma2, 1, 2));
+		shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2));
 		nlfg->push_back(f2);
 
 		// measurement between x1 and l1
-		double sigma3 = 0.2;
 		Point2 z3(0, -1);
-		shared f3(new simulated2D::Measurement(z3, sigma3, 1, 1));
+		shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1));
 		nlfg->push_back(f3);
 
 		// measurement between x2 and l1
-		double sigma4 = 0.2;
 		Point2 z4(-1.5, -1.);
-		shared f4(new simulated2D::Measurement(z4, sigma4, 2, 1));
+		shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1));
 		nlfg->push_back(f4);
 
 		return nlfg;
@@ -193,7 +188,7 @@ namespace example {
 
 			Point2 z_;
 
-			UnaryFactor(const Point2& z, const sharedGaussian& model,
+			UnaryFactor(const Point2& z, const SharedGaussian& model,
 					const simulated2D::PoseKey& key) :
 				gtsam::NonlinearFactor1<Config, simulated2D::PoseKey,
 						Point2>(model, key), z_(z) {
@@ -228,28 +223,25 @@ namespace example {
 	/* ************************************************************************* */
 	pair<Graph, Config> createNonlinearSmoother(int T) {
 
-		// noise on measurements and odometry, respectively
-		double sigma1 = 1, sigma2 = 1;
-
 		// Create
 		Graph nlfg;
 		Config poses;
 
 		// prior on x1
 		Point2 x1(1.0, 0.0);
-		shared prior(new simulated2D::Prior(x1, sigma1, 1));
+		shared prior(new simulated2D::Prior(x1, sigma1_0, 1));
 		nlfg.push_back(prior);
 		poses.insert(simulated2D::PoseKey(1), x1);
 
 		for (int t = 2; t <= T; t++) {
 			// odometry between x_t and x_{t-1}
 			Point2 odo(1.0, 0.0);
-			shared odometry(new simulated2D::Odometry(odo, sigma2, t - 1, t));
+			shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t));
 			nlfg.push_back(odometry);
 
 			// measurement on x_t is like perfect GPS
 			Point2 xt(t, 0);
-			shared measurement(new simulated2D::Prior(xt, sigma1, t));
+			shared measurement(new simulated2D::Prior(xt, sigma1_0, t));
 			nlfg.push_back(measurement);
 
 			// initial estimate
@@ -524,17 +516,14 @@ namespace example {
 		NonlinearFactorGraph<Config> nlfg;
 
 		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
-		double sigma0 = 1e-3;
+		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1)));
-		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sigma0, key(1,1)));
 		nlfg.push_back(constraint);
 
-		double sigma = 0.01;
-
 		// Create horizontal constraints, 1...N*(N-1)
 		Point2 z1(1.0, 0.0); // move right
 		for (size_t x = 1; x < N; x++)
 			for (size_t y = 1; y <= N; y++) {
-				shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y)));
+				shared f(new simulated2D::Odometry(z1, sharedSigma(2,0.01), key(x, y), key(x + 1, y)));
 				nlfg.push_back(f);
 			}
 
@@ -542,7 +531,7 @@ namespace example {
 		Point2 z2(0.0, 1.0); // move up
 		for (size_t x = 1; x <= N; x++)
 			for (size_t y = 1; y < N; y++) {
-				shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1)));
+				shared f(new simulated2D::Odometry(z2, sharedSigma(2,0.01), key(x, y), key(x, y + 1)));
 				nlfg.push_back(f);
 			}
 

cpp/testGaussianFactor.cpp

@@ -27,7 +27,7 @@ using namespace gtsam;
 using namespace example;
 using namespace boost;
 
-static sharedDiagonal
+static SharedDiagonal
 	sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
 	constraintModel = noiseModel::Constrained::All(2);
 
@@ -225,7 +225,7 @@ TEST( NonlinearFactorGraph, combine2){
 TEST( GaussianFactor, linearFactorN){
 	Matrix I = eye(2);
   vector<GaussianFactor::shared_ptr> f;
-  sharedDiagonal model = sharedSigma(2,1.0);
+  SharedDiagonal model = sharedSigma(2,1.0);
   f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2,
 			10.0, 5.0), model)));
 	f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I,

cpp/testGaussianFactorGraph.cpp

@@ -263,7 +263,7 @@ TEST( GaussianFactorGraph, add_priors )
   GaussianFactorGraph expected = createGaussianFactorGraph();
   Matrix A = eye(2);
   Vector b = zero(2);
-  sharedDiagonal sigma = sharedSigma(2,3.0);
+  SharedDiagonal sigma = sharedSigma(2,3.0);
   expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma)));
   expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma)));
   expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
@@ -629,7 +629,7 @@ TEST( GaussianFactorGraph, elimination )
 	GaussianFactorGraph fg;
 	Matrix Ap = eye(1), An = eye(1) * -1;
 	Vector b = Vector_(1, 0.0);
-  sharedDiagonal sigma = sharedSigma(2,2.0);
+  SharedDiagonal sigma = sharedSigma(2,2.0);
 	fg.add("x1", An, "x2", Ap, b, sigma);
 	fg.add("x1", Ap, b, sigma);
 	fg.add("x2", Ap, b, sigma);
@@ -735,7 +735,7 @@ TEST( GaussianFactorGraph, constrained_multi2 )
 
 /* ************************************************************************* */
 
-sharedDiagonal model = sharedSigma(2,1);
+SharedDiagonal model = sharedSigma(2,1);
 
 TEST( GaussianFactorGraph, findMinimumSpanningTree )
 {

cpp/testIterative.cpp

@@ -10,7 +10,7 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 
 // TODO: DANGEROUS, create shared pointers
-#define GTSAM_MAGIC_GAUSSIAN 3
+#define GTSAM_DANGEROUS_GAUSSIAN 3
 #define GTSAM_MAGIC_KEY
 
 #include "Ordering.h"
@@ -114,8 +114,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
 	config.insert(2, Pose2(1.5,0.,0.));
 
 	Pose2Graph graph;
-	graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
+	graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10));
-	graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
+	graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1));
 
 	VectorConfig zeros;
 	zeros.insert("x1",zero(3));
@@ -140,8 +140,8 @@ TEST( Iterative, subgraphPCG )
 	theta_bar.insert(2, Pose2(1.5,0.,0.));
 
 	Pose2Graph graph;
-	graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
+	graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10));
-	graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
+	graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1));
 
 	VectorConfig zeros;
 	zeros.insert("x1",zero(3));

cpp/testMatrix.cpp

@@ -11,7 +11,6 @@
 #include <boost/foreach.hpp>
 #include <boost/numeric/ublas/matrix_proxy.hpp>
 #include "Matrix.h"
-#include "NoiseModel.h"
 
 using namespace std;
 using namespace gtsam;
@@ -419,6 +418,8 @@ TEST( matrix, inverse )
   A(2,0)= 1;  A(2,1)=0; A(2,2)=6;
 
   Matrix Ainv = inverse(A);
+  CHECK(assert_equal(eye(3), A*Ainv));
+  CHECK(assert_equal(eye(3), Ainv*A));
 
   Matrix expected(3,3);
   expected(0,0)= 1.0909;   expected(0,1)=-0.5454; expected(0,2)=-0.0909;
@@ -426,6 +427,26 @@ TEST( matrix, inverse )
   expected(2,0)= -0.1818;  expected(2,1)= 0.0909; expected(2,2)=0.1818;
 
   CHECK(assert_equal(expected, Ainv, 1e-4));
+
+  // These two matrices failed before version 2003 because we called LU incorrectly
+  Matrix lMg(Matrix_(3,3,
+  		0.0,  1.0,-2.0,
+  	 -1.0,  0.0, 1.0,
+  		0.0,  0.0, 1.0));
+  CHECK(assert_equal(Matrix_(3,3,
+  		0.0, -1.0, 1.0,
+  		1.0,  0.0, 2.0,
+  		0.0,  0.0, 1.0),
+  		inverse(lMg)));
+  Matrix gMl(Matrix_(3,3,
+  		0.0, -1.0, 1.0,
+  		1.0,  0.0, 2.0,
+  		0.0,  0.0, 1.0));
+  CHECK(assert_equal(Matrix_(3,3,
+  		0.0,  1.0,-2.0,
+  	 -1.0,  0.0, 1.0,
+  		0.0,  0.0, 1.0),
+  		inverse(gMl)));
 }
 
 /* ************************************************************************* */
@@ -623,69 +644,6 @@ static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
 	}
 }
 
-/* ************************************************************************* */
-list<boost::tuple<Vector, double, double> >
-weighted_eliminate2(Matrix& A, Vector& b, const sharedGaussian& model) {
-	size_t m = A.size1(), n = A.size2(); // get size(A)
-	size_t maxRank = min(m,n);
-
-	// pre-whiten everything
-	model->WhitenInPlace(A);
-	b = model->whiten(b);
-
-	// create list
-	list<boost::tuple<Vector, double, double> > results;
-
-	// We loop over all columns, because the columns that can be eliminated
-	// are not necessarily contiguous. For each one, estimate the corresponding
-	// scalar variable x as d-rS, with S the separator (remaining columns).
-	// Then update A and b by substituting x with d-rS, zero-ing out x's column.
-	for (int j=0; j<n; ++j) {
-		// extract the first column of A
-		Vector a(column(A, j)); // ublas::matrix_column is slower !
-
-		// Calculate weighted pseudo-inverse and corresponding precision
-		double precision = dot(a,a);
-		Vector pseudo = a/precision;
-
-		// if precision is zero, no information on this column
-		if (precision < 1e-8) continue;
-
-		// create solution and copy into r
-		Vector r(basis(n, j));
-		for (int j2=j+1; j2<n; ++j2) // expensive !!
-			r(j2) = inner_prod(pseudo, boost::numeric::ublas::matrix_column<Matrix>(A, j2));
-
-		// create the rhs
-		double d = inner_prod(pseudo, b);
-
-		// construct solution (r, d, sigma)
-		// TODO: avoid sqrt, store precision or at least variance
-		results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
-
-		// exit after rank exhausted
-		if (results.size()>=maxRank) break;
-
-		// update A, b, expensive, suing outer product
-		// A' \define A_{S}-a*r and b'\define b-d*a
-		updateAb(A, b, j, a, r, d);
-	}
-
-	return results;
-}
-
-/* ************************************************************************* */
-void weighted_eliminate3(Matrix& A, Vector& b, const sharedGaussian& model) {
-	size_t m = A.size1(), n = A.size2(); // get size(A)
-	size_t maxRank = min(m,n);
-
-	// pre-whiten everything
-	model->WhitenInPlace(A);
-	b = model->whiten(b);
-
-	householder_(A, maxRank);
-}
-
 /* ************************************************************************* */
 TEST( matrix, weighted_elimination )
 {
@@ -727,27 +685,6 @@ TEST( matrix, weighted_elimination )
 		DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5);  // verify sigma
 		i += 1;
 	}
-
-	// perform elimination with NoiseModel
-	Matrix A2 = A; Vector b2 = b;
-	sharedGaussian model = noiseModel::Diagonal::Sigmas(sigmas);
-	std::list<boost::tuple<Vector, double, double> > solution2 =
-								weighted_eliminate2(A2, b2, model);
-
-	// unpack and verify
-	i=0;
-	BOOST_FOREACH(boost::tie(r, di, sigma), solution2) {
-		CHECK(assert_equal(r, row(expectedR, i))); // verify r
-		DOUBLES_EQUAL(d(i), di, 1e-8);             // verify d
-		DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5);  // verify sigma
-		i += 1;
-	}
-
-	// perform elimination with NoiseModel
-	weighted_eliminate3(A, b, model);
-	sharedGaussian newModel = noiseModel::Diagonal::Sigmas(newSigmas);
-//	print(A);
-//	print(newModel->Whiten(expectedR));
 }
 
 /* ************************************************************************* */

cpp/testNoiseModel.cpp

@@ -11,6 +11,8 @@
 #include <boost/foreach.hpp>
 #include <iostream>
 #include "NoiseModel.h"
+#include "SharedGaussian.h"
+#include "SharedDiagonal.h"
 
 using namespace std;
 using namespace gtsam;
@@ -148,12 +150,12 @@ TEST(NoiseModel, ConstrainedAll )
 //
 //	// Expected result
 //	Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
-//	sharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
+//	SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
 //
 //	// Call Gaussian version
-//	sharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
+//	SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
-//	sharedDiagonal actual1 = diagonal->QR(Ab1);
+//	SharedDiagonal actual1 = diagonal->QR(Ab1);
-//	sharedDiagonal expected = noiseModel::Unit::Create(4);
+//	SharedDiagonal expected = noiseModel::Unit::Create(4);
 //	CHECK(assert_equal(*expected,*actual1));
 //	Matrix expectedRd1 = Matrix_(4, 6+1,
 //			11.1803,   0.0,   -2.23607, 0.0,    -8.94427, 0.0,     2.23607,
@@ -163,8 +165,8 @@ TEST(NoiseModel, ConstrainedAll )
 //	CHECK(assert_equal(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!!
 //
 //	// Call Constrained version
-//	sharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
+//	SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
-//	sharedDiagonal actual2 = constrained->QR(Ab2);
+//	SharedDiagonal actual2 = constrained->QR(Ab2);
 //	CHECK(assert_equal(*expectedModel,*actual2));
 //	Matrix expectedRd2 = Matrix_(4, 6+1,
 //			1.,  0., -0.2,  0., -0.8, 0.,  0.2,
@@ -177,13 +179,13 @@ TEST(NoiseModel, ConstrainedAll )
 /* ************************************************************************* */
 TEST(NoiseModel, QRNan )
 {
-	sharedDiagonal constrained = noiseModel::Constrained::All(2);
+	SharedDiagonal constrained = noiseModel::Constrained::All(2);
 	Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.);
 
-	sharedDiagonal expected = noiseModel::Constrained::All(2);
+	SharedDiagonal expected = noiseModel::Constrained::All(2);
 	Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);
 
-	sharedDiagonal actual = constrained->QR(Ab);
+	SharedDiagonal actual = constrained->QR(Ab);
 	CHECK(assert_equal(*expected,*actual));
 	CHECK(assert_equal(expectedAb,Ab));
 }
@@ -192,8 +194,8 @@ TEST(NoiseModel, QRNan )
 TEST(NoiseModel, SmartCovariance )
 {
 	bool smart = true;
-	sharedGaussian expected = Unit::Create(3);
+	SharedGaussian expected = Unit::Create(3);
-	sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
+	SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
 	CHECK(assert_equal(*expected,*actual));
 }
 
@@ -201,8 +203,8 @@ TEST(NoiseModel, SmartCovariance )
 TEST(NoiseModel, ScalarOrVector )
 {
 	bool smart = true;
-	sharedGaussian expected = Unit::Create(3);
+	SharedGaussian expected = Unit::Create(3);
-	sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
+	SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
 	CHECK(assert_equal(*expected,*actual));
 }
 

cpp/testNonlinearConstraint.cpp

@@ -87,9 +87,9 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
 	boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
 
 	// verify
-	sharedDiagonal probModel = sharedSigma(p,1.0);
+	SharedDiagonal probModel = sharedSigma(p,1.0);
 	GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel);
-	sharedDiagonal constraintModel = noiseModel::Constrained::All(p);
+	SharedDiagonal constraintModel = noiseModel::Constrained::All(p);
 	GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel);
 	CHECK(assert_equal(*actualFactor, expectedFactor));
 	CHECK(assert_equal(*actualConstraint, expectedConstraint));
@@ -188,11 +188,11 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
 	boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
 
 	// verify
-	sharedDiagonal probModel = sharedSigma(p,1.0);
+	SharedDiagonal probModel = sharedSigma(p,1.0);
 	GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0),
 							 x1, Matrix_(1,1, -3.0),
 							 "L12", eye(1), zero(1), probModel);
-	sharedDiagonal constraintModel = noiseModel::Constrained::All(p);
+	SharedDiagonal constraintModel = noiseModel::Constrained::All(p);
 	GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0),
 								 x1, Matrix_(1,1, -1.0),
 								 Vector_(1, 6.0), constraintModel);
@@ -289,9 +289,9 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
 	CHECK(c1.active(config2));
 
 	// verify
-	sharedDiagonal probModel = sharedSigma(p,1.0);
+	SharedDiagonal probModel = sharedSigma(p,1.0);
 	GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel);
-	sharedDiagonal constraintModel = noiseModel::Constrained::All(p);
+	SharedDiagonal constraintModel = noiseModel::Constrained::All(p);
 	GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel);
 	CHECK(assert_equal(*actualFactor2, expectedFactor));
 	CHECK(assert_equal(*actualConstraint2, expectedConstraint));

cpp/testNonlinearEquality.cpp

@@ -31,7 +31,7 @@ TEST ( NonlinearEquality, linearization ) {
 	shared_nle nle(new NLE(key, value,vector_compare));
 
 	// check linearize
-	sharedDiagonal constraintModel = noiseModel::Constrained::All(2);
+	SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
 	GaussianFactor expLF(key, eye(2), zero(2), constraintModel);
 	GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
 	CHECK(assert_equal(*actualLF, expLF));

cpp/testNonlinearFactor.cpp

@@ -29,7 +29,7 @@ typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
 /* ************************************************************************* */
 TEST( NonlinearFactor, equals )
 {
-	sharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0));
+	SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0));
 
 	// create two nonlinear2 factors
 	Point2 z3(0.,-1.);

cpp/testNonlinearOptimizer.cpp

@@ -170,8 +170,8 @@ TEST( NonlinearOptimizer, Factorization )
 	config->insert(2, Pose2(1.5,0.,0.));
 
 	boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
-	graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
+	graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10));
-	graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
+	graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1));
 
 	boost::shared_ptr<Ordering> ordering(new Ordering);
 	ordering->push_back(Pose2Config::Key(1));
@@ -197,8 +197,8 @@ TEST( NonlinearOptimizer, SubgraphPCG )
 	config->insert(2, Pose2(1.5,0.,0.));
 
 	boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
-	graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
+	graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10));
-	graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
+	graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1));
 
 	double relativeThreshold = 1e-5;
 	double absoluteThreshold = 1e-5;

cpp/testPlanarSLAM.cpp

@@ -15,7 +15,7 @@ using namespace gtsam;
 static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
 static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
 
-sharedGaussian
+SharedGaussian
 	sigma(noiseModel::Isotropic::Sigma(1,0.1)),
 	I3(noiseModel::Unit::Create(3));
 

cpp/testPose2.cpp

@@ -181,15 +181,15 @@ Matrix matrix(const Pose2& gTl) {
 TEST( Pose2, matrix )
 {
 	Point2 origin, t(1,2);
-	Pose2 gT1(M_PI_2, t); // robot at (1,2) looking towards y
+	Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y
-  Matrix gM1 = matrix(gT1);
+  Matrix gMl = matrix(gTl);
   CHECK(assert_equal(Matrix_(3,3,
   		0.0, -1.0, 1.0,
   		1.0,  0.0, 2.0,
   		0.0,  0.0, 1.0),
-  		gM1));
+  		gMl));
-  Rot2 gR1 = gT1.r();
+  Rot2 gR1 = gTl.r();
-  CHECK(assert_equal(homogeneous(t),gM1*homogeneous(origin)));
+  CHECK(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
   Point2 x_axis(1,0), y_axis(0,1);
   CHECK(assert_equal(Matrix_(2,2,
   		0.0, -1.0,
@@ -197,14 +197,26 @@ TEST( Pose2, matrix )
   		gR1.matrix()));
   CHECK(assert_equal(Point2(0,1),gR1*x_axis));
   CHECK(assert_equal(Point2(-1,0),gR1*y_axis));
-  CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gM1*homogeneous(x_axis)));
+  CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
-  CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gM1*homogeneous(y_axis)));
+  CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));
 
   // check inverse pose
-  Matrix _1Mg = matrix(inverse(gT1));
+  Matrix lMg = matrix(inverse(gTl));
-  CHECK(assert_equal(inverse(gM1),_1Mg));
+  CHECK(assert_equal(Matrix_(3,3,
-  CHECK(assert_equal(eye(3),inverse(_1Mg)*_1Mg));
+  		0.0,  1.0,-2.0,
-  CHECK(assert_equal(eye(3),inverse(gM1)*gM1));
+  	 -1.0,  0.0, 1.0,
+  		0.0,  0.0, 1.0),
+  		lMg));
+}
+
+/* ************************************************************************* */
+TEST( Pose2, compose_matrix )
+{
+  Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
+  Pose2 _1T2(M_PI, Point2(-1,4));  // local robot at (-1,4) loooking at negative x
+  Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
+  CHECK(assert_equal(gM1*_1M2,matrix(compose(_1T2,gT1)))); // WRONG CHECKS OUT !
+  // CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); RIGHT DOES NOT
 }
 
 /* ************************************************************************* */
@@ -221,13 +233,6 @@ TEST( Pose2, between )
   Matrix actualH1,actualH2;
   Pose2 expected(M_PI_2, Point2(2,2));
   Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!!
-  GTSAM_PRINT(between(gT1,gT2));
-  // what we want is 1T2 = 1Tg * gT2 = between(2Tg,1Tg)
-//  print(matrix(gT1));
-//  print(matrix(gT2));
-//  print(inverse(matrix(gT1))*matrix(gT2)); // 1T2
-//  print(inverse(matrix(gT2))*matrix(gT1)); // 2T1
-//  GTSAM_PRINT(between(inverse(gT2),inverse(gT1)));
   Pose2 actual2 = between(gT1,gT2,actualH1,actualH2);
   CHECK(assert_equal(expected,actual1));
   CHECK(assert_equal(expected,actual2));

cpp/testPose2Factor.cpp

@@ -114,7 +114,7 @@ TEST( Pose2Factor, linearize )
 	Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
 
 	// expected linear factor
-	sharedDiagonal probModel1 = noiseModel::Unit::Create(3);
+	SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
 	GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, probModel1);
 
 	// Actual linearization

cpp/testPose2Prior.cpp

@@ -16,7 +16,7 @@ using namespace gtsam;
 
 // Common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
-static sharedGaussian sigmas = Diagonal::Sigmas(Vector_(3,sx,sy,st));
+static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st));
 
 /* ************************************************************************* */
 // Very simple test establishing Ax-b \approx z-h(x)

cpp/testPose2SLAM.cpp

@@ -79,7 +79,7 @@ TEST( Pose2Graph, linearization )
 
 	double sigma = 1;
 	Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
-	sharedDiagonal probModel1 = noiseModel::Unit::Create(3);
+	SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
 	lfg_expected.add("x1", A1, "x2", A2, b, probModel1);
 
 	CHECK(assert_equal(lfg_expected, lfg_linearized));

cpp/testPose3Factor.cpp

@@ -23,7 +23,7 @@ TEST( Pose3Factor, error )
 	Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
 
 	// Create factor
-	sharedGaussian I6(noiseModel::Unit::Create(6));
+	SharedGaussian I6(noiseModel::Unit::Create(6));
 	Pose3Factor factor(1,2, z, I6);
 
 	// Create config

cpp/testSQP.cpp

@@ -13,7 +13,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 // TODO: DANGEROUS, create shared pointers
-#define GTSAM_MAGIC_GAUSSIAN 2
+#define GTSAM_DANGEROUS_GAUSSIAN 2
 #define GTSAM_MAGIC_KEY
 
 #include <Pose3.h>
@@ -36,9 +36,9 @@ using namespace boost;
 using namespace boost::assign;
 
 // Models to use
-sharedDiagonal probModel1 = sharedSigma(1,1.0);
+SharedDiagonal probModel1 = sharedSigma(1,1.0);
-sharedDiagonal probModel2 = sharedSigma(2,1.0);
+SharedDiagonal probModel2 = sharedSigma(2,1.0);
-sharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
+SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
 
 // trick from some reading group
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@@ -279,7 +279,7 @@ TEST (SQP, two_pose_truth ) {
 
 	// measurement from x1 to l1
 	Point2 z1(0.0, 5.0);
-	sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
+	SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
 	shared f1(new simulated2D::Measurement(z1, sigma, x1,l1));
 	graph->push_back(f1);
 
@@ -387,7 +387,7 @@ TEST (SQP, two_pose ) {
 
 	// measurement from x1 to l1
 	Point2 z1(0.0, 5.0);
-	sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
+	SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
 	shared f1(new simulated2D::Measurement(z1, sigma, x1,l1));
 	graph->push_back(f1);
 

cpp/testSQPOptimizer.cpp

@@ -29,7 +29,7 @@ using namespace boost;
 using namespace boost::assign;
 using namespace simulated2D;
 	
-static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
+static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
 
 // typedefs
 typedef simulated2D::Config Config2D;

cpp/testVSLAMFactor.cpp

@@ -24,7 +24,7 @@ static double fov = 60; // degrees
 static size_t w=640,h=480;
 static Cal3_S2 K(fov,w,h);
 
-static sharedGaussian sigma(noiseModel::Unit::Create(1));
+static SharedGaussian sigma(noiseModel::Unit::Create(1));
 static shared_ptrK sK(new Cal3_S2(K));
 
 // make cameras
@@ -52,7 +52,7 @@ TEST( ProjectionFactor, error )
   Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
   Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
   Vector b = Vector_(2,3.,0.);
-  sharedDiagonal probModel1 = noiseModel::Unit::Create(2);
+  SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
   GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1);
   GaussianFactor::shared_ptr actual = factor->linearize(config);
   CHECK(assert_equal(expected,*actual,1e-3));

cpp/testVSLAMGraph.cpp

@@ -23,7 +23,7 @@ using namespace gtsam;
 using namespace gtsam::visualSLAM;
 using namespace boost;
 typedef NonlinearOptimizer<Graph,Config> Optimizer;
-static sharedGaussian sigma(noiseModel::Unit::Create(1));
+static SharedGaussian sigma(noiseModel::Unit::Create(1));
 
 /* ************************************************************************* */
 Point3 landmark1(-1.0,-1.0, 0.0);

cpp/visualSLAM.h

@@ -63,7 +63,7 @@ namespace gtsam { namespace visualSLAM {
      * @param K the constant calibration
      */
     ProjectionFactor(const Point2& z,
-					const sharedGaussian& model, PoseKey j_pose,
+					const SharedGaussian& model, PoseKey j_pose,
 					PointKey j_landmark, const shared_ptrK& K) :
 				z_(z), K_(K), Base(model, j_pose, j_landmark) {
 			}

matlab/createGaussianFactorGraph.m

@@ -6,46 +6,36 @@ c = createNoisyConfig();
 
 % Create
 fg = GaussianFactorGraph;
-sigma1=.1;
+
+% Create shared Noise models
+sigma0_1 = SharedDiagonal([0.1;0.1]);
+sigma0_2 = SharedDiagonal([0.2;0.2]);
 
 % prior on x1
 A11=eye(2);
 b = - c.get('x1');
-
+f1 = GaussianFactor('x1', A11, b, sigma0_1); % generate a Gaussian factor of odometry
-f1 = GaussianFactor('x1', A11, b, sigma1); % generate a Gaussian factor of odometry
 fg.push_back(f1);
 
 % odometry between x1 and x2
-sigma2=.1;
-
 A21=-eye(2);
 A22=eye(2);
 b = [.2;-.1];
-
+f2 = GaussianFactor('x1', A21,  'x2', A22, b,sigma0_1);
-f2 = GaussianFactor('x1', A21,  'x2', A22, b,sigma2);
-
 fg.push_back(f2);
 
 % measurement between x1 and l1
-sigma3=.2;
 A31=-eye(2);
 A33=eye(2);
 b = [0;.2];
-
+f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma0_2);
-
-f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma3);
-
 fg.push_back(f3);
 
 % measurement between x2 and l1
-sigma4=.2;
 A42=-eye(2);
 A43=eye(2);
 b = [-.2;.3];
-
+f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma0_2);
-
-f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma4);
-
 fg.push_back(f4);
 
 end

matlab/testGaussianFactor.m

@@ -25,14 +25,13 @@ Ax1 = [
 
 % the RHS
 b2=[-1;1.5;2;-1];
-
+model = SharedDiagonal([1;1]);
-combined = GaussianFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, 1);
+combined = GaussianFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, model);
 
 % eliminate the combined factor
 % NOT WORKING
-% this is not working because there is no elimination function for a linear
+% this is not working because the eliminate has to be added back to gtsam.h
-% factor. Just for the MutableGaussianFactor
+[actualCG,actualLF] = combined.eliminate('x2');
-%[actualCG,actualLF] = combined.eliminate('x2');
 
 % create expected Conditional Gaussian
 R11 = [
@@ -48,7 +47,7 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
+expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,model);
 
 % the expected linear factor
 Bl1 = [
@@ -70,5 +69,5 @@ expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, 1);
 % check if the result matches
 % NOT WORKING 
 % because can not be computed with GaussianFactor.eliminate
-%if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end
+if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end
-%if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end
+if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end

matlab/testLinearFactorGraph.m

@@ -2,7 +2,7 @@
 % equals
 fg = createGaussianFactorGraph();
 fg2 = createGaussianFactorGraph();
-CHECK('equals',fg.equals(fg2));
+CHECK('equals',fg.equals(fg2,1e-9));
 
 %-----------------------------------------------------------------------
 % error

wrap/wrap-matlab.h

@@ -15,10 +15,16 @@ extern "C" {
 #include <boost/shared_ptr.hpp>
 
 using namespace std;
-using namespace boost; // not usual, but for consiseness of generated code
+using namespace boost; // not usual, but for conciseness of generated code
 
+// start GTSAM Specifics /////////////////////////////////////////////////
 typedef numeric::ublas::vector<double> Vector;
 typedef numeric::ublas::matrix<double> Matrix;
+// to make keys be constructed from strings:
+#define GTSAM_MAGIC_KEY
+// to enable Matrix and Vector constructor for SharedGaussian:
+#define GTSAM_MAGIC_GAUSSIAN
+// end GTSAM Specifics /////////////////////////////////////////////////
 
 #ifdef __LP64__
 // 64-bit Mac