From 89e6e2730142f312b75913fb25ecec40de2abe5c Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 31 May 2014 20:29:00 -0400
Subject: [PATCH] Additional unit test

---
 gtsam/linear/NoiseModel.cpp           | 16 +++++++++++++++-
 gtsam/linear/NoiseModel.h             | 10 ++++++++++
 gtsam/linear/tests/testNoiseModel.cpp | 11 +++++++++++
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 45314e023..3d11542db 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -49,7 +49,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
 
 /* ************************************************************************* */
 // check *above the diagonal* for non-zero entries
-static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
+boost::optional<Vector> checkIfDiagonal(const Matrix M) {
   size_t m = M.rows(), n = M.cols();
   // check all non-diagonal entries
   bool full = false;
@@ -82,6 +82,20 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
   else return shared_ptr(new Gaussian(R.rows(),R));
 }
 
+/* ************************************************************************* */
+Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
+  size_t m = M.rows(), n = M.cols();
+  if (m != n) throw invalid_argument("Gaussian::Information: R not square");
+  boost::optional<Vector> diagonal = boost::none;
+  if (smart)
+    diagonal = checkIfDiagonal(M);
+  if (diagonal) return Diagonal::Precisions(*diagonal, true);
+  else {
+    Matrix R = RtR(M);
+    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();
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index e709ea543..55d01c92f 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -164,6 +164,13 @@ namespace gtsam {
        */
       static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
 
+      /**
+       * A Gaussian noise model created by specifying an information matrix.
+       * @param M The information matrix
+       * @param smart check if can be simplified to derived class
+       */
+      static shared_ptr Information(const Matrix& M, bool smart = true);
+
       /**
        * A Gaussian noise model created by specifying a covariance matrix.
        * @param covariance The square covariance Matrix
@@ -864,6 +871,9 @@ namespace gtsam {
         ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
       }
     };
+    
+    // Helper function
+    boost::optional<Vector> checkIfDiagonal(const Matrix M);
 
   } // namespace noiseModel
 
diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index d68caeabe..df0f8a774 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -285,6 +285,17 @@ TEST(NoiseModel, SmartSqrtInformation2 )
   EXPECT(assert_equal(*expected,*actual));
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, SmartInformation )
+{
+  bool smart = true;
+  gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
+  Matrix M = 0.5*eye(3);
+  EXPECT(checkIfDiagonal(M));
+  gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
+  EXPECT(assert_equal(*expected,*actual));
+}
+
 /* ************************************************************************* */
 TEST(NoiseModel, SmartCovariance )
 {