Added mu to noisemodel

gtsam/linear/NoiseModel.cpp

@@ -275,6 +275,19 @@ Vector Diagonal::sample() const {
 
 /* ************************************************************************* */
 // Constrained
+/* ************************************************************************* */
+Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& sigmas, bool smart) {
+	// FIXME: can't return a diagonal shared_ptr due to conversion
+//	if (smart) {
+//		// look for zeros to make a constraint
+//		for (size_t i=0; i< (size_t) sigmas.size(); ++i)
+//			if (sigmas(i)<1e-8)
+//				return MixedSigmas(mu, sigmas);
+//		return Diagonal::Sigmas(sigmas);
+//	}
+	return shared_ptr(new Constrained(mu, sigmas));
+}
+
 /* ************************************************************************* */
 void Constrained::print(const std::string& name) const {
 	gtsam::print(sigmas_, name + ": constrained sigmas");
@@ -283,16 +296,28 @@ void Constrained::print(const std::string& name) const {
 /* ************************************************************************* */
 Vector Constrained::whiten(const Vector& v) const {
 	// ediv_ does the right thing with the errors
+	// FIXME: solve this more effectively
 	return ediv_(v, sigmas_);
 }
 
+/* ************************************************************************* */
+double Constrained::distance(const Vector& v) const {
+	Vector w = whiten(v); // get noisemodel for constrained elements
+	for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
+		if (isinf(w[i])) // whiten makes constrained variables infinite
+			w[i] = v[i] * sqrt(mu_[i]); // FIXME: may want to store sqrt rather than rebuild
+	return w.dot(w);
+}
+
 /* ************************************************************************* */
 Matrix Constrained::Whiten(const Matrix& H) const {
+	// FIXME: replace with pass-through
 	throw logic_error("noiseModel::Constrained cannot Whiten");
 }
 
 /* ************************************************************************* */
 void Constrained::WhitenInPlace(Matrix& H) const {
+	// FIXME: replace with pass-through
 	throw logic_error("noiseModel::Constrained cannot Whiten");
 }
 

gtsam/linear/NoiseModel.h

@@ -212,6 +212,7 @@ namespace gtsam {
 
 			/**
 			 * Simple check for constrained-ness
+			 * FIXME Find a better way of handling this
 			 */
 			virtual bool isConstrained() const {return false;}
 
@@ -327,17 +328,28 @@ namespace gtsam {
 		 * 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.
+		 * FIXME: the "otherwise return infinity" does not solve anything
+		 *
+		 * The distance function in this function provides an error model
+		 * for a penalty function with a scaling function, assuming a mask of
 		 */
 		class Constrained : public Diagonal {
 		protected:
 
-			// Constrained does not have member variables
+			// Sigmas are contained in the base class
-			// Instead (possibly zero) sigmas are stored in Diagonal Base class
+
+			// Penalty function parameters
+			Vector mu_;
 
 			/** protected constructor takes sigmas */
 			// Keeps only sigmas and calculates invsigmas when necessary
 			Constrained(const Vector& sigmas = zero(1)) :
-				Diagonal(sigmas, false) {}
+				Diagonal(sigmas, false), mu_(repeat(sigmas.size(), 1000.0)) {}
+
+			// Keeps only sigmas and calculates invsigmas when necessary
+			// allows for specifying mu
+			Constrained(const Vector& mu, const Vector& sigmas) :
+				Diagonal(sigmas, false), mu_(mu) {}
 
 		public:
 
@@ -345,19 +357,38 @@ namespace gtsam {
 
 			virtual ~Constrained() {}
 
+			/// Access mu as a vector
+			const Vector& mu() const { return mu_; }
+
+			/**
+			 * A diagonal noise model created by specifying a Vector of
+			 * standard devations, some of which might be zero
+			 */
+			static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas,
+					bool smart = false);
+
 			/**
 			 * A diagonal noise model created by specifying a Vector of
 			 * standard devations, some of which might be zero
-			 * TODO: make smart - check for zeros
 			 */
 			static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = false) {
-				return shared_ptr(new Constrained(sigmas));
+				return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart);
 			}
 
 			/**
 			 * A diagonal noise model created by specifying a Vector of
 			 * standard devations, some of which might be zero
 			 */
+			static shared_ptr MixedSigmas(double m, const Vector& sigmas,
+					bool smart = false) {
+				return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart);
+			}
+
+			/**
+			 * A diagonal noise model created by specifying a Vector of
+			 * standard devations, some of which might be zero
+			 * TODO: allow for mu
+			 */
 			static shared_ptr MixedVariances(const Vector& variances) {
 				return shared_ptr(new Constrained(esqrt(variances)));
 			}
@@ -365,24 +396,42 @@ namespace gtsam {
 			/**
 			 * A diagonal noise model created by specifying a Vector of
 			 * precisions, some of which might be inf
+			 * TODO: allow for mu
 			 */
 			static shared_ptr MixedPrecisions(const Vector& precisions) {
 				return MixedVariances(reciprocal(precisions));
 			}
 
 			/**
-			 * Fully constrained. TODO: subclass ?
+			 * The distance function for a constrained noisemodel,
+			 * for non-constrained versions, uses sigmas, otherwise
+			 * uses the penalty function with mu
 			 */
+      virtual double distance(const Vector& v) const;
+
+			/** Fully constrained variations */
 			static shared_ptr All(size_t dim) {
-				return MixedSigmas(repeat(dim,0));
+				return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0)));
+			}
+
+			/** Fully constrained variations */
+			static shared_ptr All(size_t dim, const Vector& mu) {
+				return shared_ptr(new Constrained(mu, repeat(dim,0)));
+			}
+
+			/** Fully constrained variations */
+			static shared_ptr All(size_t dim, double m) {
+				return shared_ptr(new Constrained(repeat(dim, m), repeat(dim,0)));
 			}
 
 			virtual void print(const std::string& name) const;
+
+			/// Calculates error vector with weights applied
 			virtual Vector whiten(const Vector& v) const;
 
 			// Whitening Jacobians does not make sense for possibly constrained
 			// noise model and will throw an exception.
-
+			// FIXME: change to allow for use a normal linearization function
 			virtual Matrix Whiten(const Matrix& H) const;
 			virtual void WhitenInPlace(Matrix& H) const;
 
@@ -393,6 +442,7 @@ namespace gtsam {
 
 			/**
 			 * Check constrained is always true
+			 * FIXME Find a better way of handling this
 			 */
 			virtual bool isConstrained() const {return true;}
 
@@ -402,6 +452,7 @@ namespace gtsam {
 			template<class ARCHIVE>
 			void serialize(ARCHIVE & ar, const unsigned int version) {
 				ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
+				ar & BOOST_SERIALIZATION_NVP(mu_);
 			}
 
 		}; // Constrained

gtsam/linear/tests/testNoiseModel.cpp

@@ -41,10 +41,6 @@ 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();
 
@@ -108,7 +104,8 @@ TEST(NoiseModel, constructors)
 }
 
 /* ************************************************************************* */
-TEST(NoiseModel, Unit) {
+TEST(NoiseModel, Unit)
+{
 	Vector v = Vector_(3,5.0,10.0,15.0);
 	Gaussian::shared_ptr u(Unit::Create(3));
 	EXPECT(assert_equal(v,u->whiten(v)));
@@ -135,36 +132,80 @@ TEST(NoiseModel, equals)
 }
 
 /* ************************************************************************* */
-TEST(NoiseModel, sample) {
+TEST(NoiseModel, sample)
+{
 	Vector s = Vector_(3,1.0,2.0,3.0);
 	SharedDiagonal model = sharedSigmas(s);
 	Vector v = model->sample();
 	// no check as no way yet to set random seed
 }
 
+// TODO enable test once a mechanism for smart constraints exists
+///* ************************************************************************* */
+//TEST(NoiseModel, ConstrainedSmart )
+//{
+//	Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma), true);
+//	Diagonal::shared_ptr n1 = boost::shared_dynamic_cast<Diagonal>(nonconstrained);
+//	Constrained::shared_ptr n2 = boost::shared_dynamic_cast<Constrained>(nonconstrained);
+//	EXPECT(n1);
+//	EXPECT(!n2);
+//
+//	Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
+//	Diagonal::shared_ptr c1 = boost::shared_dynamic_cast<Diagonal>(constrained);
+//	Constrained::shared_ptr c2 = boost::shared_dynamic_cast<Constrained>(constrained);
+//	EXPECT(c1);
+//	EXPECT(c2);
+//}
+
+/* ************************************************************************* */
+TEST(NoiseModel, ConstrainedConstructors )
+{
+	Constrained::shared_ptr actual;
+	size_t d = 3;
+	double m = 100.0;
+	Vector sigmas = Vector_(3, sigma, 0.0, 0.0);
+	Vector mu = Vector_(3, 200.0, 300.0, 400.0);
+	actual = Constrained::All(d);
+	EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
+
+	actual = Constrained::All(d, m);
+	EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));
+
+	actual = Constrained::All(d, mu);
+	EXPECT(assert_equal(mu, actual->mu()));
+
+	actual = Constrained::MixedSigmas(mu, sigmas);
+	EXPECT(assert_equal(mu, actual->mu()));
+
+	actual = Constrained::MixedSigmas(m, sigmas);
+	EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu()));
+}
+
 /* ************************************************************************* */
 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::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
+	Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
 	EXPECT(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible)));
 	EXPECT(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);
+	DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
+	DOUBLES_EQUAL(0.5,d->distance(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);
+			 infeasible = Vector_(3, 1.0, 1.0, 1.0);
 
 	Constrained::shared_ptr i = Constrained::All(3);
 	EXPECT(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible)));
 	EXPECT(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);
+	DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
+	DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
 }
 
 /* ************************************************************************* */