Smart named constructors Covariance/Variances/Variance

cpp/GaussianFactor.h

@@ -60,7 +60,7 @@ public:
 		As_.insert(make_pair(key1, A1));
 	}
 
-	/** Construct unary factor with vector of sigmas*/
+	/** Construct unary factor with vector of sigmas */
 	GaussianFactor(const Symbol& key1, const Matrix& A1,
 			const Vector& b, const Vector& sigmas) :
 		b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
@@ -76,6 +76,15 @@ public:
 		As_.insert(make_pair(key2, A2));
 	}
 
+	/** Construct binary factor with vector of sigmas */
+	GaussianFactor(const Symbol& key1, const Matrix& A1,
+			const Symbol& key2, const Matrix& A2,
+			const Vector& b, const Vector& sigmas) :
+		b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas))  {
+		As_.insert(make_pair(key1, A1));
+		As_.insert(make_pair(key2, A2));
+	}
+
 	/** Construct ternary factor */
 	GaussianFactor(const Symbol& key1, const Matrix& A1,
 			const Symbol& key2, const Matrix& A2,

cpp/NoiseModel.cpp

@@ -8,6 +8,8 @@
 
 #include <limits>
 #include <iostream>
+#include <typeinfo>
+#include <stdexcept>
 #include <boost/numeric/ublas/lu.hpp>
 #include <boost/numeric/ublas/io.hpp>
 #include <boost/foreach.hpp>
@@ -23,13 +25,30 @@ namespace gtsam {
 	namespace noiseModel {
 
 		/* ************************************************************************* */
+		Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
+			size_t m = covariance.size1(), n = covariance.size2();
+			if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
+			if (smart) {
+				// check all non-diagonal entries
+				int i,j;
+				for (i = 0; i < m; i++)
+					for (j = 0; j < n; j++)
+						if (i != j && fabs(covariance(i, j) > 1e-9)) goto full;
+				Vector variances(n);
+				for (j = 0; j < n; j++) variances(j) = covariance(j,j);
+				return Diagonal::Variances(variances,true);
+			}
+			full: return shared_ptr(new Gaussian(inverse_square_root(covariance)));
+		}
+
     void Gaussian::print(const string& name) const {
 			gtsam::print(sqrt_information_, "Gaussian");
 		}
 
-		bool Gaussian::equals(const Base& m, double tol) const {
+		bool Gaussian::equals(const Base& expected, double tol) const {
-			const Gaussian* p = dynamic_cast<const Gaussian*> (&m);
+			const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
 			if (p == NULL) return false;
+			if (typeid(*this) != typeid(*p)) return false;
 			return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol));
 		}
 
@@ -80,6 +99,17 @@ namespace gtsam {
 							sigmas_(sigmas) {
 				}
 
+    Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
+			if (smart) {
+				// check whether all the same entry
+				int j, n = variances.size();
+				for (j = 1; j < n; j++)
+					if (variances(j) != variances(0)) goto full;
+				return Isotropic::Variance(n, variances(0), true);
+			}
+			full: return shared_ptr(new Diagonal(esqrt(variances)));
+    }
+
     void Diagonal::print(const string& name) const {
 			gtsam::print(sigmas_, "Diagonal sigmas " + name);
 		}
@@ -205,6 +235,11 @@ namespace gtsam {
 
 		/* ************************************************************************* */
 
+    Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart)  {
+    	if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim);
+    	return shared_ptr(new Isotropic(dim, sqrt(variance)));
+    }
+
     void Isotropic::print(const string& name) const {
 			cout << "Isotropic sigma " << name << " " << sigma_ << endl;
 		}

cpp/NoiseModel.h

@@ -85,6 +85,7 @@ namespace gtsam {	namespace noiseModel {
 
     /**
      * A Gaussian noise model created by specifying a square root information matrix.
+     * @param smart, check if can be simplified to derived class
      */
     static shared_ptr SqrtInformation(const Matrix& R) {
     	return shared_ptr(new Gaussian(R));
@@ -92,10 +93,9 @@ namespace gtsam {	namespace noiseModel {
 
     /**
      * A Gaussian noise model created by specifying a covariance matrix.
+     * @param smart, check if can be simplified to derived class
      */
-    static shared_ptr Covariance(const Matrix& covariance) {
+    static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
-    	return shared_ptr(new Gaussian(inverse_square_root(covariance)));
-    }
 
     /**
      * A Gaussian noise model created by specifying an information matrix.
@@ -183,10 +183,9 @@ namespace gtsam {	namespace noiseModel {
     /**
      * A diagonal noise model created by specifying a Vector of variances, i.e.
      * i.e. the diagonal of the covariance matrix.
+     * @param smart, check if can be simplified to derived class
      */
-    static shared_ptr Variances(const Vector& variances) {
+    static shared_ptr Variances(const Vector& variances, bool smart = false);
-    	return shared_ptr(new Diagonal(esqrt(variances)));
-    }
 
     /**
      * A diagonal noise model created by specifying a Vector of precisions, i.e.
@@ -288,10 +287,9 @@ namespace gtsam {	namespace noiseModel {
 
     /**
      * An isotropic noise model created by specifying a variance = sigma^2.
+     * @param smart, check if can be simplified to derived class
      */
-    static shared_ptr Variance(size_t dim, double variance)  {
+    static shared_ptr Variance(size_t dim, double variance, bool smart = false);
-    	return shared_ptr(new Isotropic(dim, sqrt(variance)));
-    }
 
     /**
      * An isotropic noise model created by specifying a precision

cpp/testGaussianFactor.cpp

@@ -221,36 +221,16 @@ TEST( NonlinearFactorGraph, combine2){
 
 /* ************************************************************************* */
 TEST( GaussianFactor, linearFactorN){
+	Matrix I = eye(2);
   vector<GaussianFactor::shared_ptr> f;
-  f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
+  f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2,
-      1.0, 0.0,
-      0.0, 1.0),
-      Vector_(2,
 			10.0, 5.0), 1)));
-  f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
+	f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I,
-      -10.0, 0.0,
+			"x2", 10 * I, Vector_(2, 1.0, -2.0), 1)));
-      0.0, -10.0),
+	f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", -10 * I,
-      "x2", Matrix_(2,2,
+			"x3", 10 * I, Vector_(2, 1.5, -1.5), 1)));
-      10.0, 0.0,
+	f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", -10 * I,
-      0.0, 10.0),
+			"x4", 10 * I, Vector_(2, 2.0, -1.0), 1)));
-      Vector_(2,
-      1.0, -2.0), 1)));
-  f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", Matrix_(2,2,
-      -10.0, 0.0,
-      0.0, -10.0),
-      "x3", Matrix_(2,2,
-      10.0, 0.0,
-      0.0, 10.0),
-      Vector_(2,
-      1.5, -1.5), 1)));
-  f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", Matrix_(2,2,
-      -10.0, 0.0,
-      0.0, -10.0),
-      "x4", Matrix_(2,2,
-      10.0, 0.0,
-      0.0, 10.0),
-      Vector_(2,
-      2.0, -1.0), 1)));
 
   GaussianFactor combinedFactor(f);
 
@@ -294,8 +274,9 @@ TEST( GaussianFactor, linearFactorN){
   Vector b = Vector_(8,
       10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
 
-  GaussianFactor expected(combinedMeasurement, b, 1.);
+  Vector sigmas = repeat(8,1.0);
-  CHECK(combinedFactor.equals(expected));
+  GaussianFactor expected(combinedMeasurement, b, sigmas);
+  CHECK(assert_equal(expected,combinedFactor));
 }
 
 /* ************************************************************************* */
@@ -336,45 +317,19 @@ TEST( GaussianFactor, eliminate )
 	boost::tie(actualCG,actualLF) = combined.eliminate("x2");
 
 	// create expected Conditional Gaussian
-	Matrix R11 = Matrix_(2,2,
+	Matrix I = eye(2);
-			1.0, 0.0,
+	Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I;
-			0.0, 1.0
-	);
-	Matrix S12 = Matrix_(2,2,
-			-0.2, 0.0,
-			+0.0,-0.2
-	);
-	Matrix S13 = Matrix_(2,2,
-			-0.8, 0.0,
-			+0.0,-0.8
-	);
 	Vector d(2); d(0) = 0.2; d(1) = -0.14;
 
-	Vector sigmas(2);
-	sigmas(0) = 1/sqrt(125.0);
-	sigmas(1) = 1/sqrt(125.0);
-
 	// Check the conditional Gaussian
-	GaussianConditional expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas);
+	GaussianConditional
+		expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1 / sqrt(125.0)));
 
 	// the expected linear factor
-	double sigma = 0.2236;
+	Matrix Bl1 = I, Bx1 = -I;
-	Matrix Bl1 = Matrix_(2,2,
-			// l1
-			1.00, 0.00,
-			0.00, 1.00
-	);
-
-	Matrix Bx1 = Matrix_(2,2,
-			// x1
-			-1.00,  0.00,
-			+0.00, -1.00
-	);
-
-	// the RHS
 	Vector b1(2); b1(0) = 0.0; b1(1) = 0.2;
 
-	GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma);
+	GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,0.2236));
 
 	// check if the result matches
 	CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@@ -452,7 +407,7 @@ TEST( GaussianFactor, eliminate2 )
 	// the RHS
 	Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
 
-	GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, sigma);
+	GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, repeat(2,sigma));
 
 	// check if the result matches
 	CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@@ -684,7 +639,7 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
 	GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l11",S12,sigmas));
 	GaussianFactor actualLF(CG);
 	//  actualLF.print();
-	GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas(0));
+	GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas);
 
 	CHECK(assert_equal(expectedLF,actualLF,1e-5));
 }

cpp/testNoiseModel.cpp

@@ -173,6 +173,23 @@ TEST( NoiseModel, QR )
 	CHECK(assert_equal(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, SmartCovariance )
+{
+	bool smart = true;
+	sharedGaussian expected = Unit::Create(3);
+	sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
+	CHECK(assert_equal(*expected,*actual));
+}
+
+/* ************************************************************************* */
+TEST(NoiseModel, ScalarOrVector )
+{
+	sharedGaussian expected = Unit::Create(3);
+	sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
+	CHECK(assert_equal(*expected,*actual));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;