From 64000bb747c7407fa4f4dbf80731ba0c5c5bd6d1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 25 Nov 2014 13:45:59 +0100
Subject: [PATCH] Cleaned up handling of constraints, removed Constrained smart
 option as was not used, and discovered there was already an "isConstrained"
 method.

---
 gtsam/linear/NoiseModel.cpp         | 89 ++++++++++++-----------------
 gtsam/linear/NoiseModel.h           | 37 +++++-------
 gtsam/nonlinear/NonlinearFactor.cpp |  5 +-
 3 files changed, 53 insertions(+), 78 deletions(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 3b9c5d79c..2031a4b73 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -16,20 +16,19 @@
  * @author Frank Dellaert
  */
 
-#include <limits>
-#include <iostream>
-#include <typeinfo>
-#include <stdexcept>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/base/timing.h>
 
 #include <boost/foreach.hpp>
 #include <boost/random/linear_congruential.hpp>
 #include <boost/random/normal_distribution.hpp>
 #include <boost/random/variate_generator.hpp>
 
-#include <gtsam/base/timing.h>
-#include <gtsam/linear/NoiseModel.h>
+#include <limits>
+#include <iostream>
+#include <typeinfo>
+#include <stdexcept>
 
-static double inf = std::numeric_limits<double>::infinity();
 using namespace std;
 
 namespace gtsam {
@@ -290,42 +289,42 @@ void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
 // Constrained
 /* ************************************************************************* */
 
+namespace internal {
+// switch precisions and invsigmas to finite value
+// TODO: why?? And, why not just ask s==0.0 below ?
+static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
+  for (size_t i = 0; i < sigmas.size(); ++i)
+    if (!std::isfinite(1. / sigmas[i])) {
+      precisions[i] = 0.0;
+      invsigmas[i] = 0.0;
+    }
+}
+}
+
 /* ************************************************************************* */
 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;
-    }
-  }
+  internal::fix(sigmas, precisions_, invsigmas_);
 }
 
 /* ************************************************************************* */
 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;
-    }
-  }
+  internal::fix(sigmas, precisions_, invsigmas_);
 }
 
 /* ************************************************************************* */
-Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& sigmas, bool smart) {
-  // FIXME: can't return a diagonal shared_ptr due to conversion
-//  if (smart) {
-//    // look for zeros to make a constraint
-//    for (size_t i=0; i< (size_t) sigmas.size(); ++i)
-//      if (sigmas(i)<1e-8)
-//        return MixedSigmas(mu, sigmas);
-//    return Diagonal::Sigmas(sigmas);
-//  }
+Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu,
+    const Vector& sigmas) {
   return shared_ptr(new Constrained(mu, sigmas));
 }
 
+/* ************************************************************************* */
+bool Constrained::constrained(size_t i) const {
+  // TODO why not just check sigmas_[i]==0.0 ?
+  return !std::isfinite(1./sigmas_[i]);
+}
+
 /* ************************************************************************* */
 void Constrained::print(const std::string& name) const {
   gtsam::print(sigmas_, name + "constrained sigmas");
@@ -352,9 +351,8 @@ 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::isfinite(1./sigmas_[i])) // whiten makes constrained variables zero
+    if (constrained(i)) // whiten makes constrained variables zero
       w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
   return w.dot(w);
 }
@@ -367,36 +365,23 @@ Matrix Constrained::Whiten(const Matrix& H) const {
 
 /* ************************************************************************* */
 void Constrained::WhitenInPlace(Matrix& H) const {
-  // selective scaling
-  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
-  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
-  // indicating a hard constraint, we leave H's row i in place.
-//  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;
-  }
+  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
+    if (!constrained(i)) // if constrained, leave row of H as is
+      H.row(i) *= invsigmas_(i);
 }
 
 /* ************************************************************************* */
 void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
-  // selective scaling
-  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
-  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
-  // indicating a hard constraint, we leave H's row i in place.
-  const Vector& _invsigmas = invsigmas();
-  for(DenseIndex row = 0; row < _invsigmas.size(); ++row)
-    if(isfinite(_invsigmas(row)))
-      H.row(row) *= _invsigmas(row);
+  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
+    if (!constrained(i)) // if constrained, leave row of H as is
+      H.row(i) *= invsigmas_(i);
 }
 
 /* ************************************************************************* */
 Constrained::shared_ptr Constrained::unit() const {
   Vector sigmas = ones(dim());
   for (size_t i=0; i<dim(); ++i)
-    if (this->sigmas_(i) == 0.0)
+    if (constrained(i))
       sigmas(i) = 0.0;
   return MixedSigmas(mu_, sigmas);
 }
@@ -472,7 +457,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
     const size_t& j  = t.get<0>();
     const Vector& rd = t.get<1>();
     precisions(i)    = t.get<2>();
-    if (precisions(i)==inf) mixed = true;
+    if (constrained(i)) mixed = true;
     for (size_t j2=0; j2<j; ++j2)
       Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
     for (size_t j2=j; j2<n+1; ++j2)
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index caad6b01a..dc6801e03 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -61,9 +61,9 @@ namespace gtsam {
       Base(size_t dim = 1):dim_(dim) {}
       virtual ~Base() {}
 
-      /** true if a constrained noise mode, saves slow/clumsy dynamic casting */
-      virtual bool is_constrained() const {
-        return false;
+      /// true if a constrained noise mode, saves slow/clumsy dynamic casting
+      virtual bool isConstrained() const {
+        return false; // default false
       }
 
       /// Dimensionality
@@ -237,12 +237,6 @@ namespace gtsam {
        */
       virtual Matrix R() const { return thisR();}
 
-      /**
-       * Simple check for constrained-ness
-       * FIXME Find a better way of handling this
-       */
-      virtual bool isConstrained() const {return false;}
-
     private:
       /** Serialization function */
       friend class boost::serialization::access;
@@ -390,11 +384,14 @@ namespace gtsam {
 
       virtual ~Constrained() {}
 
-      /** true if a constrained noise mode, saves slow/clumsy dynamic casting */
-      virtual bool is_constrained() const {
+      /// true if a constrained noise mode, saves slow/clumsy dynamic casting
+      virtual bool isConstrained() const {
         return true;
       }
 
+      /// Return true if a particular dimension is free or constrained
+      bool constrained(size_t i) const;
+
       /// Access mu as a vector
       const Vector& mu() const { return mu_; }
 
@@ -402,24 +399,22 @@ namespace gtsam {
        * A diagonal noise model created by specifying a Vector of
        * standard devations, some of which might be zero
        */
-      static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas,
-          bool smart = true);
+      static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
 
       /**
        * A diagonal noise model created by specifying a Vector of
        * standard devations, some of which might be zero
        */
-      static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = true) {
-        return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart);
+      static shared_ptr MixedSigmas(const Vector& sigmas) {
+        return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas);
       }
 
       /**
        * A diagonal noise model created by specifying a Vector of
        * standard devations, some of which might be zero
        */
-      static shared_ptr MixedSigmas(double m, const Vector& sigmas,
-          bool smart = true) {
-        return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart);
+      static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
+        return MixedSigmas(repeat(sigmas.size(), m), sigmas);
       }
 
       /**
@@ -482,12 +477,6 @@ namespace gtsam {
        */
       virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
 
-      /**
-       * Check constrained is always true
-       * FIXME Find a better way of handling this
-       */
-      virtual bool isConstrained() const { return true; }
-
       /**
        * Returns a Unit version of a constrained noisemodel in which
        * constrained sigmas remain constrained and the rest are unit scaled
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index e9b97d644..7c8337fc8 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -132,10 +132,11 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
   }
 
   // TODO pass unwhitened + noise model to Gaussian factor
-  if (noiseModel_ && noiseModel_->is_constrained())
+  using noiseModel::Constrained;
+  if (noiseModel_ && noiseModel_->isConstrained())
     return GaussianFactor::shared_ptr(
         new JacobianFactor(terms, b,
-            boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
+            boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
   else
     return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
 }