NonlinearConstraint now derives from NonlinearFactor rather than NoiseModelFactor

gtsam/nonlinear/NonlinearConstraint.h

@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include <boost/function.hpp>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -36,16 +35,18 @@ namespace gtsam {
  * significant
  */
 template <class VALUES>
-class NonlinearConstraint : public NoiseModelFactor<VALUES> {
+class NonlinearConstraint : public NonlinearFactor<VALUES> {
 
 protected:
 	typedef NonlinearConstraint<VALUES> This;
-	typedef NoiseModelFactor<VALUES> Base;
+	typedef NonlinearFactor<VALUES> Base;
+	typedef Factor<Symbol> BaseFactor;
 
 	/** default constructor to allow for serialization */
 	NonlinearConstraint() {}
 
 	double mu_;  /// gain for quadratic merit function
+	size_t dim_; /// dimension of the constraint
 
 public:
 
@@ -54,14 +55,30 @@ public:
 	 * @param keys is a boost::tuple containing the keys, e.g. \c make_tuple(key1,key2,key3)
 	 * @param mu is the gain used at error evaluation (forced to be positive)
 	 */
-	template<class TUPLE>
+	template<class U1, class U2>
-	NonlinearConstraint(const TUPLE& keys, size_t dim, double mu = 1000.0):
+	NonlinearConstraint(const boost::tuples::cons<U1,U2>& keys, size_t dim, double mu = 1000.0)
-		Base(noiseModel::Constrained::All(dim), keys), mu_(fabs(mu)) {}
+	: Base(keys), mu_(fabs(mu)), dim_(dim) {
+	}
+
+  /**
+   * Constructor
+   * @param keys The variables involved in this factor
+   */
+  template<class ITERATOR>
+  NonlinearConstraint(ITERATOR beginKeys, ITERATOR endKeys, size_t dim, double mu = 1000.0)
+  : Base(beginKeys, endKeys), mu_(fabs(mu)), dim_(dim) {
+  }
+
 	virtual ~NonlinearConstraint() {}
 
 	/** returns the gain mu */
 	double mu() const { return mu_; }
 
+  /** get the dimension of the factor (number of rows on linearization) */
+  virtual size_t dim() const {
+    return dim_;
+  }
+
 	/** Print */
 	virtual void print(const std::string& s = "") const {
     std::cout << "NonlinearConstraint " << s << std::endl;
@@ -75,7 +92,7 @@ public:
 	virtual bool equals(const NonlinearFactor<VALUES>& f, double tol=1e-9) const {
 		const This* p = dynamic_cast<const This*> (&f);
 		if (p == NULL) return false;
-		return Base::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol);
+		return BaseFactor::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol) && dim_ == p->dim_;
 	}
 
 	/** error function - returns the quadratic merit function */
@@ -95,6 +112,14 @@ public:
 	 */
 	virtual bool active(const VALUES& c) const=0;
 
+  /**
+   * Error function \f$ z-c(x) \f$.
+   * Override this method to finish implementing an N-way factor.
+   * If any of the optional Matrix reference arguments are specified, it should compute
+   * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
+   */
+	virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
+
 	/**
 	 * Linearizes around a given config
 	 * @param config is the values structure
@@ -103,8 +128,22 @@ public:
 	virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
 		if (!active(c))
 			return boost::shared_ptr<JacobianFactor>();
-    else
+
-      return Base::linearize(c, ordering);
+		// Create the set of terms - Jacobians for each index
+		Vector b;
+		// Call evaluate error to get Jacobians and b vector
+		std::vector<Matrix> A(this->size());
+		b = -unwhitenedError(c, A);
+
+		std::vector<std::pair<Index, Matrix> > terms(this->size());
+		// Fill in terms
+		for(size_t j=0; j<this->size(); ++j) {
+			terms[j].first = ordering[this->keys()[j]];
+			terms[j].second.swap(A[j]);
+		}
+
+		return GaussianFactor::shared_ptr(
+				new JacobianFactor(terms, b, noiseModel::Constrained::All(dim_)));
 	}
 
 private:
@@ -116,10 +155,11 @@ private:
 		ar & boost::serialization::make_nvp("NonlinearFactor",
 				boost::serialization::base_object<Base>(*this));
 		ar & BOOST_SERIALIZATION_NVP(mu_);
+		ar & BOOST_SERIALIZATION_NVP(dim_);
 	}
 };
 
-
+/* ************************************************************************* */
 /**
  * A unary constraint that defaults to an equality constraint
  */
@@ -186,6 +226,7 @@ private:
 	}
 };
 
+/* ************************************************************************* */
 /**
  * Unary Equality constraint - simply forces the value of active() to true
  */
@@ -222,6 +263,7 @@ private:
 
 };
 
+/* ************************************************************************* */
 /**
  * A binary constraint with arbitrary cost and jacobian functions
  */
@@ -294,6 +336,7 @@ private:
 	}
 };
 
+/* ************************************************************************* */
 /**
  * Binary Equality constraint - simply forces the value of active() to true
  */
@@ -330,6 +373,7 @@ private:
 	}
 };
 
+/* ************************************************************************* */
 /**
  * A ternary constraint
  */
@@ -410,6 +454,7 @@ private:
 	}
 };
 
+/* ************************************************************************* */
 /**
  * Ternary Equality constraint - simply forces the value of active() to true
  */
@@ -448,7 +493,7 @@ private:
 	}
 };
 
-
+/* ************************************************************************* */
 /**
  * Simple unary equality constraint - fixes a value for a variable
  */
@@ -493,7 +538,7 @@ private:
 	}
 };
 
-
+/* ************************************************************************* */
 /**
  * Simple binary equality constraint - this constraint forces two factors to
  * be the same.  This constraint requires the underlying type to a Lie type

gtsam/nonlinear/NonlinearFactor.h

@@ -37,6 +37,17 @@ namespace gtsam {
 
 using boost::make_tuple;
 
+// Helper function to fill a vector from a tuple function of any length
+template<typename CONS>
+inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
+  vector[position] = tuple.get_head();
+  __fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
+}
+template<>
+inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
+  // Do nothing
+}
+
 /* ************************************************************************* */
 /**
  * Nonlinear factor base class
@@ -62,6 +73,27 @@ public:
   NonlinearFactor() {
   }
 
+  /**
+   * Constructor
+   * @param keys A boost::tuple containing the variables involved in this factor,
+   * example: <tt>NonlinearFactor(make_tuple(symbol1, symbol2, symbol3))</tt>
+   */
+  template<class U1, class U2>
+  NonlinearFactor(const boost::tuples::cons<U1,U2>& keys) {
+    this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
+    // Use helper function to fill key vector, using 'cons' representation of tuple
+    __fill_from_tuple(this->keys(), 0, keys);
+  }
+
+  /**
+   * Constructor
+   * @param keys The variables involved in this factor
+   */
+  template<class ITERATOR>
+  NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) {
+    this->keys_.insert(this->keys_.end(), beginKeys, endKeys);
+  }
+
   /** Destructor */
   virtual ~NonlinearFactor() {}
 
@@ -88,22 +120,15 @@ public:
    * Create a symbolic factor using the given ordering to determine the
    * variable indices.
    */
-  virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0;
+  virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
+    std::vector<Index> indices(this->size());
+    for(size_t j=0; j<this->size(); ++j)
+      indices[j] = ordering[this->keys()[j]];
+    return IndexFactor::shared_ptr(new IndexFactor(indices));
+  }
 
 }; // \class NonlinearFactor
 
-
-// Helper function to fill a vector from a tuple function of any length
-template<typename CONS>
-inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
-  vector[position] = tuple.get_head();
-  __fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
-}
-template<>
-inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
-  // Do nothing
-}
-
 /* ************************************************************************* */
 /**
  * A nonlinear sum-of-squares factor with a zero-mean noise model
@@ -143,11 +168,8 @@ public:
    * example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
    */
   template<class U1, class U2>
-  NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys) :
+  NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys)
-  noiseModel_(noiseModel) {
+  : Base(keys), noiseModel_(noiseModel) {
-    this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
-    // Use helper function to fill key vector, using 'cons' representation of tuple
-    __fill_from_tuple(this->keys(), 0, keys);
   }
 
   /**
@@ -155,9 +177,8 @@ public:
    * @param keys The variables involved in this factor
    */
   template<class ITERATOR>
-  NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) :
+  NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys)
-  Base(noiseModel) {
+  : Base(beginKeys, endKeys), noiseModel_(noiseModel) {
-    this->keys_.insert(this->keys_.end(), beginKeys, endKeys);
   }
 
 protected:
@@ -253,17 +274,6 @@ public:
           new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size())));
   }
 
-  /**
-   * Create a symbolic factor using the given ordering to determine the
-   * variable indices.
-   */
-  virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
-    std::vector<Index> indices(this->size());
-    for(size_t j=0; j<this->size(); ++j)
-      indices[j] = ordering[this->keys()[j]];
-    return IndexFactor::shared_ptr(new IndexFactor(indices));
-  }
-
 private:
 
   /** Serialization function */