From dfb3e60171ad912fe17193c7af59c525b3633b7c Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgc@umich.edu>
Date: Sat, 4 Jan 2014 20:01:05 -0500
Subject: [PATCH] Fixed issue with serialization by actively changing inf/nan
 values to zero in constrained noisemodel. Updated WhitenInPlace() and
 distance(). Apparently this caused a bunch of other unit tests to fail,
 though.

---
 gtsam/linear/NoiseModel.cpp                   | 48 +++++++++++++++----
 gtsam/linear/NoiseModel.h                     | 31 ++++++------
 gtsam/linear/tests/testNoiseModel.cpp         |  5 +-
 .../linear/tests/testSerializationLinear.cpp  | 12 ++---
 4 files changed, 65 insertions(+), 31 deletions(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 359ce9968..6afd23f55 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -166,9 +166,11 @@ 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)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1))
+{
 }
 
+/* ************************************************************************* */
 Diagonal::Diagonal(const Vector& sigmas) :
     Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_(
         emul(invsigmas_, invsigmas_)) {
@@ -229,6 +231,31 @@ void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
 
 /* ************************************************************************* */
 // Constrained
+/* ************************************************************************* */
+
+/* ************************************************************************* */
+Constrained::Constrained(const Vector& sigmas)
+  : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
+  for (int i=0; i<sigmas.size(); ++i) {
+    if (!std::isfinite(1./sigmas(i))) {
+      precisions_(i) = 0.0; // Set to finite value
+      invsigmas_(i) = 0.0;
+    }
+  }
+}
+
+/* ************************************************************************* */
+Constrained::Constrained(const Vector& mu, const Vector& sigmas)
+  : Diagonal(sigmas), mu_(mu) {
+  assert(sigmas.size() == mu.size());
+  for (int i=0; i<sigmas.size(); ++i) {
+    if (!std::isfinite(1./sigmas(i))) {
+      precisions_(i) = 0.0; // Set to finite value
+      invsigmas_(i) = 0.0;
+    }
+  }
+}
+
 /* ************************************************************************* */
 Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& sigmas, bool smart) {
   // FIXME: can't return a diagonal shared_ptr due to conversion
@@ -269,12 +296,9 @@ Vector Constrained::whiten(const Vector& v) const {
 double Constrained::distance(const Vector& v) const {
   Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
   // TODO Find a better way of doing these checks
-  for (size_t i=0; i<dim_; ++i) { // add mu weights on constrained variables
-    if (std::isinf(w[i])) // whiten makes constrained variables infinite
+  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
+    if (!std::isfinite(1./sigmas_[i])) // whiten makes constrained variables zero
       w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
-    if (std::isnan(w[i])) // ensure no other invalid values make it through
-      w[i] = v[i];
-  }
   return w.dot(w);
 }
 
@@ -283,7 +307,9 @@ Matrix Constrained::Whiten(const Matrix& H) const {
   // selective scaling
   // Now allow augmented Matrix with a new additional part coming
   // from the Lagrange multiplier.
-  return vector_scale(invsigmas(), H.block(0, 0, dim(), H.cols()), true);
+  Matrix M(H.block(0, 0, dim(), H.cols()));
+  Constrained::WhitenInPlace(M);
+  return M;
 }
 
 /* ************************************************************************* */
@@ -294,7 +320,13 @@ void Constrained::WhitenInPlace(Matrix& H) const {
   // indicating a hard constraint, we leave H's row i in place.
   // Now allow augmented Matrix with a new additional part coming
   // from the Lagrange multiplier.
-  vector_scale_inplace(invsigmas(), H, true);
+//  Inlined: vector_scale_inplace(invsigmas(), H, true);
+  // vector_scale_inplace(v, A, true);
+  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
+    const double& invsigma = invsigmas_(i);
+    if (std::isfinite(1./sigmas_(i)))
+      H.row(i) *= invsigma;
+  }
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index e3bf5a10b..f72563d7a 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -258,10 +258,10 @@ namespace gtsam {
       Vector sigmas_, invsigmas_, precisions_;
 
     protected:
-      /** protected constructor takes sigmas */
+      /** protected constructor - no initializations */
       Diagonal();
 
-      /** constructor to allow for disabling initializaion of invsigmas */
+      /** constructor to allow for disabling initialization of invsigmas */
       Diagonal(const Vector& sigmas);
 
     public:
@@ -272,7 +272,7 @@ namespace gtsam {
 
       /**
        * A diagonal noise model created by specifying a Vector of sigmas, i.e.
-       * standard devations, the diagonal of the square root covariance matrix.
+       * standard deviations, the diagonal of the square root covariance matrix.
        */
       static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
 
@@ -353,19 +353,22 @@ namespace gtsam {
     protected:
 
       // Sigmas are contained in the base class
+      Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints
 
-      // Penalty function parameters
-      Vector mu_;
+      /**
+       * protected constructor takes sigmas.
+       * prevents any inf values
+       * from appearing in invsigmas or precisions.
+       * mu set to large default value (1000.0)
+       */
+      Constrained(const Vector& sigmas = zero(1));
 
-      /** protected constructor takes sigmas */
-      // Keeps only sigmas and calculates invsigmas when necessary
-      Constrained(const Vector& sigmas = zero(1)) :
-        Diagonal(sigmas), 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), mu_(mu) {}
+      /**
+       * Constructor that prevents any inf values
+       * from appearing in invsigmas or precisions.
+       * Allows for specifying mu.
+       */
+      Constrained(const Vector& mu, const Vector& sigmas);
 
     public:
 
diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 5c1b81401..9d87a163b 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -161,9 +161,8 @@ TEST(NoiseModel, ConstrainedConstructors )
   // TODO: why should this be a thousand ??? Dummy variable?
   EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
   EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas()));
-  double Inf = numeric_limits<double>::infinity();
-  EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->invsigmas()));
-  EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->precisions()));
+  EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value
+  EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value
 
   actual = Constrained::All(d, m);
   EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));
diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp
index 9432cf326..5e08a7827 100644
--- a/gtsam/linear/tests/testSerializationLinear.cpp
+++ b/gtsam/linear/tests/testSerializationLinear.cpp
@@ -75,8 +75,8 @@ TEST (Serialization, noiseModels) {
   EXPECT(equalsDereferencedBinary<noiseModel::Unit::shared_ptr>(unit3));
 
   EXPECT(equalsDereferencedBinary<noiseModel::Constrained::shared_ptr>(constrained3));
-  EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3)); // FAILS: "Exception: input stream error" - there are inf's that cannot be deserialized
-  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3)); // FAILS: "Exception: input stream error"
+  EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
+  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
 }
 
 /* ************************************************************************* */
@@ -103,8 +103,8 @@ TEST (Serialization, SharedNoiseModel_noiseModels) {
   EXPECT(equalsDereferencedBinary<SharedNoiseModel>(unit3));
 
   EXPECT(equalsDereferencedBinary<SharedNoiseModel>(constrained3));
-  EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3)); // FAILS: "Exception: input stream error"
-  EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3)); // FAILS: "Exception: input stream error"
+  EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
 }
 
 /* ************************************************************************* */
@@ -122,8 +122,8 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
   EXPECT(equalsDereferencedBinary<SharedDiagonal>(unit3));
 
   EXPECT(equalsDereferencedBinary<SharedDiagonal>(constrained3));
-  EXPECT(equalsDereferenced<SharedDiagonal>(constrained3)); // FAILS: "Exception: input stream error"
-  EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3)); // FAILS: "Exception: input stream error"
+  EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
+  EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
 }
 
 /* Create GUIDs for factors */