Smarter noise models: Diagonal::Sigmas is now actually smart, and Gaussian::SqrtInformation now has a smart flag (default is true)

gtsam/linear/NoiseModel.cpp

@@ -47,22 +47,50 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
   Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
 }
 
+/* ************************************************************************* */
+// check *above the diagonal* for non-zero entries
+static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
+  size_t m = M.rows(), n = M.cols();
+  // check all non-diagonal entries
+  bool full = false;
+  size_t i, j;
+  for (i = 0; i < m; i++)
+    if (!full)
+      for (j = i + 1; j < n; j++)
+        if (fabs(M(i, j)) > 1e-9) {
+          full = true;
+          break;
+        }
+  if (full) {
+    return boost::none;
+  } else {
+    Vector diagonal(n);
+    for (j = 0; j < n; j++)
+      diagonal(j) = M(j, j);
+    return diagonal;
+  }
+}
+
+/* ************************************************************************* */
+Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
+  size_t m = R.rows(), n = R.cols();
+  if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square");
+  boost::optional<Vector> diagonal = boost::none;
+  if (smart)
+    diagonal = checkIfDiagonal(R);
+  if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true);
+  else return shared_ptr(new Gaussian(R.rows(),R));
+}
 
 /* ************************************************************************* */
 Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
   size_t m = covariance.rows(), n = covariance.cols();
   if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
-  if (smart) {
-    // check all non-diagonal entries
-    size_t 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(n, inverse_square_root(covariance)));
+  boost::optional<Vector> variances = boost::none;
+  if (smart)
+    variances = checkIfDiagonal(covariance);
+  if (variances) return Diagonal::Variances(*variances,true);
+  else return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
 }
 
 /* ************************************************************************* */
@@ -166,7 +194,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
 // Diagonal
 /* ************************************************************************* */
 Diagonal::Diagonal() :
-    Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1))
+    Gaussian(1) // TODO: Frank asks: really sure about this?
 {
 }
 
@@ -191,12 +219,17 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
 /* ************************************************************************* */
 Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
   if (smart) {
+    DenseIndex j, n = sigmas.size();
     // look for zeros to make a constraint
-    for (size_t i=0; i< (size_t) sigmas.size(); ++i)
-      if (sigmas(i)<1e-8)
+    for (j=0; j< n; ++j)
+      if (sigmas(j)<1e-8)
         return Constrained::MixedSigmas(sigmas);
+    // check whether all the same entry
+    for (j = 1; j < n; j++)
+      if (sigmas(j) != sigmas(0)) goto full;
+    return Isotropic::Sigma(n, sigmas(0), true);
   }
-  return Diagonal::shared_ptr(new Diagonal(sigmas));
+  full: return Diagonal::shared_ptr(new Diagonal(sigmas));
 }
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -159,10 +159,10 @@ namespace gtsam {
 
       /**
        * A Gaussian noise model created by specifying a square root information matrix.
+       * @param R The (upper-triangular) 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.rows(),R));
-      }
+      static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
 
       /**
        * A Gaussian noise model created by specifying a covariance matrix.

gtsam/linear/tests/testNoiseModel.cpp

@@ -267,6 +267,24 @@ TEST(NoiseModel, QRNan )
   EXPECT(assert_equal(expectedAb,Ab));
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, SmartSqrtInformation )
+{
+  bool smart = true;
+  gtsam::SharedGaussian expected = Unit::Create(3);
+  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart);
+  EXPECT(assert_equal(*expected,*actual));
+}
+
+/* ************************************************************************* */
+TEST(NoiseModel, SmartSqrtInformation2 )
+{
+  bool smart = true;
+  gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
+  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart);
+  EXPECT(assert_equal(*expected,*actual));
+}
+
 /* ************************************************************************* */
 TEST(NoiseModel, SmartCovariance )
 {