NOISE MODEL.

This is a big edit but with no templates involed, so it should not be a big deal.

New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. 

From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable.

Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests.

cpp/BetweenFactor.h

@@ -25,7 +25,6 @@ namespace gtsam {
 		typedef NonlinearFactor2<Config, Key, T, Key, T> Base;
 
 		T measured_; /** The measurement */
-		Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
 
 	public:
 
@@ -34,10 +33,8 @@ namespace gtsam {
 
 		/** Constructor */
 		BetweenFactor(const Key& key1, const Key& key2, const T& measured,
-				const Matrix& measurement_covariance) :
+				const sharedGaussian& model) :
-			Base(1, key1, key2), measured_(measured) {
+			Base(model, key1, key2), measured_(measured) {
-			square_root_inverse_covariance_ = inverse_square_root(
-					measurement_covariance);
 		}
 
 		/** implement functions needed for Testable */
@@ -46,8 +43,6 @@ namespace gtsam {
 		void print(const std::string& s) const {
 			Base::print(s);
 			measured_.print("measured");
-			gtsam::print(square_root_inverse_covariance_,
-					"Square Root Inverse Covariance");
 		}
 
 		/** equals */
@@ -64,12 +59,8 @@ namespace gtsam {
 		Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
 				boost::none, boost::optional<Matrix&> H2 = boost::none) const {
 			T hx = between(p1, p2, H1, H2); // h(x)
-			if (H1 || H2) {
-				if (H1) *H1 = square_root_inverse_covariance_ * *H1;
-				if (H2) *H2 = square_root_inverse_covariance_ * *H2;
-			}
 			// manifold equivalent of h(x)-z -> log(z,h(x))
-			return square_root_inverse_covariance_ * logmap(measured_, hx);
+			return logmap(measured_, hx);
 		}
 
 		/** return the measured */

cpp/NoiseModel.cpp

@@ -1,73 +1,135 @@
 /*
- * NoiseModel.cpp
+ * NoiseModel
  *
  *  Created on: Jan 13, 2010
  *      Author: Richard Roberts
  *      Author: Frank Dellaert
  */
 
+#include <limits>
+#include <iostream>
 #include "NoiseModel.h"
 
 namespace ublas = boost::numeric::ublas;
 typedef ublas::matrix_column<Matrix> column;
 
+static double inf = std::numeric_limits<double>::infinity();
+
+using namespace std;
+
 namespace gtsam {
+	namespace noiseModel {
 
-	/* ************************************************************************* */
+		/* ************************************************************************* */
-	Vector GaussianNoiseModel::whiten(const Vector& v) const {
+		void Gaussian::print(const string& name) const {
-		return sqrt_information_ * v;
+			gtsam::print(sqrt_information_, "Gaussian");
-	}
-
-	Vector GaussianNoiseModel::unwhiten(const Vector& v) const {
-		return backSubstituteUpper(sqrt_information_, v);
-	}
-
-	// functional
-	Matrix GaussianNoiseModel::Whiten(const Matrix& H) const {
-		size_t m = H.size1(), n = H.size2();
-		Matrix W(m, n);
-		for (int j = 0; j < n; j++) {
-			Vector wj = whiten(column(H, j));
-			for (int i = 0; i < m; i++)
-				W(i, j) = wj(i);
 		}
-		return W;
-	}
 
-	// in place
+		bool Gaussian::equals(const Base& m, double tol) const {
-	void GaussianNoiseModel::WhitenInPlace(Matrix& H) const {
+			const Gaussian* p = dynamic_cast<const Gaussian*> (&m);
-		size_t m = H.size1(), n = H.size2();
+			if (p == NULL) return false;
-		for (int j = 0; j < n; j++) {
+			return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, tol);
-			Vector wj = whiten(column(H, j));
-			for (int i = 0; i < m; i++)
-				H(i, j) = wj(i);
 		}
-	}
 
-	/* ************************************************************************* */
+		Vector Gaussian::whiten(const Vector& v) const {
-	// TODO: can we avoid calling reciprocal twice ?
+			return sqrt_information_ * v;
-	Diagonal::Diagonal(const Vector& sigmas) :
+		}
-		GaussianNoiseModel(diag(reciprocal(sigmas))),
-				invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) {
-	}
 
-	Vector Diagonal::whiten(const Vector& v) const {
+		Vector Gaussian::unwhiten(const Vector& v) const {
-		return emul(v, invsigmas_);
+			return backSubstituteUpper(sqrt_information_, v);
-	}
+		}
 
-	Vector Diagonal::unwhiten(const Vector& v) const {
+		double Gaussian::Mahalanobis(const Vector& v) const {
-		return emul(v, sigmas_);
+			// Note: for Diagonal, which does ediv_, will be correct for constraints
-	}
+			Vector w = whiten(v);
+			return inner_prod(w, w);
+		}
+
+		// functional
+		Matrix Gaussian::Whiten(const Matrix& H) const {
+			size_t m = H.size1(), n = H.size2();
+			Matrix W(m, n);
+			for (int j = 0; j < n; j++) {
+				Vector wj = whiten(column(H, j));
+				for (int i = 0; i < m; i++)
+					W(i, j) = wj(i);
+			}
+			return W;
+		}
+
+		// in place
+		void Gaussian::WhitenInPlace(Matrix& H) const {
+			size_t m = H.size1(), n = H.size2();
+			for (int j = 0; j < n; j++) {
+				Vector wj = whiten(column(H, j));
+				for (int i = 0; i < m; i++)
+					H(i, j) = wj(i);
+			}
+		}
+
+		/* ************************************************************************* */
+		// TODO: can we avoid calling reciprocal twice ?
+		Diagonal::Diagonal(const Vector& sigmas) :
+			Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)),
+					sigmas_(sigmas) {
+		}
+
+		void Diagonal::print(const string& name) const {
+			gtsam::print(sigmas_, "Diagonal sigmas " + name);
+		}
+
+		Vector Diagonal::whiten(const Vector& v) const {
+			return emul(v, invsigmas_);
+		}
+
+		Vector Diagonal::unwhiten(const Vector& v) const {
+			return emul(v, sigmas_);
+		}
+
+		/* ************************************************************************* */
+
+		void Constrained::print(const std::string& name) const {
+			gtsam::print(sigmas_, "Constrained sigmas " + name);
+		}
+
+		Vector Constrained::whiten(const Vector& v) const {
+			// ediv_ does the right thing with the errors
+			return ediv_(v, sigmas_);
+		}
+
+		Matrix Constrained::Whiten(const Matrix& H) const {
+			throw logic_error("noiseModel::Constrained cannot Whiten");
+		}
+
+		void Constrained::WhitenInPlace(Matrix& H) const {
+			throw logic_error("noiseModel::Constrained cannot Whiten");
+		}
+
+		/* ************************************************************************* */
+
+		void Isotropic::print(const string& name) const {
+			cout << "Isotropic sigma " << name << " " << sigma_ << endl;
+		}
+
+		double Isotropic::Mahalanobis(const Vector& v) const {
+			double dot = inner_prod(v, v);
+			return dot * invsigma_ * invsigma_;
+		}
+
+		Vector Isotropic::whiten(const Vector& v) const {
+			return v * invsigma_;
+		}
+
+		Vector Isotropic::unwhiten(const Vector& v) const {
+			return v * sigma_;
+		}
+
+		/* ************************************************************************* */
+		void Unit::print(const std::string& name) const {
+			cout << "Unit " << name << endl;
+		}
 
 	/* ************************************************************************* */
 
-	Vector Isotropic::whiten(const Vector& v) const {
-		return v * invsigma_;
 	}
-
-	Vector Isotropic::unwhiten(const Vector& v) const {
-		return v * sigma_;
-	}
-
-/* ************************************************************************* */
 } // gtsam

cpp/NoiseModel.h

@@ -9,19 +9,19 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
-//#include "Testable.h" TODO
+#include "Testable.h"
 #include "Vector.h"
 #include "Matrix.h"
 
-namespace gtsam {
+namespace gtsam {	namespace noiseModel {
 
   /**
-   * NoiseModel is the abstract base class for all noise models.
+   * noiseModel::Base is the abstract base class for all noise models.
    *
-   * It must implement a 'whiten' function to normalize an error vector, and an
+   * Noise models must implement a 'whiten' function to normalize an error vector,
-   * 'unwhiten' function to unnormalize an error vector.
+   * and an 'unwhiten' function to unnormalize an error vector.
    */
-  class NoiseModel /* TODO : public Testable<NoiseModel> */ {
+  class Base : public Testable<Base> {
 
   protected:
 
@@ -29,8 +29,8 @@ namespace gtsam {
 
   public:
 
-  	NoiseModel(size_t dim):dim_(dim) {}
+  	Base(size_t dim):dim_(dim) {}
-  	virtual ~NoiseModel() {}
+  	virtual ~Base() {}
 
     /**
      * Whiten an error vector.
@@ -41,10 +41,21 @@ namespace gtsam {
      * Unwhiten an error vector.
      */
     virtual Vector unwhiten(const Vector& v) const = 0;
+
+    /** in-place whiten, override if can be done more efficiently */
+    virtual void whitenInPlace(Vector& v) const {
+    	v = whiten(v);
+    }
+
+    /** in-place unwhiten, override if can be done more efficiently */
+    virtual void unwhitenInPlace(Vector& v) const {
+    	v = unwhiten(v);
+    }
+
   };
 
   /**
-	 * GaussianNoiseModel implements the mathematical model
+	 * Gaussian implements the mathematical model
 	 *  |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
 	 * where
 	 *   y = whiten(x) = R*x
@@ -53,7 +64,7 @@ namespace gtsam {
 	 *   |y|^2 = y'*y = x'*R'*R*x
 	 * Various derived classes are available that are more efficient.
 	 */
-	struct GaussianNoiseModel: public NoiseModel {
+	struct Gaussian: public Base {
 
 	protected:
 
@@ -62,38 +73,45 @@ namespace gtsam {
 		Matrix sqrt_information_;
 
 		/** protected constructor takes square root information matrix */
-		GaussianNoiseModel(const Matrix& sqrt_information) :
+		Gaussian(const Matrix& sqrt_information) :
-			NoiseModel(sqrt_information.size1()), sqrt_information_(sqrt_information) {
+			Base(sqrt_information.size1()), sqrt_information_(sqrt_information) {
 		}
 
 	public:
 
-		typedef boost::shared_ptr<GaussianNoiseModel> shared_ptr;
+		typedef boost::shared_ptr<Gaussian> shared_ptr;
 
     /**
      * A Gaussian noise model created by specifying a square root information matrix.
      */
     static shared_ptr SqrtInformation(const Matrix& R) {
-    	return shared_ptr(new GaussianNoiseModel(R));
+    	return shared_ptr(new Gaussian(R));
     }
 
     /**
      * A Gaussian noise model created by specifying a covariance matrix.
      */
-    static shared_ptr Covariance(const Matrix& Sigma) {
+    static shared_ptr Covariance(const Matrix& covariance) {
-    	return shared_ptr(new GaussianNoiseModel(inverse_square_root(Sigma)));
+    	return shared_ptr(new Gaussian(inverse_square_root(covariance)));
     }
 
     /**
      * A Gaussian noise model created by specifying an information matrix.
      */
     static shared_ptr Information(const Matrix& Q)  {
-    	return shared_ptr(new GaussianNoiseModel(square_root_positive(Q)));
+    	return shared_ptr(new Gaussian(square_root_positive(Q)));
     }
 
+		virtual void print(const std::string& name) const;
+		virtual bool equals(const Base& expected, double tol) const;
     virtual Vector whiten(const Vector& v) const;
 		virtual Vector unwhiten(const Vector& v) const;
 
+    /**
+		 * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
+		 */
+		virtual double Mahalanobis(const Vector& v) const;
+
 		/**
 		 * Multiply a derivative with R (derivative of whiten)
 		 * Equivalent to whitening each column of the input matrix.
@@ -112,17 +130,17 @@ namespace gtsam {
 			return sqrt_information_;
 		}
 
-	}; // GaussianNoiseModel
+	}; // Gaussian
 
 	// FD: does not work, ambiguous overload :-(
-  // inline Vector operator*(const GaussianNoiseModel& R, const Vector& v) {return R.whiten(v);}
+  // inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);}
 
   /**
    * A diagonal noise model implements a diagonal covariance matrix, with the
    * elements of the diagonal specified in a Vector.  This class has no public
    * constructors, instead, use the static constructor functions Sigmas etc...
    */
-  class Diagonal : public GaussianNoiseModel {
+  class Diagonal : public Gaussian {
   protected:
 
   	/** sigmas and reciprocal */
@@ -159,10 +177,54 @@ namespace gtsam {
     	return Variances(reciprocal(precisions));
     }
 
+		virtual void print(const std::string& name) const;
     virtual Vector whiten(const Vector& v) const;
     virtual Vector unwhiten(const Vector& v) const;
   };
 
+  /**
+   * A Constrained constrained model is a specialization of Diagonal which allows
+   * some or all of the sigmas to be zero, forcing the error to be zero there.
+   * All other Gaussian models are guaranteed to have a non-singular square-root
+   * information matrix, but this class is specifically equipped to deal with
+   * singular noise models, specifically: whiten will return zero on those
+   * components that have zero sigma *and* zero error, infinity otherwise.
+   */
+  class Constrained : public Diagonal {
+  protected:
+
+    /** protected constructor takes sigmas */
+  	Constrained(const Vector& sigmas) :Diagonal(sigmas) {}
+
+  public:
+
+		typedef boost::shared_ptr<Constrained> shared_ptr;
+
+    /**
+     * A diagonal noise model created by specifying a Vector of
+     * standard devations, some of which might be zero
+     */
+    static shared_ptr Mixed(const Vector& sigmas) {
+    	return shared_ptr(new Constrained(sigmas));
+    }
+
+    /**
+     * Fully constrained. TODO: subclass ?
+     */
+    static shared_ptr All(size_t dim) {
+    	return Mixed(repeat(dim,0));
+    }
+
+		virtual void print(const std::string& name) const;
+    virtual Vector whiten(const Vector& v) const;
+
+		// Whitening Jacobians does not make sense for possibly constrained
+		// noise model and will throw an exception.
+
+		virtual Matrix Whiten(const Matrix& H) const;
+		virtual void WhitenInPlace(Matrix& H) const;
+  };
+
   /**
    * An isotropic noise model corresponds to a scaled diagonal covariance
    * To construct, use one of the static methods
@@ -200,31 +262,59 @@ namespace gtsam {
     	return Variance(dim, 1.0/precision);
     }
 
+		virtual void print(const std::string& name) const;
+		virtual double Mahalanobis(const Vector& v) const;
     virtual Vector whiten(const Vector& v) const;
     virtual Vector unwhiten(const Vector& v) const;
   };
 
   /**
-   * UnitCovariance: i.i.d. unit-variance noise on all m dimensions.
+   * Unit: i.i.d. unit-variance noise on all m dimensions.
    */
-  class UnitCovariance : public Isotropic {
+  class Unit : public Isotropic {
   protected:
 
-    UnitCovariance(size_t dim): Isotropic(dim,1.0) {}
+    Unit(size_t dim): Isotropic(dim,1.0) {}
 
   public:
 
-    typedef boost::shared_ptr<UnitCovariance> shared_ptr;
+    typedef boost::shared_ptr<Unit> shared_ptr;
 
     /**
-     * An isotropic noise model created by specifying a standard devation sigma
+     * Create a unit covariance noise model
      */
-    static shared_ptr Create(size_t dim);
+    static shared_ptr Create(size_t dim) {
+    	return shared_ptr(new Unit(dim));
+    }
 
+		virtual void print(const std::string& name) const;
+		virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); }
     virtual Vector whiten(const Vector& v) const { return v; }
     virtual Vector unwhiten(const Vector& v) const { return v; }
     virtual Matrix Whiten(const Matrix& H) const { return H; }
 	  virtual void WhitenInPlace(Matrix& H) const {}
   };
 
-}
+	} // namespace noiseModel
+
+	using namespace noiseModel;
+
+	// 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
+		};
+
+
+} // namespace gtsam

cpp/NonlinearConstraint-inl.h

@@ -32,14 +32,14 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
 					size_t dim_lagrange,
 					boost::function<Vector(const Config& config)> g,
 					bool isEquality)
-:	NonlinearFactor<Config>(1.0),
+:	NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
 	lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)),
 	g_(g), isEquality_(isEquality) {}
 
 /* ************************************************************************* */
 template <class Config>
 bool NonlinearConstraint<Config>::active(const Config& config) const {
-	return !(!isEquality_ && greaterThanOrEqual(error_vector(config), zero(p_)));
+	return !(!isEquality_ && greaterThanOrEqual(unwhitenedError(config), zero(p_)));
 }
 
 /* ************************************************************************* */

cpp/NonlinearConstraint.h

@@ -88,7 +88,7 @@ public:
 	virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
 
 	/** error function - returns the result of the constraint function */
-	inline Vector error_vector(const Config& c) const { return g_(c); }
+	inline Vector unwhitenedError(const Config& c) const { return g_(c); }
 
 	/**
 	 * Determines whether the constraint is active given a particular configuration

cpp/NonlinearEquality.h

@@ -46,7 +46,7 @@ namespace gtsam {
 		 * Constructor
 		 */
 		NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
-			Base(0, j), feasible_(feasible), compare_(compare) {
+			Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible), compare_(compare) {
 		}
 
 		void print(const std::string& s = "") const {
@@ -76,6 +76,16 @@ namespace gtsam {
 				return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
 			}
 		}
+
+		// Linearize is over-written, because base linearization tries to whiten
+		virtual boost::shared_ptr<GaussianFactor> linearize(const Config& x) const {
+			const T& xj = x[this->key_];
+			Matrix A;
+			Vector b = - evaluateError(xj, A);
+			// TODO pass unwhitened + noise model to Gaussian factor
+			return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, 0.0));
+		}
+
 	}; // NonlinearEquality
 
 } // namespace gtsam

cpp/NonlinearFactor.h

@@ -18,6 +18,7 @@
 #include "Factor.h"
 #include "Vector.h"
 #include "Matrix.h"
+#include "NoiseModel.h"
 #include "GaussianFactor.h"
 
 #define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \
@@ -27,12 +28,6 @@
 
 namespace gtsam {
 
-	// TODO class NoiseModel {};
-	// TODO class Isotropic : public NoiseModel {};
-	// TODO class Diagonal : public NoiseModel {};
-	// TODO class Full : public NoiseModel {};
-	// TODO class Robust : public NoiseModel {};
-
 	/**
 	 * Nonlinear factor which assumes zero-mean Gaussian noise on the
 	 * on a measurement predicted by a non-linear function h.
@@ -46,10 +41,10 @@ namespace gtsam {
 
 	protected:
 
-    typedef NonlinearFactor<Config> This;
+		typedef NonlinearFactor<Config> This;
 
-    double sigma_; // noise standard deviation
+		sharedGaussian noiseModel_; /** Noise model */
-		std::list<Symbol> keys_; // keys
+		std::list<Symbol> keys_; /** cached keys */
 
 	public:
 
@@ -59,41 +54,31 @@ namespace gtsam {
 
 		/**
 		 *  Constructor
-		 *  @param sigma the standard deviation
+		 *  @param noiseModel shared pointer to a noise model
-		 *  // TODO: take a NoiseModel shared pointer
 		 */
-		NonlinearFactor(const double sigma) :
+		NonlinearFactor(const sharedGaussian& noiseModel) :
-			sigma_(sigma) {
+			noiseModel_(noiseModel) {
 		}
 
 		/** print */
 		void print(const std::string& s = "") const {
 			std::cout << "NonlinearFactor " << s << std::endl;
-			std::cout << "  sigma = " << sigma_ << std::endl;
+			noiseModel_->print("noise model");
 		}
 
 		/** Check if two NonlinearFactor objects are equal */
 		bool equals(const Factor<Config>& f, double tol = 1e-9) const {
 			const This* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
 			if (p == NULL) return false;
-			return fabs(sigma_ - p->sigma_) <= tol;
+			return noiseModel_->equals(*p->noiseModel_, tol);
 		}
 
 		/**
 		 * calculate the error of the factor
-		 * TODO: use NoiseModel
 		 */
 		double error(const Config& c) const {
-			// return NoiseModel.mahalanobis(error_vector(c)); // e'*inv(C)*e
+			return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c));
-			if (sigma_ == 0.0) {
-				Vector e = error_vector(c);
-				return (inner_prod(e, e) > 0) ? std::numeric_limits<double>::infinity()
-						: 0.0;
-			}
-			Vector e = error_vector(c) / sigma_;
-			return 0.5 * inner_prod(e, e);
 		}
-		;
 
 		/** return keys */
 		std::list<Symbol> keys() const {
@@ -105,13 +90,13 @@ namespace gtsam {
 			return keys_.size();
 		}
 
-		/** get functions */
+		/** Vector of errors, unwhitened ! */
-		double sigma() const {
+		virtual Vector unwhitenedError(const Config& c) const = 0;
-			return sigma_;
-		} // TODO obsolete when using NoiseModel
 
-		/** Vector of errors */
+		/** Vector of errors, whitened ! */
-		virtual Vector error_vector(const Config& c) const = 0;
+		Vector whitenedError(const Config& c) const {
+			return noiseModel_->whiten(unwhitenedError(c));
+		}
 
 		/** linearize to a GaussianFactor */
 		virtual boost::shared_ptr<GaussianFactor>
@@ -123,7 +108,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version) {
-			ar & BOOST_SERIALIZATION_NVP(sigma_); // TODO NoiseModel
+			// TODO NoiseModel
 		}
 
 	}; // NonlinearFactor
@@ -159,8 +144,9 @@ namespace gtsam {
 		 *  @param z measurement
 		 *  @param key by which to look up X value in Config
 		 */
-		NonlinearFactor1(double sigma, const Key& key1) :
+		NonlinearFactor1(const sharedGaussian& noiseModel,
-			Base(sigma), key_(key1) {
+				const Key& key1) :
+			Base(noiseModel), key_(key1) {
 			this->keys_.push_back(key_);
 		}
 
@@ -178,9 +164,9 @@ namespace gtsam {
 			return Base::equals(*p, tol) && (key_ == p->key_);
 		}
 
-		/** error function h(x)-z */
+		/** error function h(x)-z, unwhitened !!! */
-		inline Vector error_vector(const Config& x) const {
+		inline Vector unwhitenedError(const Config& x) const {
-			Key j = key_;
+			const Key& j = key_;
 			const X& xj = x[j];
 			return evaluateError(xj);
 		}
@@ -190,12 +176,14 @@ namespace gtsam {
 		 * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
 		 * Hence b = z - h(x0) = - error_vector(x)
 		 */
-		boost::shared_ptr<GaussianFactor> linearize(const Config& x) const {
+		virtual boost::shared_ptr<GaussianFactor> linearize(const Config& x) const {
 			const X& xj = x[key_];
 			Matrix A;
-			Vector b = -evaluateError(xj, A);
+			Vector b = - evaluateError(xj, A);
-			return GaussianFactor::shared_ptr(new GaussianFactor(
+			// TODO pass unwhitened + noise model to Gaussian factor
-			    key_, A, b, this->sigma()));
+			this->noiseModel_->WhitenInPlace(A);
+			this->noiseModel_->whitenInPlace(b);
+			return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, 1.0));
 		}
 
 		/*
@@ -249,8 +237,9 @@ namespace gtsam {
 		 * @param j1 key of the first variable
 		 * @param j2 key of the second variable
 		 */
-		NonlinearFactor2(double sigma, Key1 j1, Key2 j2) :
+		NonlinearFactor2(const sharedGaussian& noiseModel, Key1 j1,
-			Base(sigma), key1_(j1), key2_(j2) {
+				Key2 j2) :
+			Base(noiseModel), key1_(j1), key2_(j2) {
 			this->keys_.push_back(key1_);
 			this->keys_.push_back(key2_);
 		}
@@ -272,7 +261,7 @@ namespace gtsam {
 		}
 
 		/** error function z-h(x1,x2) */
-		inline Vector error_vector(const Config& x) const {
+		inline Vector unwhitenedError(const Config& x) const {
 			const X1& x1 = x[key1_];
 			const X2& x2 = x[key2_];
 			return evaluateError(x1, x2);
@@ -288,9 +277,12 @@ namespace gtsam {
 			const X2& x2 = c[key2_];
 			Matrix A1, A2;
 			Vector b = -evaluateError(x1, x2, A1, A2);
-			return GaussianFactor::shared_ptr(new GaussianFactor(
+			// TODO pass unwhitened + noise model to Gaussian factor
-			    key1_, A1, key2_,
+			this->noiseModel_->WhitenInPlace(A1);
-					A2, b, this->sigma()));
+			this->noiseModel_->WhitenInPlace(A2);
+			this->noiseModel_->whitenInPlace(b);
+			return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
+					A2, b, 1.0));
 		}
 
 		/** methods to retrieve both keys */
@@ -306,9 +298,9 @@ namespace gtsam {
 		 *  If any of the optional Matrix reference arguments are specified, it should compute
 		 *  both the function evaluation and its derivative(s) in X1 (and/or X2).
 		 */
-		virtual Vector evaluateError(const X1&, const X2&,
+		virtual Vector
-				boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+		evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
-						boost::none) const = 0;
+				boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
 
 	private:
 

cpp/NonlinearFactorGraph-inl.h

@@ -23,10 +23,10 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class Config>
-	Vector NonlinearFactorGraph<Config>::error_vector(const Config& c) const {
+	Vector NonlinearFactorGraph<Config>::unwhitenedError(const Config& c) const {
 		list<Vector> errors;
 		BOOST_FOREACH(typename NonlinearFactorGraph<Config>::sharedFactor factor, this->factors_)
-			errors.push_back(factor->error_vector(c));
+			errors.push_back(factor->unwhitenedError(c));
 		return concatVectors(errors);
 	}
 

cpp/NonlinearFactorGraph.h

@@ -34,7 +34,7 @@ namespace gtsam {
 		double error(const Config& c) const;
 
 		/** all individual errors */
-		Vector error_vector(const Config& c) const;
+		Vector unwhitenedError(const Config& c) const;
 
 		/** Unnormalized probability. O(n) */
 		double probPrime(const Config& c) const {

cpp/PriorFactor.h

@@ -23,7 +23,6 @@ namespace gtsam {
 		typedef NonlinearFactor1<Config, Key, T> Base;
 
 		T prior_; /** The measurement */
-		boost::shared_ptr<GaussianNoiseModel> noiseModel_;
 
 	public:
 
@@ -32,13 +31,8 @@ namespace gtsam {
 
 		/** Constructor */
 		PriorFactor(const Key& key, const T& prior,
-				const boost::shared_ptr<GaussianNoiseModel>& model) :
+				const sharedGaussian& model) :
-			Base(1.0, key), prior_(prior), noiseModel_(model) {
+			Base(model, key), prior_(prior) {
-		}
-
-		/** Constructor */
-		PriorFactor(const Key& key, const T& prior, const Matrix& cov) :
-			Base(1.0, key), prior_(prior), noiseModel_(GaussianNoiseModel::Covariance(cov)) {
 		}
 
 		/** implement functions needed for Testable */
@@ -47,7 +41,6 @@ namespace gtsam {
 		void print(const std::string& s) const {
 			Base::print(s);
 			prior_.print("prior");
-			// Todo print NoiseModel
 		}
 
 		/** equals */
@@ -56,16 +49,15 @@ namespace gtsam {
 					Config, Key, T>*> (&expected);
 			return e != NULL && Base::equals(expected) && this->prior_.equals(
 					e->prior_, tol);
-			// Todo check NoiseModel
 		}
 
 		/** implement functions needed to derive from Factor */
 
 		/** vector of errors */
 		Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
-			if (H) (*H) = noiseModel_->R();
+			if (H) (*H) = eye(dim(p));
 			// manifold equivalent of h(x)-z -> log(z,h(x))
-			return noiseModel_->whiten(logmap(prior_, p));
+			return logmap(prior_, p);
 		}
 	};
 

cpp/SQPOptimizer-inl.h

@@ -37,7 +37,7 @@ double constraintError(const G& graph, const C& config) {
 	for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) {
 		const shared_c constraint = boost::shared_dynamic_cast<NLConstraint >(*factor);
 		if (constraint != NULL) {
-			Vector e = constraint->error_vector(config);
+			Vector e = constraint->unwhitenedError(config);
 			error += inner_prod(trans(e),e);
 		}
 	}

cpp/Simulated3D.cpp

@@ -6,10 +6,9 @@
 
 #include "Simulated3D.h"
 
+namespace gtsam {
 namespace simulated3D {
 
-using namespace gtsam;
-
 Vector prior (const Vector& x)
 {
 	return x;
@@ -56,4 +55,4 @@ Matrix Dmea2(const Vector& x, const Vector& l)
 	return H;
 }
 
-} // namespace gtsam
+}} // namespace gtsam::simulated3D

cpp/Simulated3D.h

@@ -15,9 +15,10 @@
 
 // \namespace
 
+namespace gtsam {
 namespace simulated3D {
 
-	typedef gtsam::VectorConfig VectorConfig;
+	typedef VectorConfig VectorConfig;
 
 	typedef gtsam::Symbol PoseKey;
 	typedef gtsam::Symbol PointKey;
@@ -42,14 +43,15 @@ namespace simulated3D {
 	Matrix Dmea1(const Vector& x, const Vector& l);
 	Matrix Dmea2(const Vector& x, const Vector& l);
 
-	struct Point2Prior3D: public gtsam::NonlinearFactor1<VectorConfig, PoseKey,
+	struct Point2Prior3D: public NonlinearFactor1<VectorConfig, PoseKey,
 			Vector> {
 
 		Vector z_;
 
-		Point2Prior3D(const Vector& z, double sigma, const PoseKey& j) :
+		Point2Prior3D(const Vector& z,
-			gtsam::NonlinearFactor1<VectorConfig, PoseKey, Vector>(sigma, j), z_(z) {
+					const sharedGaussian& model, const PoseKey& j) :
-		}
+				NonlinearFactor1<VectorConfig, PoseKey, Vector> (model, j), z_(z) {
+			}
 
 		Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
 				boost::none) {
@@ -58,16 +60,17 @@ namespace simulated3D {
 		}
 	};
 
-	struct Simulated3DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
+	struct Simulated3DMeasurement: public NonlinearFactor2<VectorConfig,
 			PoseKey, Vector, PointKey, Vector> {
 
 		Vector z_;
 
-		Simulated3DMeasurement(const Vector& z, double sigma, PoseKey& j1,
+		Simulated3DMeasurement(const Vector& z,
-				PointKey j2) :
+					const sharedGaussian& model, PoseKey& j1, PointKey j2) :
-			z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
+				z_(z),
-					Vector>(sigma, j1, j2) {
+						NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, Vector> (
-		}
+								model, j1, j2) {
+			}
 
 		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
 				Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
@@ -77,4 +80,4 @@ namespace simulated3D {
 		}
 	};
 
-} // namespace simulated3D
+}} // namespace simulated3D

cpp/planarSLAM.cpp

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

cpp/planarSLAM.h

@@ -35,8 +35,8 @@ namespace gtsam {
 
 		BearingFactor(); /* Default constructor */
 		BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
-				double sigma) :
+				const sharedGaussian& model) :
-			Base(sigma, i, j), z_(z) {
+			Base(model, i, j), z_(z) {
 		}
 
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
@@ -63,8 +63,9 @@ namespace gtsam {
 
 		RangeFactor(); /* Default constructor */
 
-		RangeFactor(const PoseKey& i, const PointKey& j, double z, double sigma) :
+		RangeFactor(const PoseKey& i, const PointKey& j, double z,
-			Base(sigma, i, j), z_(z) {
+				const sharedGaussian& model) :
+			Base(model, i, j), z_(z) {
 		}
 
 		/** h(x)-z */
@@ -85,16 +86,19 @@ namespace gtsam {
 
 		// Factors
 		typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;
-	  typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
+		typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
 		typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
 		typedef RangeFactor<Config, PoseKey, PointKey> Range;
 
 		// Graph
 		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 Matrix& cov);
+			void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
-			void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, double sigma);
+					const sharedGaussian& model);
-			void addRange(const PoseKey& i, const PointKey& j, double z, double sigma);
+			void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
+					const sharedGaussian& model);
+			void addRange(const PoseKey& i, const PointKey& j, double z,
+					const sharedGaussian& model);
 		};
 
 		// Optimizer

cpp/pose2SLAM.cpp

@@ -29,13 +29,15 @@ namespace gtsam {
 		}
 
 		/* ************************************************************************* */
-		void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) {
+		void Graph::addPrior(const Key& i, const Pose2& p,
-			sharedFactor factor(new Prior(i, p, cov));
+				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 Matrix& cov) {
+		void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z,
-			sharedFactor factor(new Constraint(i, j, z, cov));
+				const sharedGaussian& model) {
+			sharedFactor factor(new Constraint(i, j, z, model));
 			push_back(factor);
 		}
 

cpp/pose2SLAM.h

@@ -40,8 +40,8 @@ namespace gtsam {
 
 		// Graph
 		struct Graph: public NonlinearFactorGraph<Config> {
-			void addPrior(const Key& i, const Pose2& p, const Matrix& cov);
+			void addPrior(const Key& i, const Pose2& p, const sharedGaussian& model);
-			void addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov);
+			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

@@ -22,22 +22,25 @@ namespace gtsam {
 		/* ************************************************************************* */
 		Config circle(size_t n, double R) {
 			Config x;
-			double theta = 0, dtheta = 2*M_PI/n;
+			double theta = 0, dtheta = 2 * M_PI / n;
 			// Vehicle at p0 is looking towards y axis
 			Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
 			for (size_t i = 0; i < n; i++, theta += dtheta)
-				x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
+				x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),
+						sin(theta), 0)));
 			return x;
 		}
 
 		/* ************************************************************************* */
-		void Graph::addPrior(const Key& i, const Pose3& p, const Matrix& cov) {
+		void Graph::addPrior(const Key& i, const Pose3& p,
-			sharedFactor factor(new Prior(i, p, cov));
+				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 Matrix& cov) {
+		void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z,
-			sharedFactor factor(new Constraint(i, j, z, cov));
+				const sharedGaussian& model) {
+			sharedFactor factor(new Constraint(i, j, z, model));
 			push_back(factor);
 		}
 

cpp/pose3SLAM.h

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

cpp/simulated2D.cpp

@@ -6,32 +6,34 @@
 
 #include "simulated2D.h"
 
-namespace simulated2D {
+namespace gtsam {
+	namespace simulated2D {
 
-	static Matrix I = gtsam::eye(2);
+		static Matrix I = gtsam::eye(2);
+
+		/* ************************************************************************* */
+		Vector prior(const Vector& x, boost::optional<Matrix&> H) {
+			if (H) *H = I;
+			return x;
+		}
+
+		/* ************************************************************************* */
+		Vector odo(const Vector& x1, const Vector& x2, boost::optional<Matrix&> H1,
+				boost::optional<Matrix&> H2) {
+			if (H1) *H1 = -I;
+			if (H2) *H2 = I;
+			return x2 - x1;
+		}
+
+		/* ************************************************************************* */
+		Vector mea(const Vector& x, const Vector& l, boost::optional<Matrix&> H1,
+				boost::optional<Matrix&> H2) {
+			if (H1) *H1 = -I;
+			if (H2) *H2 = I;
+			return l - x;
+		}
 
 	/* ************************************************************************* */
-	Vector prior(const Vector& x, boost::optional<Matrix&> H) {
-		if (H) *H = I;
-		return x;
-	}
 
-	/* ************************************************************************* */
+	} // namespace simulated2D
-	Vector odo(const Vector& x1, const Vector& x2, boost::optional<Matrix&> H1,
+} // namespace gtsam
-			boost::optional<Matrix&> H2) {
-		if (H1) *H1 = -I;
-		if (H2) *H2 = I;
-		return x2 - x1;
-	}
-
-	/* ************************************************************************* */
-	Vector mea(const Vector& x, const Vector& l, boost::optional<Matrix&> H1,
-			boost::optional<Matrix&> H2) {
-		if (H1) *H1 = -I;
-		if (H2) *H2 = I;
-		return l - x;
-	}
-
-/* ************************************************************************* */
-
-} // namespace simulated2D

cpp/simulated2D.h

@@ -14,7 +14,9 @@
 
 // \namespace
 
-namespace simulated2D {
+namespace gtsam {
+
+	namespace simulated2D {
 
 	typedef gtsam::VectorConfig VectorConfig;
 	typedef gtsam::Symbol PoseKey;
@@ -43,13 +45,14 @@ namespace simulated2D {
 	/**
 	 * Unary factor encoding a soft prior on a vector
 	 */
-	struct Prior: public gtsam::NonlinearFactor1<VectorConfig, PoseKey,
+	struct Prior: public NonlinearFactor1<VectorConfig, PoseKey,
 			Vector> {
 
 		Vector z_;
 
-		Prior(const Vector& z, double sigma, const PoseKey& key) :
+		Prior(const Vector& z, const sharedGaussian& model,
-			gtsam::NonlinearFactor1<VectorConfig, PoseKey, Vector>(sigma, key),
+				const PoseKey& key) :
+			NonlinearFactor1<VectorConfig, PoseKey, Vector>(model, key),
 					z_(z) {
 		}
 
@@ -63,14 +66,14 @@ namespace simulated2D {
 	/**
 	 * Binary factor simulating "odometry" between two Vectors
 	 */
-	struct Odometry: public gtsam::NonlinearFactor2<VectorConfig, PoseKey,
+	struct Odometry: public NonlinearFactor2<VectorConfig, PoseKey,
 			Vector, PointKey, Vector> {
 		Vector z_;
 
-		Odometry(const Vector& z, double sigma, const PoseKey& j1,
+		Odometry(const Vector& z, const sharedGaussian& model,
-				const PoseKey& j2) :
+				const PoseKey& j1, const PoseKey& j2) :
-			z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
+			z_(z), NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
-					Vector>(sigma, j1, j2) {
+					Vector>(model, j1, j2) {
 		}
 
 		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
@@ -83,15 +86,15 @@ namespace simulated2D {
 	/**
 	 * Binary factor simulating "measurement" between two Vectors
 	 */
-	struct Measurement: public gtsam::NonlinearFactor2<VectorConfig, PoseKey,
+	struct Measurement: public NonlinearFactor2<VectorConfig, PoseKey,
 			Vector, PointKey, Vector> {
 
 		Vector z_;
 
-		Measurement(const Vector& z, double sigma, const PoseKey& j1,
+		Measurement(const Vector& z, const sharedGaussian& model,
-				const PointKey& j2) :
+				const PoseKey& j1, const PointKey& j2) :
-			z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
+			z_(z), NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
-					Vector>(sigma, j1, j2) {
+					Vector>(model, j1, j2) {
 		}
 
 		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
@@ -101,4 +104,5 @@ namespace simulated2D {
 
 	};
 
-} // namespace simulated2D
+	} // namespace simulated2D
+} // namespace gtsam

cpp/smallExample.cpp

@@ -12,6 +12,8 @@
 
 using namespace std;
 
+// TODO: DANGEROUS, create shared pointers
+#define GTSAM_MAGIC_GAUSSIAN 2
 #define GTSAM_MAGIC_KEY
 
 #include "Ordering.h"

cpp/testGraph.cpp

@@ -12,6 +12,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+// TODO: DANGEROUS, create shared pointers
+#define GTSAM_MAGIC_GAUSSIAN 3
+
 #include "pose2SLAM.h"
 #include "TupleConfig-inl.h"
 #include "graph-inl.h"

cpp/testIterative.cpp

@@ -9,6 +9,8 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+// TODO: DANGEROUS, create shared pointers
+#define GTSAM_MAGIC_GAUSSIAN 3
 #define GTSAM_MAGIC_KEY
 
 #include "Ordering.h"

cpp/testMatrix.cpp

@@ -16,6 +16,8 @@
 using namespace std;
 using namespace gtsam;
 
+static double inf = std::numeric_limits<double>::infinity();
+
 /* ************************************************************************* */
 TEST( matrix, constructor_data )
 {
@@ -229,7 +231,7 @@ TEST( matrix, equal_nan )
   Matrix A2(A);
 
   Matrix A3(A);
-  A3(3,3)=0.0/0.0;
+  A3(3,3)=inf;
 
   CHECK(A!=A3);
 }
@@ -567,13 +569,13 @@ 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 GaussianNoiseModel& model) {
+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);
+	model->WhitenInPlace(A);
-	b = model.whiten(b);
+	b = model->whiten(b);
 
 	// create list
 	list<boost::tuple<Vector, double, double> > results;
@@ -617,13 +619,13 @@ weighted_eliminate2(Matrix& A, Vector& b, const GaussianNoiseModel& model) {
 }
 
 /* ************************************************************************* */
-void weighted_eliminate3(Matrix& A, Vector& b, const GaussianNoiseModel& model) {
+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);
+	model->WhitenInPlace(A);
-	b = model.whiten(b);
+	b = model->whiten(b);
 
 	householder_(A, maxRank);
 }
@@ -672,9 +674,9 @@ TEST( matrix, weighted_elimination )
 
 	// perform elimination with NoiseModel
 	Matrix A2 = A; Vector b2 = b;
-	GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(sigmas);
+	sharedGaussian model = noiseModel::Diagonal::Sigmas(sigmas);
 	std::list<boost::tuple<Vector, double, double> > solution2 =
-								weighted_eliminate2(A2, b2, *model);
+								weighted_eliminate2(A2, b2, model);
 
 	// unpack and verify
 	i=0;
@@ -686,8 +688,8 @@ TEST( matrix, weighted_elimination )
 	}
 
 	// perform elimination with NoiseModel
-	weighted_eliminate3(A, b, *model);
+	weighted_eliminate3(A, b, model);
-	GaussianNoiseModel::shared_ptr newModel = Diagonal::Sigmas(newSigmas);
+	sharedGaussian newModel = noiseModel::Diagonal::Sigmas(newSigmas);
 //	print(A);
 //	print(newModel->Whiten(expectedR));
 }

cpp/testNoiseModel.cpp

@@ -14,28 +14,35 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace noiseModel;
+
+static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
+static Matrix R = Matrix_(3, 3,
+		s_1, 0.0, 0.0,
+		0.0, s_1, 0.0,
+		0.0, 0.0, s_1);
+static Matrix Sigma = Matrix_(3, 3,
+		var, 0.0, 0.0,
+		0.0, var, 0.0,
+		0.0, 0.0, var);
+static Matrix Q = Matrix_(3, 3,
+		prc, 0.0, 0.0,
+		0.0, prc, 0.0,
+		0.0, 0.0, prc);
+
+static double inf = std::numeric_limits<double>::infinity();
 
 /* ************************************************************************* */
 TEST(NoiseModel, constructors)
 {
-	double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
 	Vector whitened = Vector_(3,5.0,10.0,15.0);
 	Vector unwhitened = Vector_(3,10.0,20.0,30.0);
 
 	// Construct noise models
-	vector<GaussianNoiseModel::shared_ptr> m;
+	vector<Gaussian::shared_ptr> m;
-	m.push_back(GaussianNoiseModel::SqrtInformation(Matrix_(3, 3,
+	m.push_back(Gaussian::SqrtInformation(R));
-			s_1, 0.0, 0.0,
+	m.push_back(Gaussian::Covariance(Sigma));
-			0.0, s_1, 0.0,
+	m.push_back(Gaussian::Information(Q));
-			0.0, 0.0, s_1)));
-	m.push_back(GaussianNoiseModel::Covariance(Matrix_(3, 3,
-			var, 0.0, 0.0,
-			0.0, var, 0.0,
-			0.0, 0.0, var)));
-	m.push_back(GaussianNoiseModel::Information(Matrix_(3, 3,
-			prc, 0.0, 0.0,
-			0.0, prc, 0.0,
-			0.0, 0.0, prc)));
 	m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)));
 	m.push_back(Diagonal::Variances(Vector_(3, var, var, var)));
 	m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc)));
@@ -45,20 +52,25 @@ TEST(NoiseModel, constructors)
 
 	// test whiten
 	int i=0;
-	BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
+	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
 		CHECK(assert_equal(whitened,mi->whiten(unwhitened)));
 
 	// test unwhiten
-	BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
+	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
 		CHECK(assert_equal(unwhitened,mi->unwhiten(whitened)));
 
+	// test Mahalanobis distance
+	double distance = 5*5+10*10+15*15;
+	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+		DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
+
 	// test R matrix
 	Matrix expectedR(Matrix_(3, 3,
 			s_1, 0.0, 0.0,
 			0.0, s_1, 0.0,
 			0.0, 0.0, s_1));
 
-	BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
+	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
 		CHECK(assert_equal(expectedR,mi->R()));
 
 	// test Whiten operator
@@ -72,7 +84,7 @@ TEST(NoiseModel, constructors)
 			0.0, s_1, 0.0, s_1,
 			s_1, 0.0, 0.0, s_1));
 
-	BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
+	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
 		CHECK(assert_equal(expected,mi->Whiten(H)));
 
 	// can only test inplace version once :-)
@@ -80,6 +92,47 @@ TEST(NoiseModel, constructors)
 	CHECK(assert_equal(expected,H));
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, Unit) {
+	Vector v = Vector_(3,5.0,10.0,15.0);
+	Gaussian::shared_ptr u(Unit::Create(3));
+	CHECK(assert_equal(v,u->whiten(v)));
+}
+
+/* ************************************************************************* */
+TEST(NoiseModel, equals)
+{
+	Gaussian::shared_ptr g = Gaussian::SqrtInformation(R);
+	Diagonal::shared_ptr d = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma));
+	Isotropic::shared_ptr i = Isotropic::Sigma(3, sigma);
+	CHECK(assert_equal(*g,*g));
+}
+
+/* ************************************************************************* */
+TEST(NoiseModel, ConstrainedMixed )
+{
+	Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
+			infeasible = Vector_(3, 1.0, 1.0, 1.0);
+	Constrained::shared_ptr d = Constrained::Mixed(Vector_(3, sigma, 0.0, sigma));
+	CHECK(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible)));
+	CHECK(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
+	DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9);
+	DOUBLES_EQUAL(0.5,d->Mahalanobis(feasible),1e-9);
+}
+
+/* ************************************************************************* */
+TEST(NoiseModel, ConstrainedAll )
+{
+	Vector feasible = Vector_(3, 0.0, 0.0, 0.0),
+			infeasible = Vector_(3, 1.0, 1.0, 1.0);
+
+	Constrained::shared_ptr i = Constrained::All(3);
+	CHECK(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible)));
+	CHECK(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));
+	DOUBLES_EQUAL(inf,i->Mahalanobis(infeasible),1e-9);
+	DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;

cpp/testNonlinearConstraint.cpp

@@ -54,7 +54,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
 	config.insert("x", Vector_(1, 1.0));
 
 	// calculate the error
-	Vector actual = c1.error_vector(config);
+	Vector actual = c1.unwhitenedError(config);
 	Vector expected = Vector_(1, -4.0);
 	CHECK(assert_equal(actual, expected, 1e-5));
 }
@@ -146,7 +146,7 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
 	config.insert("y", Vector_(1, 2.0));
 
 	// calculate the error
-	Vector actual = c1.error_vector(config);
+	Vector actual = c1.unwhitenedError(config);
 	Vector expected = Vector_(1.0, -6.0);
 	CHECK(assert_equal(actual, expected, 1e-5));
 }
@@ -238,7 +238,7 @@ TEST( NonlinearConstraint1, unary_inequality ) {
 
 	// check error
 	CHECK(!c1.active(config1));
-	Vector actualError2 = c1.error_vector(config2);
+	Vector actualError2 = c1.unwhitenedError(config2);
 	CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
 	CHECK(c1.active(config2));
 }
@@ -320,7 +320,7 @@ TEST( NonlinearConstraint1, unary_binding ) {
 
 	// check error
 	CHECK(!c1.active(config1));
-	Vector actualError2 = c1.error_vector(config2);
+	Vector actualError2 = c1.unwhitenedError(config2);
 	CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
 	CHECK(c1.active(config2));
 }
@@ -371,7 +371,7 @@ TEST( NonlinearConstraint2, binary_binding ) {
 	config.insert("y", Vector_(1, 2.0));
 
 	// calculate the error
-	Vector actual = c1.error_vector(config);
+	Vector actual = c1.unwhitenedError(config);
 	Vector expected = Vector_(1.0, -6.0);
 	CHECK(assert_equal(actual, expected, 1e-5));
 }

cpp/testNonlinearEquality.cpp

@@ -69,10 +69,10 @@ TEST ( NonlinearEquality, error ) {
 	shared_nle nle(new NLE(key, value,vector_compare));
 
 	// check error function outputs
-	Vector actual = nle->error_vector(feasible);
+	Vector actual = nle->unwhitenedError(feasible);
 	CHECK(assert_equal(actual, zero(2)));
 
-	actual = nle->error_vector(bad_linearize);
+	actual = nle->unwhitenedError(bad_linearize);
 	CHECK(assert_equal(actual, repeat(2, 1.0/0.0)));
 }
 

cpp/testNonlinearFactor.cpp

@@ -11,6 +11,8 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+// TODO: DANGEROUS, create shared pointers
+#define GTSAM_MAGIC_GAUSSIAN 2
 #define GTSAM_MAGIC_KEY
 
 #include "Matrix.h"
@@ -69,12 +71,11 @@ TEST( NonlinearFactor, NonlinearFactor )
   shared_nlf factor = fg[0];
 
   // calculate the error_vector from the factor "f1"
-  Vector actual_e = factor->error_vector(cfg);
+  // the expected value for the whitened error from the factor
-  Vector e(2); e(0) = 0.1;  e(1) = 0.1;
-  CHECK(assert_equal(e,actual_e));
-
-  // the expected value for the error from the factor
   // error_vector / sigma = [0.1 0.1]/0.1 = [1;1]
+  Vector actual_e = factor->whitenedError(cfg);
+  CHECK(assert_equal(ones(2),actual_e));
+
   // error = 0.5 * [1 1] * [1;1] = 1
   double expected = 1.0; 
 

cpp/testNonlinearFactorGraph.cpp

@@ -67,14 +67,15 @@ TEST( ExampleNonlinearFactorGraph, probPrime )
 	DOUBLES_EQUAL(expected,actual,0);
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
+// TODO: Commented out until noise model is passed to Gaussian factor graph
 TEST( ExampleNonlinearFactorGraph, linearize )
 {
 	ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
 	VectorConfig initial = createNoisyConfig();
 	GaussianFactorGraph linearized = fg.linearize(initial);
 	GaussianFactorGraph expected = createGaussianFactorGraph();
-	CHECK(expected.equals(linearized));
+	CHECK(assert_equal(expected,linearized));
 }
 
 /* ************************************************************************* */

cpp/testPlanarSLAM.cpp

@@ -5,21 +5,25 @@
 
 #include <iostream>
 #include <CppUnitLite/TestHarness.h>
+
 #include "planarSLAM.h"
 
 using namespace std;
 using namespace gtsam;
 
 // some shared test values
-Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
+static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
-Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
+static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
+
+sharedGaussian
+	sigma(noiseModel::Isotropic::Sigma(1,0.1)),
+	I3(noiseModel::Unit::Create(3));
 
 /* ************************************************************************* */
 TEST( planarSLAM, BearingFactor )
 {
 	// Create factor
 	Rot2 z(M_PI_4 + 0.1); // h(x) - z = -0.1
-	double sigma = 0.1;
 	planarSLAM::Bearing factor(2, 3, z, sigma);
 
 	// create config
@@ -28,7 +32,7 @@ TEST( planarSLAM, BearingFactor )
 	c.insert(3, l3);
 
 	// Check error
-	Vector actual = factor.error_vector(c);
+	Vector actual = factor.unwhitenedError(c);
 	CHECK(assert_equal(Vector_(1,-0.1),actual));
 }
 
@@ -37,7 +41,6 @@ TEST( planarSLAM, RangeFactor )
 {
 	// Create factor
 	double z(sqrt(2) - 0.22); // h(x) - z = 0.22
-	double sigma = 0.1;
 	planarSLAM::Range factor(2, 3, z, sigma);
 
 	// create config
@@ -46,7 +49,7 @@ TEST( planarSLAM, RangeFactor )
 	c.insert(3, l3);
 
 	// Check error
-	Vector actual = factor.error_vector(c);
+	Vector actual = factor.unwhitenedError(c);
 	CHECK(assert_equal(Vector_(1,0.22),actual));
 }
 
@@ -66,20 +69,18 @@ TEST( planarSLAM, constructor )
 	G.addPoseConstraint(2, x2); // make it feasible :-)
 
 	// Add odometry
-	G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), eye(3));
+	G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3);
 
 	// Create bearing factor
 	Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1
-	double sigma1 = 0.1;
+	G.addBearing(2, 3, z1, sigma);
-	G.addBearing(2, 3, z1, sigma1);
 
 	// Create range factor
 	double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
-	double sigma2 = 0.1;
+	G.addRange(2, 3, z2, sigma);
-	G.addRange(2, 3, z2, sigma2);
 
 	Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22);
-	CHECK(assert_equal(expected,G.error_vector(c)));
+	CHECK(assert_equal(expected,G.unwhitenedError(c)));
 }
 
 /* ************************************************************************* */

cpp/testPose2Factor.cpp

@@ -16,11 +16,12 @@ using namespace gtsam;
 
 // Common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
-static Matrix covariance = Matrix_(3,3,
+static noiseModel::Gaussian::shared_ptr covariance(
-		sx*sx, 0.0, 0.0,
+		noiseModel::Gaussian::Covariance(Matrix_(3, 3,
-		0.0, sy*sy, 0.0,
+	sx*sx, 0.0, 0.0,
-		0.0, 0.0, st*st
+	0.0, sy*sy, 0.0,
-		);
+	0.0, 0.0, st*st
+	)));
 
 /* ************************************************************************* */
 // Very simple test establishing Ax-b \approx z-h(x)
@@ -45,14 +46,14 @@ TEST( Pose2Factor, error )
 	delta.insert("x1", zero(3));
 	delta.insert("x2", zero(3));
 	Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
-	CHECK(assert_equal(error_at_zero,factor.error_vector(x0)));
+	CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0)));
 	CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
 
 	// Check error after increasing p2
 	VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0));
 	Pose2Config x1 = expmap(x0, plus);
 	Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
-	CHECK(assert_equal(error_at_plus,factor.error_vector(x1)));
+	CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
 }
 
@@ -78,7 +79,7 @@ TEST( Pose2Factor, rhs )
 	Pose2 hx0 = between(p1,p2);
 	CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0));
 	Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
-	CHECK(assert_equal(expected_b,-factor.error_vector(x0)));
+	CHECK(assert_equal(expected_b,-factor.whitenedError(x0)));
 	CHECK(assert_equal(expected_b,linear->get_b()));
 }
 
@@ -86,7 +87,7 @@ TEST( Pose2Factor, rhs )
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
 // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
 Vector h(const Pose2& p1,const Pose2& p2) {
-	return factor.evaluateError(p1,p2);
+	return covariance->whiten(factor.evaluateError(p1,p2));
 }
 
 /* ************************************************************************* */
@@ -100,21 +101,16 @@ TEST( Pose2Factor, linearize )
 	x0.insert(2,p2);
 
 	// expected linearization
-	Matrix square_root_inverse_covariance = Matrix_(3,3,
+	Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,
-	    2.0, 0.0, 0.0,
-	    0.0, 2.0, 0.0,
-	    0.0, 0.0, 10.0
-	);
-	Matrix expectedH1 = square_root_inverse_covariance*Matrix_(3,3,
 	    0.0,-1.0,-2.0,
 	    1.0, 0.0,-2.0,
 	    0.0, 0.0,-1.0
-	);
+	));
-	Matrix expectedH2 = square_root_inverse_covariance*Matrix_(3,3,
+	Matrix expectedH2 = covariance->Whiten(Matrix_(3,3,
 	    1.0, 0.0, 0.0,
 	    0.0, 1.0, 0.0,
 	    0.0, 0.0, 1.0
-	);
+	));
 	Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
 
 	// expected linear factor

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 GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(Vector_(3,sx,sy,st));
+static sharedGaussian sigmas = Diagonal::Sigmas(Vector_(3,sx,sy,st));
 
 /* ************************************************************************* */
 // Very simple test establishing Ax-b \approx z-h(x)
@@ -28,7 +28,7 @@ TEST( Pose2Prior, error )
 	x0.insert(1, p1);
 
 	// Create factor
-	Pose2Prior factor(1, p1, model);
+	Pose2Prior factor(1, p1, sigmas);
 
 	// Actual linearization
 	boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
@@ -37,27 +37,27 @@ TEST( Pose2Prior, error )
 	VectorConfig delta;
 	delta.insert("x1", zero(3));
 	Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
-	CHECK(assert_equal(error_at_zero,factor.error_vector(x0)));
+	CHECK(assert_equal(error_at_zero,factor.whitenedError(x0)));
 	CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
 
 	// Check error after increasing p2
 	VectorConfig plus = delta + VectorConfig("x1", Vector_(3, 0.1, 0.0, 0.0));
 	Pose2Config x1 = expmap(x0, plus);
 	Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
-	CHECK(assert_equal(error_at_plus,factor.error_vector(x1)));
+	CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
 }
 
 /* ************************************************************************* */
 // common Pose2Prior for tests below
 static Pose2 prior(2,2,M_PI_2);
-static Pose2Prior factor(1,prior, model);
+static Pose2Prior factor(1,prior, sigmas);
 
 /* ************************************************************************* */
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
 // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
 Vector h(const Pose2& p1) {
-	return factor.evaluateError(p1);
+	return sigmas->whiten(factor.evaluateError(p1));
 }
 
 /* ************************************************************************* */

cpp/testPose2SLAM.cpp

@@ -23,11 +23,12 @@ using namespace gtsam;
 
 // common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
-static Matrix covariance = Matrix_(3,3,
+static noiseModel::Gaussian::shared_ptr covariance(
-		sx*sx, 0.0, 0.0,
+		noiseModel::Gaussian::Covariance(Matrix_(3, 3,
-		0.0, sy*sy, 0.0,
+	sx*sx, 0.0, 0.0,
-		0.0, 0.0, st*st
+	0.0, sy*sy, 0.0,
-		);
+	0.0, 0.0, st*st
+	))), I3(noiseModel::Unit::Create(3));
 
 /* ************************************************************************* */
 TEST( Pose2Graph, constructor )
@@ -162,10 +163,9 @@ TEST(Pose2Graph, optimizeCircle) {
 // test optimization with 6 poses arranged in a hexagon and a loop closure
 TEST(Pose2Graph, findMinimumSpanningTree) {
 	Pose2Graph G, T, C;
-	Matrix cov = eye(3);
+	G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
-	G.addConstraint(1, 2, Pose2(0.,0.,0.), cov);
+	G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
-	G.addConstraint(1, 3, Pose2(0.,0.,0.), cov);
+	G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
-	G.addConstraint(2, 3, Pose2(0.,0.,0.), cov);
 
 	PredecessorMap<pose2SLAM::Key> tree =
 			G.findMinimumSpanningTree<pose2SLAM::Key, Pose2Factor>();
@@ -178,10 +178,9 @@ TEST(Pose2Graph, findMinimumSpanningTree) {
 // test optimization with 6 poses arranged in a hexagon and a loop closure
 TEST(Pose2Graph, split) {
 	Pose2Graph G, T, C;
-	Matrix cov = eye(3);
+	G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
-	G.addConstraint(1, 2, Pose2(0.,0.,0.), cov);
+	G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
-	G.addConstraint(1, 3, Pose2(0.,0.,0.), cov);
+	G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
-	G.addConstraint(2, 3, Pose2(0.,0.,0.), cov);
 
 	PredecessorMap<pose2SLAM::Key> tree;
 	tree.insert(1,2);

cpp/testPose3Factor.cpp

@@ -23,8 +23,8 @@ TEST( Pose3Factor, error )
 	Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
 
 	// Create factor
-	Matrix measurement_covariance = eye(6);
+	sharedGaussian I6(noiseModel::Unit::Create(6));
-	Pose3Factor factor(1,2, z, measurement_covariance);
+	Pose3Factor factor(1,2, z, I6);
 
 	// Create config
 	Pose3Config x;
@@ -32,7 +32,7 @@ TEST( Pose3Factor, error )
 	x.insert(2,t2);
 
 	// Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2))
-	Vector actual = factor.error_vector(x);
+	Vector actual = factor.unwhitenedError(x);
 	Vector expected = logmap(z,between(t1,t2));
 	CHECK(assert_equal(expected,actual));
 }

cpp/testPose3SLAM.cpp

@@ -13,6 +13,8 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+// TODO: DANGEROUS, create shared pointers
+#define GTSAM_MAGIC_GAUSSIAN 6
 #define GTSAM_MAGIC_KEY
 
 #include "pose3SLAM.h"

cpp/testSQP.cpp

@@ -12,6 +12,8 @@
 #include <boost/shared_ptr.hpp>
 #include <CppUnitLite/TestHarness.h>
 
+// TODO: DANGEROUS, create shared pointers
+#define GTSAM_MAGIC_GAUSSIAN 2
 #define GTSAM_MAGIC_KEY
 
 #include <GaussianFactorGraph.h>

cpp/testSQPOptimizer.cpp

@@ -29,6 +29,8 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 	
+static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
+
 // typedefs
 typedef boost::shared_ptr<VectorConfig> shared_config;
 typedef NonlinearFactorGraph<VectorConfig> NLGraph;
@@ -102,13 +104,11 @@ NLGraph linearMapWarpGraph() {
 
 	// measurement from x1 to l1
 	Vector z1 = Vector_(2, 0.0, 5.0);
-	double sigma1 = 0.1;
+	shared f1(new simulated2D::Measurement(z1, sigma, "x1", "l1"));
-	shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1"));
 
 	// measurement from x2 to l2
 	Vector z2 = Vector_(2, -4.0, 0.0);
-	double sigma2 = 0.1;
+	shared f2(new simulated2D::Measurement(z2, sigma, "x2", "l2"));
-	shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2"));
 
 	// equality constraint between l1 and l2
 	list<Symbol> keys; keys += "l1", "l2";
@@ -262,13 +262,11 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
 
 	// measurement from x1 to x2
 	Vector x1x2 = Vector_(2, 5.0, 0.0);
-	double sigma1 = 0.1;
+	shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2"));
-	shared f1(new simulated2D::Odometry(x1x2, sigma1, "x1", "x2"));
 
 	// measurement from x2 to x3
 	Vector x2x3 = Vector_(2, 5.0, 0.0);
-	double sigma2 = 0.1;
+	shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3"));
-	shared f2(new simulated2D::Odometry(x2x3, sigma2, "x2", "x3"));
 
 	// create a binary inequality constraint that forces the middle point away from
 	//  the obstacle
@@ -394,13 +392,11 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
 
 	// measurement from x1 to x2
 	Vector x1x2 = Vector_(2, 5.0, 0.0);
-	double sigma1 = 0.1;
+	shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2"));
-	shared f1(new simulated2D::Odometry(x1x2, sigma1, "x1", "x2"));
 
 	// measurement from x2 to x3
 	Vector x2x3 = Vector_(2, 5.0, 0.0);
-	double sigma2 = 0.1;
+	shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3"));
-	shared f2(new simulated2D::Odometry(x2x3, sigma2, "x2", "x3"));
 
 	double radius = 1.0;
 

cpp/testVSLAMFactor.cpp

@@ -15,13 +15,17 @@ using namespace gtsam;
 using namespace gtsam::visualSLAM;
 
 // make cube
-Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1),
+static Point3
-		x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1);
+	x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1),
+	x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1);
 
 // make a realistic calibration matrix
-double fov = 60; // degrees
+static double fov = 60; // degrees
-size_t w=640,h=480;
+static size_t w=640,h=480;
-Cal3_S2 K(fov,w,h);
+static Cal3_S2 K(fov,w,h);
+
+static sharedGaussian sigma(noiseModel::Unit::Create(1));
+static shared_ptrK sK(new Cal3_S2(K));
 
 // make cameras
 
@@ -30,17 +34,16 @@ TEST( ProjectionFactor, error )
 {
 	// Create the factor with a measurement that is 3 pixels off in x
 	Point2 z(323.,240.);
-	double sigma=1.0;
 	int cameraFrameNumber=1, landmarkNumber=1;
 	boost::shared_ptr<ProjectionFactor>
-		factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K))));
+		factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
 
   // For the following configuration, the factor predicts 320,240
   Config config;
   Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
   Point3 l1;  config.insert(1, l1);
   // Point should project to Point2(320.,240.)
-  CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->error_vector(config)));
+  CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
 
   // Which yields an error of 3^2/2 = 4.5
   DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
@@ -76,13 +79,12 @@ TEST( ProjectionFactor, equals )
 {
 	// Create two identical factors and make sure they're equal
 	Vector z = Vector_(2,323.,240.);
-	double sigma=1.0;
 	int cameraFrameNumber=1, landmarkNumber=1;
 	boost::shared_ptr<ProjectionFactor>
-	  factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K))));
+	  factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
 
 	boost::shared_ptr<ProjectionFactor>
-		factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K))));
+		factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
 
 	CHECK(assert_equal(*factor1, *factor2));
 }

cpp/testVSLAMGraph.cpp

@@ -23,6 +23,7 @@ using namespace gtsam;
 using namespace gtsam::visualSLAM;
 using namespace boost;
 typedef NonlinearOptimizer<Graph,Config> Optimizer;
+static sharedGaussian sigma(noiseModel::Unit::Create(1));
 
 /* ************************************************************************* */
 Point3 landmark1(-1.0,-1.0, 0.0);
@@ -55,7 +56,6 @@ Graph testGraph() {
 	Point2 z23( 125,-125);
 	Point2 z24( 125, 125);
 
-  double sigma = 1;
   shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
   Graph g;
   g.add(ProjectionFactor(z11, sigma, 1, 1, sK));

cpp/visualSLAM.h

@@ -62,8 +62,11 @@ namespace gtsam { namespace visualSLAM {
      * @param landmarkNumber is the index of the landmark
      * @param K the constant calibration
      */
-    ProjectionFactor(const Point2& z, double sigma, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K) :
+    ProjectionFactor(const Point2& z,
-        z_(z), K_(K), Base(sigma, j_pose, j_landmark) {}
+					const sharedGaussian& model, PoseKey j_pose,
+					PointKey j_landmark, const shared_ptrK& K) :
+				z_(z), K_(K), Base(model, j_pose, j_landmark) {
+			}
 
     /**
      * print

myconfigure

@@ -1,4 +1,4 @@
 ./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/
 
 # for maximum performance on Intel Core2 platform:
-#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -ftree-vectorizer-verbose=2 -march=nocona -mtune=generic -DNDEBUG" --disable-static
+#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -ftree-vectorizer-verbose=2 -march=core2 -DNDEBUG" --disable-static