Large gtsam refactoring

To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.

The following are the biggest changes:

1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.

2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just  says something like

	if (H) *H = Matrix_(3,6,....);

3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>

4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !

cpp/BetweenFactor.h

@@ -17,14 +17,14 @@ namespace gtsam {
 	 * A class for a measurement predicted by "between(config[key1],config[key2])"
 	 * T is the Lie group type, Config where the T's are gotten from
 	 */
-	template<class T, class Config>
+	template<class Config, class Key, class T>
-	class BetweenFactor: public NonlinearFactor<Config> {
+	class BetweenFactor: public NonlinearFactor2<Config, Key, T, Key, T> {
 
 	private:
 
+		typedef NonlinearFactor2<Config, Key, T, Key, T> Base;
+
 		T measured_; /** The measurement */
-		std::string key1_, key2_; /** The keys of the two poses, order matters */
-		std::list<std::string> keys_; /** The keys as a list */
 		Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
 
 	public:
@@ -33,72 +33,50 @@ namespace gtsam {
 		typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
 
 		/** Constructor */
-		BetweenFactor(const std::string& key1, const std::string& key2,
+		BetweenFactor(const Key& key1, const Key& key2, const T& measured,
-				const T& measured, const Matrix& measurement_covariance) :
+				const Matrix& measurement_covariance) :
-			key1_(key1), key2_(key2), measured_(measured) {
+			Base(1, key1, key2), measured_(measured) {
 			square_root_inverse_covariance_ = inverse_square_root(
 					measurement_covariance);
-			keys_.push_back(key1);
-			keys_.push_back(key2);
 		}
 
 		/** implement functions needed for Testable */
 
 		/** print */
-		void print(const std::string& name) const {
+		void print(const std::string& s) const {
-			std::cout << name << std::endl;
+			Base::print(s);
-			std::cout << "Factor " << std::endl;
-			std::cout << "key1 " << key1_ << std::endl;
-			std::cout << "key2 " << key2_ << std::endl;
 			measured_.print("measured ");
 			gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance");
 		}
 
 		/** equals */
 		bool equals(const NonlinearFactor<Config>& expected, double tol) const {
-			return key1_ == expected.key1_ && key2_ == expected.key2_
+			const BetweenFactor<Config, Key, T> *e =
-					&& measured_.equals(expected.measured_, tol);
+					dynamic_cast<const BetweenFactor<Config, Key, T>*> (&expected);
+			return e != NULL && Base::equals(expected)
+					&& this->measured_.equals(e->measured_, tol);
 		}
 
 		/** implement functions needed to derive from Factor */
 
 		/** vector of errors */
-		Vector error_vector(const T& p1, const T& p2) const {
+		Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
-			//z-h
+				boost::none, boost::optional<Matrix&> H2 = boost::none) const {
-			T hx = between(p1,p2);
+			// h - z
-			// manifold equivalent of z-h(x) -> log(h(x),z)
+			T hx = between(p1, p2);
-			return square_root_inverse_covariance_ * logmap(hx,measured_);
+			// TODO should be done by noise model
+			if (H1) *H1 = square_root_inverse_covariance_ * Dbetween1(p1, p2);
+			if (H2) *H2 = square_root_inverse_covariance_ * Dbetween2(p1, p2);
+			// manifold equivalent of h(x)-z -> log(z,h(x))
+			// TODO use noise model, error vector is not whitened yet
+			return square_root_inverse_covariance_ * logmap(measured_, hx);
 		}
 
-		/** vector of errors */
-		Vector error_vector(const Config& c) const {
-		  return error_vector(c.get(key1_), c.get(key2_));
-		}
-
-		/** methods to retrieve both keys */
-		inline const std::string& key1() const { return key1_;}
-		inline const std::string& key2() const { return key2_;}
-
 		/** return the measured */
 		inline const T measured() const {return measured_;}
 
-		/** keys as a list */
-		inline std::list<std::string> keys() const { return keys_;}
-
 		/** number of variables attached to this factor */
 		inline std::size_t size() const { return 2;}
-
-		/** linearize */
-		boost::shared_ptr<GaussianFactor> linearize(const Config& x0) const {
-			const T& p1 = x0.get(key1_), p2 = x0.get(key2_);
-			Matrix A1 = Dbetween1(p1, p2);
-			Matrix A2 = Dbetween2(p1, p2);
-			Vector b = error_vector(p1, p2); // already has sigmas in !
-			boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
-					key1_, square_root_inverse_covariance_ * A1,
-					key2_, square_root_inverse_covariance_ * A2, b, 1.0));
-			return linearized;
-		}
 	};
 
 } /// namespace gtsam

cpp/FactorGraph-inl.h

@@ -269,7 +269,7 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
   }
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 template<class Factor>
 map<string, string> FactorGraph<Factor>::findMinimumSpanningTree() const {
 

cpp/FactorGraph.h

@@ -120,14 +120,14 @@ namespace gtsam {
 		/**
 		 * find the minimum spanning tree.
 		 */
-		std::map<std::string, std::string> findMinimumSpanningTree() const;
+//		std::map<std::string, std::string> findMinimumSpanningTree() const;
 
 		/**
 		 * Split the graph into two parts: one corresponds to the given spanning tre,
 		 * and the other corresponds to the rest of the factors
 		 */
-		void split(std::map<std::string, std::string> tree,
+//		void split(std::map<std::string, std::string> tree,
-				FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
+//				FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
 
 	private:
 		/** Associate factor index with the variables connected to the factor */

cpp/Key.h

@@ -0,0 +1,58 @@
+/*
+ * Key.h
+ *
+ *  Created on: Jan 12, 2010
+ *  @author: Frank Dellaert
+ *  @author: Richard Roberts
+ */
+
+#pragma once
+
+#include <boost/format.hpp>
+#include <boost/serialization/serialization.hpp>
+
+namespace gtsam {
+
+	/**
+	 * Symbol key class is templated on
+	 * 1) the type T it is supposed to retrieve, for extra type checking
+	 * 2) the character constant used for its string representation
+	 * TODO: make Testable
+	 */
+	template <class T, char C>
+	class Symbol {
+
+	private:
+		size_t j_;
+
+	public:
+
+		// Constructors:
+
+		Symbol():j_(999999) {}
+		Symbol(size_t j):j_(j) {}
+
+		// Get stuff:
+
+		size_t index() const { return j_;}
+		operator std::string() const { return (boost::format("%c%d") % C % j_).str(); }
+		std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); }
+
+		// logic:
+
+		bool operator< (const Symbol& compare) const { return j_<compare.j_;}
+		bool operator== (const Symbol& compare) const { return j_==compare.j_;}
+
+	private:
+
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & BOOST_SERIALIZATION_NVP(j_);
+		}
+
+	};
+
+} // namespace gtsam
+

cpp/LieConfig-inl.h

@@ -17,58 +17,57 @@
 
 using namespace std;
 
-#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
-
 namespace gtsam {
 
-  template<class T>
+  template<class J, class T>
-  void LieConfig<T>::print(const string &s) const {
+  void LieConfig<J,T>::print(const string &s) const {
        cout << "LieConfig " << s << ", size " << values_.size() << "\n";
-       pair<string, T> v;
+       BOOST_FOREACH(const typename Values::value_type& v, values_)
-       BOOST_FOREACH(v, values_)
+         gtsam::print(v.second, (string)(v.first));
-         gtsam::print(v.second, v.first + ": ");
      }
 
-  template<class T>
+  template<class J, class T>
-  bool LieConfig<T>::equals(const LieConfig<T>& expected, double tol) const {
+  bool LieConfig<J,T>::equals(const LieConfig<J,T>& expected, double tol) const {
     if (values_.size() != expected.values_.size()) return false;
-    pair<string, T> v;
+    BOOST_FOREACH(const typename Values::value_type& v, values_) {
-    BOOST_FOREACH(v, values_) {
+    	if (!exists(v.first)) return false;
-      boost::optional<const T&> expval = expected.gettry(v.first);
+      if(!gtsam::equal(v.second, expected[v.first], tol))
-      if(!expval || !gtsam::equal(v.second, *expval, tol))
         return false;
     }
     return true;
   }
 
-  template<class T>
+  template<class J, class T>
-  const T& LieConfig<T>::get(const std::string& key) const {
+  const T& LieConfig<J,T>::at(const Key& key) const {
-    iterator it = values_.find(key);
-    if (it == values_.end()) throw std::invalid_argument("invalid key");
-    else return it->second;
-  }
-
-  template<class T>
-  boost::optional<const T&> LieConfig<T>::gettry(const std::string& key) const {
     const_iterator it = values_.find(key);
-    if (it == values_.end()) return boost::optional<const T&>();
+    if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
     else return it->second;
   }
 
-  template<class T>
+  template<class J, class T>
-  void LieConfig<T>::insert(const std::string& name, const T& val) {
+  void LieConfig<J,T>::insert(const Key& name, const T& val) {
     values_.insert(make_pair(name, val));
     dim_ += dim(val);
   }
 
+  template<class J, class T>
+  void LieConfig<J,T>::erase(const Key& key) {
+    iterator it = values_.find(key);
+    if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
+    values_.erase(it);
+  }
+
   // todo: insert for every element is inefficient
-  template<class T>
+  template<class J, class T>
-  LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) {
+  LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) {
-		LieConfig<T> newConfig;
+		LieConfig<J,T> newConfig;
-		string j; T pj;
+		typedef pair<typename LieConfig<J,T>::Key,T> Value;
-		FOREACH_PAIR(j, pj, c) {
+		BOOST_FOREACH(const Value& value, c) {
-			if (delta.contains(j)) {
+			const typename LieConfig<J,T>::Key& j = value.first;
-				const Vector& dj = delta[j];
+			const T& pj = value.second;
+			string key = (string)j;
+			if (delta.contains(key)) {
+				const Vector& dj = delta[key];
 				newConfig.insert(j, expmap(pj,dj));
 			} else
 			newConfig.insert(j, pj);
@@ -77,21 +76,22 @@ namespace gtsam {
 	}
 
   // todo: insert for every element is inefficient
-  template<class T>
+  template<class J, class T>
-  LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta) {
+  LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta) {
     if(delta.size() != dim(c))
       throw invalid_argument("Delta vector length does not match config dimensionality.");
-    LieConfig<T> newConfig;
+    LieConfig<J,T> newConfig;
-    pair<string, T> value;
     int delta_offset = 0;
-    BOOST_FOREACH(value, c) {
+		typedef pair<typename LieConfig<J,T>::Key,T> Value;
-      int cur_dim = dim(value.second);
+		BOOST_FOREACH(const Value& value, c) {
-      newConfig.insert(value.first,
+			const typename LieConfig<J,T>::Key& j = value.first;
-          expmap(value.second,
+			const T& pj = value.second;
-            sub(delta, delta_offset, delta_offset+cur_dim)));
+      int cur_dim = dim(pj);
+      newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim)));
       delta_offset += cur_dim;
     }
     return newConfig;
   }
-
 }
+
+

cpp/LieConfig.h

@@ -9,61 +9,92 @@
 
 #pragma once
 
-
 #include <map>
 
 #include "Vector.h"
 #include "Testable.h"
 #include "Lie.h"
+#include "Key.h"
 
 namespace boost { template<class T> class optional; }
-namespace gtsam { class VectorConfig; }
 
 namespace gtsam {
 
-  template<class T> class LieConfig;
+	class VectorConfig;
+
+	// TODO: why are these defined *before* the class ?
+  template<class J, class T> class LieConfig;
 
   /** Dimensionality of the tangent space */
-  template<class T>
+  template<class J, class T>
-  size_t dim(const LieConfig<T>& c);
+  size_t dim(const LieConfig<J,T>& c);
 
   /** Add a delta config */
-  template<class T>
+  template<class J, class T>
-  LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta);
+  LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta);
 
   /** Add a delta vector, uses iterator order */
-  template<class T>
+  template<class J, class T>
-  LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta);
+  LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta);
 
+	/**
+	 * Lie type configuration
+	 */
+  template<class J, class T>
+  class LieConfig : public Testable<LieConfig<J,T> > {
+
+  public:
+
+    /**
+     * Typedefs
+     */
+  	typedef J Key;
+    typedef std::map<Key, T> Values;
+    typedef typename Values::iterator iterator;
+    typedef typename Values::const_iterator const_iterator;
 
-  template<class T>
-  class LieConfig : public Testable<LieConfig<T> > {
   private:
-    std::map<std::string, T> values_;
+
+    Values values_;
     size_t dim_;
 
   public:
-    typedef typename std::map<std::string, T>::const_iterator iterator;
-    typedef iterator const_iterator;
 
     LieConfig() : dim_(0) {}
     LieConfig(const LieConfig& config) :
       values_(config.values_), dim_(dim(config)) {}
     virtual ~LieConfig() {}
 
+    /** print */
+    void print(const std::string &s) const;
+
+    /** Test whether configs are identical in keys and values */
+    bool equals(const LieConfig& expected, double tol=1e-9) const;
+
     /** Retrieve a variable by key, throws std::invalid_argument if not found */
-    const T& get(const std::string& key) const;
+    const T& at(const Key& key) const;
 
     /** operator[] syntax for get */
-		inline const T& operator[](const std::string& name) const {
+		inline const T& operator[](const Key& key) const { return at(key);}
-			return get(name);
-		}
 
-    /** Retrieve a variable by key, returns nothing if not found */
+	  /** Check if a variable exists */
-    boost::optional<const T&> gettry(const std::string& key) const;
+	  bool exists(const Key& i) const {return values_.find(i)!=values_.end();}
+
+    /** The number of variables in this config */
+    int size() const { return values_.size(); }
+
+    const_iterator begin() const { return values_.begin(); }
+    const_iterator end() const { return values_.end(); }
+    iterator begin() { return values_.begin(); }
+    iterator end() { return values_.end(); }
+
+    // imperative methods:
 
     /** Add a variable with the given key */
-    void insert(const std::string& name, const T& val);
+    void insert(const Key& key, const T& val);
+
+    /** Remove a variable from the config */
+    void erase(const Key& key) ;
 
     /** Replace all keys and variables */
     LieConfig& operator=(const LieConfig& rhs) {
@@ -78,27 +109,14 @@ namespace gtsam {
       dim_ = 0;
     }
 
-    /** The number of variables in this config */
+//    friend LieConfig<J,T> expmap<T>(const LieConfig<J,T>& c, const VectorConfig& delta);
-    int size() { return values_.size(); }
+//    friend LieConfig<J,T> expmap<T>(const LieConfig<J,T>& c, const Vector& delta);
-
+    friend size_t dim<J,T>(const LieConfig<J,T>& c);
-    /** Test whether configs are identical in keys and values */
-    bool equals(const LieConfig& expected, double tol=1e-9) const;
-
-    void print(const std::string &s) const;
-
-    const_iterator begin() const { return values_.begin(); }
-    const_iterator end() const { return values_.end(); }
-    iterator begin() { return values_.begin(); }
-    iterator end() { return values_.end(); }
-
-    friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const VectorConfig& delta);
-    friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const Vector& delta);
-    friend size_t dim<T>(const LieConfig<T>& c);
 
   };
 
   /** Dimensionality of the tangent space */
-  template<class T>
+  template<class J, class T>
-  size_t dim(const LieConfig<T>& c) { return c.dim_; }
+  size_t dim(const LieConfig<J,T>& c) { return c.dim_; }
 }
 

cpp/Makefile.am

@@ -66,7 +66,7 @@ testSymbolicBayesNet_LDADD      = libgtsam.la
 
 # Inference
 headers += inference.h inference-inl.h
-headers += graph-inl.h
+headers += graph.h graph-inl.h
 headers += FactorGraph.h FactorGraph-inl.h
 headers += BayesNet.h BayesNet-inl.h
 headers += BayesTree.h BayesTree-inl.h
@@ -134,7 +134,7 @@ testSubgraphPreconditioner_LDADD   = libgtsam.la
 #timeGaussianFactorGraph_LDADD = libgtsam.la
 
 # Nonlinear inference 
-headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
+headers += Key.h NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
 sources += NonlinearFactor.cpp 
 check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer

cpp/NonlinearConstraint-inl.h

@@ -22,7 +22,7 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
 					size_t dim_lagrange,
 					Vector (*g)(const Config& config),
 					bool isEquality)
-:	NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
+:	NonlinearFactor<Config>(1.0),z_(zero(dim_lagrange)),
 	lagrange_key_(lagrange_key), p_(dim_lagrange),
 	isEquality_(isEquality), g_(boost::bind(g, _1)) {}
 
@@ -32,8 +32,8 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
 					size_t dim_lagrange,
 					boost::function<Vector(const Config& config)> g,
 					bool isEquality)
-:	NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
+:	NonlinearFactor<Config>(1.0),
-	lagrange_key_(lagrange_key), p_(dim_lagrange),
+	lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)),
 	g_(g), isEquality_(isEquality) {}
 
 /* ************************************************************************* */

cpp/NonlinearConstraint.h

@@ -27,6 +27,10 @@ template <class Config>
 class NonlinearConstraint : public NonlinearFactor<Config> {
 
 protected:
+
+	/** Lagrange multipliers? */
+	Vector z_;
+
 	/** key for the lagrange multipliers */
 	std::string lagrange_key_;
 

cpp/NonlinearEquality.h

@@ -8,19 +8,17 @@
 
 #include <limits>
 #include <iostream>
+
+#include "Key.h"
 #include "NonlinearFactor.h"
 
 namespace gtsam {
 
 	/**
-	 * Template default compare function that assumes Congig.get yields a testable T
+	 * Template default compare function that assumes a testable T
 	 */
-	template<class Config, class T>
+	template<class T>
-	bool compare(const std::string& key, const Config& feasible, const Config& input) {
+	bool compare(const T& a, const T& b) {return a.equals(b);	}
-		const T& t1 = feasible.get(key);
-		const T& t2 = input.get(key);
-		return t1.equals(t2);
-	}
 
 
 	/**
@@ -28,85 +26,54 @@ namespace gtsam {
 	 * or a set of variables to be equal to each other.
 	 * Throws an error at linearization if the constraints are not met.
 	 */
-	template<class Config>
+	template<class Config, class Key, class T>
-	class NonlinearEquality: public NonlinearFactor<Config> {
+	class NonlinearEquality: public NonlinearFactor1<Config, Key, T> {
 	private:
 
-		// node to constrain
+		// feasible value
-		std::string key_;
+		T feasible_;
-
-		// config containing the necessary feasible point
-		Config feasible_;
-
-		// dimension of the variable
-		size_t dim_;
 
 	public:
 
 		/**
-		 * Function that compares a value from a config with
+		 * Function that compares two values
-		 * another to determine whether a linearization point is
-		 * a feasible point.
-		 * @param key is the identifier for the key
-		 * @param feasible is the value which is constrained
-		 * @param input is the config to be tested for feasibility
-		 * @return true if the linearization point is feasible
 		 */
-		bool (*compare_)(const std::string& key, const Config& feasible,
+		bool (*compare_)(const T& a, const T& b);
-				const Config& input);
 
-		/** Constructor */
+		typedef NonlinearFactor1<Config, Key, T> Base;
-		NonlinearEquality(const std::string& key, const Config& feasible,
-				size_t dim, bool(*compare)(const std::string& key,
-						const Config& feasible, const Config& input)) :
-			key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
-
-		}
 
 		/**
-		 * Constructor with default compare
+		 * Constructor
-		 * Needs class T to have dim()
-		 * and Config to have insert and get
 		 */
-		template <class T>
+		NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
-		NonlinearEquality(const std::string& key, const T& feasible) :
+			Base(0, j), feasible_(feasible), compare_(compare) {
-			key_(key), dim_(dim(feasible)), compare_(compare<Config,T>) {
-			feasible_.insert(key,feasible);
 		}
 
 		void print(const std::string& s = "") const {
-			std::cout << "Constraint: " << s << " on [" << key_ << "]\n";
+			std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
-			feasible_.print("Feasible Point");
+			gtsam::print(feasible_,"Feasible Point");
-			std::cout << "Variable Dimension: " << dim_ << std::endl;
+			std::cout << "Variable Dimension: " << dim(feasible_) << std::endl;
 		}
 
 		/** Check if two factors are equal */
 		bool equals(const Factor<Config>& f, double tol = 1e-9) const {
-			const NonlinearEquality<Config>* p =
+			const NonlinearEquality<Config,Key,T>* p =
-					dynamic_cast<const NonlinearEquality<Config>*> (&f);
+					dynamic_cast<const NonlinearEquality<Config,Key,T>*> (&f);
 			if (p == NULL) return false;
-			if (key_ != p->key_) return false;
+			if (!Base::equals(*p)) return false;
-			if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value
+			return compare_(feasible_, p->feasible_);
-			return dim_ == p->dim_;
 		}
 
 		/** error function */
-		inline Vector error_vector(const Config& c) const {
+		inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H) const {
-			if (!compare_(key_, feasible_, c))
+			size_t nj = dim(feasible_);
-				return repeat(dim_, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
+			if (compare_(feasible_,xj)) {
-			else
+				if (H) *H = eye(nj);
-				return zero(dim_); // set error to zero if equal
+				return zero(nj); // set error to zero if equal
-		}
-
-		/** linearize a nonlinear constraint into a linear constraint */
-		boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
-			if (!compare_(key_, feasible_, c)) {
-				throw std::invalid_argument("Linearization point not feasible for "
-						+ key_ + "!");
 			} else {
-				GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_),
+				if (H) throw std::invalid_argument(
-						zero(dim_), 0.0));
+						"Linearization point not feasible for " + (std::string)(this->key_) + "!");
-				return ret;
+				return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
 			}
 		}
 	}; // NonlinearEquality

cpp/NonlinearFactor.cpp

@@ -2,10 +2,10 @@
  * @file    NonlinearFactor.cpp
  * @brief   nonlinear factor versions which assume a Gaussian noise on a measurement 
  * @brief   predicted by a non-linear function h nonlinearFactor
- * @author  Kai Ni
- * @author  Carlos Nieto
- * @author  Christian Potthast
  * @author  Frank Dellaert
+ * @author  Richard Roberts
+ *
+ * Earlier prototype contributors:  Kai Ni, Carlos Nieto, Christian Potthast
  */
 
 #include "NonlinearFactor.h"
@@ -14,105 +14,3 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-NonlinearFactor1::NonlinearFactor1(const Vector& z,		                    
-                                   const double sigma,	                       
-                                   Vector (*h)(const Vector&),                      
-                                   const string& key1,                   
-                                   Matrix (*H)(const Vector&)) 
-  : NonlinearFactor<VectorConfig>(z, sigma), h_(h), key_(key1), H_(H)
-{
-  keys_.push_front(key1);
-}
-
-/* ************************************************************************* */
-void NonlinearFactor1::print(const string& s) const {
-	cout << "NonlinearFactor1 " << s << endl;
-  cout << "h  : " << (void*)h_ << endl;
-  cout << "key: " << key_      << endl;
-  cout << "H  : " << (void*)H_ << endl;
-	NonlinearFactor<VectorConfig>::print("parent");
-}
-
-/* ************************************************************************* */
-GaussianFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
-	// get argument 1 from config
-	Vector x1 = c[key_];
-
-	// Jacobian A = H(x1)/sigma
-	Matrix A = H_(x1);
-
-	// Right-hand-side b = error(c) = (z - h(x1))/sigma
-	Vector b = (z_ - h_(x1));
-
-	GaussianFactor::shared_ptr p(new GaussianFactor(key_, A, b, sigma_));
-	return p;
-}
-
-/* ************************************************************************* */
-/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html       */
-/* ************************************************************************* */
-bool NonlinearFactor1::equals(const Factor<VectorConfig>& f, double tol) const {
-	const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*> (&f);
-	if (p == NULL) return false;
-	return NonlinearFactor<VectorConfig>::equals(*p, tol)
-	&& (h_   == p->h_) 
-	&& (key_ == p->key_)
-	&& (H_   == p->H_);
-}
-
-/* ************************************************************************* */
-NonlinearFactor2::NonlinearFactor2(const Vector& z,
-		const double sigma,
-		Vector (*h)(const Vector&, const Vector&),
-		const string& key1,
-		Matrix (*H1)(const Vector&, const Vector&),
-		const string& key2,
-		Matrix (*H2)(const Vector&, const Vector&)
-)
-: NonlinearFactor<VectorConfig>(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
-{
-	keys_.push_front(key1);
-	keys_.push_front(key2);
-}
-
-/* ************************************************************************* */
-void NonlinearFactor2::print(const string& s) const {
-	cout << "NonlinearFactor2 " << s << endl;
-  cout << "h   : " << (void*)h_  << endl;
-  cout << "key1: " << key1_      << endl;
-  cout << "H2  : " << (void*)H2_ << endl;
-  cout << "key2: " << key2_      << endl;
-  cout << "H1  : " << (void*)H1_ << endl;
-	NonlinearFactor<VectorConfig>::print("parent");
-}
-
-/* ************************************************************************* */
-GaussianFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) const {
-	// get arguments from config
-	Vector x1 = c[key1_];
-	Vector x2 = c[key2_];
-
-	// Jacobian A = H(x)/sigma
-	Matrix A1 = H1_(x1, x2);
-	Matrix A2 = H2_(x1, x2);
-
-	// Right-hand-side b = (z - h(x))/sigma
-	Vector b = (z_ - h_(x1, x2));
-
-	GaussianFactor::shared_ptr p(new GaussianFactor(key1_, A1, key2_, A2, b, sigma_));
-	return p;
-}
-
-/* ************************************************************************* */
-bool NonlinearFactor2::equals(const Factor<VectorConfig>& f, double tol) const {
-	const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*> (&f);
-	if (p == NULL) return false;
-	return NonlinearFactor<VectorConfig>::equals(*p, tol)
-    && (h_    == p->h_)
-    && (key1_ == p->key1_)
-    && (H1_   == p->H1_)
-    && (key2_ == p->key2_)
-    && (H2_   == p->H2_);
-}
-
-/* ************************************************************************* */

cpp/NonlinearFactor.h

@@ -1,13 +1,10 @@
 /**
  * @file    NonlinearFactor.h
  * @brief   Non-linear factor class
- * @author  Kai Ni
- * @author  Carlos Nieto
- * @author  Christian Potthast
  * @author  Frank Dellaert
+ * @author  Richard Roberts
  */
 
-
 // \callgraph
 
 #pragma once
@@ -16,92 +13,104 @@
 #include <limits>
 
 #include <boost/shared_ptr.hpp>
-#include <boost/serialization/list.hpp>
+#include <boost/serialization/base_object.hpp>
 
 #include "Factor.h"
 #include "Vector.h"
 #include "Matrix.h"
 #include "GaussianFactor.h"
 
-/**
- * Base Class
- * Just has the measurement and noise model
- */ 
-
 namespace gtsam {
 
-	// forward declaration of GaussianFactor
+	// TODO class NoiseModel {};
-	//class GaussianFactor;
+	// TODO class Isotropic : public NoiseModel {};
-	//typedef boost::shared_ptr<GaussianFactor> shared_ptr;
+	// TODO class Diagonal : public NoiseModel {};
+	// TODO class Full : public NoiseModel {};
+	// TODO class Robust : public NoiseModel {};
 
 	/**
-   * Nonlinear factor which assumes Gaussian noise on a measurement
+	 * Nonlinear factor which assumes zero-mean Gaussian noise on the
-   * predicted by a non-linear function h.
+	 * on a measurement predicted by a non-linear function h.
 	 *
-   * Templated on a configuration type. The configurations are typically more general
+	 * Templated on a configuration type. The configurations are typically
-	 * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
+	 * more general than just vectors, e.g., Rot3 or Pose3,
+	 * which are objects in non-linear manifolds (Lie groups).
 	 */
-	template <class Config>
+	template<class Config>
-  class NonlinearFactor : public Factor<Config>
+	class NonlinearFactor: public Factor<Config> {
-  {
+
 	protected:
 
-    Vector z_;     // measurement
 		double sigma_; // noise standard deviation
 		std::list<std::string> keys_; // keys
 
+		typedef NonlinearFactor<Config> This;
+
 	public:
 
-    /** Default constructor, with easily identifiable bogus values */
+		/** Default constructor for I/O only */
-    NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {}
+		NonlinearFactor() {
+		}
 
 		/**
 		 *  Constructor
-     *  @param z the measurement
 		 *  @param sigma the standard deviation
+		 *  // TODO: take a NoiseModel shared pointer
 		 */
-    NonlinearFactor(const Vector& z, const double sigma) {
+		NonlinearFactor(const double sigma) :
-    	z_ = z;
+			sigma_(sigma) {
-    	sigma_ = sigma;
 		}
 
 		/** print */
 		void print(const std::string& s = "") const {
 			std::cout << "NonlinearFactor " << s << std::endl;
-    	gtsam::print(z_, "  z = ");
 			std::cout << "  sigma = " << sigma_ << std::endl;
 		}
 
 		/** Check if two NonlinearFactor objects are equal */
-    bool equals(const Factor<Config>& f, double tol=1e-9) const {
+		bool equals(const Factor<Config>& f, double tol = 1e-9) const {
-    	const NonlinearFactor<Config>* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
+			const This* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
 			if (p == NULL) return false;
-      return equal_with_abs_tol(z_,p->z_,tol) && fabs(sigma_ - p->sigma_)<=tol;
+			return fabs(sigma_ - p->sigma_) <= tol;
 		}
 
+		/**
+		 * calculate the error of the factor
+		 * TODO: use NoiseModel
+		 */
+		double error(const Config& c) const {
+			// return NoiseModel.mahalanobis(error_vector(c)); // e'*inv(C)*e
+			if (sigma_ == 0.0) {
+				Vector e = error_vector(c);
+				return (inner_prod(e, e) > 0) ? std::numeric_limits<double>::infinity()
+						: 0.0;
+			}
+			Vector e = error_vector(c) / sigma_;
+			return 0.5 * inner_prod(e, e);
+		}
+		;
+
+		/** return keys */
+		std::list<std::string> keys() const {
+			return keys_;
+		}
+
+		/** get the size of the factor */
+		std::size_t size() const {
+			return keys_.size();
+		}
+
+		/** get functions */
+		double sigma() const {
+			return sigma_;
+		} // TODO obsolete when using NoiseModel
+
 		/** Vector of errors */
 		virtual Vector error_vector(const Config& c) const = 0;
 
 		/** linearize to a GaussianFactor */
-    virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const = 0;
+		virtual boost::shared_ptr<GaussianFactor>
-
+		linearize(const Config& c) const = 0;
-    /** get functions */
-    double sigma() const {return sigma_;}
-    Vector measurement() const {return z_;}
-    std::list<std::string> keys() const {return keys_;}
-
-    /** calculate the error of the factor */
-    double error(const Config& c) const {
-    	if (sigma_==0.0) {
-        Vector e = error_vector(c);
-    		return (inner_prod(e,e)>0) ? std::numeric_limits<double>::infinity() : 0.0;
-    	}
-      Vector e = error_vector(c) / sigma_;
-      return 0.5 * inner_prod(e,e);
-    };
-		
-    /** get the size of the factor */
-    std::size_t size() const{return keys_.size();}
 
 	private:
 
@@ -109,91 +118,199 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version) {
-			ar & BOOST_SERIALIZATION_NVP(z_);
+			ar & BOOST_SERIALIZATION_NVP(sigma_); // TODO NoiseModel
-			ar & BOOST_SERIALIZATION_NVP(sigma_);
-			ar & BOOST_SERIALIZATION_NVP(keys_);
 		}
 
 	}; // NonlinearFactor
 
 
 	/**
-   * a Gaussian nonlinear factor that takes 1 parameter
+	 * A Gaussian nonlinear factor that takes 1 parameter
-   * Note: cannot be serialized as contains function pointers
+	 * implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C
-   * Specialized derived classes could do this
+	 * Templated on the parameter type X and the configuration Config
+	 * There is no return type specified for h(x). Instead, we require
+	 * the derived class implements error_vector(c) = h(x)-z \approx Ax-b
+	 * This allows a graph to have factors with measurements of mixed type.
 	 */
-  class NonlinearFactor1 : public NonlinearFactor<VectorConfig> {
+	template<class Config, class Key, class X>
-  private:
+	class NonlinearFactor1: public NonlinearFactor<Config> {
 
-		std::string key_;
+	protected:
+
+		// The value of the key. Not const to allow serialization
+		Key key_;
+
+		typedef NonlinearFactor<Config> Base;
+		typedef NonlinearFactor1<Config, Key, X> This;
 
 	public:
 
-		Vector (*h_)(const Vector&);
-		Matrix (*H_)(const Vector&);
-
-    /** Constructor */
-    NonlinearFactor1(const Vector& z,		  // measurement
-		     const double sigma,	  // variance
-		     Vector (*h)(const Vector&),  // measurement function
-		     const std::string& key1,     // key of the variable
-		     Matrix (*H)(const Vector&)); // derivative of the measurement function
-
-    void print(const std::string& s = "") const;
-
-    /** Check if two factors are equal */
-    bool equals(const Factor<VectorConfig>& f, double tol=1e-9) const;
-
-    /** error function */
-    inline Vector error_vector(const VectorConfig& c) const {
-      return z_ - h_(c[key_]);
-    }
-
-    /** linearize a non-linearFactor1 to get a linearFactor1 */
-    boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const;
-  };
-
 		/**
-	 * a Gaussian nonlinear factor that takes 2 parameters
+		 *  Constructor
-	 * Note: cannot be serialized as contains function pointers
+		 *  @param z measurement
-	 * Specialized derived classes could do this
+		 *  @param key by which to look up X value in Config
 		 */
-  class NonlinearFactor2 : public NonlinearFactor<VectorConfig> {
+		NonlinearFactor1(double sigma, const Key& key1) :
+			Base(sigma), key_(key1) {
+			this->keys_.push_back(key_);
+		}
+
+		/* print */
+		void print(const std::string& s = "") const {
+			std::cout << "NonlinearFactor1 " << s << std::endl;
+			std::cout << "key: " << (std::string) key_ << std::endl;
+			Base::print("parent");
+		}
+
+		/** Check if two factors are equal. Note type is Factor and needs cast. */
+		bool equals(const Factor<Config>& f, double tol = 1e-9) const {
+			const This* p = dynamic_cast<const This*> (&f);
+			if (p == NULL) return false;
+			return Base::equals(*p, tol) && (key_ == p->key_);
+		}
+
+		/** error function z-h(x) */
+		inline Vector error_vector(const Config& x) const {
+			Key j = key_;
+			const X& xj = x[j];
+			return evaluateError(xj);
+		}
+
+		/**
+		 * Linearize a non-linearFactor1 to get a GaussianFactor
+		 * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
+		 * Hence b = z - h(x0) = - error_vector(x)
+		 */
+		boost::shared_ptr<GaussianFactor> linearize(const Config& x) const {
+			const X& xj = x[key_];
+			Matrix A;
+			Vector b = -evaluateError(xj, A);
+			return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
+					this->sigma()));
+		}
+
+		/*
+		 *  Override this method to finish implementing a unary factor.
+		 *  If the optional Matrix reference argument is specified, it should compute
+		 *  both the function evaluation and its derivative in X.
+		 */
+		virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
+				boost::none) const = 0;
+
 
 	private:
 
-    std::string key1_;
+		/** Serialization function */
-    std::string key2_;
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object<NonlinearFactor>(*this));
+			ar & BOOST_SERIALIZATION_NVP(key_);
+		}
+
+};
+
+	/**
+	 * A Gaussian nonlinear factor that takes 2 parameters
+	 * Note: cannot be serialized as contains function pointers
+	 * Specialized derived classes could do this
+	 */
+	template<class Config, class Key1, class X1, class Key2, class X2>
+	class NonlinearFactor2: public NonlinearFactor<Config> {
+
+	protected:
+
+		// The values of the keys. Not const to allow serialization
+		Key1 key1_;
+		Key2 key2_;
+
+		typedef NonlinearFactor<Config> Base;
+		typedef NonlinearFactor2<Config, Key1, X1, Key2, X2> This;
 
 	public:
 
-  	Vector (*h_)(const Vector&, const Vector&);
+		/**
-    Matrix (*H1_)(const Vector&, const Vector&);
+		 * Default Constructor for I/O
-    Matrix (*H2_)(const Vector&, const Vector&);
+		 */
+		NonlinearFactor2();
 
-    /** Constructor */
+		/**
-    NonlinearFactor2(const Vector& z,	               // the measurement
+		 * Constructor
-		     const double sigma,	                       // the variance
+		 * @param j1 key of the first variable
-		     Vector (*h)(const Vector&, const Vector&),  // the measurement function
+		 * @param j2 key of the second variable
-		     const std::string& key1,                    // key of the first variable
+		 */
-		     Matrix (*H1)(const Vector&, const Vector&), // derivative of h in first variable
+		NonlinearFactor2(double sigma, Key1 j1, Key2 j2) :
-		     const std::string& key2,                    // key of the second variable
+			Base(sigma), key1_(j1), key2_(j2) {
-		     Matrix (*H2)(const Vector&, const Vector&));// derivative of h in second variable
+			this->keys_.push_back(key1_);
-			
+			this->keys_.push_back(key2_);
-    /** Print */
+		}
-    void print(const std::string& s = "") const;
+
-
+		/** Print */
-    /** Check if two factors are equal */
+		void print(const std::string& s = "") const {
-    bool equals(const Factor<VectorConfig>& f, double tol=1e-9) const;
+			std::cout << "NonlinearFactor2 " << s << std::endl;
-
+			std::cout << "key1: " << (std::string) key1_ << std::endl;
-    /** error function */
+			std::cout << "key2: " << (std::string) key2_ << std::endl;
-    inline Vector error_vector(const VectorConfig& c) const {
+			Base::print("parent");
-      return z_ - h_(c[key1_], c[key2_]); 
+		}
+
+		/** Check if two factors are equal */
+		bool equals(const Factor<Config>& f, double tol = 1e-9) const {
+			const This* p = dynamic_cast<const This*> (&f);
+			if (p == NULL) return false;
+			return Base::equals(*p, tol) && (key1_ == p->key1_)
+					&& (key2_ == p->key2_);
+		}
+
+		/** error function z-h(x1,x2) */
+		inline Vector error_vector(const Config& x) const {
+			const X1& x1 = x[key1_];
+			const X2& x2 = x[key2_];
+			return evaluateError(x1, x2);
+		}
+
+		/**
+		 * Linearize a non-linearFactor1 to get a GaussianFactor
+		 * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
+		 * Hence b = z - h(x1,x2) = - error_vector(x)
+		 */
+		boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
+			const X1& x1 = c[key1_];
+			const X2& x2 = c[key2_];
+			Matrix A1, A2;
+			Vector b = -evaluateError(x1, x2, A1, A2);
+			return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
+					A2, b, this->sigma()));
+		}
+
+		/** methods to retrieve both keys */
+		inline const Key1& key1() const {
+			return key1_;
+		}
+		inline const Key2& key2() const {
+			return key2_;
+		}
+
+		/*
+		 *  Override this method to finish implementing a binary 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).
+		 */
+		virtual Vector evaluateError(const X1&, const X2&,
+				boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+						boost::none) const = 0;
+
+	private:
+
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object<NonlinearFactor>(*this));
+			ar & BOOST_SERIALIZATION_NVP(key1_);
+			ar & BOOST_SERIALIZATION_NVP(key2_);
 		}
 
-    /** Linearize a non-linearFactor2 to get a linearFactor2 */
-    boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const;
 	};
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
 }

cpp/Ordering.cpp

@@ -20,6 +20,7 @@ using namespace boost::assign;
 
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
 
+/*
 class ordering_key_visitor : public boost::default_bfs_visitor {
 public:
 	ordering_key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {}
@@ -36,8 +37,8 @@ public:
 		//boost::add_edge(v1, v2, g);
 	}
 };
-
+*/
-/* ************************************************************************* */
+/* ************************************************************************* *
 Ordering::Ordering(const map<string, string>& p_map) {
 
 	SGraph g;

cpp/Ordering.h

@@ -42,7 +42,7 @@ namespace gtsam {
 		/**
 		 * Generate the ordering from a spanning tree represented by its parent map
 		 */
-		Ordering(const std::map<std::string, std::string>& p_map);
+		//Ordering(const std::map<std::string, std::string>& p_map);
 
 		/**
 		 * Remove a set of keys from an ordering

cpp/Point2.h

@@ -70,7 +70,7 @@ namespace gtsam {
   /** Lie group functions */
 
   /** Global print calls member function */
-  inline void print(const Point2& p, std::string& s) { p.print(s); }
+  inline void print(const Point2& p, const std::string& s) { p.print(s); }
   inline void print(const Point2& p) { p.print(); }
 
   /** Dimensionality of the tangent space */

cpp/Point2Prior.h

@@ -4,13 +4,21 @@
 #include "NonlinearFactor.h"
 #include "simulated2D.h"
 
-namespace gtsam {
+namespace simulated2D {
 
-	class Point2Prior: public NonlinearFactor1 {
+	struct Point2Prior: public gtsam::NonlinearFactor1<VectorConfig, std::string, Vector> {
-	public:
+
-		Point2Prior(const Vector& mu, double sigma, const std::string& key) :
+		Vector z_;
-			NonlinearFactor1(mu, sigma, prior, key, Dprior) {
+
+		Point2Prior(const Vector& z, double sigma, const std::string& key) :
+			gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key), z_(z) {
 		}
+
+	  Vector evaluateError(const Vector& x, boost::optional<Matrix&> H = boost::none) const {
+	    if (H) *H = Dprior(x);
+	    return prior(x) - z_;
+	  }
+
 	};
 
-} // namespace gtsam
+} // namespace simulated2D

cpp/Point3.h

@@ -79,7 +79,7 @@ namespace gtsam {
 
 
   /** Global print calls member function */
-  inline void print(const Point3& p, std::string& s) { p.print(s); }
+  inline void print(const Point3& p, const std::string& s) { p.print(s); }
   inline void print(const Point3& p) { p.print(); }
 
   /** return DOF, dimensionality of tangent space */

cpp/Pose2Config.cpp

@@ -12,25 +12,17 @@ using namespace std;
 namespace gtsam {
 
   /** Explicit instantiation of templated methods and functions */
-  template class LieConfig<Pose2>;
+  template class LieConfig<Symbol<Pose2,'x'>,Pose2>;
-  template size_t dim(const LieConfig<Pose2>& c);
+  template size_t dim(const Pose2Config& c);
-  template LieConfig<Pose2> expmap(const LieConfig<Pose2>& c, const Vector& delta);
+  template Pose2Config expmap(const Pose2Config& c, const Vector& delta);
-  template LieConfig<Pose2> expmap(const LieConfig<Pose2>& c, const VectorConfig& delta);
+  template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
 
 	/* ************************************************************************* */
-	// TODO: local version, should probably defined in LieConfig
+	Pose2Config pose2Circle(size_t n, double R) {
-	static string symbol(char c, int index) {
-		stringstream ss;
-		ss << c << index;
-		return ss.str();
-	}
-
-	/* ************************************************************************* */
-	Pose2Config pose2Circle(size_t n, double R, char c) {
 		Pose2Config x;
 		double theta = 0, dtheta = 2*M_PI/n;
 		for(size_t i=0;i<n;i++, theta+=dtheta)
-			x.insert(symbol(c,i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
+			x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
 		return x;
 	}
 

cpp/Pose2Config.h

@@ -14,7 +14,7 @@ namespace gtsam {
   /**
    * Pose2Config is now simply a typedef
    */
-  typedef LieConfig<Pose2> Pose2Config;
+  typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
 
   /**
    * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
@@ -23,6 +23,6 @@ namespace gtsam {
    * @param c character to use for keys
    * @return circle of n 2D poses
    */
-	Pose2Config pose2Circle(size_t n, double R, char c = 'p');
+	Pose2Config pose2Circle(size_t n, double R);
 
 } // namespace

cpp/Pose2Factor.h

@@ -12,6 +12,6 @@
 namespace gtsam {
 
 	/** This is just a typedef now */
-	typedef BetweenFactor<Pose2, Pose2Config> Pose2Factor;
+	typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
 
 } /// namespace gtsam

cpp/Pose2Graph.cpp

@@ -3,10 +3,11 @@
  * @brief   A factor graph for the 2D PoseSLAM problem
  * @authors Frank Dellaert, Viorela Ila
  */
+
+#include "Pose2Graph.h"
 #include "FactorGraph-inl.h"
 #include "NonlinearFactorGraph-inl.h"
 #include "graph-inl.h"
-#include "Pose2Graph.h"
 
 using namespace std;
 using namespace gtsam;
@@ -14,7 +15,7 @@ using namespace gtsam;
 namespace gtsam {
 
 // explicit instantiation so all the code is there and we can link with it
-template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
+template class FactorGraph<NonlinearFactor<Pose2Config> > ;
 template class NonlinearFactorGraph<Pose2Config> ;
 //template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
 

cpp/Pose2Graph.h

@@ -7,10 +7,10 @@
 
 #pragma once
 
-#include "NonlinearFactorGraph.h"
-#include "NonlinearEquality.h"
 #include "Pose2Factor.h"
 #include "Pose2Config.h"
+#include "NonlinearFactorGraph.h"
+#include "NonlinearEquality.h"
 
 namespace gtsam {
 
@@ -33,7 +33,7 @@ namespace gtsam {
 		/**
 		 * Add a factor without having to do shared factor dance
 		 */
-		inline void add(const std::string& key1, const std::string& key2,
+		inline void add(const Pose2Config::Key& key1, const Pose2Config::Key& key2,
 				const Pose2& measured, const Matrix& covariance) {
 			push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
 		}
@@ -43,8 +43,8 @@ namespace gtsam {
 		 *  @param key of pose
 		 *  @param pose which pose to constrain it to
 		 */
-		inline void addConstraint(const std::string& key, const Pose2& pose =	Pose2()) {
+		inline void addConstraint(const Pose2Config::Key& key, const Pose2& pose =	Pose2()) {
-			push_back(sharedFactor(new NonlinearEquality<Pose2Config>(key, pose)));
+			push_back(sharedFactor(new NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2>(key, pose)));
 		}
 
 	private:

cpp/Pose3Config.cpp

@@ -12,10 +12,10 @@ using namespace std;
 namespace gtsam {
 
   /** Explicit instantiation of templated methods and functions */
-  template class LieConfig<Pose3>;
+  template class LieConfig<Symbol<Pose3,'x'>,Pose3>;
-  template size_t dim(const LieConfig<Pose3>& c);
+  template size_t dim(const Pose3Config& c);
-  template LieConfig<Pose3> expmap(const LieConfig<Pose3>& c, const Vector& delta);
+  template Pose3Config expmap(const Pose3Config& c, const Vector& delta);
-  template LieConfig<Pose3> expmap(const LieConfig<Pose3>& c, const VectorConfig& delta);
+  template Pose3Config expmap(const Pose3Config& c, const VectorConfig& delta);
 
 	/* ************************************************************************* */
 	// TODO: local version, should probably defined in LieConfig
@@ -26,13 +26,13 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	Pose3Config pose3Circle(size_t n, double R, char c) {
+	Pose3Config pose3Circle(size_t n, double R) {
 		Pose3Config x;
 		double theta = 0, dtheta = 2*M_PI/n;
 		// Vehicle at p0 is looking towards y axis
 		Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
 		for (size_t i = 0; i < n; i++, theta += dtheta)
-			x.insert(symbol(c,i), Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
+			x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
 		return x;
 	}
 

cpp/Pose3Config.h

@@ -14,7 +14,7 @@ namespace gtsam {
   /**
    * Pose3Config is now simply a typedef
    */
-  typedef LieConfig<Pose3> Pose3Config;
+  typedef LieConfig<Symbol<Pose3,'x'>,Pose3> Pose3Config;
 
   /**
    * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0,0)
@@ -24,6 +24,6 @@ namespace gtsam {
    * @param c character to use for keys
    * @return circle of n 2D poses
    */
-	Pose3Config pose3Circle(size_t n, double R, char c = 'p');
+	Pose3Config pose3Circle(size_t n, double R);
 
 } // namespace

cpp/Pose3Factor.h

@@ -14,6 +14,6 @@ namespace gtsam {
 	 * A Factor for 3D pose measurements
 	 * This is just a typedef now
 	 */
-	typedef BetweenFactor<Pose3, Pose3Config> Pose3Factor;
+	typedef BetweenFactor<Pose3Config,Pose3Config::Key,Pose3> Pose3Factor;
 
 } /// namespace gtsam

cpp/Pose3Graph.h

@@ -7,10 +7,10 @@
 
 #pragma once
 
-#include "NonlinearFactorGraph.h"
-#include "NonlinearEquality.h"
 #include "Pose3Factor.h"
 #include "Pose3Config.h"
+#include "NonlinearFactorGraph.h"
+#include "NonlinearEquality.h"
 
 namespace gtsam {
 
@@ -33,7 +33,7 @@ namespace gtsam {
 		/**
 		 * Add a factor without having to do shared factor dance
 		 */
-		inline void add(const std::string& key1, const std::string& key2,
+		inline void add(const Pose3Config::Key& key1, const Pose3Config::Key& key2,
 				const Pose3& measured, const Matrix& covariance) {
 			push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
 		}
@@ -43,8 +43,8 @@ namespace gtsam {
 		 *  @param key of pose
 		 *  @param pose which pose to constrain it to
 		 */
-		inline void addConstraint(const std::string& key, const Pose3& pose =	Pose3()) {
+		inline void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3()) {
-			push_back(sharedFactor(new NonlinearEquality<Pose3Config> (key, pose)));
+			push_back(sharedFactor(new NonlinearEquality<Pose3Config,Pose3Config::Key,Pose3> (key, pose)));
 		}
 
 	private:

cpp/Simulated2DMeasurement.h

@@ -4,11 +4,26 @@
 #include "NonlinearFactor.h"
 #include "simulated2D.h"
 
-namespace gtsam {
+namespace simulated2D {
+
+	struct Simulated2DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
+			PoseKey, Vector, PointKey, Vector> {
+
+		Vector z_;
+
+		Simulated2DMeasurement(const Vector& z, double sigma, const std::string& j1,
+				const std::string& j2) :
+			z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
+					Vector>(sigma, j1, j2) {
+		}
+
+		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
+				Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+			if (H1) *H1 = Dmea1(x1, x2);
+			if (H2) *H2 = Dmea2(x1, x2);
+			return mea(x1, x2) - z_;
+		}
 
-  struct Simulated2DMeasurement : public NonlinearFactor2 {
-  Simulated2DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
-    NonlinearFactor2(z, sigma, mea, key1, Dmea1, key2, Dmea2) {}
 	};
 
-} // namespace gtsam
+} // namespace simulated2D

cpp/Simulated2DOdometry.h

@@ -4,11 +4,25 @@
 #include "NonlinearFactor.h"
 #include "simulated2D.h"
 
-namespace gtsam {
+namespace simulated2D {
+
+	struct Simulated2DOdometry: public gtsam::NonlinearFactor2<VectorConfig,
+			PoseKey, Vector, PointKey, Vector> {
+		Vector z_;
+
+		Simulated2DOdometry(const Vector& z, double sigma, const std::string& j1,
+				const std::string& j2) :
+			z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
+					Vector>(sigma, j1, j2) {
+		}
+
+		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
+				Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+			if (H1) *H1 = Dodo1(x1, x2);
+			if (H2) *H2 = Dodo2(x1, x2);
+			return odo(x1, x2) - z_;
+		}
 
-  struct Simulated2DOdometry : public NonlinearFactor2 {
-  Simulated2DOdometry(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
-    NonlinearFactor2(z, sigma, odo, key1, Dodo1, key2, Dodo2) {}
 	};
 
 } // namespace gtsam

cpp/Simulated3D.cpp

@@ -6,49 +6,51 @@
 
 #include "Simulated3D.h"
 
-namespace gtsam {
+namespace simulated3D {
 
-Vector prior_3D (const Vector& x)
+using namespace gtsam;
+
+Vector prior (const Vector& x)
 {
 	return x;
 }
 
-Matrix Dprior_3D(const Vector& x)
+Matrix Dprior(const Vector& x)
 {
 	Matrix H = eye((int) x.size());
 	return H;
 }
 
-Vector odo_3D(const Vector& x1, const Vector& x2)
+Vector odo(const Vector& x1, const Vector& x2)
 {
 	return x2 - x1;
 }
 
-Matrix Dodo1_3D(const Vector& x1, const Vector& x2)
+Matrix Dodo1(const Vector& x1, const Vector& x2)
 {
 	Matrix H = -1 * eye((int) x1.size());
 	return H;
 }
 
-Matrix Dodo2_3D(const Vector& x1, const Vector& x2)
+Matrix Dodo2(const Vector& x1, const Vector& x2)
 {
 	Matrix H = eye((int) x1.size());
 	return H;
 }
 
 
-Vector mea_3D(const Vector& x,  const Vector& l)
+Vector mea(const Vector& x,  const Vector& l)
 {
 	return l - x;
 }
 
-Matrix Dmea1_3D(const Vector& x, const Vector& l)
+Matrix Dmea1(const Vector& x, const Vector& l)
 {
 	Matrix H = -1 * eye((int) x.size());
 	return H;
 }
 
-Matrix Dmea2_3D(const Vector& x, const Vector& l)
+Matrix Dmea2(const Vector& x, const Vector& l)
 {
 	Matrix H = eye((int) x.size());
 	return H;

cpp/Simulated3D.h

@@ -1,47 +1,81 @@
 /**
-* @file   Simulated3D.h
+ * @file   Simulated3D.h
-* @brief  measurement functions and derivatives for simulated 3D robot
+ * @brief  measurement functions and derivatives for simulated 3D robot
-* @author Alex Cunningham
+ * @author Alex Cunningham
-**/
+ **/
 
 // \callgraph
 
 #pragma once
 
+#include "Matrix.h"
+#include "VectorConfig.h"
 #include "NonlinearFactor.h"
 
 // \namespace
 
-namespace gtsam {
+namespace simulated3D {
+
+	typedef gtsam::VectorConfig VectorConfig;
+
+	struct PoseKey: public std::string {
+	};
+	struct PointKey: public std::string {
+	};
 
 	/**
 	 * Prior on a single pose
 	 */
-	Vector prior_3D (const Vector& x); 
+	Vector prior(const Vector& x);
-	Matrix Dprior_3D(const Vector& x);
+	Matrix Dprior(const Vector& x);
 
 	/**
 	 * odometry between two poses
 	 */
-	Vector odo_3D(const Vector& x1, const Vector& x2);
+	Vector odo(const Vector& x1, const Vector& x2);
-	Matrix Dodo1_3D(const Vector& x1, const Vector& x2);
+	Matrix Dodo1(const Vector& x1, const Vector& x2);
-	Matrix Dodo2_3D(const Vector& x1, const Vector& x2);
+	Matrix Dodo2(const Vector& x1, const Vector& x2);
 
 	/**
 	 *  measurement between landmark and pose
 	 */
-	Vector mea_3D(const Vector& x,  const Vector& l);
+	Vector mea(const Vector& x, const Vector& l);
-	Matrix Dmea1_3D(const Vector& x, const Vector& l);
+	Matrix Dmea1(const Vector& x, const Vector& l);
-	Matrix Dmea2_3D(const Vector& x, const Vector& l);
+	Matrix Dmea2(const Vector& x, const Vector& l);
 
-	struct Point2Prior3D : public NonlinearFactor1 {
+	struct Point2Prior3D: public gtsam::NonlinearFactor1<VectorConfig, PoseKey,
-		Point2Prior3D(const Vector& mu, double sigma, const std::string& key):
+			Vector> {
-		NonlinearFactor1(mu, sigma, prior_3D, key, Dprior_3D) {}
+
+		Vector z_;
+
+		Point2Prior3D(const Vector& z, double sigma, const PoseKey& j) :
+			gtsam::NonlinearFactor1<VectorConfig, PoseKey, Vector>(sigma, j), z_(z) {
+		}
+
+		Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
+				boost::none) {
+			if (H) *H = Dprior(x);
+			return prior(x) - z_;
+		}
 	};
 
-	struct Simulated3DMeasurement : public NonlinearFactor2 {
+	struct Simulated3DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
-		Simulated3DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
+			PoseKey, Vector, PointKey, Vector> {
-		NonlinearFactor2(z, sigma, mea_3D, key1, Dmea1_3D, key2, Dmea2_3D) {}
+
+		Vector z_;
+
+		Simulated3DMeasurement(const Vector& z, double sigma, PoseKey& j1,
+				PointKey j2) :
+			z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
+					Vector>(sigma, j1, j2) {
+		}
+
+		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
+				Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
+			if (H1) *H1 = Dmea1(x1, x2);
+			if (H2) *H2 = Dmea2(x1, x2);
+			return mea(x1, x2) - z_;
+		}
 	};
 
-} // namespace gtsam
+} // namespace simulated3D

cpp/VSLAMConfig.cpp

@@ -11,109 +11,32 @@
 #include <boost/tuple/tuple.hpp>
 
 #include "VSLAMConfig.h"
+#include "LieConfig-inl.h"
 
 using namespace std;
 
-// trick from some reading group
-#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
-
-#define SIGMA 1.0
-
 namespace gtsam {
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
-// Exponential map
+	// Exponential map
-VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta) {
+	VSLAMConfig expmap(const VSLAMConfig& x0, const VectorConfig & delta) {
-
+		VSLAMConfig x;
-	VSLAMConfig newConfig;
+		x.poses_ = expmap(x0.poses_, delta);
-
+		x.points_ = expmap(x0.points_, delta);
-	for (map<string, Vector>::const_iterator it = delta.begin(); it
+		return x;
-			!= delta.end(); it++) {
-		string key = it->first;
-		if (key[0] == 'x') {
-			int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str());
-			if (c.cameraPoseExists(cameraNumber)) {
-				Pose3 basePose = c.cameraPose(cameraNumber);
-				newConfig.addCameraPose(cameraNumber, expmap(basePose, it->second));
-			}
-		}
-		if (key[0] == 'l') {
-			int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str());
-			if (c.landmarkPointExists(landmarkNumber)) {
-				Point3 basePoint = c.landmarkPoint(landmarkNumber);
-				newConfig.addLandmarkPoint(landmarkNumber, expmap(basePoint, it->second));
-			}
-		}
 	}
 
-	return newConfig;
+	/* ************************************************************************* */
-}
+	void VSLAMConfig::print(const std::string& s) const {
-
-/* ************************************************************************* */
-void VSLAMConfig::print(const std::string& s) const
-{
 		printf("%s:\n", s.c_str());
-  printf("Camera Poses:\n");
+		poses_.print("Poses");
-  for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++)
+		points_.print("Points");
-  {
-    printf("x%d:\n", it->first);
-    it->second.print();
-  }
-  printf("Landmark Points:\n");
-  for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++)
-  {
-    printf("l%d:\n", (*it).first);
-    (*it).second.print();
-  }
-}
-
-/* ************************************************************************* */
-bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
-	for (PoseMap::const_iterator it = cameraIteratorBegin(); it
-			!= cameraIteratorEnd(); it++) {
-		if (!c.cameraPoseExists(it->first)) return false;
-		if (!it->second.equals(c.cameraPose(it->first), tol)) return false;
 	}
 
-	for (PointMap::const_iterator it = landmarkIteratorBegin(); it
+	/* ************************************************************************* */
-			!= landmarkIteratorEnd(); it++) {
+	bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
-		if (!c.landmarkPointExists(it->first)) return false;
+		return poses_.equals(c.poses_, tol) && points_.equals(c.points_, tol);
-		if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false;
 	}
-	return true;
-}
-
-/* ************************************************************************* */
-void VSLAMConfig::addCameraPose(const int i, Pose3 cp)
-{
-  pair<int, Pose3> camera;
-  camera.first = i;
-  camera.second = cp;
-  cameraPoses_.insert(camera);
-}
-
-/* ************************************************************************* */
-void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp)
-{
-  pair<int, Point3> landmark;
-  landmark.first = i;
-  landmark.second = lp;
-  landmarkPoints_.insert(landmark);
-}
-
-/* ************************************************************************* */
-void VSLAMConfig::removeCameraPose(const int i)
-{
-  if(cameraPoseExists(i))
-    cameraPoses_.erase(i);
-}
-
-/* ************************************************************************* */
-void VSLAMConfig::removeLandmarkPose(const int i)
-{
-  if(landmarkPointExists(i))
-    landmarkPoints_.erase(i);
-}
 
 /* ************************************************************************* */
 

cpp/VSLAMConfig.h

@@ -9,10 +9,8 @@
 #include <map>
 #include <vector>
 #include <fstream>
-#include "VectorConfig.h"
+
-#include "Pose3.h"
+#include "Pose3Config.h"
-#include "Cal3_S2.h"
-#include "Testable.h"
 
 #pragma once
 
@@ -23,91 +21,59 @@ namespace gtsam{
  */
 class VSLAMConfig : Testable<VSLAMConfig> {
 
- private:
+public:
-  typedef std::map<int, Pose3> PoseMap;
+
-  typedef std::map<int, Point3> PointMap;
+ typedef Symbol<Pose3,'x'> PoseKey;
-  PointMap landmarkPoints_;
+ typedef Symbol<Point3,'l'> PointKey;
-  PoseMap cameraPoses_;
+
+private:
+
+  LieConfig<PoseKey,  Pose3>  poses_;
+	LieConfig<PointKey, Point3> points_;
+
+public:
 
- public:
-  typedef std::map<std::string, Vector>::const_iterator const_iterator;
-  typedef PoseMap::const_iterator const_Pose_iterator;
-  typedef PointMap::const_iterator const_Point_iterator;
   /**
    * default constructor
    */
   VSLAMConfig() {}
 
-  /*
-   * copy constructor
-   */
-  VSLAMConfig(const VSLAMConfig& original):
-  	cameraPoses_(original.cameraPoses_), landmarkPoints_(original.landmarkPoints_){}
-
-  PoseMap::const_iterator cameraIteratorBegin() const  { return cameraPoses_.begin();}
-  PoseMap::const_iterator cameraIteratorEnd() const   { return cameraPoses_.end();}
-  PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();}
-  PointMap::const_iterator landmarkIteratorEnd() const  { return landmarkPoints_.end();}
-
   /**
    * print
    */
   void print(const std::string& s = "") const;
 
-  /**
-   * Retrieve robot pose
-   */
-  bool cameraPoseExists(int i) const
-  {
-    PoseMap::const_iterator it = cameraPoses_.find(i);
-    if (it==cameraPoses_.end())
-      return false;
-    return true;
-  }
-
-  Pose3 cameraPose(int i) const {
-    PoseMap::const_iterator it = cameraPoses_.find(i);
-    if (it==cameraPoses_.end())
-      throw(std::invalid_argument("robotPose: invalid key"));
-    return it->second;
-  }
-
-  /**
-   * Check whether a landmark point exists
-   */
-  bool landmarkPointExists(int i) const
-  {
-    PointMap::const_iterator it = landmarkPoints_.find(i);
-    if (it==landmarkPoints_.end())
-      return false;
-    return true;
-  }
-
-  /**
-   * Retrieve landmark point
-   */
-  Point3 landmarkPoint(int i) const {
-    PointMap::const_iterator it = landmarkPoints_.find(i);
-    if (it==landmarkPoints_.end())
-      throw(std::invalid_argument("markerPose: invalid key"));
-    return it->second;
-  }
-
   /**
    * check whether two configs are equal
    */
   bool equals(const VSLAMConfig& c, double tol=1e-6) const;
-  void addCameraPose(const int i, Pose3 cp);
-  void addLandmarkPoint(const int i, Point3 lp);
 
-  void removeCameraPose(const int i);
+  /**
-  void removeLandmarkPose(const int i);
+   * Get Poses or Points
+   */
+  inline const Pose3&  operator[](const PoseKey&  key) const {return poses_[key];}
+  inline const Point3& operator[](const PointKey& key) const {return points_[key];}
 
-  void clear() {landmarkPoints_.clear(); cameraPoses_.clear();}
+  // (Awkwardly named) backwards compatibility:
 
-  inline size_t size(){
+  inline bool cameraPoseExists   (const PoseKey&  key) const {return  poses_.exists(key);}
-    return landmarkPoints_.size() + cameraPoses_.size();
+  inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);}
-  }
+
+  inline Pose3  cameraPose   (const PoseKey&  key) const {return  poses_[key];}
+  inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];}
+
+  inline size_t size() const {return points_.size() + poses_.size();}
+  inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);}
+
+  // Imperative functions:
+
+  inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);}
+  inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);}
+
+  inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);}
+  inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);}
+
+  inline void clear() {points_.clear(); poses_.clear();}
 
   friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
 };
@@ -119,7 +85,5 @@ class VSLAMConfig : Testable<VSLAMConfig> {
  * Needed for use in nonlinear optimization
  */
 VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
-
-
 } // namespace gtsam
 

cpp/VSLAMFactor.cpp

@@ -4,88 +4,37 @@
  * @author  Alireza Fathi
  */
 
-#include "VSLAMConfig.h"
+#include <boost/bind.hpp>
+#include <boost/bind/placeholders.hpp>
+
 #include "VSLAMFactor.h"
-#include "Pose3.h"
 #include "SimpleCamera.h"
 
 using namespace std;
-namespace gtsam{
+namespace gtsam {
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
-VSLAMFactor::VSLAMFactor() {
+	VSLAMFactor::VSLAMFactor() {
 		/// Arbitrary values
-	cameraFrameNumber_ = 111;
+		K_ = shared_ptrK(new Cal3_S2(444, 555, 666, 777, 888));
-	landmarkNumber_    = 222;
+	}
-	cameraFrameName_ = symbol('x',cameraFrameNumber_);
+	/* ************************************************************************* */
-	landmarkName_    = symbol('l',landmarkNumber_);
+	VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln,
-  keys_.push_back(cameraFrameName_);
+			const shared_ptrK &K) :
-  keys_.push_back(landmarkName_);
+		VSLAMFactorBase(sigma, cn, ln), z_(z), K_(K) {
-	K_ = shared_ptrK(new Cal3_S2(444,555,666,777,888));
+	}
-}
-/* ************************************************************************* */
-VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const shared_ptrK &K)
-  : NonlinearFactor<VSLAMConfig>(z.vector(), sigma)
-{
-  cameraFrameNumber_ = cn;
-  landmarkNumber_    = ln;
-  cameraFrameName_ = symbol('x',cameraFrameNumber_);
-  landmarkName_    = symbol('l',landmarkNumber_);
-  keys_.push_back(cameraFrameName_);
-  keys_.push_back(landmarkName_);
-  K_ = K;
-}
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
-void VSLAMFactor::print(const std::string& s) const {
+	void VSLAMFactor::print(const std::string& s) const {
-  printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
+		VSLAMFactorBase::print(s);
-  gtsam::print(this->z_, s+".z");
+		z_.print(s + ".z");
-}
+	}
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
-bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
+	bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
-  if (&p == NULL) return false;
+		return VSLAMFactorBase::equals(p, tol) && z_.equals(p.z_, tol)
-  if (cameraFrameNumber_ != p.cameraFrameNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
+				&& K_->equals(*p.K_, tol);
-  if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false;
+	}
-  return true;
-}
-
-/* ************************************************************************* */
-Vector VSLAMFactor::predict(const VSLAMConfig& c) const {
-  Pose3 pose = c.cameraPose(cameraFrameNumber_);
-  Point3 landmark = c.landmarkPoint(landmarkNumber_);
-  return project(SimpleCamera(*K_,pose), landmark).vector();
-}
-
-/* ************************************************************************* */
-Vector VSLAMFactor::error_vector(const VSLAMConfig& c) const {
-  // Right-hand-side b = (z - h(x))/sigma
-  Vector h = predict(c);
-  return (this->z_ - h);
-}
-
-/* ************************************************************************* */
-GaussianFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
-{
-  // get arguments from config
-  Pose3 pose = c.cameraPose(cameraFrameNumber_);
-  Point3 landmark = c.landmarkPoint(landmarkNumber_);
-
-  SimpleCamera camera(*K_,pose);
-
-  // Jacobians
-  Matrix Dh1 = Dproject_pose(camera, landmark);
-  Matrix Dh2 = Dproject_point(camera, landmark);
-
-  // Right-hand-side b = (z - h(x))
-  Vector h = project(camera, landmark).vector();
-  Vector b = this->z_ - h;
-
-  // Make new linearfactor, divide by sigma
-  GaussianFactor::shared_ptr
-    p(new GaussianFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
-  return p;
-}
 
 /* ************************************************************************* */
 } // namespace gtsam

cpp/VSLAMFactor.h

@@ -6,31 +6,33 @@
 
 #pragma once
 
+#include <boost/optional.hpp>
+
 #include "NonlinearFactor.h"
-#include "GaussianFactor.h"
+#include "SimpleCamera.h"
+#include "VSLAMConfig.h"
 #include "Cal3_S2.h"
-#include "Testable.h"
 
 namespace gtsam {
 
-class VSLAMConfig;
+	typedef NonlinearFactor2<VSLAMConfig, VSLAMConfig::PoseKey,
+	Pose3, VSLAMConfig::PointKey, Point3> VSLAMFactorBase;
 
-/**
+	/**
 	 * Non-linear factor for a constraint derived from a 2D measurement,
 	 * i.e. the main building block for visual SLAM.
 	 */
-class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
+	class VSLAMFactor: public VSLAMFactorBase , Testable<VSLAMFactor> {
-{
+	private:
-private:
 
-	int cameraFrameNumber_, landmarkNumber_;
+		// Keep a copy of measurement and calibration for I/O
-	std::string cameraFrameName_, landmarkName_;
+		Point2 z_;
-	boost::shared_ptr<Cal3_S2> K_; // Calibration stored in each factor. FD: need to think about this.
+		boost::shared_ptr<Cal3_S2> K_;
-	typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
 
-public:
+	public:
 
-	typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
+		 // shorthand for a smart pointer to a factor
+		typedef boost::shared_ptr<VSLAMFactor> shared_ptr;
 
 		/**
 		 * Default constructor
@@ -45,49 +47,45 @@ public:
 		 * @param landmarkNumber is the index of the landmark
 		 * @param K the constant calibration
 		 */
-	VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const shared_ptrK & K);
+		VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber,
-
+				int landmarkNumber, const shared_ptrK & K);
 
 		/**
 		 * print
 		 * @param s optional string naming the factor
 		 */
-	void print(const std::string& s="VSLAMFactor") const;
+		void print(const std::string& s = "VSLAMFactor") const;
 
 		/**
 		 * equals
 		 */
-	bool equals(const VSLAMFactor&, double tol=1e-9) const;
+		bool equals(const VSLAMFactor&, double tol = 1e-9) const;
 
-	/**
+		/** h(x) */
-	 * predict the measurement
+		Point2 predict(const Pose3& pose, const Point3& point) const {
-	 */
+			return SimpleCamera(*K_, pose).project(point);
-	Vector predict(const VSLAMConfig&) const;
+		}
 
-	/**
+		/** h(x)-z */
-	 * calculate the error of the factor
+		Vector evaluateError(const Pose3& pose, const Point3& point,
-	 */
+				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-	Vector error_vector(const VSLAMConfig&) const;
+			SimpleCamera camera(*K_, pose);
+			if (H1) *H1=Dproject_pose(camera,point);
+			if (H2) *H2=Dproject_point(camera,point);
+			Point2 reprojectionError(project(camera, point) - z_);
+			return reprojectionError.vector();
+		}
 
-	/**
+	private:
-	 * linerarization
-	 */
-	GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const;
-
-	int getCameraFrameNumber() const { return cameraFrameNumber_; }
-	int getLandmarkNumber()    const { return landmarkNumber_;    }
-
-private:
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version) {
-		ar & BOOST_SERIALIZATION_NVP(cameraFrameNumber_);
+			ar & BOOST_SERIALIZATION_NVP(key1_);
-		ar & BOOST_SERIALIZATION_NVP(landmarkNumber_);
+			ar & BOOST_SERIALIZATION_NVP(key2_);
-		ar & BOOST_SERIALIZATION_NVP(cameraFrameName_);
+			ar & BOOST_SERIALIZATION_NVP(z_);
-		ar & BOOST_SERIALIZATION_NVP(landmarkName_);
 			ar & BOOST_SERIALIZATION_NVP(K_);
 		}
-};
+	};
 
 }

cpp/VSLAMGraph.cpp

@@ -33,10 +33,8 @@ bool compareLandmark(const std::string& key,
 
 /* ************************************************************************* */
 void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
-  typedef NonlinearEquality<VSLAMConfig> NLE;
+  typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PointKey,Point3> NLE;
-  VSLAMConfig feasible;
+  boost::shared_ptr<NLE> factor(new NLE(j, p));
-  feasible.addLandmarkPoint(j,p);
-  boost::shared_ptr<NLE> factor(new NLE(symbol('l',j), feasible, 3, compareLandmark));
   push_back(factor);
 }
 
@@ -50,10 +48,8 @@ bool compareCamera(const std::string& key,
 
 /* ************************************************************************* */
 void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) {
-  typedef NonlinearEquality<VSLAMConfig> NLE;
+  typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PoseKey,Pose3> NLE;
-  VSLAMConfig feasible;
+  boost::shared_ptr<NLE> factor(new NLE(j,p));
-  feasible.addCameraPose(j,p);
-  boost::shared_ptr<NLE> factor(new NLE(symbol('x',j), feasible, 6, compareCamera));
   push_back(factor);
 }
 

cpp/VSLAMGraph.h

@@ -12,9 +12,9 @@
 #include <set>
 #include <fstream>
 
+#include "VSLAMFactor.h"
 #include "NonlinearFactorGraph.h"
 #include "FactorGraph-inl.h"
-#include "VSLAMFactor.h"
 #include "VSLAMConfig.h"
 #include "Testable.h"
 

cpp/graph-inl.h

@@ -1,19 +1,15 @@
 /*
  * graph-inl.h
- *
+ * @brief Graph algorithm using boost library
- *   Created on: Jan 11, 2010
+ * @author: Kai Ni
- *       Author: nikai
- *  Description: Graph algorithm using boost library
  */
 
 #pragma once
 
 #include <boost/foreach.hpp>
-#include <boost/graph/graph_traits.hpp>
-#include <boost/graph/adjacency_list.hpp>
 #include <boost/graph/breadth_first_search.hpp>
-#include <boost/shared_ptr.hpp>
 
+#include "graph.h"
 
 using namespace std;
 
@@ -21,31 +17,22 @@ using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
+// some typedefs we need
-/**
+
- * type definitions
+//typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
- */
+
-typedef boost::adjacency_list<
+//typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
-	boost::vecS, boost::vecS, boost::undirectedS,
+//		boost::property<boost::vertex_name_t, std::string> > SGraph;
-	boost::property<boost::vertex_name_t, std::string>,
+//typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
-	boost::property<boost::edge_weight_t, double> > SDGraph;
-typedef boost::graph_traits<SDGraph>::vertex_descriptor BoostVertex;
-typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
 
-typedef boost::adjacency_list<
-	boost::vecS, boost::vecS, boost::directedS,
-	boost::property<boost::vertex_name_t, string> > SGraph;
-typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
 
 /* ************************************************************************* */
-/**
+template<class G, class F, class Key>
- * Convert the factor graph to a boost undirected graph
+SDGraph<Key> toBoostGraph(const G& graph) {
- */
-template<class G, class F>
-SDGraph toBoostGraph(const G& graph) {
 	// convert the factor graph to boost graph
-	SDGraph g(0);
+	SDGraph<Key> g(0);
-	map<string, BoostVertex> key2vertex;
+	typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
+	map<Key, BoostVertex> key2vertex;
 	BoostVertex v1, v2;
 	BOOST_FOREACH(F factor, graph) {
 		if (factor->keys().size() > 2)
@@ -54,8 +41,8 @@ SDGraph toBoostGraph(const G& graph) {
 		if (factor->keys().size() == 1)
 			continue;
 
-		string key1 = factor->keys().front();
+		Key key1 = factor->key1();
-		string key2 = factor->keys().back();
+		Key key2 = factor->key2();
 
 		if (key2vertex.find(key1) == key2vertex.end()) {
 				 v1 = add_vertex(key1, g);
@@ -77,16 +64,13 @@ SDGraph toBoostGraph(const G& graph) {
 }
 
 /* ************************************************************************* */
-/**
+template<class G, class V, class Key>
- *  build the graph corresponding to the predecessor map. Excute action for each edge.
+boost::tuple<G, V, map<Key, V> > predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
- */
-template<class G, class V>
-boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, string>& p_map) {
 
 	G g(0);
-	map<string, V> key2vertex;
+	map<Key, V> key2vertex;
 	V v1, v2, root;
-	string child, parent;
+	Key child, parent;
 	bool foundRoot = false;
 	FOREACH_PAIR(child, parent, p_map) {
 		if (key2vertex.find(child) == key2vertex.end()) {
@@ -101,7 +85,7 @@ boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, strin
 		 } else
 			 v2 = key2vertex[parent];
 
-		if (child.compare(parent) == 0) {
+		if (child==parent) {
 			root = v1;
 			foundRoot = true;
 		} else
@@ -111,49 +95,47 @@ boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, strin
 	if (!foundRoot)
 		throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
 
-	return boost::tuple<G, V, map<string, V> >(g, root, key2vertex);
+	return boost::tuple<G, V, map<Key, V> >(g, root, key2vertex);
 }
 
 /* ************************************************************************* */
-/**
+template <class V,class Pose, class Config>
- * Visit each edge and compose the poses
- */
-template <class V, class Pose, class PoseConfig>
 class compose_key_visitor : public boost::default_bfs_visitor {
-public:
-	compose_key_visitor(boost::shared_ptr<PoseConfig> config_in) { config = config_in; }
-	template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
-			string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
-			string key_to   = boost::get(boost::vertex_name, g, boost::target(edge, g));
-			Pose relativePose = boost::get(boost::edge_weight, g, edge);
-			config->insert(key_to, compose(relativePose, config->get(key_from)));
-	}
 
 private:
-	boost::shared_ptr<PoseConfig> config;
+	boost::shared_ptr<Config> config_;
+
+public:
+
+	compose_key_visitor(boost::shared_ptr<Config> config_in) {config_ = config_in;}
+
+	template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
+		typename Config::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
+		typename Config::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
+		Pose relativePose = boost::get(boost::edge_weight, g, edge);
+		config_->insert(key_to, compose(relativePose, (*config_)[key_from]));
+	}
 
 };
 
 /* ************************************************************************* */
-/**
- * Compose the poses by following the chain sepcified by the spanning tree
- */
 template<class G, class Factor, class Pose, class Config>
-boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>& tree,
+boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree,
 		const Pose& rootPose) {
 
 	//TODO: change edge_weight_t to edge_pose_t
 	typedef typename boost::adjacency_list<
 		boost::vecS, boost::vecS, boost::directedS,
-		boost::property<boost::vertex_name_t, string>,
+		boost::property<boost::vertex_name_t, typename Config::Key>,
 		boost::property<boost::edge_weight_t, Pose> > PoseGraph;
 	typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
 	typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
 
 	PoseGraph g;
 	PoseVertex root;
-	map<string, PoseVertex> key2vertex;
+	map<typename Config::Key, PoseVertex> key2vertex;
-	boost::tie(g, root, key2vertex) = predecessorMap2Graph<PoseGraph, PoseVertex>(tree);
+	boost::tie(g, root, key2vertex) =
+			predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree);
 
 	// attach the relative poses to the edges
 	PoseEdge edge1, edge2;
@@ -167,8 +149,11 @@ boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>
 		boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
 		if (!factor) continue;
 
-		PoseVertex v_from = key2vertex.find(factor->keys().front())->second;
+		typename Config::Key key1 = factor->key1();
-		PoseVertex v_to = key2vertex.find(factor->keys().back())->second;
+		typename Config::Key key2 = factor->key2();
+
+		PoseVertex v_from = key2vertex.find(key1)->second;
+		PoseVertex v_to = key2vertex.find(key2)->second;
 
 		Pose measured = factor->measured();
 		tie(edge1, found1) = boost::edge(v_from, v_to, g);
@@ -183,7 +168,8 @@ boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>
 
 	// compose poses
 	boost::shared_ptr<Config> config(new Config);
-	config->insert(boost::get(boost::vertex_name, g, root), rootPose);
+	typename Config::Key rootKey = boost::get(boost::vertex_name, g, root);
+	config->insert(rootKey, rootPose);
 	compose_key_visitor<PoseVertex, Pose, Config> vis(config);
 	boost::breadth_first_search(g, root, boost::visitor(vis));
 

cpp/graph.h

@@ -0,0 +1,59 @@
+/*
+ *  graph.h
+ *  @brief Graph algorithm using boost library
+ *  @author: Kai Ni
+ *  Created on: Jan 11, 2010
+ */
+
+#pragma once
+
+#include <map>
+
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace gtsam {
+
+	// type definitions :
+
+	/**
+	 * SDGraph is undirected graph with variable keys and double edge weights
+	 */
+	template<class Key>
+	class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
+	boost::property<boost::vertex_name_t, Key>, boost::property<
+	boost::edge_weight_t, double> > {
+	};
+
+	/**
+	 * Map from variable key to parent key
+	 */
+	template <class Key>
+	class PredecessorMap : public std::map<Key,Key> {};
+
+	/**
+	 * Convert the factor graph to an SDGraph
+	 * G = Graph type
+	 * F = Factor type
+	 * Key = Key type
+	 */
+	template<class G, class F, class Key> SDGraph<Key> toBoostGraph(const G& graph);
+
+	/**
+	 * Build takes a predecessor map, and builds a directed graph corresponding to the tree.
+	 * G = Graph type
+	 * V = Vertex type
+	 */
+	template<class G, class V, class Key>
+	boost::tuple<G, V, std::map<Key, V> >
+		predecessorMap2Graph(const PredecessorMap<Key>& p_map);
+
+	/**
+	 * Compose the poses by following the chain specified by the spanning tree
+	 */
+	template<class G, class Factor, class Pose, class Config>
+	boost::shared_ptr<Config>
+		composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree, const Pose& rootPose);
+
+} // namespace gtsam

cpp/simulated2D.cpp

@@ -6,7 +6,7 @@
 
 #include "simulated2D.h"
 
-namespace gtsam {
+namespace simulated2D {
 
 
 /** prior on a single pose */
@@ -64,4 +64,4 @@ Matrix Dmea2(const Vector& x, const Vector& l) {
 }
 
 /* ************************************************************************* */
-} // namespace gtsam
+} // namespace simulated2D

cpp/simulated2D.h

@@ -8,16 +8,30 @@
 
 #pragma once
 
+#include "VectorConfig.h"
 #include "NonlinearFactor.h"
 
 // \namespace
 
-namespace gtsam {
+namespace simulated2D {
+
+	typedef gtsam::VectorConfig VectorConfig;
+
+	struct PoseKey: public std::string {
+		PoseKey(const std::string&s) :
+			std::string(s) {
+		}
+	};
+	struct PointKey: public std::string {
+		PointKey(const std::string&s) :
+			std::string(s) {
+		}
+	};
 
 	/**
 	 * Prior on a single pose
 	 */
-  Vector prior (const Vector& x);
+	Vector prior(const Vector& x);
 	Matrix Dprior(const Vector& x);
 
 	/**
@@ -34,4 +48,4 @@ namespace gtsam {
 	Matrix Dmea1(const Vector& x, const Vector& l);
 	Matrix Dmea2(const Vector& x, const Vector& l);
 
-} // namespace gtsam
+} // namespace simulated2D

cpp/smallExample.cpp

@@ -8,6 +8,7 @@
 
 #include <iostream>
 #include <string>
+#include <boost/optional.hpp>
 
 using namespace std;
 
@@ -37,7 +38,7 @@ namespace gtsam {
 		// prior on x1
 		double sigma1 = 0.1;
 		Vector mu = zero(2);
-		shared f1(new Point2Prior(mu, sigma1, "x1"));
+		shared f1(new simulated2D::Point2Prior(mu, sigma1, "x1"));
 		nlfg->push_back(f1);
 
 		// odometry between x1 and x2
@@ -45,7 +46,7 @@ namespace gtsam {
 		Vector z2(2);
 		z2(0) = 1.5;
 		z2(1) = 0;
-		shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2"));
+		shared f2(new simulated2D::Simulated2DOdometry(z2, sigma2, "x1", "x2"));
 		nlfg->push_back(f2);
 
 		// measurement between x1 and l1
@@ -53,7 +54,7 @@ namespace gtsam {
 		Vector z3(2);
 		z3(0) = 0.;
 		z3(1) = -1.;
-		shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
+		shared f3(new simulated2D::Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
 		nlfg->push_back(f3);
 
 		// measurement between x2 and l1
@@ -61,7 +62,7 @@ namespace gtsam {
 		Vector z4(2);
 		z4(0) = -1.5;
 		z4(1) = -1.;
-		shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
+		shared f4(new simulated2D::Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
 		nlfg->push_back(f4);
 
 		return nlfg;
@@ -173,16 +174,35 @@ namespace gtsam {
 	// Some nonlinear functions to optimize
 	/* ************************************************************************* */
 	namespace smallOptimize {
+
 		Vector h(const Vector& v) {
 			double x = v(0);
 			return Vector_(2, cos(x), sin(x));
 		}
-		;
+
 		Matrix H(const Vector& v) {
 			double x = v(0);
 			return Matrix_(2, 1, -sin(x), cos(x));
 		}
-		;
+
+		struct UnaryFactor: public gtsam::NonlinearFactor1<VectorConfig,
+				std::string, Vector> {
+
+			Vector z_;
+
+			UnaryFactor(const Vector& z, double sigma, const std::string& key) :
+				gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key),
+						z_(z) {
+			}
+
+			Vector evaluateError(const Vector& x, boost::optional<Matrix&> A =
+					boost::none) const {
+				if (A) *A = H(x);
+				return h(x) - z_;
+			}
+
+		};
+
 	}
 
 	/* ************************************************************************* */
@@ -191,8 +211,8 @@ namespace gtsam {
 				new ExampleNonlinearFactorGraph);
 		Vector z = Vector_(2, 1.0, 0.0);
 		double sigma = 0.1;
-		boost::shared_ptr<NonlinearFactor1> factor(new NonlinearFactor1(z, sigma,
+		boost::shared_ptr<smallOptimize::UnaryFactor> factor(new smallOptimize::UnaryFactor(
-				&smallOptimize::h, "x", &smallOptimize::H));
+				z, sigma, "x"));
 		fg->push_back(factor);
 		return fg;
 	}
@@ -214,7 +234,7 @@ namespace gtsam {
 		// prior on x1
 		Vector x1 = Vector_(2, 1.0, 0.0);
 		string key1 = symbol('x', 1);
-		shared prior(new Point2Prior(x1, sigma1, key1));
+		shared prior(new simulated2D::Point2Prior(x1, sigma1, key1));
 		nlfg.push_back(prior);
 		poses.insert(key1, x1);
 
@@ -222,13 +242,13 @@ namespace gtsam {
 			// odometry between x_t and x_{t-1}
 			Vector odo = Vector_(2, 1.0, 0.0);
 			string key = symbol('x', t);
-			shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
+			shared odometry(new simulated2D::Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
 					key));
 			nlfg.push_back(odometry);
 
 			// measurement on x_t is like perfect GPS
 			Vector xt = Vector_(2, (double) t, 0.0);
-			shared measurement(new Point2Prior(xt, sigma1, key));
+			shared measurement(new simulated2D::Point2Prior(xt, sigma1, key));
 			nlfg.push_back(measurement);
 
 			// initial estimate
@@ -509,7 +529,7 @@ namespace gtsam {
 
 		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
 		double sigma0 = 1e-3;
-		shared constraint(new Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
+		shared constraint(new simulated2D::Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
 		nlfg.push_back(constraint);
 
 		double sigma = 0.01;
@@ -518,7 +538,7 @@ namespace gtsam {
 		Vector z1 = Vector_(2, 1.0, 0.0); // move right
 		for (size_t x = 1; x < N; x++)
 			for (size_t y = 1; y <= N; y++) {
-				shared f(new Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
+				shared f(new simulated2D::Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
 				nlfg.push_back(f);
 			}
 
@@ -526,7 +546,7 @@ namespace gtsam {
 		Vector z2 = Vector_(2, 0.0, 1.0); // move up
 		for (size_t x = 1; x <= N; x++)
 			for (size_t y = 1; y < N; y++) {
-				shared f(new Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
+				shared f(new simulated2D::Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
 				nlfg.push_back(f);
 			}
 

cpp/testGaussianFactorGraph.cpp

@@ -730,7 +730,7 @@ TEST( GaussianFactorGraph, constrained_multi2 )
 	CHECK(assert_equal(expected, actual));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( GaussianFactorGraph, findMinimumSpanningTree )
 {
 	GaussianFactorGraph g;
@@ -750,7 +750,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree )
 	CHECK(tree["x4"].compare("x1")==0);
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( GaussianFactorGraph, split )
 {
 	GaussianFactorGraph g;

cpp/testGraph.cpp

@@ -13,6 +13,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include "Pose2Graph.h"
+#include "LieConfig-inl.h"
 #include "graph-inl.h"
 
 using namespace std;
@@ -24,22 +25,23 @@ TEST( Graph, composePoses )
 {
 	Pose2Graph graph;
 	Matrix cov = eye(3);
-	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov)));
+	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(1,2, Pose2(2.0, 0.0, 0.0), cov)));
-	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov)));
+	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(2,3, Pose2(3.0, 0.0, 0.0), cov)));
 
-	map<string, string> tree;
+	PredecessorMap<Pose2Config::Key> tree;
-	tree.insert(make_pair("x1", "x2"));
+	tree.insert(make_pair(1,2));
-	tree.insert(make_pair("x2", "x2"));
+	tree.insert(make_pair(2,2));
-	tree.insert(make_pair("x3", "x2"));
+	tree.insert(make_pair(3,2));
 
 	Pose2 rootPose(3.0, 0.0, 0.0);
 
-	boost::shared_ptr<Pose2Config> actual = composePoses<Pose2Graph, Pose2Factor, Pose2, Pose2Config>
+	boost::shared_ptr<Pose2Config> actual = composePoses<Pose2Graph, Pose2Factor,
-		(graph, tree, rootPose);
+			Pose2, Pose2Config> (graph, tree, rootPose);
+
 	Pose2Config expected;
-	expected.insert("x1", Pose2(1.0, 0.0, 0.0));
+	expected.insert(1, Pose2(1.0, 0.0, 0.0));
-	expected.insert("x2", Pose2(3.0, 0.0, 0.0));
+	expected.insert(2, Pose2(3.0, 0.0, 0.0));
-	expected.insert("x3", Pose2(6.0, 0.0, 0.0));
+	expected.insert(3, Pose2(6.0, 0.0, 0.0));
 
 	LONGS_EQUAL(3, actual->size());
 	CHECK(assert_equal(expected, *actual));

cpp/testLieConfig.cpp

@@ -18,10 +18,10 @@ using namespace std;
 /* ************************************************************************* */
 TEST( LieConfig, equals1 )
 {
-  LieConfig<Vector> expected;
+  LieConfig<string,Vector> expected;
   Vector v = Vector_(3, 5.0, 6.0, 7.0);
   expected.insert("a",v);
-  LieConfig<Vector> actual;
+  LieConfig<string,Vector> actual;
   actual.insert("a",v);
   CHECK(assert_equal(expected,actual));
 }
@@ -29,7 +29,7 @@ TEST( LieConfig, equals1 )
 /* ************************************************************************* */
 TEST( LieConfig, equals2 )
 {
-  LieConfig<Vector> cfg1, cfg2;
+  LieConfig<string,Vector> cfg1, cfg2;
   Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
   Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
   cfg1.insert("x", v1);
@@ -41,7 +41,7 @@ TEST( LieConfig, equals2 )
 /* ************************************************************************* */
 TEST( LieConfig, equals_nan )
 {
-  LieConfig<Vector> cfg1, cfg2;
+  LieConfig<string,Vector> cfg1, cfg2;
   Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
   Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0);
   cfg1.insert("x", v1);
@@ -53,7 +53,7 @@ TEST( LieConfig, equals_nan )
 /* ************************************************************************* */
 TEST(LieConfig, expmap_a)
 {
-  LieConfig<Vector> config0;
+  LieConfig<string,Vector> config0;
   config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
   config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
 
@@ -61,7 +61,7 @@ TEST(LieConfig, expmap_a)
   increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2));
   increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5));
 
-  LieConfig<Vector> expected;
+  LieConfig<string,Vector> expected;
   expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2));
   expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
 
@@ -71,14 +71,14 @@ TEST(LieConfig, expmap_a)
 /* ************************************************************************* */
 TEST(LieConfig, expmap_b)
 {
-  LieConfig<Vector> config0;
+  LieConfig<string,Vector> config0;
   config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
   config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
 
   VectorConfig increment;
   increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5));
 
-  LieConfig<Vector> expected;
+  LieConfig<string,Vector> expected;
   expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
   expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
 
@@ -88,7 +88,7 @@ TEST(LieConfig, expmap_b)
 /* ************************************************************************* */
 TEST(LieConfig, expmap_c)
 {
-  LieConfig<Vector> config0;
+  LieConfig<string,Vector> config0;
   config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
   config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
 
@@ -96,7 +96,7 @@ TEST(LieConfig, expmap_c)
       1.0, 1.1, 1.2,
       1.3, 1.4, 1.5);
 
-  LieConfig<Vector> expected;
+  LieConfig<string,Vector> expected;
   expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2));
   expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
 
@@ -106,14 +106,14 @@ TEST(LieConfig, expmap_c)
 /* ************************************************************************* */
 TEST(LieConfig, expmap_d)
 {
-  LieConfig<Vector> config0;
+  LieConfig<string,Vector> config0;
   config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
   config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
 
-  LieConfig<Pose2> poseconfig;
+  LieConfig<string,Pose2> poseconfig;
   poseconfig.insert("p1", Pose2(1,2,3));
   poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
   //poseconfig.print("poseconfig");

cpp/testNonlinearEquality.cpp

@@ -10,27 +10,22 @@
 using namespace std;
 using namespace gtsam;
 
-typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_nle;
+typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
+typedef boost::shared_ptr<NLE> shared_nle;
 
-bool vector_compare(const std::string& key,
+bool vector_compare(const Vector& a, const Vector& b) {
-					const VectorConfig& feasible,
+	return equal_with_abs_tol(a, b, 1e-5);
-					const VectorConfig& input) {
-	Vector feas, lin;
-	feas = feasible[key];
-	lin = input[key];
-	return equal_with_abs_tol(lin, feas, 1e-5);
 }
 
 /* ************************************************************************* */
 TEST ( NonlinearEquality, linearization ) {
 	string key = "x";
 	Vector value = Vector_(2, 1.0, 2.0);
-	VectorConfig feasible, linearize;
+	VectorConfig linearize;
-	feasible.insert(key, value);
 	linearize.insert(key, value);
 
 	// create a nonlinear equality constraint
-	shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
+	shared_nle nle(new NLE(key, value,vector_compare));
 
 	// check linearize
 	GaussianFactor expLF(key, eye(2), zero(2), 0.0);
@@ -38,17 +33,16 @@ TEST ( NonlinearEquality, linearization ) {
 	CHECK(assert_equal(*actualLF, expLF));
 }
 
-/* ************************************************************************* */
+/* ********************************************************************** */
 TEST ( NonlinearEquality, linearization_fail ) {
 	string key = "x";
 	Vector value = Vector_(2, 1.0, 2.0);
 	Vector wrong = Vector_(2, 3.0, 4.0);
-	VectorConfig feasible, bad_linearize;
+	VectorConfig bad_linearize;
-	feasible.insert(key, value);
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
-	shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
+	shared_nle nle(new NLE(key, value,vector_compare));
 
 	// check linearize to ensure that it fails for bad linearization points
 	try {
@@ -69,7 +63,7 @@ TEST ( NonlinearEquality, error ) {
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
-	shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
+	shared_nle nle(new NLE(key, value,vector_compare));
 
 	// check error function outputs
 	Vector actual = nle->error_vector(feasible);
@@ -84,23 +78,16 @@ TEST ( NonlinearEquality, equals ) {
 	string key1 = "x";
 	Vector value1 = Vector_(2, 1.0, 2.0);
 	Vector value2 = Vector_(2, 3.0, 4.0);
-	VectorConfig feasible1, feasible2, feasible3;
-	feasible1.insert(key1, value1);
-	feasible2.insert(key1, value2);
-	feasible3.insert(key1, value1);
-	feasible3.insert("y", value2);
 
 	// create some constraints to compare
-	shared_nle nle1(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
+	shared_nle nle1(new NLE(key1, value1,vector_compare));
-	shared_nle nle2(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
+	shared_nle nle2(new NLE(key1, value1,vector_compare));
-	shared_nle nle3(new NonlinearEquality<VectorConfig>(key1, feasible2, 2, *vector_compare));
+	shared_nle nle3(new NLE(key1, value2,vector_compare));
-	shared_nle nle4(new NonlinearEquality<VectorConfig>(key1, feasible3, 2, *vector_compare));
 
 	// verify
 	CHECK(nle1->equals(*nle2));  // basic equality = true
 	CHECK(nle2->equals(*nle1));  // test symmetry of equals()
 	CHECK(!nle1->equals(*nle3)); // test config
-	CHECK(nle4->equals(*nle1));  // test the full feasible set isn't getting compared
 }
 
 

cpp/testNonlinearFactor.cpp

@@ -27,12 +27,12 @@ TEST( NonlinearFactor, equals )
 	double sigma = 1.0;
 
 	// create two nonlinear2 factors
-	Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
+	Vector z3 = Vector_(2,0.,-1.);
-	Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
+	simulated2D::Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
 
 	// measurement between x2 and l1
-	Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
+	Vector z4 = Vector_(2,-1.5, -1.);
-	Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
+	simulated2D::Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
 
 	CHECK(assert_equal(f0,f0));
 	CHECK(f0.equals(f0));
@@ -68,7 +68,7 @@ TEST( NonlinearFactor, NonlinearFactor )
 
   // calculate the error_vector from the factor "f1"
   Vector actual_e = factor->error_vector(cfg);
-  Vector e(2); e(0) = -0.1;  e(1) = -0.1;
+  Vector e(2); e(0) = 0.1;  e(1) = 0.1;
   CHECK(assert_equal(e,actual_e));
 
   // the expected value for the error from the factor
@@ -81,7 +81,7 @@ TEST( NonlinearFactor, NonlinearFactor )
   DOUBLES_EQUAL(expected,actual,0.00000001);
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( NonlinearFactor, linearize_f1 )
 {
   // Grab a non-linear factor
@@ -103,7 +103,7 @@ TEST( NonlinearFactor, linearize_f1 )
 	CHECK(assert_equal(nlf->error_vector(c),actual->get_b()));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( NonlinearFactor, linearize_f2 )
 {
   // Grab a non-linear factor
@@ -121,7 +121,7 @@ TEST( NonlinearFactor, linearize_f2 )
   CHECK(expected->equals(*actual));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( NonlinearFactor, linearize_f3 )
 {
   // Grab a non-linear factor
@@ -139,7 +139,7 @@ TEST( NonlinearFactor, linearize_f3 )
   CHECK(expected->equals(*actual));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( NonlinearFactor, linearize_f4 )
 {
   // Grab a non-linear factor

cpp/testOrdering.cpp

@@ -11,11 +11,11 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 // x1 -> x2
 //		-> x3 -> x4
 //    -> x5
-TEST ( Ordering, constructor ) {
+TEST ( Ordering, constructorFromTree ) {
 	map<string, string> p_map;
 	p_map.insert(make_pair("x1", "x1"));
 	p_map.insert(make_pair("x2", "x1"));

cpp/testPose2Config.cpp

@@ -16,12 +16,12 @@ TEST( Pose2Config, pose2Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
 	Pose2Config expected;
-	expected.insert("p0", Pose2( 1,  0,   M_PI_2));
+	expected.insert(0, Pose2( 1,  0,   M_PI_2));
-	expected.insert("p1", Pose2( 0,  1, - M_PI  ));
+	expected.insert(1, Pose2( 0,  1, - M_PI  ));
-	expected.insert("p2", Pose2(-1,  0, - M_PI_2));
+	expected.insert(2, Pose2(-1,  0, - M_PI_2));
-	expected.insert("p3", Pose2( 0, -1,   0     ));
+	expected.insert(3, Pose2( 0, -1,   0     ));
 
-	Pose2Config actual = pose2Circle(4,1.0,'p');
+	Pose2Config actual = pose2Circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -30,14 +30,14 @@ TEST( Pose2Config, expmap )
 {
 	// expected is circle shifted to right
 	Pose2Config expected;
-	expected.insert("p0", Pose2( 1.1,  0,   M_PI_2));
+	expected.insert(0, Pose2( 1.1,  0,   M_PI_2));
-	expected.insert("p1", Pose2( 0.1,  1, - M_PI  ));
+	expected.insert(1, Pose2( 0.1,  1, - M_PI  ));
-	expected.insert("p2", Pose2(-0.9,  0, - M_PI_2));
+	expected.insert(2, Pose2(-0.9,  0, - M_PI_2));
-	expected.insert("p3", Pose2( 0.1, -1,   0     ));
+	expected.insert(3, Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
 	Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0);
-	Pose2Config actual = expmap(pose2Circle(4,1.0,'p'),delta);
+	Pose2Config actual = expmap(pose2Circle(4,1.0),delta);
 	CHECK(assert_equal(expected,actual));
 }
 

cpp/testPose2Factor.cpp

@@ -20,43 +20,43 @@ static Matrix covariance = Matrix_(3,3,
 		);
 
 /* ************************************************************************* */
-// Very simple test establishing Ax-b \approx h(x)-z
+// Very simple test establishing Ax-b \approx z-h(x)
 TEST( Pose2Factor, error )
 {
 	// Choose a linearization point
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
 	Pose2Config x0;
-	x0.insert("p1", p1);
+	x0.insert(1, p1);
-	x0.insert("p2", p2);
+	x0.insert(2, p2);
 
 	// Create factor
 	Pose2 z = between(p1,p2);
-	Pose2Factor factor("p1", "p2", z, covariance);
+	Pose2Factor factor(1, 2, z, covariance);
 
 	// Actual linearization
 	boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
 
-	// Check error at x0 = zero !
+	// Check error at x0, i.e. delta = zero !
 	VectorConfig delta;
-	delta.insert("p1", zero(3));
+	delta.insert("x1", zero(3));
-	delta.insert("p2", zero(3));
+	delta.insert("x2", zero(3));
 	Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
 	CHECK(assert_equal(error_at_zero,factor.error_vector(x0)));
 	CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
 
 	// Check error after increasing p2
-	VectorConfig plus = delta + VectorConfig("p2", Vector_(3, 0.1, 0.0, 0.0));
+	VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0));
 	Pose2Config x1 = expmap(x0, plus);
-	Vector error_at_plus = Vector_(3,-0.1/sx,0.0,0.0);
+	Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
 	CHECK(assert_equal(error_at_plus,factor.error_vector(x1)));
-	CHECK(assert_equal(-error_at_plus,linear->error_vector(plus)));
+	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
 }
 
 /* ************************************************************************* */
 // common Pose2Factor for tests below
 static Pose2 measured(2,2,M_PI_2);
-static Pose2Factor factor("p1","p2",measured, covariance);
+static Pose2Factor factor(1,2,measured, covariance);
 
 /* ************************************************************************* */
 TEST( Pose2Factor, rhs )
@@ -65,8 +65,8 @@ TEST( Pose2Factor, rhs )
 	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
 	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
 	Pose2Config x0;
-	x0.insert("p1",p1);
+	x0.insert(1,p1);
-	x0.insert("p2",p2);
+	x0.insert(2,p2);
 
 	// Actual linearization
 	boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
@@ -75,7 +75,7 @@ TEST( Pose2Factor, rhs )
 	Pose2 hx0 = between(p1,p2);
 	CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0));
 	Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
-	CHECK(assert_equal(expected_b,factor.error_vector(x0)));
+	CHECK(assert_equal(expected_b,-factor.error_vector(x0)));
 	CHECK(assert_equal(expected_b,linear->get_b()));
 }
 
@@ -83,10 +83,7 @@ TEST( Pose2Factor, rhs )
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
 // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
 Vector h(const Pose2& p1,const Pose2& p2) {
-	Pose2Config x;
+	return factor.evaluateError(p1,p2);
-	x.insert("p1",p1);
-	x.insert("p2",p2);
-	return -factor.error_vector(x);
 }
 
 /* ************************************************************************* */
@@ -96,8 +93,8 @@ TEST( Pose2Factor, linearize )
 	Pose2 p1(1,2,M_PI_2);
 	Pose2 p2(-1,4,M_PI);
 	Pose2Config x0;
-	x0.insert("p1",p1);
+	x0.insert(1,p1);
-	x0.insert("p2",p2);
+	x0.insert(2,p2);
 
 	// expected linearization
 	Matrix square_root_inverse_covariance = Matrix_(3,3,
@@ -118,7 +115,7 @@ TEST( Pose2Factor, linearize )
 	Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
 
 	// expected linear factor
-	GaussianFactor expected("p1", expectedH1, "p2", expectedH2, expected_b, 1.0);
+	GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, 1.0);
 
 	// Actual linearization
 	boost::shared_ptr<GaussianFactor> actual = factor.linearize(x0);

cpp/testPose2Graph.cpp

@@ -32,9 +32,9 @@ TEST( Pose2Graph, constructor )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 measured(2,2,M_PI_2);
-	Pose2Factor constraint("x1","x2",measured, covariance);
+	Pose2Factor constraint(1,2,measured, covariance);
 	Pose2Graph graph;
-	graph.add("x1","x2",measured, covariance);
+	graph.add(1,2,measured, covariance);
 	// get the size of the graph
 	size_t actual = graph.size();
 	// verify
@@ -48,16 +48,16 @@ TEST( Pose2Graph, linerization )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 measured(2,2,M_PI_2);
-	Pose2Factor constraint("x1","x2",measured, covariance);
+	Pose2Factor constraint(1,2,measured, covariance);
 	Pose2Graph graph;
-	graph.add("x1","x2",measured, covariance);
+	graph.add(1,2,measured, covariance);
 
 	// Choose a linearization point
 	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
 	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
 	Pose2Config config;
-	config.insert("x1",p1);
+	config.insert(1,p1);
-	config.insert("x2",p2);
+	config.insert(2,p2);
 	// Linearize
 	GaussianFactorGraph lfg_linearized = graph.linearize(config);
 	//lfg_linearized.print("lfg_actual");
@@ -86,17 +86,17 @@ TEST(Pose2Graph, optimize) {
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose2Graph> fg(new Pose2Graph);
-  fg->addConstraint("p0", Pose2(0,0,0));
+  fg->addConstraint(0, Pose2(0,0,0));
-  fg->add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
+  fg->add(0, 1, Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
   boost::shared_ptr<Pose2Config> initial(new Pose2Config());
-  initial->insert("p0", Pose2(0,0,0));
+  initial->insert(0, Pose2(0,0,0));
-  initial->insert("p1", Pose2(0,0,0));
+  initial->insert(1, Pose2(0,0,0));
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "p0","p1";
+  *ordering += "x0","x1";
   typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
   Optimizer optimizer0(fg, ordering, initial);
   Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
@@ -105,8 +105,8 @@ TEST(Pose2Graph, optimize) {
 
   // Check with expected config
   Pose2Config expected;
-  expected.insert("p0", Pose2(0,0,0));
+  expected.insert(0, Pose2(0,0,0));
-  expected.insert("p1", Pose2(1,2,M_PI_2));
+  expected.insert(1, Pose2(1,2,M_PI_2));
   CHECK(assert_equal(expected, *optimizer.config()));
 }
 
@@ -115,32 +115,32 @@ TEST(Pose2Graph, optimize) {
 TEST(Pose2Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
-	Pose2Config hexagon = pose2Circle(6,1.0,'p');
+	Pose2Config hexagon = pose2Circle(6,1.0);
-  Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"];
+  Pose2 p0 = hexagon[0], p1 = hexagon[1];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose2Graph> fg(new Pose2Graph);
-  fg->addConstraint("p0", p0);
+  fg->addConstraint(0, p0);
   Pose2 delta = between(p0,p1);
-  fg->add("p0", "p1", delta, covariance);
+  fg->add(0, 1, delta, covariance);
-  fg->add("p1", "p2", delta, covariance);
+  fg->add(1,2, delta, covariance);
-  fg->add("p2", "p3", delta, covariance);
+  fg->add(2,3, delta, covariance);
-  fg->add("p3", "p4", delta, covariance);
+  fg->add(3,4, delta, covariance);
-  fg->add("p4", "p5", delta, covariance);
+  fg->add(4,5, delta, covariance);
-  fg->add("p5", "p0", delta, covariance);
+  fg->add(5, 0, delta, covariance);
 
   // Create initial config
   boost::shared_ptr<Pose2Config> initial(new Pose2Config());
-  initial->insert("p0", p0);
+  initial->insert(0, p0);
-  initial->insert("p1", expmap(hexagon["p1"],Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(1, expmap(hexagon[1],Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert("p2", expmap(hexagon["p2"],Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(2, expmap(hexagon[2],Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert("p3", expmap(hexagon["p3"],Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(3, expmap(hexagon[3],Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert("p4", expmap(hexagon["p4"],Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(4, expmap(hexagon[4],Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(5, expmap(hexagon[5],Vector_(3,-0.1, 0.1,-0.1)));
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "p0","p1","p2","p3","p4","p5";
+  *ordering += "x0","x1","x2","x3","x4","x5";
   typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
   Optimizer optimizer0(fg, ordering, initial);
   Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
@@ -153,7 +153,7 @@ TEST(Pose2Graph, optimizeCircle) {
   CHECK(assert_equal(hexagon, actual));
 
   // Check loop closure
-  CHECK(assert_equal(delta,between(actual["p5"],actual["p0"])));
+  CHECK(assert_equal(delta,between(actual[5],actual[0])));
 }
 
 /* ************************************************************************* */

cpp/testPose3Config.cpp

@@ -23,12 +23,12 @@ TEST( Pose3Config, pose3Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
 	Pose3Config expected;
-	expected.insert("p0", Pose3(R1, Point3( 1, 0, 0)));
+	expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
-	expected.insert("p1", Pose3(R2, Point3( 0, 1, 0)));
+	expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
-	expected.insert("p2", Pose3(R3, Point3(-1, 0, 0)));
+	expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
-	expected.insert("p3", Pose3(R4, Point3( 0,-1, 0)));
+	expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
 
-	Pose3Config actual = pose3Circle(4,1.0,'p');
+	Pose3Config actual = pose3Circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -37,10 +37,10 @@ TEST( Pose3Config, expmap )
 {
 	// expected is circle shifted to East
 	Pose3Config expected;
-	expected.insert("p0", Pose3(R1, Point3( 1.1, 0, 0)));
+	expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0)));
-	expected.insert("p1", Pose3(R2, Point3( 0.1, 1, 0)));
+	expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0)));
-	expected.insert("p2", Pose3(R3, Point3(-0.9, 0, 0)));
+	expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0)));
-	expected.insert("p3", Pose3(R4, Point3( 0.1,-1, 0)));
+	expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0)));
 
 	// Note expmap coordinates are in global coordinates with non-compose expmap
 	// so shifting to East requires little thought, different from with Pose2 !!!
@@ -49,7 +49,7 @@ TEST( Pose3Config, expmap )
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0);
-	Pose3Config actual = expmap(pose3Circle(4,1.0,'p'),delta);
+	Pose3Config actual = expmap(pose3Circle(4,1.0),delta);
 	CHECK(assert_equal(expected,actual));
 }
 

cpp/testPose3Factor.cpp

@@ -25,16 +25,16 @@ TEST( Pose3Factor, error )
 
 	// Create factor
 	Matrix measurement_covariance = eye(6);
-	Pose3Factor factor("t1", "t2", z, measurement_covariance);
+	Pose3Factor factor(1,2, z, measurement_covariance);
 
 	// Create config
 	Pose3Config x;
-	x.insert("t1",t1);
+	x.insert(1,t1);
-	x.insert("t2",t2);
+	x.insert(2,t2);
 
-	// Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z)
+	// Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2))
 	Vector actual = factor.error_vector(x);
-	Vector expected = logmap(between(t1,t2),z);
+	Vector expected = logmap(z,between(t1,t2));
 	CHECK(assert_equal(expected,actual));
 }
 

cpp/testPose3Graph.cpp

@@ -13,10 +13,10 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#include "Pose3Graph.h"
 #include "NonlinearOptimizer-inl.h"
 #include "NonlinearEquality.h"
 #include "Ordering.h"
-#include "Pose3Graph.h"
 
 using namespace std;
 using namespace gtsam;
@@ -29,32 +29,32 @@ static Matrix covariance = eye(6);
 TEST(Pose3Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
-	Pose3Config hexagon = pose3Circle(6,1.0,'p');
+	Pose3Config hexagon = pose3Circle(6,1.0);
-  Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"];
+  Pose3 p0 = hexagon[0], p1 = hexagon[1];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose3Graph> fg(new Pose3Graph);
-  fg->addConstraint("p0", p0);
+  fg->addConstraint(0, p0);
   Pose3 delta = between(p0,p1);
-  fg->add("p0", "p1", delta, covariance);
+  fg->add(0,1, delta, covariance);
-  fg->add("p1", "p2", delta, covariance);
+  fg->add(1,2, delta, covariance);
-  fg->add("p2", "p3", delta, covariance);
+  fg->add(2,3, delta, covariance);
-  fg->add("p3", "p4", delta, covariance);
+  fg->add(3,4, delta, covariance);
-  fg->add("p4", "p5", delta, covariance);
+  fg->add(4,5, delta, covariance);
-  fg->add("p5", "p0", delta, covariance);
+  fg->add(5,0, delta, covariance);
 
   // Create initial config
   boost::shared_ptr<Pose3Config> initial(new Pose3Config());
-  initial->insert("p0", p0);
+  initial->insert(0, p0);
-  initial->insert("p1", expmap(hexagon["p1"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(1, expmap(hexagon[1],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert("p2", expmap(hexagon["p2"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(2, expmap(hexagon[2],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert("p3", expmap(hexagon["p3"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(3, expmap(hexagon[3],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert("p4", expmap(hexagon["p4"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(4, expmap(hexagon[4],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(5, expmap(hexagon[5],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "p0","p1","p2","p3","p4","p5";
+  *ordering += "x0","x1","x2","x3","x4","x5";
   typedef NonlinearOptimizer<Pose3Graph, Pose3Config> Optimizer;
   Optimizer optimizer0(fg, ordering, initial);
   Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
@@ -67,7 +67,7 @@ TEST(Pose3Graph, optimizeCircle) {
   CHECK(assert_equal(hexagon, actual,1e-5));
 
   // Check loop closure
-  CHECK(assert_equal(delta,between(actual["p5"],actual["p0"]),1e-5));
+  CHECK(assert_equal(delta,between(actual[5],actual[0]),1e-5));
 }
 
 /* ************************************************************************* */

cpp/testSQP.cpp

@@ -242,19 +242,16 @@ TEST (SQP, problem1_sqp ) {
 
 /* ********************************************************************* */
 // components for nonlinear factor graphs
-bool vector_compare(const std::string& key,
+bool vector_compare(const Vector& a, const Vector& b) {
-					const VectorConfig& feasible,
+	return equal_with_abs_tol(a, b, 1e-5);
-					const VectorConfig& input) {
-	Vector feas, lin;
-	feas = feasible[key];
-	lin = input[key];
-	return equal_with_abs_tol(lin, feas, 1e-5);
 }
 
 typedef NonlinearFactorGraph<VectorConfig> NLGraph;
 typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
 typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
-typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_eq;
+
+typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
+typedef boost::shared_ptr<NLE> shared_eq;
 typedef boost::shared_ptr<VectorConfig> shared_cfg;
 typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
 
@@ -268,24 +265,26 @@ TEST (SQP, two_pose_truth ) {
 	// position (1, 1) constraint for x1
 	// position (5, 6) constraint for x2
 	VectorConfig feas;
-	feas.insert("x1", Vector_(2, 1.0, 1.0));
+	Vector feas1 = Vector_(2, 1.0, 1.0);
-	feas.insert("x2", Vector_(2, 5.0, 6.0));
+	Vector feas2 = Vector_(2, 5.0, 6.0);
+	feas.insert("x1", feas1);
+	feas.insert("x2", feas2);
 
 	// constant constraint on x1
-	shared_eq ef1(new NonlinearEquality<VectorConfig>("x1", feas, 2, *vector_compare));
+	shared_eq ef1(new NLE("x1", feas1, vector_compare));
 
 	// constant constraint on x2
-	shared_eq ef2(new NonlinearEquality<VectorConfig>("x2", feas, 2, *vector_compare));
+	shared_eq ef2(new NLE("x2", feas2, vector_compare));
 
 	// measurement from x1 to l1
 	Vector z1 = Vector_(2, 0.0, 5.0);
 	double sigma1 = 0.1;
-	shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
+	shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
 
 	// measurement from x2 to l1
 	Vector z2 = Vector_(2, -4.0, 0.0);
 	double sigma2 = 0.1;
-	shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
+	shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
 
 	// construct the graph
 	shared_ptr<NLGraph> graph(new NLGraph());
@@ -377,12 +376,12 @@ TEST (SQP, two_pose ) {
 	// measurement from x1 to l1
 	Vector z1 = Vector_(2, 0.0, 5.0);
 	double sigma1 = 0.1;
-	shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
+	shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
 
 	// measurement from x2 to l2
 	Vector z2 = Vector_(2, -4.0, 0.0);
 	double sigma2 = 0.1;
-	shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
+	shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
 
 	// equality constraint between l1 and l2
 	list<string> keys2; keys2 += "l1", "l2";

cpp/testSQPOptimizer.cpp

@@ -102,12 +102,12 @@ NLGraph linearMapWarpGraph() {
 	// measurement from x1 to l1
 	Vector z1 = Vector_(2, 0.0, 5.0);
 	double sigma1 = 0.1;
-	shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
+	shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
 
 	// measurement from x2 to l2
 	Vector z2 = Vector_(2, -4.0, 0.0);
 	double sigma2 = 0.1;
-	shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
+	shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
 
 	// equality constraint between l1 and l2
 	list<string> keys; keys += "l1", "l2";
@@ -208,20 +208,15 @@ TEST ( SQPOptimizer, map_warp ) {
 // states, which enforces a minimum distance.
 /* ********************************************************************* */
 
-bool vector_compare(const std::string& key,
+bool vector_compare(const Vector& a, const Vector& b) {
-					const VectorConfig& feasible,
+	return equal_with_abs_tol(a, b, 1e-5);
-					const VectorConfig& input) {
-	Vector feas, lin;
-	feas = feasible[key];
-	lin = input[key];
-	return equal_with_abs_tol(lin, feas, 1e-5);
 }
 
 typedef NonlinearConstraint1<VectorConfig> NLC1;
 typedef boost::shared_ptr<NLC1> shared_NLC1;
 typedef NonlinearConstraint2<VectorConfig> NLC2;
 typedef boost::shared_ptr<NLC2> shared_NLC2;
-typedef NonlinearEquality<VectorConfig> NLE;
+typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
 typedef boost::shared_ptr<NLE> shared_NLE;
 
 namespace sqp_avoid1 {
@@ -255,22 +250,24 @@ Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
 pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
 	// fix start, end, obstacle positions
 	VectorConfig feasible;
-	feasible.insert("x1", Vector_(2, 0.0, 0.0));
+	Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 =
-	feasible.insert("x3", Vector_(2, 10.0, 0.0));
+			Vector_(2, 5.0, -0.5);
-	feasible.insert("obs", Vector_(2, 5.0, -0.5));
+	feasible.insert("x1", feas1);
-	shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare));
+	feasible.insert("x3", feas2);
-	shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare));
+	feasible.insert("obs", feas3);
-	shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare));
+	shared_NLE e1(new NLE("x1", feas1, vector_compare));
+	shared_NLE e2(new NLE("x3", feas2, vector_compare));
+	shared_NLE e3(new NLE("obs", feas3, vector_compare));
 
 	// measurement from x1 to x2
 	Vector x1x2 = Vector_(2, 5.0, 0.0);
 	double sigma1 = 0.1;
-	shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
+	shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
 
 	// measurement from x2 to x3
 	Vector x2x3 = Vector_(2, 5.0, 0.0);
 	double sigma2 = 0.1;
-	shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
+	shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
 
 	// create a binary inequality constraint that forces the middle point away from
 	//  the obstacle
@@ -385,22 +382,24 @@ Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
 pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
 	// fix start, end, obstacle positions
 	VectorConfig feasible;
-	feasible.insert("x1", Vector_(2, 0.0, 0.0));
+	Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 =
-	feasible.insert("x3", Vector_(2, 10.0, 0.0));
+			Vector_(2, 5.0, -0.5);
-	feasible.insert("obs", Vector_(2, 5.0, -0.5));
+	feasible.insert("x1", feas1);
-	shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare));
+	feasible.insert("x3", feas2);
-	shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare));
+	feasible.insert("obs", feas3);
-	shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare));
+	shared_NLE e1(new NLE("x1", feas1,vector_compare));
+	shared_NLE e2(new NLE("x3", feas2, vector_compare));
+	shared_NLE e3(new NLE("obs", feas3, vector_compare));
 
 	// measurement from x1 to x2
 	Vector x1x2 = Vector_(2, 5.0, 0.0);
 	double sigma1 = 0.1;
-	shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
+	shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
 
 	// measurement from x2 to x3
 	Vector x2x3 = Vector_(2, 5.0, 0.0);
 	double sigma2 = 0.1;
-	shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
+	shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
 
 	double radius = 1.0;
 

cpp/testSimulated2D.cpp

@@ -13,6 +13,7 @@
 
 using namespace gtsam;
 using namespace std;
+using namespace simulated2D;
 
 /* ************************************************************************* */
 TEST( simulated2D, Dprior )

cpp/testSimulated3D.cpp

@@ -12,14 +12,15 @@
 #include "Simulated3D.h"
 
 using namespace gtsam;
+using namespace simulated3D;
 
 /* ************************************************************************* */
-TEST( simulated3D, Dprior_3D )
+TEST( simulated3D, Dprior )
 {
 	Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
 	Vector v = logmap(x1);
-	Matrix numerical = numericalDerivative11(prior_3D,v);
+	Matrix numerical = numericalDerivative11(prior,v);
-	Matrix computed = Dprior_3D(v);
+	Matrix computed = Dprior(v);
 	CHECK(assert_equal(numerical,computed,1e-9));
 }
 
@@ -30,8 +31,8 @@ TEST( simulated3D, DOdo1 )
 	Vector v1 = logmap(x1);
 	Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
 	Vector v2 = logmap(x2);
-	Matrix numerical = numericalDerivative21(odo_3D,v1,v2);
+	Matrix numerical = numericalDerivative21(odo,v1,v2);
-	Matrix computed = Dodo1_3D(v1,v2);
+	Matrix computed = Dodo1(v1,v2);
 	CHECK(assert_equal(numerical,computed,1e-9));
 }
 
@@ -42,8 +43,8 @@ TEST( simulated3D, DOdo2 )
 	Vector v1 = logmap(x1);
 	Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
 	Vector v2 = logmap(x2);
-	Matrix numerical = numericalDerivative22(odo_3D,v1,v2);
+	Matrix numerical = numericalDerivative22(odo,v1,v2);
-	Matrix computed = Dodo2_3D(v1,v2);
+	Matrix computed = Dodo2(v1,v2);
 	CHECK(assert_equal(numerical,computed,1e-9));
 }
 

cpp/testVSLAMConfig.cpp

@@ -5,7 +5,8 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
-#include <VSLAMConfig.h>
+#include "VectorConfig.h"
+#include "VSLAMConfig.h"
 
 using namespace std;
 using namespace gtsam;
@@ -18,17 +19,17 @@ TEST( VSLAMConfig, update_with_large_delta) {
 	init.addCameraPose(1, Pose3());
 	init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0));
 
-	VectorConfig delta;
-	delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));
-	delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1));
-	delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
-
-	VSLAMConfig actual = expmap(init, delta);
 	VSLAMConfig expected;
 	expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
 	expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1));
 
-	CHECK(assert_equal(actual, expected));
+	VectorConfig delta;
+	delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));
+	delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1));
+	delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
+	VSLAMConfig actual = expmap(init, delta);
+
+	CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */

cpp/testVSLAMFactor.cpp

@@ -38,7 +38,7 @@ TEST( VSLAMFactor, error )
   VSLAMConfig config;
   Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1);
   Point3 l1;  config.addLandmarkPoint(1, l1);
-  CHECK(assert_equal(Vector_(2,320.,240.),factor->predict(config)));
+  CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1)));
 
   // Which yields an error of 3^2/2 = 4.5
   DOUBLES_EQUAL(4.5,factor->error(config),1e-9);