Merged in feature/constraints (pull request #49)

Cleaning up constraints a bit

gtsam/linear/NoiseModel.cpp

@@ -16,20 +16,19 @@
  * @author Frank Dellaert
  */
 
-#include <limits>
+#include <gtsam/linear/NoiseModel.h>
-#include <iostream>
+#include <gtsam/base/timing.h>
-#include <typeinfo>
-#include <stdexcept>
 
 #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 <limits>
-#include <gtsam/linear/NoiseModel.h>
+#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) {
+  internal::fix(sigmas, precisions_, invsigmas_);
-    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());
+  internal::fix(sigmas, precisions_, invsigmas_);
-  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) {
+Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu,
-  // FIXME: can't return a diagonal shared_ptr due to conversion
+    const Vector& sigmas) {
-//  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);
-//  }
   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
+  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
-  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
+    if (!constrained(i)) // if constrained, leave row of H as is
-  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
+      H.row(i) *= invsigmas_(i);
-  // 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;
-  }
 }
 
 /* ************************************************************************* */
 void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
-  // selective scaling
+  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
-  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
+    if (!constrained(i)) // if constrained, leave row of H as is
-  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
+      H.row(i) *= invsigmas_(i);
-  // 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);
 }
 
 /* ************************************************************************* */
 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)

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 */
+      /// true if a constrained noise mode, saves slow/clumsy dynamic casting
-      virtual bool is_constrained() const {
+      virtual bool isConstrained() const {
-        return false;
+        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 */
+      /// true if a constrained noise mode, saves slow/clumsy dynamic casting
-      virtual bool is_constrained() const {
+      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,
+      static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
-          bool smart = true);
 
       /**
        * 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) {
+      static shared_ptr MixedSigmas(const Vector& sigmas) {
-        return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart);
+        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,
+      static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
-          bool smart = true) {
+        return MixedSigmas(repeat(sigmas.size(), m), sigmas);
-        return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart);
       }
 
       /**
@@ -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

gtsam/nonlinear/NonlinearEquality.h

@@ -17,13 +17,13 @@
 
 #pragma once
 
-#include <limits>
-#include <iostream>
-
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
 
+#include <limits>
+#include <iostream>
+
 namespace gtsam {
 
 /**
@@ -216,16 +216,22 @@ protected:
   X value_; /// fixed value for variable
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(X)
-  ;GTSAM_CONCEPT_TESTABLE_TYPE(X)
+
-  ;
+  GTSAM_CONCEPT_TESTABLE_TYPE(X)
 
 public:
 
   typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
 
-  ///TODO: comment
+  /**
-  NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) :
+   * Constructor
-      Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(
+   * @param value feasible value that values(key) shouild be equal to
+   * @param key the key for the unknown variable to be constrained
+   * @param mu a parameter which really turns this into a strong prior
+   *
+   */
+  NonlinearEquality1(const X& value, Key key, double mu = 1000.0) :
+      Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key), value_(
           value) {
   }
 
@@ -285,7 +291,6 @@ protected:
   typedef NonlinearEquality2<VALUE> This;
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(X)
-  ;
 
   /** default constructor to allow for serialization */
   NonlinearEquality2() {

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));
 }

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -90,7 +90,7 @@ public:
 
     // Create a writeable JacobianFactor in advance
     // In case noise model is constrained, we need to provide a noise model
-    bool constrained = noiseModel_->is_constrained();
+    bool constrained = noiseModel_->isConstrained();
     boost::shared_ptr<JacobianFactor> factor(
         constrained ? new JacobianFactor(keys_, dims_, Dim,
             boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
@@ -107,7 +107,6 @@ public:
     T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
 
     // Evaluate error and set RHS vector b
-    // TODO(PTF) Is this a place for custom charts?
     DefaultChart<T> chart;
     Ab(size()).col(0) = -chart.local(measurement_, value);