Merge branch 'fix_serialization' into develop

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
+  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
-    if (std::isinf(w[i])) // whiten makes constrained variables infinite
+    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;
+  }
 }
 
 /* ************************************************************************* */

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
+       * Constructor that prevents any inf values
-      Constrained(const Vector& sigmas = zero(1)) :
+       * from appearing in invsigmas or precisions.
-        Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {}
+       * Allows for specifying mu.
-
+       */
-      // Keeps only sigmas and calculates invsigmas when necessary
+      Constrained(const Vector& mu, const Vector& sigmas);
-      // allows for specifying mu
-      Constrained(const Vector& mu, const Vector& sigmas) :
-        Diagonal(sigmas), mu_(mu) {}
 
     public:
 

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, 0), actual->invsigmas())); // Actually zero as dummy value
-  EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->invsigmas()));
+  EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value
-  EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->precisions()));
 
   actual = Constrained::All(d, m);
   EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));

gtsam/linear/tests/testSerializationLinear.cpp

@@ -70,13 +70,13 @@ TEST (Serialization, noiseModels) {
   EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
   EXPECT(equalsDereferencedBinary<noiseModel::Isotropic::shared_ptr>(iso3));
 
-  EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
-  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
-  EXPECT(equalsDereferencedBinary<noiseModel::Constrained::shared_ptr>(constrained3));
-
   EXPECT(equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
   EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
   EXPECT(equalsDereferencedBinary<noiseModel::Unit::shared_ptr>(unit3));
+
+  EXPECT(equalsDereferencedBinary<noiseModel::Constrained::shared_ptr>(constrained3));
+  EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
+  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
 }
 
 /* ************************************************************************* */
@@ -102,9 +102,9 @@ TEST (Serialization, SharedNoiseModel_noiseModels) {
   EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
   EXPECT(equalsDereferencedBinary<SharedNoiseModel>(unit3));
 
+  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(constrained3));
   EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
   EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
-  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(constrained3));
 }
 
 /* ************************************************************************* */
@@ -121,9 +121,9 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
   EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
   EXPECT(equalsDereferencedBinary<SharedDiagonal>(unit3));
 
+  EXPECT(equalsDereferencedBinary<SharedDiagonal>(constrained3));
   EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
   EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
-  EXPECT(equalsDereferencedBinary<SharedDiagonal>(constrained3));
 }
 
 /* Create GUIDs for factors */