From 98a4d83735482457925dcd26488abbf0ad71d708 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 14 Jan 2012 22:24:06 +0000
Subject: [PATCH 02/88] (in branch) Added abstract Value base class

---
 gtsam/base/Makefile.am |   2 +-
 gtsam/base/Value.h     | 173 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 174 insertions(+), 1 deletion(-)
 create mode 100644 gtsam/base/Value.h

diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am
index f32de8eb3..ced6530f8 100644
--- a/gtsam/base/Makefile.am
+++ b/gtsam/base/Makefile.am
@@ -26,7 +26,7 @@ sources += timing.cpp debug.cpp
 check_PROGRAMS += tests/testDebug tests/testTestableAssertions
 
 # Manifolds and Lie Groups
-headers += Manifold.h Group.h
+headers += Value.h Manifold.h Group.h
 headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
 sources += LieVector.cpp
 check_PROGRAMS += tests/testLieVector tests/testLieScalar
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
new file mode 100644
index 000000000..220b3885a
--- /dev/null
+++ b/gtsam/base/Value.h
@@ -0,0 +1,173 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Value.h
+ * @brief The interface class for any variable that can be optimized or used in a factor.
+ * @author Richard Roberts
+ * @date Jan 14, 2012
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <gtsam/base/Vector.h>
+
+namespace gtsam {
+
+  /**
+   * This is the interface class for any value that may be used as a variable
+   * assignment in a factor graph, and which you must derive to create new
+   * variable types to use with gtsam.  Examples of built-in classes
+   * implementing this are mainly in geometry, including Rot3, Pose2, etc.
+   *
+   * This interface specifies pure virtual retract_ and localCoordinates_
+   * functions that work with pointers to this interface class.  When you
+   * implement these functions in the derived class, the objects behind these
+   * pointers must always be instances of the proper derived class:
+   * \code
+     // This is example code you would never write, but illustrates that
+     // consistent derived value instances are passed to and from retract
+     // and localCoordinates as base class (Value) pointers.
+
+     // This is a base class pointer that is actually a Rot3:
+     auto_ptr<Value> value = new Rot3();
+
+     // The retract implementation must always returns a Rot3 instance as a
+     // base class pointer, i.e. this code must run successfully:
+     auto_ptr<Value> retracted = value->retract_(delta);
+     Rot3* rot3retracted = dynamic_cast<Rot3*>(retracted.get());
+
+     // localCoordinates will always be passed a derived class instance as
+     // a base class pointer:
+     Vector coordinates = value->localCoordinates_(retracted);
+     \endcode
+   *
+   * The reason we have require functions is so that containers, such as
+   * Values can operate generically on Value objects, retracting or computing
+   * local coordinates for many Value objects of different types.
+   *
+   * When you implement retract_() and localCoordinates_(), we suggest first
+   * implementing versions of these functions that work directly with derived
+   * objects, then using the provided helper functions to implement the
+   * generic Value versions.  This makes your implementation easier, and also
+   * improves performance in situations where the derived type is in fact
+   * known, such as in most implementations of \c evaluateError() in classes
+   * derived from NonlinearFactor.
+   *
+   * Using the above practice, here is an example of implementing a typical
+   * class derived from Value:
+   * \code
+     class Rot3 : public Value {
+     public:
+       // Constructor, there is never a need to call the Value base class constructor.
+       Rot3() { ... }
+
+       // Tangent space dimensionality (virtual, overrides Value::dim())
+       virtual size_t dim() cosnt {
+         return 3;
+       }
+
+       // retract working directly with Rot3 objects (non-virtual, non-overriding!)
+       Rot3 retract(const Vector& delta) const {
+         // Math to implement a 3D rotation retraction e.g. exponential map
+         return Rot3(result);
+       }
+
+       // localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!)
+       Vector localCoordinates(const Rot3& r2) const {
+         // Math to implement 3D rotation localCoordinates, e.g. logarithm map
+         return Vector(result);
+       }
+
+       // retract implementing the generic Value interface (virtual, overrides Value::retract())
+       virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
+         // Call our provided helper function to call your Rot3-specific
+         // retract and do the appropriate casting and allocation.
+         return CallDerivedRetract(this, delta);
+       }
+
+       // localCoordinates implementing the generic Value interface (virtual, overrides Value::localCoordinates())
+       virtual Vector localCoordinates_(const Value& value) const {
+         // Call our provided helper function to call your Rot3-specific
+         // localCoordinates and do the appropriate casting.
+         return CallDerivedLocalCoordinates(this, value);
+       }
+     };
+     \endcode
+   */
+  class Value {
+  public:
+    /** Return the dimensionality of the tangent space of this value.  This is
+     * the dimensionality of \c delta passed into retract() and of the vector
+     * returned by localCoordinates().
+     * @return The dimensionality of the tangent space
+     */
+    virtual size_t dim() const = 0;
+
+    /** Increment the value, by mapping from the vector delta in the tangent
+     * space of the current value back to the manifold to produce a new,
+     * incremented value.
+     * @param delta The delta vector in the tangent space of this value, by
+     * which to increment this value.
+     */
+    virtual std::auto_ptr<Value> retract_(const Vector& delta) const = 0;
+
+    /** Compute the coordinates in the tangent space of this value that
+     * retract() would map to \c value.
+     * @param value The value whose coordinates should be determined in the
+     * tangent space of the value on which this function is called.
+     * @return The coordinates of \c value in the tangent space of \c this.
+     */
+    virtual Vector localCoordinates_(const Value& value) const = 0;
+
+    /** Virutal destructor */
+    virtual ~Value() {}
+
+  protected:
+    /** This is a convenience function to make it easy for you to implement the
+     * generic Value inferface, see the example at the top of the Value
+     * documentation.
+     * @param derived A pointer to the derived class on which to call retract
+     * @param delta The delta vector to pass to the derived retract
+     * @return The result of retract on the derived class, stored as a Value pointer
+     */
+    template<class Derived>
+    static std::auto_ptr<Value> CallDerivedRetract(const Derived* derived, const Vector& delta) {
+      // Call retract on the derived class
+      const Derived retractResult = derived->retract(delta);
+
+      // Create a Value pointer copy of the result
+      std::auto_ptr<Value> resultAsValue(new Derived(retractResult));
+
+      // Return the pointer to the Value base class
+      return resultAsValue;
+    }
+
+    /** This is a convenience function to make it easy for you to implement the
+     * generic Value inferface, see the example at the top of the Value
+     * documentation.
+     * @param value1 The object on which to call localCoordinates, stored as a Value pointer
+     * @param value2 The argument to pass to the derived localCoordinates function
+     * @return The result of localCoordinates of the derived class
+     */
+    template<class Derived>
+    static Vector CallDerivedLocalCoordinates(const Derived* value1, const Value& value2) {
+      // Cast the base class Value pointer to a derived class pointer
+      const Derived& derivedValue2 = dynamic_cast<const Derived&>(value2);
+
+      // Return the result of calling localCoordinates on the derived class
+      return value1->localCoordinates(derivedValue2);
+    }
+  };
+
+} /* namespace gtsam */

From 402ab48df8c9146bcab764659e765be77699b148 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 14 Jan 2012 23:12:05 +0000
Subject: [PATCH 03/88] (in branch) Added print and equals and changed comments

---
 gtsam/base/Value.h | 88 +++++++++++++++++++++++++++-------------------
 1 file changed, 52 insertions(+), 36 deletions(-)

diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 220b3885a..3a4b56c72 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -30,39 +30,19 @@ namespace gtsam {
    * variable types to use with gtsam.  Examples of built-in classes
    * implementing this are mainly in geometry, including Rot3, Pose2, etc.
    *
-   * This interface specifies pure virtual retract_ and localCoordinates_
-   * functions that work with pointers to this interface class.  When you
-   * implement these functions in the derived class, the objects behind these
-   * pointers must always be instances of the proper derived class:
-   * \code
-     // This is example code you would never write, but illustrates that
-     // consistent derived value instances are passed to and from retract
-     // and localCoordinates as base class (Value) pointers.
-
-     // This is a base class pointer that is actually a Rot3:
-     auto_ptr<Value> value = new Rot3();
-
-     // The retract implementation must always returns a Rot3 instance as a
-     // base class pointer, i.e. this code must run successfully:
-     auto_ptr<Value> retracted = value->retract_(delta);
-     Rot3* rot3retracted = dynamic_cast<Rot3*>(retracted.get());
-
-     // localCoordinates will always be passed a derived class instance as
-     // a base class pointer:
-     Vector coordinates = value->localCoordinates_(retracted);
-     \endcode
-   *
-   * The reason we have require functions is so that containers, such as
+   * This interface specifies pure virtual retract_(), localCoordinates_() and
+   * equals_() functions that work with pointers and references to this interface
+   * class, i.e. the base class.  These functions allow containers, such as
    * Values can operate generically on Value objects, retracting or computing
    * local coordinates for many Value objects of different types.
    *
-   * When you implement retract_() and localCoordinates_(), we suggest first
-   * implementing versions of these functions that work directly with derived
-   * objects, then using the provided helper functions to implement the
-   * generic Value versions.  This makes your implementation easier, and also
-   * improves performance in situations where the derived type is in fact
-   * known, such as in most implementations of \c evaluateError() in classes
-   * derived from NonlinearFactor.
+   * When you implement retract_(), localCoordinates_(), and equals_(), we
+   * suggest first implementing versions of these functions that work directly
+   * with derived objects, then using the provided helper functions to
+   * implement the generic Value versions.  This makes your implementation
+   * easier, and also improves performance in situations where the derived type
+   * is in fact known, such as in most implementations of \c evaluateError() in
+   * classes derived from NonlinearFactor.
    *
    * Using the above practice, here is an example of implementing a typical
    * class derived from Value:
@@ -72,8 +52,14 @@ namespace gtsam {
        // Constructor, there is never a need to call the Value base class constructor.
        Rot3() { ... }
 
-       // Tangent space dimensionality (virtual, overrides Value::dim())
-       virtual size_t dim() cosnt {
+       // Print for unit tests and debugging (virtual, implements Value::print())
+       virtual void print(const std::string& str = "") const;
+
+       // Equals working directly with Rot3 objects (non-virtual, non-overriding!)
+       bool equals(const Rot3& other, double tol = 1e-9) const;
+
+       // Tangent space dimensionality (virtual, implements Value::dim())
+       virtual size_t dim() const {
          return 3;
        }
 
@@ -89,14 +75,21 @@ namespace gtsam {
          return Vector(result);
        }
 
-       // retract implementing the generic Value interface (virtual, overrides Value::retract())
+       // Equals implementing the generic Value interface (virtual, implements Value::equals_())
+       virtual bool equals_(const Value& other, double tol = 1e-9) const {
+         // Call our provided helper function to call your Rot3-specific
+         // equals with appropriate casting.
+         return CallDerivedEquals(this, other, tol);
+       }
+
+       // retract implementing the generic Value interface (virtual, implements Value::retract_())
        virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
          // Call our provided helper function to call your Rot3-specific
          // retract and do the appropriate casting and allocation.
          return CallDerivedRetract(this, delta);
        }
 
-       // localCoordinates implementing the generic Value interface (virtual, overrides Value::localCoordinates())
+       // localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
        virtual Vector localCoordinates_(const Value& value) const {
          // Call our provided helper function to call your Rot3-specific
          // localCoordinates and do the appropriate casting.
@@ -107,6 +100,13 @@ namespace gtsam {
    */
   class Value {
   public:
+
+    /** Print this value, for debugging and unit tests */
+    virtual void print(const std::string& str = "") const = 0;
+
+    /** Compare this Value with another for equality. */
+    virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
+
     /** Return the dimensionality of the tangent space of this value.  This is
      * the dimensionality of \c delta passed into retract() and of the vector
      * returned by localCoordinates().
@@ -134,6 +134,22 @@ namespace gtsam {
     virtual ~Value() {}
 
   protected:
+    /** This is a convenience function to make it easy for you to implement the
+     * generic Value inferface, see the example at the top of the Value
+     * documentation.
+     * @param value1 The object on which to call equals, stored as a derived class pointer
+     * @param value2 The argument to pass to the derived equals function, strored as a Value reference
+     * @return The result of equals of the derived class
+     */
+    template<class Derived>
+    static bool CallDerivedEquals(const Derived* value1, const Value& value2, double tol) {
+      // Cast the base class Value pointer to a derived class pointer
+      const Derived& derivedValue2 = dynamic_cast<const Derived&>(value2);
+
+      // Return the result of calling equals on the derived class
+      return value1->equals(derivedValue2, tol);
+    }
+
     /** This is a convenience function to make it easy for you to implement the
      * generic Value inferface, see the example at the top of the Value
      * documentation.
@@ -156,8 +172,8 @@ namespace gtsam {
     /** This is a convenience function to make it easy for you to implement the
      * generic Value inferface, see the example at the top of the Value
      * documentation.
-     * @param value1 The object on which to call localCoordinates, stored as a Value pointer
-     * @param value2 The argument to pass to the derived localCoordinates function
+     * @param value1 The object on which to call localCoordinates, stored as a derived class pointer
+     * @param value2 The argument to pass to the derived localCoordinates function, stored as a Value reference
      * @return The result of localCoordinates of the derived class
      */
     template<class Derived>

From 8f4eb68cb5617574fb5ad473c106fd627db9fa93 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 14 Jan 2012 23:12:28 +0000
Subject: [PATCH 04/88] (in branch) Made Rot3 derive from Value

---
 gtsam/geometry/Rot3.h | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index a0775c449..4f5322961 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -32,6 +32,7 @@
 #endif
 #endif
 
+#include <gtsam/base/Value.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/3rdparty/Eigen/Eigen/Geometry>
 
@@ -49,7 +50,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Rot3 {
+  class Rot3 : public Value {
   public:
     static const size_t dimension = 3;
 
@@ -92,6 +93,9 @@ namespace gtsam {
      */
     Rot3(const Quaternion& q);
 
+    /** Virtual destructor */
+    virtual ~Rot3() {}
+
     /* Static member function to generate some well known rotations */
 
     /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
@@ -184,6 +188,9 @@ namespace gtsam {
     /** equals with an tolerance */
     bool equals(const Rot3& p, double tol = 1e-9) const;
 
+    /** equals implementing generic Value interface */
+    virtual bool equals_(const Value& p, double tol = 1e-9) const { return CallDerivedEquals(this, p, tol); }
+
     /// @}
     /// @name Group
     /// @{
@@ -235,7 +242,7 @@ namespace gtsam {
     static size_t Dim() { return dimension; }
 
     /// return dimensionality of tangent space, DOF = 3
-    size_t dim() const { return dimension; }
+    virtual size_t dim() const { return dimension; }
 
     /**
      * The method retract() is used to map from the tangent space back to the manifold.
@@ -265,6 +272,12 @@ namespace gtsam {
     /// Returns local retract coordinates in neighborhood around current rotation
     Vector localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 
+    /// Generic Value interface version of retract
+    virtual std::auto_ptr<Value> retract_(const Vector& omega) const { return CallDerivedRetract(this, omega); }
+
+    /// Generic Value interface version of localCoordinates
+    virtual Vector localCoordinates_(const Value& value) const { return CallDerivedLocalCoordinates(this, value); }
+
     /// @}
     /// @name Lie Group
     /// @{

From ca53ed5fb7be60e84c6a6b23ef567eccafcc4193 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 14 Jan 2012 23:12:59 +0000
Subject: [PATCH 05/88] (in branch) Added incomplete DynamicValues

---
 gtsam/nonlinear/DynamicValues-inl.h |  38 ++++
 gtsam/nonlinear/DynamicValues.cpp   |  47 +++++
 gtsam/nonlinear/DynamicValues.h     | 305 ++++++++++++++++++++++++++++
 3 files changed, 390 insertions(+)
 create mode 100644 gtsam/nonlinear/DynamicValues-inl.h
 create mode 100644 gtsam/nonlinear/DynamicValues.cpp
 create mode 100644 gtsam/nonlinear/DynamicValues.h

diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
new file mode 100644
index 000000000..451077d9c
--- /dev/null
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DynamicValues.h
+ * @author Richard Roberts
+ *
+ * @brief A non-templated config holding any types of Manifold-group elements
+ *
+ *  Detailed story:
+ *  A values structure is a map from keys to values. It is used to specify the value of a bunch
+ *  of variables in a factor graph. A Values is a values structure which can hold variables that
+ *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
+ *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
+ */
+
+#include <gtsam/nonlinear/DynamicValues.h> // Only so Eclipse finds class definition
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  const char* DynamicValuesIncorrectType::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
+          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
+    return message_.c_str();
+  }
+
+}
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
new file mode 100644
index 000000000..79270d060
--- /dev/null
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -0,0 +1,47 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DynamicValues.h
+ * @author Richard Roberts
+ *
+ * @brief A non-templated config holding any types of Manifold-group elements
+ *
+ *  Detailed story:
+ *  A values structure is a map from keys to values. It is used to specify the value of a bunch
+ *  of variables in a factor graph. A Values is a values structure which can hold variables that
+ *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
+ *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
+ */
+
+#include <gtsam/nonlinear/DynamicValues.h>
+
+#include <boost/foreach.hpp>
+
+using namespace std;
+
+namespace gtsam {
+
+  void DynamicValues::print(const string& str) const {
+    cout << str << "DynamicValues with " << size() << " values:\n" << endl;
+    BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) {
+      cout << "  " << (string)key_value.first << ": ";
+      key_value.second->print("");
+    }
+  }
+
+  bool DynamicValues::equals(const DynamicValues& other, double tol) const {
+    if(this->size() != other.size())
+      return false;
+    for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2)
+  }
+
+}
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
new file mode 100644
index 000000000..747741bfb
--- /dev/null
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -0,0 +1,305 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DynamicValues.h
+ * @author Richard Roberts
+ *
+ * @brief A non-templated config holding any types of Manifold-group elements
+ *
+ *  Detailed story:
+ *  A values structure is a map from keys to values. It is used to specify the value of a bunch
+ *  of variables in a factor graph. A Values is a values structure which can hold variables that
+ *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
+ *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
+ */
+
+#pragma once
+
+#include <string>
+
+#include <gtsam/base/Value.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Ordering.h>
+
+namespace gtsam {
+
+  class DynamicValues {
+
+  private:
+
+    typedef FastMap<Symbol, std::auto_ptr<Value> > KeyValueMap;
+    KeyValueMap values_;
+
+  public:
+
+    typedef KeyValueMap::iterator iterator;
+    typedef KeyValueMap::const_iterator const_iterator;
+    typedef KeyValueMap::reverse_iterator reverse_iterator;
+    typedef KeyValueMap::const_reverse_iterator const_reverse_iterator;
+
+    /** Default constructor creates an empty DynamicValues class */
+    DynamicValues() {}
+
+    /** Copy constructor duplicates all keys and values */
+    DynamicValues(const DynamicValues& other) {}
+
+    /// @name Testable
+    /// @{
+
+    /** print method for testing and debugging */
+    void print(const std::string& str = "") const;
+
+    /** Test whether the sets of keys and values are identical */
+    bool equals(const DynamicValues& other, double tol=1e-9) const;
+
+    /// @}
+
+    /** Retrieve a variable by key \c j.  The type of the value associated with
+     * this key is supplied as a template argument to this function.
+     * @param j Retrieve the value associated with this key
+     * @tparam Value The type of the value stored with this key, this method
+     * throws DynamicValuesIncorrectType if this requested type is not correct.
+     * @return A const reference to the stored value
+     */
+    template<typename Value>
+    const Value& at(const Symbol& j) const;
+
+    /** Retrieve a variable using a special key (typically TypedSymbol), which
+     * contains the type of the value associated with the key, and which must
+     * be conversion constructible to a Symbol, e.g.
+     * <tt>Symbol(const TypedKey&)</tt>.  Throws DynamicValuesKeyDoesNotExist
+     * the key is not found, and DynamicValuesIncorrectType if the value type
+     * associated with the requested key does not match the stored value type.
+     */
+    template<class TypedKey>
+    const typename TypedKey::Value& at(const TypedKey& j) const;
+
+    /** operator[] syntax for at(const TypedKey& j) */
+    template<class TypedKey>
+    const typename TypedKey::Value& operator[](const TypedKey& j) const {
+      return at(j); }
+
+    /** Check if a value exists with key \c j.  See exists<>(const Symbol& j)
+     * and exists(const TypedKey& j) for versions that return the value if it
+     * exists. */
+    bool exists(const Symbol& i) const;
+
+    /** Check if a value with key \c j exists, returns the value with type
+     * \c Value if the key does exist, or boost::none if it does not exist.
+     * Throws DynamicValuesIncorrectType if the value type associated with the
+     * requested key does not match the stored value type. */
+    template<typename Value>
+    boost::optional<Value> exists(const Symbol& j) const;
+
+    /** Check if a value with key \c j exists, returns the value with type
+     * \c Value if the key does exist, or boost::none if it does not exist.
+     * Uses a special key (typically TypedSymbol), which contains the type of
+     * the value associated with the key, and which must be conversion
+     * constructible to a Symbol, e.g. <tt>Symbol(const TypedKey&)</tt>. Throws
+     * DynamicValuesIncorrectType if the value type associated with the
+     * requested key does not match the stored value type.
+     */
+    template<class TypedKey>
+    boost::optional<typename TypedKey::Value> exists(const TypedKey& j) const;
+
+    /** The number of variables in this config */
+    size_t size() const { return values_.size(); }
+
+    /** whether the config is empty */
+    bool empty() const { return values_.empty(); }
+
+    /** Get a zero VectorValues of the correct structure */
+    VectorValues zero(const Ordering& ordering) 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(); }
+    const_reverse_iterator rbegin() const { return values_.rbegin(); }
+    const_reverse_iterator rend() const { return values_.rend(); }
+    reverse_iterator rbegin() { return values_.rbegin(); }
+    reverse_iterator rend() { return values_.rend(); }
+
+    /// @name Manifold Operations
+    /// @{
+
+    /** The dimensionality of the tangent space */
+    size_t dim() const;
+
+    /** Add a delta config to current config and returns a new config */
+    DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const;
+
+    /** Get a delta config about a linearization point c0 (*this) */
+    VectorValues localCoordinates(const DynamicValues& cp, const Ordering& ordering) const;
+
+    /** Get a delta config about a linearization point c0 (*this) */
+    void localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& delta) const;
+
+    ///@}
+
+    // imperative methods:
+
+    /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
+    template<class ValueType>
+    void insert(const Symbol& j, const ValueType& val);
+
+    /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
+    void insert(const DynamicValues& cfg);
+
+    /** update the current available values without adding new ones */
+    void update(const DynamicValues& cfg);
+
+    /** single element change of existing element */
+    template<class ValueType>
+    void update(const Symbol& j, const ValueType& val);
+
+    /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
+    void erase(const Symbol& j);
+
+    /** Remove a variable from the config while returning the dimensionality of
+     * the removed element (normally not needed by user code), throws
+     * KeyDoesNotExist<J> if j is not present.
+     */
+    //void erase(const J& j, size_t& dim);
+
+    /**
+     * Returns a set of keys in the config
+     * Note: by construction, the list is ordered
+     */
+    std::list<Symbol> keys() const;
+
+    /** Replace all keys and variables */
+    DynamicValues& operator=(const DynamicValues& rhs) {
+      values_ = rhs.values_;
+      return (*this);
+    }
+
+    /** Remove all variables from the config */
+    void clear() {
+      values_.clear();
+    }
+
+    /**
+     * Apply a class with an application operator() to a const_iterator over
+     * every <key,value> pair.  The operator must be able to handle such an
+     * iterator for every type in the Values, (i.e. through templating).
+     */
+    template<typename A>
+    void apply(A& operation) {
+      for(iterator it = begin(); it != end(); ++it)
+        operation(it);
+    }
+    template<typename A>
+    void apply(A& operation) const {
+      for(const_iterator it = begin(); it != end(); ++it)
+        operation(it);
+    }
+
+    /** Create an array of variable dimensions using the given ordering */
+    std::vector<size_t> dims(const Ordering& ordering) const;
+
+    /**
+     * Generate a default ordering, simply in key sort order.  To instead
+     * create a fill-reducing ordering, use
+     * NonlinearFactorGraph::orderingCOLAMD().  Alternatively, you may permute
+     * this ordering yourself (as orderingCOLAMD() does internally).
+     */
+    Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
+
+  };
+
+  /* ************************************************************************* */
+  template<class ValueType>
+  class DynamicValuesKeyAlreadyExists : public std::exception {
+  protected:
+    const Symbol key_; ///< The key that already existed
+    const Value value_; ///< The value attempted to be inserted
+
+  private:
+    mutable std::string message_;
+
+  public:
+    /// Construct with the key-value pair attemped to be added
+    DynamicValuesKeyAlreadyExists(const Symbol& key, const Value& value) throw() :
+      key_(key), value_(value) {}
+
+    virtual ~DynamicValuesKeyAlreadyExists() throw() {}
+
+    /// The duplicate key that was attemped to be added
+    const Symbol& key() const throw() { return key_; }
+
+    /// The value that was attempted to be added
+    const Value& value() const throw() { return value_; }
+
+    /// The message to be displayed to the user
+    virtual const char* what() const throw();
+  };
+
+  /* ************************************************************************* */
+  class DynamicValuesKeyDoesNotExist : public std::exception {
+  protected:
+    const char* operation_; ///< The operation that attempted to access the key
+    const Symbol key_; ///< The key that does not exist
+
+  private:
+    mutable std::string message_;
+
+  public:
+    /// Construct with the key that does not exist in the values
+    DynamicValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
+      operation_(operation), key_(key) {}
+
+    virtual ~DynamicValuesKeyDoesNotExist() throw() {}
+
+    /// The key that was attempted to be accessed that does not exist
+    const Symbol& key() const throw() { return key_; }
+
+    /// The message to be displayed to the user
+    virtual const char* what() const throw();
+  };
+
+  /* ************************************************************************* */
+  class DynamicValuesIncorrectType : public std::exception {
+  protected:
+    const Symbol key_; ///< The key requested
+    const std::type_info& storedTypeId_;
+    const std::type_info& requestedTypeId_;
+
+  private:
+    mutable std::string message_;
+
+  public:
+    /// Construct with the key that does not exist in the values
+    DynamicValuesIncorrectType(const Symbol& key,
+        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
+      key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
+
+    virtual ~DynamicValuesIncorrectType() throw() {}
+
+    /// The key that was attempted to be accessed that does not exist
+    const Symbol& key() const throw() { return key_; }
+
+    /// The typeid of the value stores in the DynamicValues
+    const std::type_info& storedTypeId() const { return storedTypeId_; }
+
+    /// The requested typeid
+    const std::type_info& requestedTypeId() const { return requestedTypeId_; }
+
+    /// The message to be displayed to the user
+    virtual const char* what() const throw();
+  };
+
+}
+
+#include <gtsam/nonlinear/DynamicValues-inl.h>

From ef8a82c8d78a6aebdcade8921baf0b0616c9904c Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 14 Jan 2012 23:13:14 +0000
Subject: [PATCH 06/88] (in branch) Added incomplete DynamicValues

---
 gtsam/nonlinear/Makefile.am | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 58503466c..1e791a622 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -16,7 +16,8 @@ check_PROGRAMS =
 #----------------------------------------------------------------------------------------------------
 
 # Lie Groups
-headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h 
+headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
+sources += DynamicValues.cpp
 check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
 
 # Nonlinear nonlinear

From d323d5b963fdb8b7203b985bbaf6fe08ea14d0ec Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sun, 15 Jan 2012 23:06:01 +0000
Subject: [PATCH 07/88] (in branch) more implementation for DynamicValues

---
 gtsam/nonlinear/DynamicValues-inl.h | 70 +++++++++++++++++++++++++++++
 gtsam/nonlinear/DynamicValues.cpp   | 59 +++++++++++++++++++++++-
 gtsam/nonlinear/DynamicValues.h     | 39 +++++++++-------
 gtsam/nonlinear/Key.h               | 10 +++++
 4 files changed, 161 insertions(+), 17 deletions(-)

diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index 451077d9c..d54cf8b47 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -35,4 +35,74 @@ namespace gtsam {
     return message_.c_str();
   }
 
+  /* ************************************************************************* */
+  template<typename Value>
+  const Value& DynamicValues::at(const Symbol& j) const {
+    // Find the item
+    const_iterator item = values_.find(j);
+
+    // Throw exception if it does not exist
+    if(item == values_.end())
+      throw DynamicValuesKeyDoesNotExist("retrieve", j);
+
+    // Check the type and throw exception if incorrect
+    if(typeid(*item->second) != typeid(Value))
+      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+
+    // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
+    return static_cast<const Value&>(*item->second);
+  }
+
+  /* ************************************************************************* */
+  template<typename TypedKey>
+  const typename TypedKey::Value& DynamicValues::at(const TypedKey& j) const {
+    // Convert to Symbol
+    const Symbol symbol(j.symbol());
+
+    // Call at with the Value type from the key
+    return at<typename TypedKey::Value>(symbol);
+  }
+
+  /* ************************************************************************* */
+  template<typename Value>
+  boost::optional<Value> DynamicValues::exists(const Symbol& j) const {
+    // Find the item
+    const_iterator item = values_.find(j);
+
+    if(item != values_.end()) {
+      // Check the type and throw exception if incorrect
+      if(typeid(*item->second) != typeid(Value))
+        throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+
+      // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
+      return static_cast<const Value&>(*item->second);
+    } else {
+      return boost::none;
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class TypedKey>
+  boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const {
+    // Convert to Symbol
+    const Symbol symbol(j.symbol());
+
+    // Call exists with the Value type from the key
+    return exists<typename TypedKey::Value>(symbol);
+  }
+
+  /* ************************************************************************* */
+  template<class ValueType>
+  void DynamicValues::insert(const Symbol& j, const ValueType& val) {
+    pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val))));
+    if(!insertResult.second)
+      throw DynamicValuesKeyAlreadyExists(j);
+  }
+
+  /* ************************************************************************* */
+  void DynamicValues::insert(const DynamicValues& values) {
+    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+      insert(key_value.first, key_value.)
+    }
+  }
 }
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index 79270d060..90f41ddd8 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -30,6 +30,12 @@ using namespace std;
 
 namespace gtsam {
 
+  /* ************************************************************************* */
+  DynamicValues::DynamicValues(const DynamicValues& other) {
+
+  }
+
+  /* ************************************************************************* */
   void DynamicValues::print(const string& str) const {
     cout << str << "DynamicValues with " << size() << " values:\n" << endl;
     BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) {
@@ -38,10 +44,61 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
   bool DynamicValues::equals(const DynamicValues& other, double tol) const {
     if(this->size() != other.size())
       return false;
-    for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2)
+    for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
+      if(typeid(*it1->second) != typeid(*it2->second))
+        return false;
+      if(it1->first != it2->first)
+        return false;
+      if(!it1->second->equals_(*it2->second, tol))
+        return false;
+    }
+    return true; // We return false earlier if we find anything that does not match
   }
 
+  /* ************************************************************************* */
+  bool DynamicValues::exists(const Symbol& j) const {
+    return values_.find(j) != values_.end();
+  }
+
+  /* ************************************************************************* */
+  VectorValues DynamicValues::zeroVectors(const Ordering& ordering) const {
+    return VectorValues::Zero(this->dims(ordering));
+  }
+
+  /* ************************************************************************* */
+  DynamicValues DynamicValues::retract(const VectorValues& delta, const Ordering& ordering) const {
+    DynamicValues result;
+
+    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
+      const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value
+      const std::auto_ptr<const Value> retractedValue = key_value.second->retract_(singleDelta); // Retract
+      result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values
+    }
+
+    return result;
+  }
+
+  /* ************************************************************************* */
+  VectorValues DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering) const {
+    VectorValues result(this->dims(ordering));
+    localCoordinates(cp, ordering, result);
+    return result;
+  }
+
+  /* ************************************************************************* */
+  void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const {
+    if(this->size() != cp.size())
+      throw DynamicValuesMismatched();
+    for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
+      if(it1->first != it2->first)
+        throw DynamicValuesMismatched(); // If keys do not match
+      result[ordering[it1->first]] = it1->second->localCoordinates_(*it2->second); // Will throw a dynamic_cast exception if types do not match
+    }
+  }
+
+
 }
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index 747741bfb..27831fab9 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -38,7 +38,9 @@ namespace gtsam {
 
   private:
 
-    typedef FastMap<Symbol, std::auto_ptr<Value> > KeyValueMap;
+    typedef std::auto_ptr<const Value> ValuePtr;
+    typedef FastMap<Symbol, std::auto_ptr<const Value> > KeyValueMap;
+    typedef KeyValueMap::value_type KeyValuePair;
     KeyValueMap values_;
 
   public:
@@ -52,7 +54,7 @@ namespace gtsam {
     DynamicValues() {}
 
     /** Copy constructor duplicates all keys and values */
-    DynamicValues(const DynamicValues& other) {}
+    DynamicValues(const DynamicValues& other);
 
     /// @name Testable
     /// @{
@@ -93,14 +95,14 @@ namespace gtsam {
     /** Check if a value exists with key \c j.  See exists<>(const Symbol& j)
      * and exists(const TypedKey& j) for versions that return the value if it
      * exists. */
-    bool exists(const Symbol& i) const;
+    bool exists(const Symbol& j) const;
 
     /** Check if a value with key \c j exists, returns the value with type
      * \c Value if the key does exist, or boost::none if it does not exist.
      * Throws DynamicValuesIncorrectType if the value type associated with the
      * requested key does not match the stored value type. */
     template<typename Value>
-    boost::optional<Value> exists(const Symbol& j) const;
+    boost::optional<const Value&> exists(const Symbol& j) const;
 
     /** Check if a value with key \c j exists, returns the value with type
      * \c Value if the key does exist, or boost::none if it does not exist.
@@ -111,7 +113,7 @@ namespace gtsam {
      * requested key does not match the stored value type.
      */
     template<class TypedKey>
-    boost::optional<typename TypedKey::Value> exists(const TypedKey& j) const;
+    boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const;
 
     /** The number of variables in this config */
     size_t size() const { return values_.size(); }
@@ -120,7 +122,7 @@ namespace gtsam {
     bool empty() const { return values_.empty(); }
 
     /** Get a zero VectorValues of the correct structure */
-    VectorValues zero(const Ordering& ordering) const;
+    VectorValues zeroVectors(const Ordering& ordering) const;
 
     const_iterator begin() const { return values_.begin(); }
     const_iterator end() const { return values_.end(); }
@@ -134,9 +136,6 @@ namespace gtsam {
     /// @name Manifold Operations
     /// @{
 
-    /** The dimensionality of the tangent space */
-    size_t dim() const;
-
     /** Add a delta config to current config and returns a new config */
     DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const;
 
@@ -220,28 +219,23 @@ namespace gtsam {
   };
 
   /* ************************************************************************* */
-  template<class ValueType>
   class DynamicValuesKeyAlreadyExists : public std::exception {
   protected:
     const Symbol key_; ///< The key that already existed
-    const Value value_; ///< The value attempted to be inserted
 
   private:
     mutable std::string message_;
 
   public:
     /// Construct with the key-value pair attemped to be added
-    DynamicValuesKeyAlreadyExists(const Symbol& key, const Value& value) throw() :
-      key_(key), value_(value) {}
+    DynamicValuesKeyAlreadyExists(const Symbol& key) throw() :
+      key_(key) {}
 
     virtual ~DynamicValuesKeyAlreadyExists() throw() {}
 
     /// The duplicate key that was attemped to be added
     const Symbol& key() const throw() { return key_; }
 
-    /// The value that was attempted to be added
-    const Value& value() const throw() { return value_; }
-
     /// The message to be displayed to the user
     virtual const char* what() const throw();
   };
@@ -300,6 +294,19 @@ namespace gtsam {
     virtual const char* what() const throw();
   };
 
+  /* ************************************************************************* */
+  class DynamicValuesMismatched : public std::exception {
+
+  public:
+    DynamicValuesMismatched() throw() {}
+
+    virtual ~DynamicValuesMismatched() throw() {}
+
+    virtual const char* what() const throw() {
+      return "The Values 'this' and the argument passed to DynamicValues::localCoordinates have mismatched keys and values";
+    }
+  };
+
 }
 
 #include <gtsam/nonlinear/DynamicValues-inl.h>
diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h
index de609d413..a4624b640 100644
--- a/gtsam/nonlinear/Key.h
+++ b/gtsam/nonlinear/Key.h
@@ -76,6 +76,7 @@ public:
   std::string latex() const {
     return (boost::format("%c_{%d}") % C % j_).str();
   }
+  Symbol symbol() const;
 
   // logic:
 
@@ -298,6 +299,9 @@ public:
     std::string label_s = (boost::format("%1%") % label_).str();
     return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
   }
+  Symbol symbol() const {
+    return Symbol(*this);
+  }
 
   // Needed for conversion to LabeledSymbol
   size_t convertLabel() const {
@@ -354,5 +358,11 @@ private:
   }
 };
 
+/* ************************************************************************* */
+template<class T, char C>
+Symbol TypedSymbol<T,C>::symbol() const {
+  return Symbol(*this);
+}
+
 } // namespace gtsam
 

From 9cdb1e08fe936c5bfdc58eddcd18a28eb7643808 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 16 Jan 2012 22:54:19 +0000
Subject: [PATCH 08/88] (in branch) more implementation for DynamicValues

---
 gtsam/base/Value.h                  |  3 ++
 gtsam/geometry/Rot3.h               |  5 +++
 gtsam/nonlinear/DynamicValues-inl.h | 10 +++--
 gtsam/nonlinear/DynamicValues.cpp   | 60 ++++++++++++++++++++++++++++-
 gtsam/nonlinear/DynamicValues.h     | 31 +++------------
 gtsam/nonlinear/Key.h               |  3 ++
 6 files changed, 81 insertions(+), 31 deletions(-)

diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 3a4b56c72..eb86e69f1 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -101,6 +101,9 @@ namespace gtsam {
   class Value {
   public:
 
+    /** Allocate and construct a clone of this value */
+    virtual std::auto_ptr<Value> clone_() const = 0;
+
     /** Print this value, for debugging and unit tests */
     virtual void print(const std::string& str = "") const = 0;
 
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 4f5322961..d2e72abeb 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -178,6 +178,11 @@ namespace gtsam {
     static Rot3 rodriguez(double wx, double wy, double wz)
       { return rodriguez(Vector_(3,wx,wy,wz));}
 
+    /**
+     * Create a duplicate object returned as a pointer to the generic Value interface
+     */
+    virtual std::auto_ptr<Value> clone_() const { return std::auto_ptr<Value>(new Rot3(*this)); }
+
     /// @}
     /// @name Testable
     /// @{
diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index d54cf8b47..9c81b0293 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -100,9 +100,11 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void DynamicValues::insert(const DynamicValues& values) {
-    BOOST_FOREACH(const KeyValuePair& key_value, values) {
-      insert(key_value.first, key_value.)
-    }
+  template<class ValueType>
+  void DynamicValues::update(const Symbol& j, const ValueType& val) {
+    iterator item = values_.find(j);
+    if(item == values_end)
+      throw DynamicValuesKeyDoesNotExist("update", j);
+    item->second = val.clone_();
   }
 }
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index 90f41ddd8..6a93a29f5 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -24,7 +24,10 @@
 
 #include <gtsam/nonlinear/DynamicValues.h>
 
+#include <list>
+
 #include <boost/foreach.hpp>
+#include <boost/iterator/transform_iterator.hpp>
 
 using namespace std;
 
@@ -32,7 +35,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   DynamicValues::DynamicValues(const DynamicValues& other) {
-
+    this->insert(other);
   }
 
   /* ************************************************************************* */
@@ -100,5 +103,60 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  void DynamicValues::insert(const DynamicValues& values) {
+    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+      insert(key_value.first, key_value.second);
+    }
+  }
 
+  /* ************************************************************************* */
+  void DynamicValues::update(const DynamicValues& values) {
+    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+      this->update(key_value.first, *key_value.second);
+    }
+  }
+
+  /* ************************************************************************* */
+  void DynamicValues::erase(const Symbol& j) {
+    iterator item = values_.find(j);
+    if(item == values_end)
+      throw DynamicValuesKeyDoesNotExist("erase", j);
+    values_.erase(item);
+  }
+
+  /* ************************************************************************* */
+  FastList<Symbol> DynamicValues::keys() const {
+    return list<Symbol>(
+        boost::make_transform_iterator(values_.begin(), &KeyValuePair::first),
+        boost::make_transform_iterator(values_.end(), &KeyValuePair::first));
+  }
+
+  /* ************************************************************************* */
+  DynamicValues& DynamicValues::operator=(const DynamicValues& rhs) {
+    this->clear();
+    this->insert(rhs);
+    return *this;
+  }
+
+  /* ************************************************************************* */
+  vector<size_t> DynamicValues::dims(const Ordering& ordering) const {
+    vector<size_t> result(values_.size());
+    // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
+    result.assign(
+        boost::make_transform_iterator(values_.begin(),
+            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second)))),
+        boost::make_transform_iterator(values_.begin(),
+            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second)))));
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar = 0) const {
+    Ordering::shared_ptr ordering(new Ordering);
+    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
+      ordering->insert(key_value.first, firstVar++);
+    }
+    return ordering;
+  }
 }
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index 27831fab9..7d7dbadd7 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -154,10 +154,10 @@ namespace gtsam {
     void insert(const Symbol& j, const ValueType& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
-    void insert(const DynamicValues& cfg);
+    void insert(const DynamicValues& values);
 
     /** update the current available values without adding new ones */
-    void update(const DynamicValues& cfg);
+    void update(const DynamicValues& values);
 
     /** single element change of existing element */
     template<class ValueType>
@@ -176,34 +176,13 @@ namespace gtsam {
      * Returns a set of keys in the config
      * Note: by construction, the list is ordered
      */
-    std::list<Symbol> keys() const;
+    FastList<Symbol> keys() const;
 
     /** Replace all keys and variables */
-    DynamicValues& operator=(const DynamicValues& rhs) {
-      values_ = rhs.values_;
-      return (*this);
-    }
+    DynamicValues& operator=(const DynamicValues& rhs);
 
     /** Remove all variables from the config */
-    void clear() {
-      values_.clear();
-    }
-
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      for(iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-      for(const_iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
+    void clear() { values_.clear(); }
 
     /** Create an array of variable dimensions using the given ordering */
     std::vector<size_t> dims(const Ordering& ordering) const;
diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h
index a4624b640..dcccc1b44 100644
--- a/gtsam/nonlinear/Key.h
+++ b/gtsam/nonlinear/Key.h
@@ -31,6 +31,9 @@
 
 namespace gtsam {
 
+  // Forward declarations
+  class Symbol;
+
 /**
  * TypedSymbol key class is templated on
  * 1) the type T it is supposed to retrieve, for extra type checking

From 64703d09da3340816b33b86454cf9c54cc62c88d Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 18 Jan 2012 14:37:41 +0000
Subject: [PATCH 09/88] (in branch) more implementation for DynamicValues and
 unit tests

---
 gtsam/nonlinear/DynamicValues-inl.h         |   6 +-
 gtsam/nonlinear/DynamicValues.h             |   1 +
 gtsam/nonlinear/Makefile.am                 |   2 +-
 gtsam/nonlinear/tests/testDynamicValues.cpp | 285 ++++++++++++++++++++
 4 files changed, 290 insertions(+), 4 deletions(-)
 create mode 100644 gtsam/nonlinear/tests/testDynamicValues.cpp

diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index 9c81b0293..f11b2f9f5 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -65,7 +65,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<typename Value>
-  boost::optional<Value> DynamicValues::exists(const Symbol& j) const {
+  boost::optional<const Value&> DynamicValues::exists(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
@@ -83,7 +83,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class TypedKey>
-  boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const {
+  boost::optional<const typename TypedKey::Value&> DynamicValues::exists(const TypedKey& j) const {
     // Convert to Symbol
     const Symbol symbol(j.symbol());
 
@@ -103,7 +103,7 @@ namespace gtsam {
   template<class ValueType>
   void DynamicValues::update(const Symbol& j, const ValueType& val) {
     iterator item = values_.find(j);
-    if(item == values_end)
+    if(item == values_.end())
       throw DynamicValuesKeyDoesNotExist("update", j);
     item->second = val.clone_();
   }
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index 7d7dbadd7..745384cd7 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -25,6 +25,7 @@
 #pragma once
 
 #include <string>
+#include <utility>
 
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 1e791a622..defcd51e4 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -18,7 +18,7 @@ check_PROGRAMS =
 # Lie Groups
 headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
 sources += DynamicValues.cpp
-check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
+check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering
 
 # Nonlinear nonlinear
 headers += Key.h 
diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp
new file mode 100644
index 000000000..4cd9e8068
--- /dev/null
+++ b/gtsam/nonlinear/tests/testDynamicValues.cpp
@@ -0,0 +1,285 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testDynamicValues.cpp
+ * @author Richard Roberts
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <stdexcept>
+#include <limits>
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/LieVector.h>
+#include <gtsam/nonlinear/DynamicValues.h>
+
+using namespace gtsam;
+using namespace std;
+static double inf = std::numeric_limits<double>::infinity();
+
+typedef TypedSymbol<LieVector, 'v'> VecKey;
+typedef DynamicValues<VecKey> DynamicValues;
+
+VecKey key1(1), key2(2), key3(3), key4(4);
+
+/* ************************************************************************* */
+TEST( DynamicValues, equals1 )
+{
+  DynamicValues expected;
+  Vector v = Vector_(3, 5.0, 6.0, 7.0);
+  expected.insert(key1,v);
+  DynamicValues actual;
+  actual.insert(key1,v);
+  CHECK(assert_equal(expected,actual));
+}
+
+/* ************************************************************************* */
+TEST( DynamicValues, equals2 )
+{
+  DynamicValues cfg1, cfg2;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
+  cfg1.insert(key1, v1);
+  cfg2.insert(key1, v2);
+  CHECK(!cfg1.equals(cfg2));
+  CHECK(!cfg2.equals(cfg1));
+}
+
+/* ************************************************************************* */
+TEST( DynamicValues, equals_nan )
+{
+  DynamicValues cfg1, cfg2;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, inf, inf, inf);
+  cfg1.insert(key1, v1);
+  cfg2.insert(key1, v2);
+  CHECK(!cfg1.equals(cfg2));
+  CHECK(!cfg2.equals(cfg1));
+}
+
+/* ************************************************************************* */
+TEST( DynamicValues, insert_good )
+{
+  DynamicValues cfg1, cfg2, expected;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  cfg1.insert(key1, v1);
+  cfg1.insert(key2, v2);
+  cfg2.insert(key3, v4);
+
+  cfg1.insert(cfg2);
+
+  expected.insert(key1, v1);
+  expected.insert(key2, v2);
+  expected.insert(key3, v4);
+
+  CHECK(assert_equal(cfg1, expected));
+}
+
+/* ************************************************************************* */
+TEST( DynamicValues, insert_bad )
+{
+  DynamicValues cfg1, cfg2;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  cfg1.insert(key1, v1);
+  cfg1.insert(key2, v2);
+  cfg2.insert(key2, v3);
+  cfg2.insert(key3, v4);
+
+  CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
+}
+
+/* ************************************************************************* */
+TEST( DynamicValues, update_element )
+{
+  DynamicValues cfg;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+
+  cfg.insert(key1, v1);
+  CHECK(cfg.size() == 1);
+  CHECK(assert_equal(v1, cfg.at(key1)));
+
+  cfg.update(key1, v2);
+  CHECK(cfg.size() == 1);
+  CHECK(assert_equal(v2, cfg.at(key1)));
+}
+
+///* ************************************************************************* */
+//TEST(DynamicValues, dim_zero)
+//{
+//  DynamicValues config0;
+//  config0.insert(key1, Vector_(2, 2.0, 3.0));
+//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//  LONGS_EQUAL(5,config0.dim());
+//
+//  VectorValues expected;
+//  expected.insert(key1, zero(2));
+//  expected.insert(key2, zero(3));
+//  CHECK(assert_equal(expected, config0.zero()));
+//}
+
+/* ************************************************************************* */
+TEST(DynamicValues, expmap_a)
+{
+  DynamicValues config0;
+  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+
+  Ordering ordering(*config0.orderingArbitrary());
+  VectorValues increment(config0.dims(ordering));
+  increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
+  increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
+
+  DynamicValues expected;
+  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
+  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
+}
+
+///* ************************************************************************* */
+//TEST(DynamicValues, expmap_b)
+//{
+//  DynamicValues config0;
+//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//
+//  Ordering ordering(*config0.orderingArbitrary());
+//  VectorValues increment(config0.dims(ordering));
+//  increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
+//
+//  DynamicValues expected;
+//  expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+//
+//  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
+//}
+
+///* ************************************************************************* */
+//TEST(DynamicValues, expmap_c)
+//{
+//  DynamicValues config0;
+//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//
+//  Vector increment = Vector_(6,
+//      1.0, 1.1, 1.2,
+//      1.3, 1.4, 1.5);
+//
+//  DynamicValues expected;
+//  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
+//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+//
+//  CHECK(assert_equal(expected, config0.retract(increment)));
+//}
+
+/* ************************************************************************* */
+/*TEST(DynamicValues, expmap_d)
+{
+  DynamicValues config0;
+  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  //config0.print("config0");
+  CHECK(equal(config0, config0));
+  CHECK(config0.equals(config0));
+
+  DynamicValues<string,Pose2> poseconfig;
+  poseconfig.insert("p1", Pose2(1,2,3));
+  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
+  //poseconfig.print("poseconfig");
+  CHECK(equal(config0, config0));
+  CHECK(config0.equals(config0));
+}*/
+
+/* ************************************************************************* */
+/*TEST(DynamicValues, extract_keys)
+{
+	typedef TypedSymbol<Pose2, 'x'> PoseKey;
+	DynamicValues<PoseKey, Pose2> config;
+
+	config.insert(PoseKey(1), Pose2());
+	config.insert(PoseKey(2), Pose2());
+	config.insert(PoseKey(4), Pose2());
+	config.insert(PoseKey(5), Pose2());
+
+	list<PoseKey> expected, actual;
+	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
+	actual = config.keys();
+
+	CHECK(actual.size() == expected.size());
+	list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
+		CHECK(assert_equal(*itExp, *itAct));
+	}
+}*/
+
+/* ************************************************************************* */
+TEST(DynamicValues, exists_)
+{
+	DynamicValues config0;
+	config0.insert(key1, Vector_(1, 1.));
+	config0.insert(key2, Vector_(1, 2.));
+
+	boost::optional<LieVector> v = config0.exists_(key1);
+	CHECK(assert_equal(Vector_(1, 1.),*v));
+}
+
+/* ************************************************************************* */
+TEST(DynamicValues, update)
+{
+	DynamicValues config0;
+	config0.insert(key1, Vector_(1, 1.));
+	config0.insert(key2, Vector_(1, 2.));
+
+	DynamicValues superset;
+	superset.insert(key1, Vector_(1, -1.));
+	superset.insert(key2, Vector_(1, -2.));
+	superset.insert(key3, Vector_(1, -3.));
+	config0.update(superset);
+
+	DynamicValues expected;
+	expected.insert(key1, Vector_(1, -1.));
+	expected.insert(key2, Vector_(1, -2.));
+	CHECK(assert_equal(expected,config0));
+}
+
+/* ************************************************************************* */
+TEST(DynamicValues, dummy_initialization)
+{
+	typedef TypedSymbol<LieVector, 'z'> Key;
+
+	DynamicValues init1;
+	init1.insert(Key(1), Vector_(2, 1.0, 2.0));
+	init1.insert(Key(2), Vector_(2, 4.0, 3.0));
+
+	DynamicValues init2;
+	init2.insert(key1, Vector_(2, 1.0, 2.0));
+	init2.insert(key2, Vector_(2, 4.0, 3.0));
+
+	DynamicValues actual1(init2);
+	DynamicValues actual2(init1);
+
+	EXPECT(actual1.empty());
+	EXPECT(actual2.empty());
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

From f76c7be5b29555c1bd782b67767cd4b54aaf15e2 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 20 Jan 2012 16:26:45 +0000
Subject: [PATCH 10/88] (in branch) more implementation for DynamicValues

---
 gtsam/nonlinear/DynamicValues-inl.h | 4 +++-
 gtsam/nonlinear/DynamicValues.cpp   | 8 ++++----
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index f11b2f9f5..deb81d567 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -22,6 +22,8 @@
  *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
  */
 
+#include <utility>
+
 #include <gtsam/nonlinear/DynamicValues.h> // Only so Eclipse finds class definition
 
 namespace gtsam {
@@ -94,7 +96,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   void DynamicValues::insert(const Symbol& j, const ValueType& val) {
-    pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val))));
+    std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val))));
     if(!insertResult.second)
       throw DynamicValuesKeyAlreadyExists(j);
   }
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index 6a93a29f5..1901ab40e 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -78,7 +78,7 @@ namespace gtsam {
 
     BOOST_FOREACH(const KeyValuePair& key_value, values_) {
       const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value
-      const std::auto_ptr<const Value> retractedValue = key_value.second->retract_(singleDelta); // Retract
+      ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract
       result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values
     }
 
@@ -96,7 +96,7 @@ namespace gtsam {
   void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const {
     if(this->size() != cp.size())
       throw DynamicValuesMismatched();
-    for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
+    for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
       if(it1->first != it2->first)
         throw DynamicValuesMismatched(); // If keys do not match
       result[ordering[it1->first]] = it1->second->localCoordinates_(*it2->second); // Will throw a dynamic_cast exception if types do not match
@@ -120,14 +120,14 @@ namespace gtsam {
   /* ************************************************************************* */
   void DynamicValues::erase(const Symbol& j) {
     iterator item = values_.find(j);
-    if(item == values_end)
+    if(item == values_.end())
       throw DynamicValuesKeyDoesNotExist("erase", j);
     values_.erase(item);
   }
 
   /* ************************************************************************* */
   FastList<Symbol> DynamicValues::keys() const {
-    return list<Symbol>(
+    return FastList<Symbol>(
         boost::make_transform_iterator(values_.begin(), &KeyValuePair::first),
         boost::make_transform_iterator(values_.end(), &KeyValuePair::first));
   }

From a7a845a803eaedf07ba228e2a80b1f66e86d0925 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 26 Jan 2012 06:04:55 +0000
Subject: [PATCH 11/88] (in branch) Fixed some compile errors

---
 gtsam/nonlinear/DynamicValues-inl.h |  2 +-
 gtsam/nonlinear/DynamicValues.cpp   | 17 +++++++++--------
 2 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index deb81d567..b1d67de08 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -96,7 +96,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   void DynamicValues::insert(const Symbol& j, const ValueType& val) {
-    std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val))));
+    std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, new ValueType(val)));
     if(!insertResult.second)
       throw DynamicValuesKeyAlreadyExists(j);
   }
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index 1901ab40e..1e7996788 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -27,6 +27,7 @@
 #include <list>
 
 #include <boost/foreach.hpp>
+#include <boost/bind.hpp>
 #include <boost/iterator/transform_iterator.hpp>
 
 using namespace std;
@@ -79,7 +80,7 @@ namespace gtsam {
     BOOST_FOREACH(const KeyValuePair& key_value, values_) {
       const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value
       ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract
-      result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values
+      result.values_[key_value.first] = retractedValue; // Add retracted result directly to result values
     }
 
     return result;
@@ -105,8 +106,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void DynamicValues::insert(const DynamicValues& values) {
-    BOOST_FOREACH(const KeyValuePair& key_value, values) {
-      insert(key_value.first, key_value.second);
+    BOOST_FOREACH(const KeyValuePair& key_value, values.values_) {
+      values_.insert(make_pair(key_value.first, key_value.second->clone_()));
     }
   }
 
@@ -128,8 +129,8 @@ namespace gtsam {
   /* ************************************************************************* */
   FastList<Symbol> DynamicValues::keys() const {
     return FastList<Symbol>(
-        boost::make_transform_iterator(values_.begin(), &KeyValuePair::first),
-        boost::make_transform_iterator(values_.end(), &KeyValuePair::first));
+        boost::make_transform_iterator(values_.begin(), boost::bind(&KeyValuePair::first, _1)),
+        boost::make_transform_iterator(values_.end(), boost::bind(&KeyValuePair::first, _1)));
   }
 
   /* ************************************************************************* */
@@ -145,14 +146,14 @@ namespace gtsam {
     // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
     result.assign(
         boost::make_transform_iterator(values_.begin(),
-            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second)))),
+            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))),
         boost::make_transform_iterator(values_.begin(),
-            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second)))));
+            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))));
     return result;
   }
 
   /* ************************************************************************* */
-  Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar = 0) const {
+  Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar) const {
     Ordering::shared_ptr ordering(new Ordering);
     BOOST_FOREACH(const KeyValuePair& key_value, values_) {
       ordering->insert(key_value.first, firstVar++);

From 9e00963d54f249b074da03f63ee685b0b2330ecf Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Thu, 26 Jan 2012 23:10:55 +0000
Subject: [PATCH 12/88] Derived value

---
 gtsam/base/DerivedValue.cpp | 21 ++++++++++
 gtsam/base/DerivedValue.h   | 76 +++++++++++++++++++++++++++++++++++++
 gtsam/base/Value.h          | 60 +++--------------------------
 gtsam/geometry/Rot3.h       | 20 ++--------
 4 files changed, 106 insertions(+), 71 deletions(-)
 create mode 100644 gtsam/base/DerivedValue.cpp
 create mode 100644 gtsam/base/DerivedValue.h

diff --git a/gtsam/base/DerivedValue.cpp b/gtsam/base/DerivedValue.cpp
new file mode 100644
index 000000000..386919d6f
--- /dev/null
+++ b/gtsam/base/DerivedValue.cpp
@@ -0,0 +1,21 @@
+/*
+ * DerivedValue.cpp
+ *
+ *  Created on: Jan 26, 2012
+ *      Author: thduynguyen
+ */
+
+#include "DerivedValue.h"
+
+namespace gtsam {
+
+DerivedValue::DerivedValue() {
+	// TODO Auto-generated constructor stub
+
+}
+
+DerivedValue::~DerivedValue() {
+	// TODO Auto-generated destructor stub
+}
+
+} /* namespace gtsam */
diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
new file mode 100644
index 000000000..d0608059e
--- /dev/null
+++ b/gtsam/base/DerivedValue.h
@@ -0,0 +1,76 @@
+/*
+ * DerivedValue.h
+ *
+ *  Created on: Jan 26, 2012
+ *      Author: thduynguyen
+ */
+
+#pragma once
+
+#include <boost/pool/singleton_pool.hpp>
+#include <gtsam/base/Value.h>
+namespace gtsam {
+
+template<class DERIVED>
+class DerivedValue : public Value {
+private:
+	/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
+	struct PoolTag { };
+
+public:
+
+	DerivedValue() {}
+
+	virtual ~DerivedValue() {}
+
+  /**
+   * Create a duplicate object returned as a pointer to the generic Value interface.
+   * For the shake of performance, this function use singleton pool allocator instead of the normal heap allocator
+   */
+  virtual Value* clone_() const {
+  	void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+  	DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
+  	return ptr;
+  }
+
+  /**
+   * Create a duplicate object returned as a pointer to the generic Value interface
+   */
+  virtual void deallocate_() const {
+  	boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
+  }
+
+  /// equals implementing generic Value interface
+  virtual bool equals_(const Value& p, double tol = 1e-9) const {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
+
+    // Return the result of calling equals on the derived class
+    return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
+//  	return CallDerivedEquals(this, p, tol);
+  }
+
+  /// Generic Value interface version of retract
+	virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
+    // Call retract on the derived class
+    const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
+
+    // Create a Value pointer copy of the result
+    std::auto_ptr<Value> resultAsValue(new DERIVED(retractResult));
+
+    // Return the pointer to the Value base class
+    return resultAsValue;
+	}
+
+	/// Generic Value interface version of localCoordinates
+	virtual Vector localCoordinates_(const Value& value2) const {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);
+
+    // Return the result of calling localCoordinates on the derived class
+    return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
+	}
+
+};
+
+} /* namespace gtsam */
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index eb86e69f1..dcb4a9c16 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -102,14 +102,17 @@ namespace gtsam {
   public:
 
     /** Allocate and construct a clone of this value */
-    virtual std::auto_ptr<Value> clone_() const = 0;
+    virtual Value* clone_() const = 0;
 
-    /** Print this value, for debugging and unit tests */
-    virtual void print(const std::string& str = "") const = 0;
+    /** Deallocate a raw pointer of this value */
+    virtual void deallocate_() const = 0;
 
     /** Compare this Value with another for equality. */
     virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
 
+    /** Print this value, for debugging and unit tests */
+    virtual void print(const std::string& str = "") const = 0;
+
     /** Return the dimensionality of the tangent space of this value.  This is
      * the dimensionality of \c delta passed into retract() and of the vector
      * returned by localCoordinates().
@@ -136,57 +139,6 @@ namespace gtsam {
     /** Virutal destructor */
     virtual ~Value() {}
 
-  protected:
-    /** This is a convenience function to make it easy for you to implement the
-     * generic Value inferface, see the example at the top of the Value
-     * documentation.
-     * @param value1 The object on which to call equals, stored as a derived class pointer
-     * @param value2 The argument to pass to the derived equals function, strored as a Value reference
-     * @return The result of equals of the derived class
-     */
-    template<class Derived>
-    static bool CallDerivedEquals(const Derived* value1, const Value& value2, double tol) {
-      // Cast the base class Value pointer to a derived class pointer
-      const Derived& derivedValue2 = dynamic_cast<const Derived&>(value2);
-
-      // Return the result of calling equals on the derived class
-      return value1->equals(derivedValue2, tol);
-    }
-
-    /** This is a convenience function to make it easy for you to implement the
-     * generic Value inferface, see the example at the top of the Value
-     * documentation.
-     * @param derived A pointer to the derived class on which to call retract
-     * @param delta The delta vector to pass to the derived retract
-     * @return The result of retract on the derived class, stored as a Value pointer
-     */
-    template<class Derived>
-    static std::auto_ptr<Value> CallDerivedRetract(const Derived* derived, const Vector& delta) {
-      // Call retract on the derived class
-      const Derived retractResult = derived->retract(delta);
-
-      // Create a Value pointer copy of the result
-      std::auto_ptr<Value> resultAsValue(new Derived(retractResult));
-
-      // Return the pointer to the Value base class
-      return resultAsValue;
-    }
-
-    /** This is a convenience function to make it easy for you to implement the
-     * generic Value inferface, see the example at the top of the Value
-     * documentation.
-     * @param value1 The object on which to call localCoordinates, stored as a derived class pointer
-     * @param value2 The argument to pass to the derived localCoordinates function, stored as a Value reference
-     * @return The result of localCoordinates of the derived class
-     */
-    template<class Derived>
-    static Vector CallDerivedLocalCoordinates(const Derived* value1, const Value& value2) {
-      // Cast the base class Value pointer to a derived class pointer
-      const Derived& derivedValue2 = dynamic_cast<const Derived&>(value2);
-
-      // Return the result of calling localCoordinates on the derived class
-      return value1->localCoordinates(derivedValue2);
-    }
   };
 
 } /* namespace gtsam */
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index d2e72abeb..18f370941 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -32,7 +32,7 @@
 #endif
 #endif
 
-#include <gtsam/base/Value.h>
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/3rdparty/Eigen/Eigen/Geometry>
 
@@ -50,7 +50,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Rot3 : public Value {
+  class Rot3 : public DerivedValue<Rot3> {
   public:
     static const size_t dimension = 3;
 
@@ -178,11 +178,6 @@ namespace gtsam {
     static Rot3 rodriguez(double wx, double wy, double wz)
       { return rodriguez(Vector_(3,wx,wy,wz));}
 
-    /**
-     * Create a duplicate object returned as a pointer to the generic Value interface
-     */
-    virtual std::auto_ptr<Value> clone_() const { return std::auto_ptr<Value>(new Rot3(*this)); }
-
     /// @}
     /// @name Testable
     /// @{
@@ -193,9 +188,6 @@ namespace gtsam {
     /** equals with an tolerance */
     bool equals(const Rot3& p, double tol = 1e-9) const;
 
-    /** equals implementing generic Value interface */
-    virtual bool equals_(const Value& p, double tol = 1e-9) const { return CallDerivedEquals(this, p, tol); }
-
     /// @}
     /// @name Group
     /// @{
@@ -247,7 +239,7 @@ namespace gtsam {
     static size_t Dim() { return dimension; }
 
     /// return dimensionality of tangent space, DOF = 3
-    virtual size_t dim() const { return dimension; }
+    size_t dim() const { return dimension; }
 
     /**
      * The method retract() is used to map from the tangent space back to the manifold.
@@ -277,12 +269,6 @@ namespace gtsam {
     /// Returns local retract coordinates in neighborhood around current rotation
     Vector localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 
-    /// Generic Value interface version of retract
-    virtual std::auto_ptr<Value> retract_(const Vector& omega) const { return CallDerivedRetract(this, omega); }
-
-    /// Generic Value interface version of localCoordinates
-    virtual Vector localCoordinates_(const Value& value) const { return CallDerivedLocalCoordinates(this, value); }
-
     /// @}
     /// @name Lie Group
     /// @{

From 6dde91f63686104d2f175dd063b59ef61cb4c431 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 27 Jan 2012 15:34:54 +0000
Subject: [PATCH 13/88] More implementation for DynamicValues - switched to
 ptr_map

---
 gtsam/base/DerivedValue.h                   | 16 +++--
 gtsam/base/Value.h                          |  2 +-
 gtsam/nonlinear/DynamicValues-inl.h         | 46 +++++++------
 gtsam/nonlinear/DynamicValues.cpp           | 72 +++++++++++++++------
 gtsam/nonlinear/DynamicValues.h             | 41 ++++++++++--
 gtsam/nonlinear/tests/testDynamicValues.cpp |  5 +-
 6 files changed, 125 insertions(+), 57 deletions(-)

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index d0608059e..c30258919 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -17,15 +17,16 @@ private:
 	/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
 	struct PoolTag { };
 
-public:
-
+protected:
 	DerivedValue() {}
 
+public:
+
 	virtual ~DerivedValue() {}
 
   /**
    * Create a duplicate object returned as a pointer to the generic Value interface.
-   * For the shake of performance, this function use singleton pool allocator instead of the normal heap allocator
+   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
    */
   virtual Value* clone_() const {
   	void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
@@ -34,9 +35,10 @@ public:
   }
 
   /**
-   * Create a duplicate object returned as a pointer to the generic Value interface
+   * Destroy and deallocate this object, only if it was originally allocated using clone_().
    */
   virtual void deallocate_() const {
+    this->~Value();
   	boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
   }
 
@@ -47,16 +49,16 @@ public:
 
     // Return the result of calling equals on the derived class
     return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
-//  	return CallDerivedEquals(this, p, tol);
   }
 
   /// Generic Value interface version of retract
-	virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
+	virtual Value* retract_(const Vector& delta) const {
     // Call retract on the derived class
     const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
 
     // Create a Value pointer copy of the result
-    std::auto_ptr<Value> resultAsValue(new DERIVED(retractResult));
+    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+    Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
 
     // Return the pointer to the Value base class
     return resultAsValue;
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index dcb4a9c16..1e3c809c3 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -126,7 +126,7 @@ namespace gtsam {
      * @param delta The delta vector in the tangent space of this value, by
      * which to increment this value.
      */
-    virtual std::auto_ptr<Value> retract_(const Vector& delta) const = 0;
+    virtual Value* retract_(const Vector& delta) const = 0;
 
     /** Compute the coordinates in the tangent space of this value that
      * retract() would map to \c value.
diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index b1d67de08..3c30b7c41 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -29,17 +29,17 @@
 namespace gtsam {
 
   /* ************************************************************************* */
-  const char* DynamicValuesIncorrectType::what() const throw() {
-    if(message_.empty())
-      message_ =
-          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
-          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
-    return message_.c_str();
-  }
+  class ValueCloneAllocator {
+  public:
+    static Value* allocate_clone(const Value& a) { return a.clone_(); }
+    static void deallocate_clone(const Value* a) { a->deallocate_(); }
+  private:
+    ValueCloneAllocator() {}
+  };
 
   /* ************************************************************************* */
-  template<typename Value>
-  const Value& DynamicValues::at(const Symbol& j) const {
+  template<typename ValueType>
+  const ValueType& DynamicValues::at(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
@@ -48,11 +48,11 @@ namespace gtsam {
       throw DynamicValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    if(typeid(*item->second) != typeid(Value))
-      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+    if(typeid(*item->second) != typeid(ValueType))
+      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
 
     // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-    return static_cast<const Value&>(*item->second);
+    return static_cast<const ValueType&>(*item->second);
   }
 
   /* ************************************************************************* */
@@ -66,18 +66,18 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<typename Value>
-  boost::optional<const Value&> DynamicValues::exists(const Symbol& j) const {
+  template<typename ValueType>
+  boost::optional<const ValueType&> DynamicValues::exists(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
     if(item != values_.end()) {
       // Check the type and throw exception if incorrect
-      if(typeid(*item->second) != typeid(Value))
-        throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+      if(typeid(*item->second) != typeid(ValueType))
+        throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
 
       // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-      return static_cast<const Value&>(*item->second);
+      return static_cast<const ValueType&>(*item->second);
     } else {
       return boost::none;
     }
@@ -96,7 +96,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   void DynamicValues::insert(const Symbol& j, const ValueType& val) {
-    std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, new ValueType(val)));
+    std::pair<iterator,bool> insertResult = values_.insert(j, val);
     if(!insertResult.second)
       throw DynamicValuesKeyAlreadyExists(j);
   }
@@ -104,9 +104,17 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   void DynamicValues::update(const Symbol& j, const ValueType& val) {
+    // Find the value to update
     iterator item = values_.find(j);
     if(item == values_.end())
       throw DynamicValuesKeyDoesNotExist("update", j);
-    item->second = val.clone_();
+
+    // Cast to the derived type
+    if(typeid(*item->second) != typeid(Value))
+      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+    ValueType& valueAsDerived = static_cast<ValueType&>(*item->second);
+
+    // Assign
+    valueAsDerived = val;
   }
 }
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index 1e7996788..c77894666 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -41,10 +41,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void DynamicValues::print(const string& str) const {
-    cout << str << "DynamicValues with " << size() << " values:\n" << endl;
-    BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) {
-      cout << "  " << (string)key_value.first << ": ";
-      key_value.second->print("");
+    cout << str << "Values with " << size() << " values:\n" << endl;
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      cout << "  " << (string)key_value->first << ": ";
+      key_value->second->print("");
     }
   }
 
@@ -77,10 +77,12 @@ namespace gtsam {
   DynamicValues DynamicValues::retract(const VectorValues& delta, const Ordering& ordering) const {
     DynamicValues result;
 
-    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
-      const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value
-      ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract
-      result.values_[key_value.first] = retractedValue; // Add retracted result directly to result values
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
+      Symbol key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
+      Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract
+      result.values_.insert(key, retractedValue); // Add retracted result directly to result values
+      retractedValue->deallocate_();
     }
 
     return result;
@@ -100,21 +102,23 @@ namespace gtsam {
     for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
       if(it1->first != it2->first)
         throw DynamicValuesMismatched(); // If keys do not match
-      result[ordering[it1->first]] = it1->second->localCoordinates_(*it2->second); // Will throw a dynamic_cast exception if types do not match
+      // Will throw a dynamic_cast exception if types do not match
+      result.insert(ordering[it1->first], it1->second->localCoordinates_(*it2->second));
     }
   }
 
   /* ************************************************************************* */
   void DynamicValues::insert(const DynamicValues& values) {
-    BOOST_FOREACH(const KeyValuePair& key_value, values.values_) {
-      values_.insert(make_pair(key_value.first, key_value.second->clone_()));
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
+      values_.insert(key, key_value->second->clone_());
     }
   }
 
   /* ************************************************************************* */
   void DynamicValues::update(const DynamicValues& values) {
-    BOOST_FOREACH(const KeyValuePair& key_value, values) {
-      this->update(key_value.first, *key_value.second);
+    for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+      this->update(key_value->first, *key_value->second);
     }
   }
 
@@ -128,9 +132,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   FastList<Symbol> DynamicValues::keys() const {
-    return FastList<Symbol>(
-        boost::make_transform_iterator(values_.begin(), boost::bind(&KeyValuePair::first, _1)),
-        boost::make_transform_iterator(values_.end(), boost::bind(&KeyValuePair::first, _1)));
+    FastList<Symbol> result;
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value)
+      result.push_back(key_value->first);
+    return result;
   }
 
   /* ************************************************************************* */
@@ -146,18 +151,45 @@ namespace gtsam {
     // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
     result.assign(
         boost::make_transform_iterator(values_.begin(),
-            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))),
+            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
         boost::make_transform_iterator(values_.begin(),
-            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))));
+            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
     return result;
   }
 
   /* ************************************************************************* */
   Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar) const {
     Ordering::shared_ptr ordering(new Ordering);
-    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
-      ordering->insert(key_value.first, firstVar++);
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      ordering->insert(key_value->first, firstVar++);
     }
     return ordering;
   }
+
+  /* ************************************************************************* */
+  const char* DynamicValuesKeyAlreadyExists::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  const char* DynamicValuesKeyDoesNotExist::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to " + std::string(operation_) + " the key \"" +
+          (std::string)key_ + "\", which does not exist in the Values.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  const char* DynamicValuesIncorrectType::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
+          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
+    return message_.c_str();
+  }
+
 }
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index 745384cd7..de9a42b84 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -27,6 +27,9 @@
 #include <string>
 #include <utility>
 
+#include <boost/pool/pool_alloc.hpp>
+#include <boost/ptr_container/ptr_map.hpp>
+
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/linear/VectorValues.h>
@@ -35,15 +38,39 @@
 
 namespace gtsam {
 
+  // Forward declarations
+  class ValueCloneAllocator;
+
+/**
+  * A non-templated config holding any types of Manifold-group elements.  A
+  * values structure is a map from keys to values. It is used to specify the
+  * value of a bunch of variables in a factor graph. A Values is a values
+  * structure which can hold variables that are elements on manifolds, not just
+  * vectors. It then, as a whole, implements a aggregate type which is also a
+  * manifold element, and hence supports operations dim, retract, and
+  * localCoordinates.
+  */
   class DynamicValues {
 
   private:
 
-    typedef std::auto_ptr<const Value> ValuePtr;
-    typedef FastMap<Symbol, std::auto_ptr<const Value> > KeyValueMap;
-    typedef KeyValueMap::value_type KeyValuePair;
+    // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
+    // below) to clone and deallocate the Value objects, and a boost
+    // fast_pool_allocator to allocate map nodes.  In this way, all memory is
+    // allocated in a boost memory pool.
+    typedef boost::ptr_map<
+        Symbol,
+        Value,
+        std::less<Symbol>,
+        ValueCloneAllocator,
+        boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
+
+    // The member to store the values, see just above
     KeyValueMap values_;
 
+    // Type obtained by iterating
+    typedef KeyValueMap::const_iterator::value_type KeyValuePair;
+
   public:
 
     typedef KeyValueMap::iterator iterator;
@@ -75,8 +102,8 @@ namespace gtsam {
      * throws DynamicValuesIncorrectType if this requested type is not correct.
      * @return A const reference to the stored value
      */
-    template<typename Value>
-    const Value& at(const Symbol& j) const;
+    template<typename ValueType>
+    const ValueType& at(const Symbol& j) const;
 
     /** Retrieve a variable using a special key (typically TypedSymbol), which
      * contains the type of the value associated with the key, and which must
@@ -102,8 +129,8 @@ namespace gtsam {
      * \c Value if the key does exist, or boost::none if it does not exist.
      * Throws DynamicValuesIncorrectType if the value type associated with the
      * requested key does not match the stored value type. */
-    template<typename Value>
-    boost::optional<const Value&> exists(const Symbol& j) const;
+    template<typename ValueType>
+    boost::optional<const ValueType&> exists(const Symbol& j) const;
 
     /** Check if a value with key \c j exists, returns the value with type
      * \c Value if the key does exist, or boost::none if it does not exist.
diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp
index 4cd9e8068..cd76389ec 100644
--- a/gtsam/nonlinear/tests/testDynamicValues.cpp
+++ b/gtsam/nonlinear/tests/testDynamicValues.cpp
@@ -29,7 +29,6 @@ using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
 typedef TypedSymbol<LieVector, 'v'> VecKey;
-typedef DynamicValues<VecKey> DynamicValues;
 
 VecKey key1(1), key2(2), key3(3), key4(4);
 
@@ -102,7 +101,7 @@ TEST( DynamicValues, insert_bad )
   cfg2.insert(key2, v3);
   cfg2.insert(key3, v4);
 
-  CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
+  CHECK_EXCEPTION(cfg1.insert(cfg2), DynamicValuesKeyAlreadyExists);
 }
 
 /* ************************************************************************* */
@@ -237,7 +236,7 @@ TEST(DynamicValues, exists_)
 	config0.insert(key1, Vector_(1, 1.));
 	config0.insert(key2, Vector_(1, 2.));
 
-	boost::optional<LieVector> v = config0.exists_(key1);
+	boost::optional<const LieVector&> v = config0.exists(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
 }
 

From 0eaf241340ece5f14dc4a97fcf40fdf4cf24c21b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 27 Jan 2012 19:50:46 +0000
Subject: [PATCH 14/88] (in branch) added DerivedValue base class to geometry
 objects

---
 gtsam/geometry/Cal3Bundler.h       | 3 ++-
 gtsam/geometry/Cal3DS2.h           | 3 ++-
 gtsam/geometry/Cal3_S2.h           | 3 ++-
 gtsam/geometry/Cal3_S2Stereo.h     | 2 +-
 gtsam/geometry/CalibratedCamera.h  | 3 ++-
 gtsam/geometry/CalibratedCameraT.h | 4 +++-
 gtsam/geometry/GeneralCameraT.h    | 3 ++-
 gtsam/geometry/Point2.h            | 4 +++-
 gtsam/geometry/Point3.h            | 3 ++-
 gtsam/geometry/Pose2.h             | 3 ++-
 gtsam/geometry/Pose3.h             | 3 ++-
 gtsam/geometry/Rot2.h              | 4 +++-
 gtsam/geometry/SimpleCamera.h      | 3 ++-
 gtsam/geometry/StereoCamera.h      | 2 ++
 gtsam/geometry/StereoPoint2.h      | 3 ++-
 15 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 9c7561f61..9e7b4c327 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,7 +28,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Cal3Bundler {
+class Cal3Bundler : public DerivedValue<Cal3Bundler> {
 
 private:
 	double f_, k1_, k2_ ;
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 437a68a68..86a397daa 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,7 +28,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Cal3DS2 {
+class Cal3DS2 : public DerivedValue<Cal3DS2> {
 
 private:
 
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 71adc58bc..29b905932 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -30,7 +31,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Cal3_S2 {
+	class Cal3_S2 : public DerivedValue<Cal3_S2> {
 	private:
 		double fx_, fy_, s_, u0_, v0_;
 
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index a8366e35f..31265fbac 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -26,7 +26,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Cal3_S2Stereo: public Cal3_S2 {
+	class Cal3_S2Stereo: public DerivedValue<Cal3_S2Stereo>, public Cal3_S2 {
 	private:
 		double b_;
 
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index a53cb52df..f1e647046 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 
@@ -35,7 +36,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class CalibratedCamera {
+	class CalibratedCamera : public DerivedValue<CalibratedCamera> {
 	private:
 		Pose3 pose_; // 6DOF pose
 
diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h
index 6a70905a4..fde771398 100644
--- a/gtsam/geometry/CalibratedCameraT.h
+++ b/gtsam/geometry/CalibratedCameraT.h
@@ -8,6 +8,8 @@
 #pragma once
 
 #include <boost/optional.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 
@@ -23,7 +25,7 @@ namespace gtsam {
    * \nosubgrouping
    */
   template <typename Calibration>
-  class CalibratedCameraT {
+  class CalibratedCameraT : public DerivedValue<CalibratedCameraT<Calibration> > {
   private:
     Pose3 pose_; // 6DOF pose
     Calibration k_;
diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h
index d93c56472..6852fa475 100644
--- a/gtsam/geometry/GeneralCameraT.h
+++ b/gtsam/geometry/GeneralCameraT.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
@@ -31,7 +32,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template <typename Camera, typename Calibration>
-class GeneralCameraT {
+class GeneralCameraT : public DerivedValue<GeneralCameraT<Camera, Calibration> > {
 
 private:
 	Camera calibrated_; // Calibrated camera
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 558d8bd4d..4a8be8edf 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -18,6 +18,8 @@
 #pragma once
 
 #include <boost/serialization/nvp.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Lie.h>
 
@@ -30,7 +32,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Point2 {
+class Point2 : DerivedValue<Point2> {
 public:
 	/// dimension of the variable - used to autodetect sizes
 	static const size_t dimension = 2;
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 269331490..73274c676 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -24,6 +24,7 @@
 #include <boost/serialization/nvp.hpp>
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
 
 namespace gtsam {
@@ -33,7 +34,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Point3 {
+  class Point3 : public DerivedValue<Point3> {
   public:
 	  /// dimension of the variable - used to autodetect sizes
 	  static const size_t dimension = 3;
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 506ae59c9..85b2826dc 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -22,6 +22,7 @@
 
 #include <boost/optional.hpp>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
 
@@ -32,7 +33,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Pose2 {
+class Pose2 : public DerivedValue<Pose2> {
 
 public:
 	static const size_t dimension = 3;
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 48933a691..2cf836800 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -22,6 +22,7 @@
 #define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
 #endif
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
 
@@ -34,7 +35,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Pose3 {
+  class Pose3 : public DerivedValue<Pose3> {
   public:
     static const size_t dimension = 6;
 
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index eb6d1f1e9..b8cb4255d 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -19,6 +19,8 @@
 #pragma once
 
 #include <boost/optional.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -29,7 +31,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Rot2 {
+	class Rot2 : public DerivedValue<Rot2> {
 
 	public:
 		/** get the dimension by the type */
diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index 88eabb0f5..ae80ad96e 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 
@@ -29,7 +30,7 @@ namespace gtsam {
 	 * to produce measurements in pixels.
 	 * Not a sublass as a SimpleCamera *is not* a CalibratedCamera.
 	 */
-	class SimpleCamera {
+	class SimpleCamera : public DerivedValue<SimpleCamera> {
 	private:
 		CalibratedCamera calibrated_; // Calibrated camera
 		Cal3_S2 K_; // Calibration
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index cd3dbd19e..f5e07e66c 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -18,6 +18,8 @@
 #pragma once
 
 #include <boost/tuple/tuple.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/StereoPoint2.h>
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 0bdf37e0e..63538b6d5 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,7 +28,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class StereoPoint2 {
+	class StereoPoint2 : public DerivedValue<StereoPoint2> {
 	public:
 		static const size_t dimension = 3;
 	private:

From 79a3855316e9bd0de072b6704ecc91d9f3180259 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 27 Jan 2012 21:48:23 +0000
Subject: [PATCH 15/88] testDynamicValues passed

---
 gtsam/base/DerivedValue.cpp                 |  21 ---
 gtsam/base/LieVector.h                      |   3 +-
 gtsam/geometry/Cal3Bundler.h                |   2 +-
 gtsam/geometry/Cal3DS2.h                    |   2 +-
 gtsam/geometry/Cal3_S2Stereo.h              |   2 +-
 gtsam/geometry/CalibratedCamera.h           |   5 +
 gtsam/geometry/Point2.h                     |   2 +-
 gtsam/geometry/SimpleCamera.h               |   8 +-
 gtsam/nonlinear/DynamicValues-inl.h         |  24 ---
 gtsam/nonlinear/DynamicValues.cpp           |  29 +++-
 gtsam/nonlinear/DynamicValues.h             |  10 +-
 gtsam/nonlinear/Makefile.am                 |   2 +-
 gtsam/nonlinear/tests/testDynamicValues.cpp | 157 +++++++++-----------
 13 files changed, 117 insertions(+), 150 deletions(-)
 delete mode 100644 gtsam/base/DerivedValue.cpp

diff --git a/gtsam/base/DerivedValue.cpp b/gtsam/base/DerivedValue.cpp
deleted file mode 100644
index 386919d6f..000000000
--- a/gtsam/base/DerivedValue.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * DerivedValue.cpp
- *
- *  Created on: Jan 26, 2012
- *      Author: thduynguyen
- */
-
-#include "DerivedValue.h"
-
-namespace gtsam {
-
-DerivedValue::DerivedValue() {
-	// TODO Auto-generated constructor stub
-
-}
-
-DerivedValue::~DerivedValue() {
-	// TODO Auto-generated destructor stub
-}
-
-} /* namespace gtsam */
diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h
index 5a1b3d580..6a6b3408d 100644
--- a/gtsam/base/LieVector.h
+++ b/gtsam/base/LieVector.h
@@ -19,13 +19,14 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/DerivedValue.h>
 
 namespace gtsam {
 
 /**
  * LieVector is a wrapper around vector to allow it to be a Lie type
  */
-struct LieVector : public Vector {
+struct LieVector : public Vector, public DerivedValue<LieVector> {
 
 	/** default constructor - should be unnecessary */
 	LieVector() {}
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 9e7b4c327..65dd1a19a 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -95,7 +95,7 @@ public:
 	Vector localCoordinates(const Cal3Bundler& T2) const ;
 
 	///TODO: comment
-	int dim() const { return 3 ; }	//TODO: make a final dimension variable (also, usually size_t in other classes  e.g. Pose2)
+	virtual size_t dim() const { return 3 ; }	//TODO: make a final dimension variable (also, usually size_t in other classes  e.g. Pose2)
 
 	///TODO: comment
 	static size_t Dim() { return 3; }	//TODO: make a final dimension variable
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 86a397daa..0044f1c35 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -98,7 +98,7 @@ public:
 	Vector localCoordinates(const Cal3DS2& T2) const ;
 
 	///TODO: comment
-	int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
+	virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
 
 	///TODO: comment
 	static size_t Dim() { return 9; }	//TODO: make a final dimension variable
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 31265fbac..a8366e35f 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -26,7 +26,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Cal3_S2Stereo: public DerivedValue<Cal3_S2Stereo>, public Cal3_S2 {
+	class Cal3_S2Stereo: public Cal3_S2 {
 	private:
 		double b_;
 
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index f1e647046..7c98d522e 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -62,6 +62,11 @@ namespace gtsam {
 		/// @name Testable
 		/// @{
 
+		virtual void print(const std::string& s = "") const {
+			std::cout << s;
+			pose_.print("pose: ");
+		}
+
 		/// check equality to another camera
 		bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
 			return pose_.equals(camera.pose(), tol) ;
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 4a8be8edf..45b9de308 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -32,7 +32,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Point2 : DerivedValue<Point2> {
+class Point2 : public DerivedValue<Point2> {
 public:
 	/// dimension of the variable - used to autodetect sizes
 	static const size_t dimension = 2;
diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index ae80ad96e..2763d6a95 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -30,7 +30,7 @@ namespace gtsam {
 	 * to produce measurements in pixels.
 	 * Not a sublass as a SimpleCamera *is not* a CalibratedCamera.
 	 */
-	class SimpleCamera : public DerivedValue<SimpleCamera> {
+	class SimpleCamera {
 	private:
 		CalibratedCamera calibrated_; // Calibrated camera
 		Cal3_S2 K_; // Calibration
@@ -77,6 +77,12 @@ namespace gtsam {
 			    boost::optional<Matrix&> H1 = boost::none,
 			    boost::optional<Matrix&> H2 = boost::none) const;
 
+		virtual void print(const std::string& s = "") const {
+			std::cout << s;
+			calibrated_.print("calibrated: ");
+			K_.print("K: ");
+		}
+
 		bool equals(const SimpleCamera& X, double tol=1e-9) const {
 			return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol);
 		}
diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h
index 3c30b7c41..7686c2293 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/DynamicValues-inl.h
@@ -93,28 +93,4 @@ namespace gtsam {
     return exists<typename TypedKey::Value>(symbol);
   }
 
-  /* ************************************************************************* */
-  template<class ValueType>
-  void DynamicValues::insert(const Symbol& j, const ValueType& val) {
-    std::pair<iterator,bool> insertResult = values_.insert(j, val);
-    if(!insertResult.second)
-      throw DynamicValuesKeyAlreadyExists(j);
-  }
-
-  /* ************************************************************************* */
-  template<class ValueType>
-  void DynamicValues::update(const Symbol& j, const ValueType& val) {
-    // Find the value to update
-    iterator item = values_.find(j);
-    if(item == values_.end())
-      throw DynamicValuesKeyDoesNotExist("update", j);
-
-    // Cast to the derived type
-    if(typeid(*item->second) != typeid(Value))
-      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
-    ValueType& valueAsDerived = static_cast<ValueType&>(*item->second);
-
-    // Assign
-    valueAsDerived = val;
-  }
 }
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index c77894666..5b8d89a83 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -82,7 +82,6 @@ namespace gtsam {
       Symbol key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
       Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract
       result.values_.insert(key, retractedValue); // Add retracted result directly to result values
-      retractedValue->deallocate_();
     }
 
     return result;
@@ -107,14 +106,36 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  void DynamicValues::insert(const Symbol& j, const Value& val) {
+  	Symbol key = j; // Non-const duplicate to deal with non-const insert argument
+  	std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
+  	if(!insertResult.second)
+  		throw DynamicValuesKeyAlreadyExists(j);
+  }
+
   /* ************************************************************************* */
   void DynamicValues::insert(const DynamicValues& values) {
-    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+    for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
       Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
-      values_.insert(key, key_value->second->clone_());
+      insert(key, *key_value->second);
     }
   }
 
+  /* ************************************************************************* */
+  void DynamicValues::update(const Symbol& j, const Value& val) {
+  	// Find the value to update
+  	iterator item = values_.find(j);
+  	if(item == values_.end())
+  		throw DynamicValuesKeyDoesNotExist("update", j);
+
+  	// Cast to the derived type
+  	if(typeid(*item->second) != typeid(val))
+  		throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(val));
+
+  	values_.replace(item, val.clone_());
+  }
+
   /* ************************************************************************* */
   void DynamicValues::update(const DynamicValues& values) {
     for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
@@ -152,7 +173,7 @@ namespace gtsam {
     result.assign(
         boost::make_transform_iterator(values_.begin(),
             boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
-        boost::make_transform_iterator(values_.begin(),
+        boost::make_transform_iterator(values_.end(),
             boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
     return result;
   }
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index de9a42b84..7b19c963f 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -178,19 +178,17 @@ namespace gtsam {
     // imperative methods:
 
     /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
-    template<class ValueType>
-    void insert(const Symbol& j, const ValueType& val);
+    void insert(const Symbol& j, const Value& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
     void insert(const DynamicValues& values);
 
+    /** single element change of existing element */
+    void update(const Symbol& j, const Value& val);
+
     /** update the current available values without adding new ones */
     void update(const DynamicValues& values);
 
-    /** single element change of existing element */
-    template<class ValueType>
-    void update(const Symbol& j, const ValueType& val);
-
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(const Symbol& j);
 
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index defcd51e4..f5e448a31 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -63,7 +63,7 @@ AM_CXXFLAGS =
 #----------------------------------------------------------------------------------------------------
 TESTS = $(check_PROGRAMS)
 AM_LDFLAGS += $(boost_serialization)
-LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
+LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la
 LDADD += ../../CppUnitLite/libCppUnitLite.a
 AM_DEFAULT_SOURCE_EXT = .cpp
 
diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp
index cd76389ec..f7493f1b1 100644
--- a/gtsam/nonlinear/tests/testDynamicValues.cpp
+++ b/gtsam/nonlinear/tests/testDynamicValues.cpp
@@ -22,6 +22,7 @@ using namespace boost::assign;
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/LieVector.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/DynamicValues.h>
 
 using namespace gtsam;
@@ -36,7 +37,7 @@ VecKey key1(1), key2(2), key3(3), key4(4);
 TEST( DynamicValues, equals1 )
 {
   DynamicValues expected;
-  Vector v = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v(3, 5.0, 6.0, 7.0);
   expected.insert(key1,v);
   DynamicValues actual;
   actual.insert(key1,v);
@@ -47,8 +48,8 @@ TEST( DynamicValues, equals1 )
 TEST( DynamicValues, equals2 )
 {
   DynamicValues cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 5.0, 6.0, 8.0);
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -59,8 +60,8 @@ TEST( DynamicValues, equals2 )
 TEST( DynamicValues, equals_nan )
 {
   DynamicValues cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, inf, inf, inf);
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, inf, inf, inf);
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -71,10 +72,10 @@ TEST( DynamicValues, equals_nan )
 TEST( DynamicValues, insert_good )
 {
   DynamicValues cfg1, cfg2, expected;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
+  LieVector v4(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key3, v4);
@@ -85,17 +86,17 @@ TEST( DynamicValues, insert_good )
   expected.insert(key2, v2);
   expected.insert(key3, v4);
 
-  CHECK(assert_equal(cfg1, expected));
+  CHECK(assert_equal(expected, cfg1));
 }
 
 /* ************************************************************************* */
 TEST( DynamicValues, insert_bad )
 {
   DynamicValues cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
+  LieVector v4(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key2, v3);
@@ -108,8 +109,8 @@ TEST( DynamicValues, insert_bad )
 TEST( DynamicValues, update_element )
 {
   DynamicValues cfg;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
@@ -124,9 +125,9 @@ TEST( DynamicValues, update_element )
 //TEST(DynamicValues, dim_zero)
 //{
 //  DynamicValues config0;
-//  config0.insert(key1, Vector_(2, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
-//  LONGS_EQUAL(5,config0.dim());
+//  config0.insert(key1, LieVector(2, 2.0, 3.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+//  LONGS_EQUAL(5, config0.dim());
 //
 //  VectorValues expected;
 //  expected.insert(key1, zero(2));
@@ -138,8 +139,8 @@ TEST( DynamicValues, update_element )
 TEST(DynamicValues, expmap_a)
 {
   DynamicValues config0;
-  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
   Ordering ordering(*config0.orderingArbitrary());
   VectorValues increment(config0.dims(ordering));
@@ -147,94 +148,95 @@ TEST(DynamicValues, expmap_a)
   increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
 
   DynamicValues expected;
-  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
-  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 
   CHECK(assert_equal(expected, config0.retract(increment, ordering)));
 }
 
-///* ************************************************************************* */
-//TEST(DynamicValues, expmap_b)
-//{
-//  DynamicValues config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
-//
-//  Ordering ordering(*config0.orderingArbitrary());
-//  VectorValues increment(config0.dims(ordering));
-//  increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
-//
-//  DynamicValues expected;
-//  expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
-//
-//  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
-//}
+/* ************************************************************************* */
+TEST(DynamicValues, expmap_b)
+{
+  DynamicValues config0;
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
-///* ************************************************************************* */
+  Ordering ordering(*config0.orderingArbitrary());
+  VectorValues increment(config0.dims(ordering));
+  increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
+
+  DynamicValues expected;
+  expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
+}
+
+/* ************************************************************************* */
 //TEST(DynamicValues, expmap_c)
 //{
 //  DynamicValues config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 //
-//  Vector increment = Vector_(6,
+//  Vector increment = LieVector(6,
 //      1.0, 1.1, 1.2,
 //      1.3, 1.4, 1.5);
 //
 //  DynamicValues expected;
-//  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+//  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
+//  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 //
 //  CHECK(assert_equal(expected, config0.retract(increment)));
 //}
 
 /* ************************************************************************* */
-/*TEST(DynamicValues, expmap_d)
+TEST(DynamicValues, expmap_d)
 {
   DynamicValues config0;
-  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
 
-  DynamicValues<string,Pose2> poseconfig;
-  poseconfig.insert("p1", Pose2(1,2,3));
-  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
-  //poseconfig.print("poseconfig");
+  typedef TypedSymbol<Pose2, 'p'> PoseKey;
+  DynamicValues poseconfig;
+  poseconfig.insert(PoseKey(1), Pose2(1,2,3));
+  poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
+
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
-}*/
+}
 
 /* ************************************************************************* */
-/*TEST(DynamicValues, extract_keys)
+TEST(DynamicValues, extract_keys)
 {
 	typedef TypedSymbol<Pose2, 'x'> PoseKey;
-	DynamicValues<PoseKey, Pose2> config;
+	DynamicValues config;
 
 	config.insert(PoseKey(1), Pose2());
 	config.insert(PoseKey(2), Pose2());
 	config.insert(PoseKey(4), Pose2());
 	config.insert(PoseKey(5), Pose2());
 
-	list<PoseKey> expected, actual;
+	FastList<Symbol> expected, actual;
 	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
-	list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
 	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
 		CHECK(assert_equal(*itExp, *itAct));
 	}
-}*/
+}
 
 /* ************************************************************************* */
 TEST(DynamicValues, exists_)
 {
 	DynamicValues config0;
-	config0.insert(key1, Vector_(1, 1.));
-	config0.insert(key2, Vector_(1, 2.));
+	config0.insert(key1, LieVector(Vector_(1, 1.)));
+	config0.insert(key2, LieVector(Vector_(1, 2.)));
 
 	boost::optional<const LieVector&> v = config0.exists(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
@@ -244,41 +246,20 @@ TEST(DynamicValues, exists_)
 TEST(DynamicValues, update)
 {
 	DynamicValues config0;
-	config0.insert(key1, Vector_(1, 1.));
-	config0.insert(key2, Vector_(1, 2.));
+	config0.insert(key1, LieVector(1, 1.));
+	config0.insert(key2, LieVector(1, 2.));
 
 	DynamicValues superset;
-	superset.insert(key1, Vector_(1, -1.));
-	superset.insert(key2, Vector_(1, -2.));
-	superset.insert(key3, Vector_(1, -3.));
+	superset.insert(key1, LieVector(1, -1.));
+	superset.insert(key2, LieVector(1, -2.));
 	config0.update(superset);
 
 	DynamicValues expected;
-	expected.insert(key1, Vector_(1, -1.));
-	expected.insert(key2, Vector_(1, -2.));
+	expected.insert(key1, LieVector(1, -1.));
+	expected.insert(key2, LieVector(1, -2.));
 	CHECK(assert_equal(expected,config0));
 }
 
-/* ************************************************************************* */
-TEST(DynamicValues, dummy_initialization)
-{
-	typedef TypedSymbol<LieVector, 'z'> Key;
-
-	DynamicValues init1;
-	init1.insert(Key(1), Vector_(2, 1.0, 2.0));
-	init1.insert(Key(2), Vector_(2, 4.0, 3.0));
-
-	DynamicValues init2;
-	init2.insert(key1, Vector_(2, 1.0, 2.0));
-	init2.insert(key2, Vector_(2, 4.0, 3.0));
-
-	DynamicValues actual1(init2);
-	DynamicValues actual2(init1);
-
-	EXPECT(actual1.empty());
-	EXPECT(actual2.empty());
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From ee0b7f8a74985ac966ceab17bfd86a4f725dce1e Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 27 Jan 2012 23:00:29 +0000
Subject: [PATCH 16/88] remove VALUES in other classes. New testNonlinearFactor
 passed.

---
 gtsam/nonlinear/DynamicValues.h               |  17 ++
 gtsam/nonlinear/GaussianISAM2.h               |  10 +-
 gtsam/nonlinear/ISAM2-impl-inl.h              |  40 ++--
 gtsam/nonlinear/ISAM2-inl.h                   |  50 ++---
 gtsam/nonlinear/ISAM2.h                       |  20 +-
 gtsam/nonlinear/Makefile.am                   |   2 +-
 gtsam/nonlinear/NonlinearFactor.h             |  91 ++++----
 gtsam/nonlinear/NonlinearFactorGraph-inl.h    |  32 +--
 gtsam/nonlinear/NonlinearFactorGraph.h        |  19 +-
 gtsam/nonlinear/tests/testNonlinearFactor.cpp | 208 ++++++++++++++++++
 10 files changed, 352 insertions(+), 137 deletions(-)
 create mode 100644 gtsam/nonlinear/tests/testNonlinearFactor.cpp

diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index 7b19c963f..d11a23ec9 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -213,6 +213,23 @@ namespace gtsam {
     /** Create an array of variable dimensions using the given ordering */
     std::vector<size_t> dims(const Ordering& ordering) const;
 
+    /**
+     * Apply a class with an application operator() to a const_iterator over
+     * every <key,value> pair.  The operator must be able to handle such an
+     * iterator for every type in the Values, (i.e. through templating).
+     */
+    template<typename A>
+    void apply(A& operation) {
+      for(iterator it = begin(); it != end(); ++it)
+        operation(it);
+    }
+
+    template<typename A>
+    void apply(A& operation) const {
+      for(const_iterator it = begin(); it != end(); ++it)
+        operation(it);
+    }
+
     /**
      * Generate a default ordering, simply in key sort order.  To instead
      * create a fill-reducing ordering, use
diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h
index 4da1d3053..0bc6199a4 100644
--- a/gtsam/nonlinear/GaussianISAM2.h
+++ b/gtsam/nonlinear/GaussianISAM2.h
@@ -36,15 +36,15 @@ namespace gtsam {
  * variables.
  * @tparam GRAPH The NonlinearFactorGraph structure to store factors.  Defaults to standard NonlinearFactorGraph<VALUES>
  */
-template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
-class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
-  typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
+template <class GRAPH = NonlinearFactorGraph>
+class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
+  typedef ISAM2<GaussianConditional, GRAPH> Base;
 public:
   /** Create an empty ISAM2 instance */
-  GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
+  GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
 
   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
-  GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
+  GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
 
   void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
     boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index fc48720f9..8b0c0b99c 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -19,10 +19,10 @@ namespace gtsam {
 
 using namespace std;
 
-template<class CONDITIONAL, class VALUES, class GRAPH>
-struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
+template<class CONDITIONAL, class GRAPH>
+struct ISAM2<CONDITIONAL, GRAPH>::Impl {
 
-  typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
+  typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
 
   struct PartialSolveResult {
     typename ISAM2Type::sharedClique bayesTree;
@@ -45,7 +45,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
    * @param ordering Current ordering to be augmented with new variables
    * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
    */
-  static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
+  static void AddVariables(const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
 
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
@@ -95,7 +95,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
    * where we might expmap something twice, or expmap it but then not
    * recalculate its delta.
    */
-  static void ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
+  static void ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
       const Ordering& ordering, const std::vector<bool>& mask,
       boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
 
@@ -134,9 +134,9 @@ struct _VariableAdder {
 };
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
-    const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
+template<class CONDITIONAL, class GRAPH>
+void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
+    const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
   theta.insert(newTheta);
@@ -162,10 +162,10 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
+template<class CONDITIONAL, class GRAPH>
+FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
   FastSet<Index> indices;
-  BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
+  BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
     BOOST_FOREACH(const Symbol& key, factor->keys()) {
       indices.insert(ordering[key]);
     }
@@ -174,8 +174,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const O
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
+template<class CONDITIONAL, class GRAPH>
+FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
   FastSet<Index> relinKeys;
 
   if(relinearizeThreshold.type() == typeid(double)) {
@@ -202,8 +202,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
+template<class CONDITIONAL, class GRAPH>
+void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
   static const bool debug = false;
   // does the separator contain any of the variables?
   bool found = false;
@@ -256,8 +256,8 @@ struct _SelectiveExpmapAndClear {
 };
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
+template<class CONDITIONAL, class GRAPH>
+void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
     const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
   // If debugging, invalidate if requested, otherwise do not invalidate.
   // Invalidating means setting expmapped entries to Inf, to trigger assertions
@@ -271,9 +271,9 @@ void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
-ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
+template<class CONDITIONAL, class GRAPH>
+typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
+ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
     const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
 
   static const bool debug = ISDEBUG("ISAM2 recalculate");
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 338427b2c..231fbdea7 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -39,8 +39,8 @@ static const bool disableReordering = false;
 static const double batchThreshold = 0.65;
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
+template<class CONDITIONAL, class GRAPH>
+ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
     delta_(Permutation(), deltaUnpermuted_), params_(params) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@@ -48,8 +48,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
+template<class CONDITIONAL, class GRAPH>
+ISAM2<CONDITIONAL, GRAPH>::ISAM2():
     delta_(Permutation(), deltaUnpermuted_) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@@ -57,14 +57,14 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
+template<class CONDITIONAL, class GRAPH>
+FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
   static const bool debug = false;
   if(debug) cout << "Getting affected factors for ";
   if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
   if(debug) cout << endl;
 
-  FactorGraph<NonlinearFactor<VALUES> > allAffected;
+  FactorGraph<NonlinearFactor > allAffected;
   FastList<size_t> indices;
   BOOST_FOREACH(const Index key, keys) {
 //    const list<size_t> l = nonlinearFactors_.factors(key);
@@ -86,9 +86,9 @@ FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const Fas
 /* ************************************************************************* */
 // retrieve all factors that ONLY contain the affected variables
 // (note that the remaining stuff is summarized in the cached factors)
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
 FactorGraph<GaussianFactor>::shared_ptr
-ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
+ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
 
   tic(1,"getAffectedFactors");
   FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@@ -125,9 +125,9 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Ind
 
 /* ************************************************************************* */
 // find intermediate (linearized) factors from cache that are passed into the affected area
-template<class CONDITIONAL, class VALUES, class GRAPH>
-FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
+template<class CONDITIONAL, class GRAPH>
+FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
+ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
 
   static const bool debug = false;
 
@@ -151,8 +151,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
   return cachedBoundary;
 }
 
-template<class CONDITIONAL, class VALUES, class GRAPH>
-boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
+template<class CONDITIONAL, class GRAPH>
+boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
     const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
     const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
 
@@ -395,8 +395,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
+template<class CONDITIONAL, class GRAPH>
+ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
     const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
     const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
 
@@ -579,36 +579,36 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
+template<class CONDITIONAL, class GRAPH>
+DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
   // We use ExpmapMasked here instead of regular expmap because the former
   // handles Permuted<VectorValues>
-	VALUES ret(theta_);
+	DynamicValues ret(theta_);
   vector<bool> mask(ordering_.nVars(), true);
   Impl::ExpmapMasked(ret, delta_, ordering_, mask);
   return ret;
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
 template<class KEY>
-typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const {
+typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key) const {
   const Index index = getOrdering()[key];
   const SubVector delta = getDelta()[index];
   return getLinearizationPoint()[key].retract(delta);
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
+template<class CONDITIONAL, class GRAPH>
+DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
   VectorValues delta(theta_.dims(ordering_));
   optimize2(this->root(), delta);
   return theta_.retract(delta, ordering_);
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
+template<class CONDITIONAL, class GRAPH>
+VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
   VectorValues delta = *allocateVectorValues(isam);
   optimize2(isam.root(), delta);
   return delta;
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 7e2a51ffa..85db8b691 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -265,13 +265,13 @@ private:
  * estimate of all variables.
  *
  */
-template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
+template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
 class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
 
 protected:
 
   /** The current linearization point */
-  VALUES theta_;
+  DynamicValues theta_;
 
   /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
   VariableIndex variableIndex_;
@@ -313,8 +313,8 @@ private:
 public:
 
   typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
-  typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
-  typedef VALUES Values;
+  typedef ISAM2<CONDITIONAL> This; ///< This class
+  typedef DynamicValues Values;
   typedef GRAPH Graph;
 
   /** Create an empty ISAM2 instance */
@@ -367,19 +367,19 @@ public:
    * (Params::relinearizeSkip).
    * @return An ISAM2Result struct containing information about the update
    */
-  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(),
+  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const DynamicValues& newTheta = DynamicValues(),
       const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
       const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
       bool force_relinearize = false);
 
   /** Access the current linearization point */
-  const VALUES& getLinearizationPoint() const {return theta_;}
+  const DynamicValues& getLinearizationPoint() const {return theta_;}
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only
    * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
    */
-  VALUES calculateEstimate() const;
+  DynamicValues calculateEstimate() const;
 
   /** Compute an estimate for a single variable using its incomplete linear delta computed
    * during the last update.  This is faster than calling the no-argument version of
@@ -398,7 +398,7 @@ public:
 
   /** Compute an estimate using a complete delta computed by a full back-substitution.
    */
-  VALUES calculateBestEstimate() const;
+  DynamicValues calculateBestEstimate() const;
 
   /** Access the current delta, computed during the last call to update */
   const Permuted<VectorValues>& getDelta() const { return delta_; }
@@ -434,8 +434,8 @@ private:
 }; // ISAM2
 
 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam);
+template<class CONDITIONAL, class GRAPH>
+VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
 
 } /// namespace gtsam
 
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index f5e448a31..40a96ba5f 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -18,7 +18,7 @@ check_PROGRAMS =
 # Lie Groups
 headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
 sources += DynamicValues.cpp
-check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering
+check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
 
 # Nonlinear nonlinear
 headers += Key.h 
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index c65b4ac08..c392c05d7 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -31,6 +31,7 @@
 #include <gtsam/linear/SharedNoiseModel.h>
 #include <gtsam/linear/JacobianFactor.h>
 
+#include <gtsam/nonlinear/DynamicValues.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
@@ -56,18 +57,17 @@ inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vec
  * more general than just vectors, e.g., Rot3 or Pose3,
  * which are objects in non-linear manifolds (Lie groups).
  */
-template<class VALUES>
 class NonlinearFactor: public Factor<Symbol> {
 
 protected:
 
   // Some handy typedefs
   typedef Factor<Symbol> Base;
-  typedef NonlinearFactor<VALUES> This;
+  typedef NonlinearFactor This;
 
 public:
 
-  typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
+  typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
 
   /** Default constructor for I/O only */
   NonlinearFactor() {
@@ -107,7 +107,7 @@ public:
    * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
    * You can override this for systems with unusual noise models.
    */
-  virtual double error(const VALUES& c) const = 0;
+  virtual double error(const DynamicValues& c) const = 0;
 
   /** get the dimension of the factor (number of rows on linearization) */
   virtual size_t dim() const = 0;
@@ -122,11 +122,11 @@ public:
 	 * when the constraint is *NOT* fulfilled.
 	 * @return true if the constraint is active
 	 */
-	virtual bool active(const VALUES& c) const { return true; }
+	virtual bool active(const DynamicValues& c) const { return true; }
 
   /** linearize to a GaussianFactor */
   virtual boost::shared_ptr<GaussianFactor>
-  linearize(const VALUES& c, const Ordering& ordering) const = 0;
+  linearize(const DynamicValues& c, const Ordering& ordering) const = 0;
 
   /**
    * Create a symbolic factor using the given ordering to determine the
@@ -152,20 +152,19 @@ public:
 
  * The noise model is typically Gaussian, but robust and constrained error models are also supported.
  */
-template<class VALUES>
-class NoiseModelFactor: public NonlinearFactor<VALUES> {
+class NoiseModelFactor: public NonlinearFactor {
 
 protected:
 
   // handy typedefs
-  typedef NonlinearFactor<VALUES> Base;
-  typedef NoiseModelFactor<VALUES> This;
+  typedef NonlinearFactor Base;
+  typedef NoiseModelFactor This;
 
   SharedNoiseModel noiseModel_; /** Noise model */
 
 public:
 
-  typedef boost::shared_ptr<NoiseModelFactor<VALUES> > shared_ptr;
+  typedef boost::shared_ptr<NoiseModelFactor > shared_ptr;
 
   /** Default constructor for I/O only */
   NoiseModelFactor() {
@@ -212,7 +211,7 @@ public:
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const {
+  virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const {
     return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
   }
 
@@ -232,13 +231,13 @@ public:
    * If any of the optional Matrix reference arguments are specified, it should compute
    * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
    */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
 
   /**
    * Vector of errors, whitened
    * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
    */
-  Vector whitenedError(const VALUES& c) const {
+  Vector whitenedError(const DynamicValues& c) const {
     return noiseModel_->whiten(unwhitenedError(c));
   }
 
@@ -248,7 +247,7 @@ public:
    * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
    * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
    */
-  virtual double error(const VALUES& c) const {
+  virtual double error(const DynamicValues& c) const {
   	if (this->active(c))
   		return 0.5 * noiseModel_->distance(unwhitenedError(c));
   	else
@@ -260,7 +259,7 @@ public:
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
-  boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& x, const Ordering& ordering) const {
   	// Only linearize if the factor is active
 		if (!this->active(x))
 			return boost::shared_ptr<JacobianFactor>();
@@ -308,8 +307,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 1
  * variable.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY>
-class NonlinearFactor1: public NoiseModelFactor<VALUES> {
+template<class KEY>
+class NonlinearFactor1: public NoiseModelFactor {
 
 public:
 
@@ -321,8 +320,8 @@ protected:
   // The value of the key. Not const to allow serialization
   KEY key_;
 
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor1<VALUES, KEY> This;
+  typedef NoiseModelFactor Base;
+  typedef NonlinearFactor1<KEY> This;
 
 public:
 
@@ -344,7 +343,7 @@ public:
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X& x1 = x[key_];
       if(H) {
@@ -387,8 +386,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 2
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2>
-class NonlinearFactor2: public NoiseModelFactor<VALUES> {
+template<class KEY1, class KEY2>
+class NonlinearFactor2: public NoiseModelFactor {
 
 public:
 
@@ -402,8 +401,8 @@ protected:
   KEY1 key1_;
   KEY2 key2_;
 
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
+  typedef NoiseModelFactor Base;
+  typedef NonlinearFactor2<KEY1, KEY2> This;
 
 public:
 
@@ -428,7 +427,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X1& x1 = x[key1_];
       const X2& x2 = x[key2_];
@@ -475,8 +474,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 3
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3>
-class NonlinearFactor3: public NoiseModelFactor<VALUES> {
+template<class KEY1, class KEY2, class KEY3>
+class NonlinearFactor3: public NoiseModelFactor {
 
 public:
 
@@ -492,8 +491,8 @@ protected:
   KEY2 key2_;
   KEY3 key3_;
 
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
+  typedef NoiseModelFactor Base;
+  typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
 
 public:
 
@@ -520,7 +519,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
@@ -569,8 +568,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 4
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4>
-class NonlinearFactor4: public NoiseModelFactor<VALUES> {
+template<class KEY1, class KEY2, class KEY3, class KEY4>
+class NonlinearFactor4: public NoiseModelFactor {
 
 public:
 
@@ -588,8 +587,8 @@ protected:
   KEY3 key3_;
   KEY4 key4_;
 
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This;
+  typedef NoiseModelFactor Base;
+  typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
 
 public:
 
@@ -618,7 +617,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
   			return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -669,8 +668,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 5
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
-class NonlinearFactor5: public NoiseModelFactor<VALUES> {
+template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
+class NonlinearFactor5: public NoiseModelFactor {
 
 public:
 
@@ -690,8 +689,8 @@ protected:
   KEY4 key4_;
   KEY5 key5_;
 
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This;
+  typedef NoiseModelFactor Base;
+  typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
 
 public:
 
@@ -722,7 +721,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -776,8 +775,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 6
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
-class NonlinearFactor6: public NoiseModelFactor<VALUES> {
+template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
+class NonlinearFactor6: public NoiseModelFactor {
 
 public:
 
@@ -799,8 +798,8 @@ protected:
   KEY5 key5_;
   KEY6 key6_;
 
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
+  typedef NoiseModelFactor Base;
+  typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
 
 public:
 
@@ -833,7 +832,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph-inl.h
index 473107429..445dbf20e 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph-inl.h
@@ -29,20 +29,17 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
+	double NonlinearFactorGraph::probPrime(const DynamicValues& c) const {
 		return exp(-0.5 * error(c));
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
+	void NonlinearFactorGraph::print(const std::string& str) const {
 		Base::print(str);
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
+	double NonlinearFactorGraph::error(const DynamicValues& c) const {
 		double total_error = 0.;
 		// iterate over all the factors_ to accumulate the log probabilities
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
@@ -53,8 +50,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
+	std::set<Symbol> NonlinearFactorGraph::keys() const {
 		std::set<Symbol> keys;
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
 		  if(factor)
@@ -64,9 +60,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
-			const VALUES& config) const {
+	Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
+			const DynamicValues& config) const {
 
 		// Create symbolic graph and initial (iterator) ordering
 		SymbolicFactorGraph::shared_ptr symbolic;
@@ -91,8 +86,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(const Ordering& ordering) const {
+	SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
 		// Generate the symbolic factor graph
 		SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
 		symbolicfg->reserve(this->size());
@@ -108,21 +102,19 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
-			VALUES>::symbolic(const VALUES& config) const {
+	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
+		const DynamicValues& config) const {
 		// Generate an initial key ordering in iterator order
 		Ordering::shared_ptr ordering(config.orderingArbitrary());
 		return make_pair(symbolic(*ordering), ordering);
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
-	typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
-			const VALUES& config, const Ordering& ordering) const {
+	GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
+			const DynamicValues& config, const Ordering& ordering) const {
 
 		// create an empty linear FG
-		typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
+		GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
 		linearFG->reserve(this->size());
 
 		// linearize all factors
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 605842179..64b035bc0 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -35,14 +35,13 @@ namespace gtsam {
 	 * tangent vector space at the linearization point. Because the tangent space is a true
 	 * vector space, the config type will be an VectorValues in that linearized factor graph.
 	 */
-	template<class VALUES>
-	class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
+	class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
 
 	public:
 
-	  typedef FactorGraph<NonlinearFactor<VALUES> > Base;
-	  typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
-		typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
+	  typedef FactorGraph<NonlinearFactor> Base;
+	  typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
+		typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
 
     /** print just calls base class */
     void print(const std::string& str = "NonlinearFactorGraph: ") const;
@@ -51,10 +50,10 @@ namespace gtsam {
     std::set<Symbol> keys() const;
 
 		/** unnormalized error */
-		double error(const VALUES& c) const;
+		double error(const DynamicValues& c) const;
 
 		/** Unnormalized probability. O(n) */
-		double probPrime(const VALUES& c) const;
+		double probPrime(const DynamicValues& c) const;
 
 		template<class F>
 		void add(const F& factor) {
@@ -73,20 +72,20 @@ namespace gtsam {
 		 * ordering is found.
 		 */
 		std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
-		symbolic(const VALUES& config) const;
+		symbolic(const DynamicValues& config) const;
 
     /**
      * Compute a fill-reducing ordering using COLAMD.  This returns the
      * ordering and a VariableIndex, which can later be re-used to save
      * computation.
      */
-		Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
+		Ordering::shared_ptr orderingCOLAMD(const DynamicValues& config) const;
 
 		/**
 		 * linearize a nonlinear factor graph
 		 */
 		boost::shared_ptr<GaussianFactorGraph >
-				linearize(const VALUES& config, const Ordering& ordering) const;
+				linearize(const DynamicValues& config, const Ordering& ordering) const;
 
 	private:
 
diff --git a/gtsam/nonlinear/tests/testNonlinearFactor.cpp b/gtsam/nonlinear/tests/testNonlinearFactor.cpp
new file mode 100644
index 000000000..558c40036
--- /dev/null
+++ b/gtsam/nonlinear/tests/testNonlinearFactor.cpp
@@ -0,0 +1,208 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testDynamicValues.cpp
+ * @author Duy-Nguyen Ta
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/LieVector.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+typedef TypedSymbol<Pose2, 'p'> PoseKey;
+
+/* ************************************************* */
+class TestFactor1: public NonlinearFactor1<PoseKey> {
+	typedef NonlinearFactor1<PoseKey> Base;
+public:
+	TestFactor1() : Base(sharedSigmas(Vector_(1, 1.0)), 1) {}
+	virtual ~TestFactor1() {}
+
+	virtual Vector evaluateError(const Pose2& pose, boost::optional<Matrix&> H = boost::none) const {
+		if (H) *H = zeros(1,3);
+		return zero(1);
+	}
+};
+
+/* ************************************************************************* */
+TEST(NonlinearFactor, TestFactor1) {
+	TestFactor1 factor1;
+	Vector actualError = factor1.evaluateError(Pose2());
+	Vector expectedError = zero(1);
+	CHECK(assert_equal(expectedError, actualError));
+}
+
+/* ************************************************************************* */
+typedef TypedSymbol<LieVector, 'x'> TestKey;
+
+/* ************************************************************************* */
+class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
+public:
+  typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
+  TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
+  virtual ~TestFactor4() {}
+
+  virtual Vector
+    evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none) const {
+    if(H1) {
+      *H1 = Matrix_(1,1, 1.0);
+      *H2 = Matrix_(1,1, 2.0);
+      *H3 = Matrix_(1,1, 3.0);
+      *H4 = Matrix_(1,1, 4.0);
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NonlinearFactor4) {
+  TestFactor4 tf;
+  DynamicValues tv;
+  tv.insert(TestKey(1), LieVector(1, 1.0));
+  tv.insert(TestKey(2), LieVector(1, 2.0));
+  tv.insert(TestKey(3), LieVector(1, 3.0));
+  tv.insert(TestKey(4), LieVector(1, 4.0));
+  EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
+  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
+  LONGS_EQUAL(jf.keys()[0], 0);
+  LONGS_EQUAL(jf.keys()[1], 1);
+  LONGS_EQUAL(jf.keys()[2], 2);
+  LONGS_EQUAL(jf.keys()[3], 3);
+  EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
+  EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal(Vector_(1, -5.0), jf.getb()));
+}
+
+/* ************************************************************************* */
+class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> {
+public:
+  typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
+  virtual ~TestFactor5() {}
+
+  virtual Vector
+    evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none,
+        boost::optional<Matrix&> H5 = boost::none) const {
+    if(H1) {
+      *H1 = Matrix_(1,1, 1.0);
+      *H2 = Matrix_(1,1, 2.0);
+      *H3 = Matrix_(1,1, 3.0);
+      *H4 = Matrix_(1,1, 4.0);
+      *H5 = Matrix_(1,1, 5.0);
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NonlinearFactor5) {
+  TestFactor5 tf;
+  DynamicValues tv;
+  tv.insert(TestKey(1), LieVector(1, 1.0));
+  tv.insert(TestKey(2), LieVector(1, 2.0));
+  tv.insert(TestKey(3), LieVector(1, 3.0));
+  tv.insert(TestKey(4), LieVector(1, 4.0));
+  tv.insert(TestKey(5), LieVector(1, 5.0));
+  EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
+  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
+  LONGS_EQUAL(jf.keys()[0], 0);
+  LONGS_EQUAL(jf.keys()[1], 1);
+  LONGS_EQUAL(jf.keys()[2], 2);
+  LONGS_EQUAL(jf.keys()[3], 3);
+  LONGS_EQUAL(jf.keys()[4], 4);
+  EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
+  EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
+  EXPECT(assert_equal(Vector_(1, -7.5), jf.getb()));
+}
+
+/* ************************************************************************* */
+class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
+public:
+  typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
+  virtual ~TestFactor6() {}
+
+  virtual Vector
+    evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none,
+        boost::optional<Matrix&> H5 = boost::none,
+        boost::optional<Matrix&> H6 = boost::none) const {
+    if(H1) {
+      *H1 = Matrix_(1,1, 1.0);
+      *H2 = Matrix_(1,1, 2.0);
+      *H3 = Matrix_(1,1, 3.0);
+      *H4 = Matrix_(1,1, 4.0);
+      *H5 = Matrix_(1,1, 5.0);
+      *H6 = Matrix_(1,1, 6.0);
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NonlinearFactor6) {
+  TestFactor6 tf;
+  DynamicValues tv;
+  tv.insert(TestKey(1), LieVector(1, 1.0));
+  tv.insert(TestKey(2), LieVector(1, 2.0));
+  tv.insert(TestKey(3), LieVector(1, 3.0));
+  tv.insert(TestKey(4), LieVector(1, 4.0));
+  tv.insert(TestKey(5), LieVector(1, 5.0));
+  tv.insert(TestKey(6), LieVector(1, 6.0));
+  EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
+  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
+  LONGS_EQUAL(jf.keys()[0], 0);
+  LONGS_EQUAL(jf.keys()[1], 1);
+  LONGS_EQUAL(jf.keys()[2], 2);
+  LONGS_EQUAL(jf.keys()[3], 3);
+  LONGS_EQUAL(jf.keys()[4], 4);
+  LONGS_EQUAL(jf.keys()[5], 5);
+  EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
+  EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
+  EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5)));
+  EXPECT(assert_equal(Vector_(1, -10.5), jf.getb()));
+
+}
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

From d3d5ee3b399a780c88f5109d43f197d29f32388b Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Sat, 28 Jan 2012 20:47:43 +0000
Subject: [PATCH 17/88] [inprogress] switching to DynamicValues

---
 gtsam/nonlinear/NonlinearEquality.h         | 28 +++++------
 gtsam/nonlinear/NonlinearOptimization-inl.h | 36 ++++++-------
 gtsam/nonlinear/NonlinearOptimization.h     |  4 +-
 gtsam/nonlinear/NonlinearOptimizer-inl.h    | 42 ++++++++--------
 gtsam/nonlinear/NonlinearOptimizer.h        | 42 ++++++++--------
 gtsam/slam/BearingFactor.h                  | 10 ++--
 gtsam/slam/BearingRangeFactor.h             | 10 ++--
 gtsam/slam/BetweenFactor.h                  | 10 ++--
 gtsam/slam/BoundingConstraint.h             | 20 ++++----
 gtsam/slam/PriorFactor.h                    | 11 ++--
 gtsam/slam/dataset.cpp                      | 18 +++----
 gtsam/slam/dataset.h                        |  6 +--
 gtsam/slam/pose2SLAM.cpp                    |  8 +--
 gtsam/slam/pose2SLAM.h                      | 20 +++-----
 gtsam/slam/simulated2D.h                    | 56 +++++++++++----------
 gtsam/slam/simulated2DOriented.h            | 16 +++---
 gtsam/slam/simulated3D.h                    | 13 ++---
 gtsam/slam/smallExample.cpp                 |  9 ++--
 gtsam/slam/smallExample.h                   |  2 +-
 19 files changed, 176 insertions(+), 185 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 4b47e133e..e0066f21a 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -45,8 +45,8 @@ namespace gtsam {
 	 *   - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
 	 *   - ONLY_EXACT   : throws error at linearization if not at exact feasible point, and infinite error
 	 */
-	template<class VALUES, class KEY>
-	class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
+	template<class KEY>
+	class NonlinearEquality: public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value T;
@@ -69,7 +69,7 @@ namespace gtsam {
 		 */
 		bool (*compare_)(const T& a, const T& b);
 
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
 
 		/** default constructor - only for serialization */
 		NonlinearEquality() {}
@@ -101,14 +101,14 @@ namespace gtsam {
 		}
 
 		/** Check if two factors are equal */
-		bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
+		bool equals(const NonlinearEquality<KEY>& f, double tol = 1e-9) const {
 			if (!Base::equals(f)) return false;
 			return feasible_.equals(f.feasible_, tol) &&
 					fabs(error_gain_ - f.error_gain_) < tol;
 		}
 
 		/** actual error function calculation */
-		virtual double error(const VALUES& c) const {
+		virtual double error(const DynamicValues& c) const {
 			const T& xj = c[this->key_];
 			Vector e = this->unwhitenedError(c);
 			if (allow_error_ || !compare_(xj, feasible_)) {
@@ -135,7 +135,7 @@ namespace gtsam {
 		}
 
 		// Linearize is over-written, because base linearization tries to whiten
-		virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
+		virtual GaussianFactor::shared_ptr linearize(const DynamicValues& x, const Ordering& ordering) const {
 			const T& xj = x[this->key_];
 			Matrix A;
 			Vector b = evaluateError(xj, A);
@@ -162,14 +162,14 @@ namespace gtsam {
 	/**
 	 * Simple unary equality constraint - fixes a value for a variable
 	 */
-	template<class VALUES, class KEY>
-	class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
+	template<class KEY>
+	class NonlinearEquality1 : public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value X;
 
 	protected:
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
 
 		/** default constructor to allow for serialization */
 		NonlinearEquality1() {}
@@ -181,7 +181,7 @@ namespace gtsam {
 
 	public:
 
-		typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr;
 
 		NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
 			: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
@@ -220,13 +220,13 @@ namespace gtsam {
 	 * Simple binary equality constraint - this constraint forces two factors to
 	 * be the same.
 	 */
-	template<class VALUES, class KEY>
-	class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
+	template<class KEY>
+	class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
 	public:
 		typedef typename KEY::Value X;
 
 	protected:
-		typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
+		typedef NonlinearFactor2<KEY, KEY> Base;
 
 		GTSAM_CONCEPT_MANIFOLD_TYPE(X);
 
@@ -235,7 +235,7 @@ namespace gtsam {
 
 	public:
 
-		typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr;
 
 		NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
 			: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h
index 36d3cccd0..97da0c367 100644
--- a/gtsam/nonlinear/NonlinearOptimization-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimization-inl.h
@@ -31,17 +31,17 @@ namespace gtsam {
 	/**
 	 * The Elimination solver
 	 */
-	template<class G, class T>
-	T	optimizeSequential(const G& graph, const T& initialEstimate,
+	template<class G>
+	DynamicValues	optimizeSequential(const G& graph, const DynamicValues& initialEstimate,
 			const NonlinearOptimizationParameters& parameters, bool useLM) {
 
 		// Use a variable ordering from COLAMD
 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
 
 		// Create an nonlinear Optimizer that uses a Sequential Solver
-	  typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
+	  typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
-	  		boost::make_shared<const T>(initialEstimate), ordering,
+	  		boost::make_shared<const DynamicValues>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
 
 	  // Now optimize using either LM or GN methods.
@@ -54,17 +54,17 @@ namespace gtsam {
 	/**
 	 * The multifrontal solver
 	 */
-	template<class G, class T>
-	T	optimizeMultiFrontal(const G& graph, const T& initialEstimate,
+	template<class G>
+	DynamicValues	optimizeMultiFrontal(const G& graph, const DynamicValues& initialEstimate,
 			const NonlinearOptimizationParameters& parameters, bool useLM) {
 
 		// Use a variable ordering from COLAMD
 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
 
 		// Create an nonlinear Optimizer that uses a Multifrontal Solver
-	  typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
+	  typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
-	  		boost::make_shared<const T>(initialEstimate), ordering,
+	  		boost::make_shared<const DynamicValues>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
 
 	  // now optimize using either LM or GN methods
@@ -77,20 +77,20 @@ namespace gtsam {
 	/**
 	 * The sparse preconditioned conjugate gradient solver
 	 */
-	template<class G, class T>
-	T	optimizeSPCG(const G& graph, const T& initialEstimate,
+	template<class G>
+	DynamicValues	optimizeSPCG(const G& graph, const DynamicValues& initialEstimate,
 			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			bool useLM = true) {
 
 		// initial optimization state is the same in both cases tested
-		typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
+		typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver;
 		typedef boost::shared_ptr<Solver> shared_Solver;
-		typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
+		typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, Solver> SPCGOptimizer;
 		shared_Solver solver = boost::make_shared<Solver>(
 				graph, initialEstimate, IterativeOptimizationParameters());
 		SPCGOptimizer optimizer(
 				boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(initialEstimate),
+				boost::make_shared<const DynamicValues>(initialEstimate),
 				solver->ordering(),
 				solver,
 				boost::make_shared<NonlinearOptimizationParameters>(parameters));
@@ -105,19 +105,19 @@ namespace gtsam {
 	/**
 	 * optimization that returns the values
 	 */
-	template<class G, class T>
-	T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
+	template<class G>
+	DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters,
 			const LinearSolver& solver,
 			const NonlinearOptimizationMethod& nonlinear_method) {
 		switch (solver) {
 		case SEQUENTIAL:
-			return optimizeSequential<G,T>(graph, initialEstimate, parameters,
+			return optimizeSequential<G,DynamicValues>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 		case MULTIFRONTAL:
-			return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters,
+			return optimizeMultiFrontal<G,DynamicValues>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 		case SPCG:
-//			return optimizeSPCG<G,T>(graph, initialEstimate, parameters,
+//			return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
 //								nonlinear_method == LM);
 			throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
 		}
diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h
index a477d4fd5..86ab7a3c8 100644
--- a/gtsam/nonlinear/NonlinearOptimization.h
+++ b/gtsam/nonlinear/NonlinearOptimization.h
@@ -42,8 +42,8 @@ namespace gtsam {
 	/**
 	 * optimization that returns the values
 	 */
-	template<class G, class T>
-	T optimize(const G& graph, const T& initialEstimate,
+	template<class G>
+	DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate,
 			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			const LinearSolver& solver = MULTIFRONTAL,
 			const NonlinearOptimizationMethod& nonlinear_method = LM);
diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h
index 02ead8b0d..2e7a83f76 100644
--- a/gtsam/nonlinear/NonlinearOptimizer-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h
@@ -30,8 +30,8 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
 			shared_values values, shared_ordering ordering, shared_parameters parameters) :
 			graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
 			parameters_(parameters), iterations_(0),
@@ -47,8 +47,8 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	// FIXME: remove this constructor
-	template<class G, class C, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
 			shared_values values, shared_ordering ordering,
 			shared_solver spcg_solver, shared_parameters parameters) :
 			graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
@@ -66,8 +66,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	// One iteration of Gauss Newton
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterate() const {
 
 		Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
 
@@ -86,7 +86,7 @@ namespace gtsam {
 		if (verbosity >= Parameters::DELTA) delta.print("delta");
 
 		// take old values and update it
-		shared_values newValues(new C(values_->retract(delta, *ordering_)));
+		shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_)));
 
 		// maybe show output
 		if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@@ -99,8 +99,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::gaussNewton() const {
 		static W writer(error_);
 
 		if (error_ < parameters_->sumError_ ) {
@@ -130,8 +130,8 @@ namespace gtsam {
 	// TODO: in theory we can't infinitely recurse, but maybe we should put a max.
 	// Reminder: the parameters are Graph type $G$, Values class type $T$,
 	// linear system class $L$, the non linear solver type $S$, and the writer type $W$
-	template<class G, class T, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::try_lambda(const L& linearSystem) {
 
 		const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
 		const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
@@ -178,7 +178,7 @@ namespace gtsam {
 		    if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
 
 		    // update values
-		    shared_values newValues(new T(values_->retract(delta, *ordering_)));
+		    shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_)));
 
 		    // create new optimization state with more adventurous lambda
 		    double error = graph_->error(*newValues);
@@ -228,8 +228,8 @@ namespace gtsam {
 	// One iteration of Levenberg Marquardt
 	// Reminder: the parameters are Graph type $G$, Values class type $T$,
 	// linear system class $L$, the non linear solver type $S$, and the writer type $W$
-	template<class G, class T, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateLM() {
 
 		const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
 		const double lambda = parameters_->lambda_ ;
@@ -253,8 +253,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	// Reminder: the parameters are Graph type $G$, Values class type $T$,
 	// linear system class $L$, the non linear solver type $S$, and the writer type $W$
-	template<class G, class T, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
+	template<class G, class L, class S, class W>
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::levenbergMarquardt() {
 
 		// Initialize
 		bool converged = false;
@@ -299,20 +299,20 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  template<class G, class T, class L, class S, class W>
-  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
+  template<class G, class L, class S, class W>
+  NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateDogLeg() {
 
     S solver(*graph_->linearize(*values_, *ordering_));
     DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
         parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
         *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR);
-    shared_values newValues(new T(values_->retract(result.dx_d, *ordering_)));
+    shared_values newValues(new DynamicValues(values_->retract(result.dx_d, *ordering_)));
     return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
   }
 
   /* ************************************************************************* */
-  template<class G, class T, class L, class S, class W>
-  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() {
+  template<class G, class L, class S, class W>
+  NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
     static W writer(error_);
 
     // check if we're already close enough
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index 4073df424..e87b44a5d 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -37,19 +37,19 @@ public:
  * and then one of the optimization routines is called. These iterate
  * until convergence. All methods are functional and return a new state.
  *
- * The class is parameterized by the Graph type $G$, Values class type $T$,
+ * The class is parameterized by the Graph type $G$, Values class type $DynamicValues$,
  * linear system class $L$, the non linear solver type $S$, and the writer type $W$
  *
- * The values class type $T$ is in order to be able to optimize over non-vector values structures.
+ * The values class type $DynamicValues$ is in order to be able to optimize over non-vector values structures.
  *
  * A nonlinear system solver $S$
- * Concept NonLinearSolver<G,T,L> implements
- *   linearize: G * T -> L
- *   solve : L -> T
+ * Concept NonLinearSolver<G,DynamicValues,L> implements
+ *   linearize: G * DynamicValues -> L
+ *   solve : L -> DynamicValues
  *
  * The writer $W$ generates output to disk or the screen.
  *
- * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
+ * For example, in a 2D case, $G$ can be Pose2Graph, $DynamicValues$ can be Pose2Values,
  * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
  * The solver class has two main functions: linearize and optimize. The first one
  * linearizes the nonlinear cost function around the current estimate, and the second
@@ -57,12 +57,12 @@ public:
  *
  * To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
  */
-template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
+template<class G, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
 class NonlinearOptimizer {
 public:
 
 	// For performance reasons in recursion, we store values in a shared_ptr
-	typedef boost::shared_ptr<const T> shared_values; ///Prevent memory leaks in Values
+	typedef boost::shared_ptr<const DynamicValues> shared_values; ///Prevent memory leaks in Values
 	typedef boost::shared_ptr<const G> shared_graph;  /// Prevent memory leaks in Graph
 	typedef boost::shared_ptr<L> shared_linear;    /// Not sure
 	typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
@@ -73,7 +73,7 @@ public:
 
 private:
 
-	typedef NonlinearOptimizer<G, T, L, GS> This;
+	typedef NonlinearOptimizer<G, L, GS> This;
 	typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
 
 	/// keep a reference to const version of the graph
@@ -167,7 +167,7 @@ public:
 	/**
 	 * Copy constructor
 	 */
-	NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
+	NonlinearOptimizer(const NonlinearOptimizer<G, L, GS> &optimizer) :
 		graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
 		ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
 		iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
@@ -222,7 +222,7 @@ public:
 
 	/**
 	 *  linearize and optimize
-	 *  This returns an VectorValues, i.e., vectors in tangent space of T
+	 *  This returns an VectorValues, i.e., vectors in tangent space of DynamicValues
 	 */
 	VectorValues linearizeAndOptimizeForDelta() const {
 		return *createSolver()->optimize();
@@ -309,18 +309,18 @@ public:
 	 * Static interface to LM optimization (no shared_ptr arguments) - see above
 	 */
 	static shared_values optimizeLM(const G& graph,
-			const T& values,
+			const DynamicValues& values,
 			const Parameters parameters = Parameters()) {
 		return optimizeLM(boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(values),
+				boost::make_shared<const DynamicValues>(values),
 				boost::make_shared<Parameters>(parameters));
 	}
 
 	static shared_values optimizeLM(const G& graph,
-			const T& values,
+			const DynamicValues& values,
 			Parameters::verbosityLevel verbosity) {
 		return optimizeLM(boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(values),
+				boost::make_shared<const DynamicValues>(values),
 				verbosity);
 	}
 
@@ -360,18 +360,18 @@ public:
    * Static interface to Dogleg optimization (no shared_ptr arguments) - see above
    */
   static shared_values optimizeDogLeg(const G& graph,
-      const T& values,
+      const DynamicValues& values,
       const Parameters parameters = Parameters()) {
     return optimizeDogLeg(boost::make_shared<const G>(graph),
-        boost::make_shared<const T>(values),
+        boost::make_shared<const DynamicValues>(values),
         boost::make_shared<Parameters>(parameters));
   }
 
   static shared_values optimizeDogLeg(const G& graph,
-      const T& values,
+      const DynamicValues& values,
       Parameters::verbosityLevel verbosity) {
     return optimizeDogLeg(boost::make_shared<const G>(graph),
-        boost::make_shared<const T>(values),
+        boost::make_shared<const DynamicValues>(values),
         verbosity);
   }
 
@@ -398,9 +398,9 @@ public:
 	/**
 	 * Static interface to GN optimization (no shared_ptr arguments) - see above
 	 */
-	static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) {
+	static shared_values optimizeGN(const G& graph, const DynamicValues& values, const Parameters parameters = Parameters()) {
 		return optimizeGN(boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(values),
+				boost::make_shared<const DynamicValues>(values),
 				boost::make_shared<Parameters>(parameters));
 	}
 };
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index d17dd135e..eaaf3d4e9 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -25,16 +25,16 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class VALUES, class POSEKEY, class POINTKEY>
-	class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
+	template<class POSEKEY, class POINTKEY>
+	class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
 	private:
 
 		typedef typename POSEKEY::Value Pose;
 		typedef typename POSEKEY::Value::Rotation Rot;
 		typedef typename POINTKEY::Value Point;
 
-		typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
+		typedef BearingFactor<POSEKEY, POINTKEY> This;
+		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
 
 		Rot z_; /** measurement */
 
@@ -68,7 +68,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol);
 		}
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 833708b5d..ef71f8f13 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -27,16 +27,16 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class VALUES, class POSEKEY, class POINTKEY>
-	class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
+	template<class POSEKEY, class POINTKEY>
+	class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
 	private:
 
 		typedef typename POSEKEY::Value Pose;
 		typedef typename POSEKEY::Value::Rotation Rot;
 		typedef typename POINTKEY::Value Point;
 
-		typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
+		typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
+		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
 
 		// the measurement
 		Rot bearing_;
@@ -68,7 +68,7 @@ namespace gtsam {
 	  }
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) &&
 					fabs(this->range_ - e->range_) < tol &&
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index dee18ebc7..09d54d524 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -29,13 +29,13 @@ namespace gtsam {
 	 * KEY1::Value is the Lie Group type
 	 * T is the measurement type, by default the same
 	 */
-	template<class VALUES, class KEY1, class T = typename KEY1::Value>
-	class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> {
+	template<class KEY1, class T = typename KEY1::Value>
+	class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
 
 	private:
 
-		typedef BetweenFactor<VALUES, KEY1, T> This;
-		typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base;
+		typedef BetweenFactor<KEY1, T> This;
+		typedef NonlinearFactor2<KEY1, KEY1> Base;
 
 		T measured_; /** The measurement */
 
@@ -71,7 +71,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
 		}
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index e8f59cb2d..274c0d83d 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -28,11 +28,11 @@ namespace gtsam {
  * will need to have its value function implemented to return
  * a scalar for comparison.
  */
-template<class VALUES, class KEY>
-struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
+template<class KEY>
+struct BoundingConstraint1: public NonlinearFactor1<KEY> {
 	typedef typename KEY::Value X;
-	typedef NonlinearFactor1<VALUES, KEY> Base;
-	typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
+	typedef NonlinearFactor1<KEY> Base;
+	typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
 
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
@@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
 			boost::none) const = 0;
 
 	/** active when constraint *NOT* met */
-	bool active(const VALUES& c) const {
+	bool active(const DynamicValues& c) const {
 		// note: still active at equality to avoid zigzagging
 		double x = value(c[this->key_]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@@ -95,13 +95,13 @@ private:
  * Binary scalar inequality constraint, with a similar value() function
  * to implement for specific systems
  */
-template<class VALUES, class KEY1, class KEY2>
-struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
+template<class KEY1, class KEY2>
+struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
 	typedef typename KEY1::Value X1;
 	typedef typename KEY2::Value X2;
 
-	typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
-	typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
+	typedef NonlinearFactor2<KEY1, KEY2> Base;
+	typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
 
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
@@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
 			boost::optional<Matrix&> H2 = boost::none) const = 0;
 
 	/** active when constraint *NOT* met */
-	bool active(const VALUES& c) const {
+	bool active(const DynamicValues& c) const {
 		// note: still active at equality to avoid zigzagging
 		double x = value(c[this->key1_], c[this->key2_]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 2d7b631cc..fa33a9bc7 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -28,15 +28,15 @@ namespace gtsam {
 	 * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
 	 * a simple type like int will not work
 	 */
-	template<class VALUES, class KEY>
-	class PriorFactor: public NonlinearFactor1<VALUES, KEY> {
+	template<class KEY>
+	class PriorFactor: public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value T;
 
 	private:
 
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
 
 		T prior_; /** The measurement */
 
@@ -69,9 +69,8 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
-			const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
-					VALUES, KEY>*> (&expected);
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+			const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
 		}
 
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index b5322ee47..712125588 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -30,7 +30,7 @@ using namespace gtsam;
 #define LINESIZE 81920
 
 typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
-typedef boost::shared_ptr<Pose2Values> sharedPose2Values;
+typedef boost::shared_ptr<DynamicValues> sharedPose2Values;
 
 namespace gtsam {
 
@@ -81,7 +81,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 		exit(-1);
 	}
 
-	sharedPose2Values poses(new Pose2Values);
+	sharedPose2Values poses(new DynamicValues);
 	sharedPose2Graph graph(new Pose2Graph);
 
 	string tag;
@@ -96,7 +96,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 			is >> id >> x >> y >> yaw;
 			// optional filter
 			if (maxID && id >= maxID) continue;
-			poses->insert(id, Pose2(x, y, yaw));
+			poses->insert(pose2SLAM::Key(id), Pose2(x, y, yaw));
 		}
 		is.ignore(LINESIZE, '\n');
 	}
@@ -133,8 +133,8 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 				l1Xl2 = l1Xl2.retract((*model)->sample());
 
 			// Insert vertices if pure odometry file
-			if (!poses->exists(id1)) poses->insert(id1, Pose2());
-			if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2);
+			if (!poses->exists(pose2SLAM::Key(id1))) poses->insert(pose2SLAM::Key(id1), Pose2());
+			if (!poses->exists(pose2SLAM::Key(id2))) poses->insert(pose2SLAM::Key(id2), poses->at(pose2SLAM::Key(id1)) * l1Xl2);
 
 			Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
 			graph->push_back(factor);
@@ -149,14 +149,14 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 }
 
 /* ************************************************************************* */
-void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiagonal model,
+void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDiagonal model,
 		const string& filename) {
-	typedef Pose2Values::Key Key;
+//	typedef DynamicValues::Key Key;
 
 	fstream stream(filename.c_str(), fstream::out);
 
 	// save poses
-	Pose2Values::Key key;
+	pose2SLAM::Key key;
 	Pose2 pose;
 	BOOST_FOREACH(boost::tie(key, pose), config)
 		stream << "VERTEX2 " << key.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
@@ -164,7 +164,7 @@ void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiag
 	// save edges
 	Matrix R = model->R();
 	Matrix RR = trans(R)*R;//prod(trans(R),R);
-	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<Pose2Values> > factor_, graph) {
+	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
 		boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
 		if (!factor) continue;
 
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index d808dfa24..2b339db18 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -41,16 +41,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
  * @param maxID, if non-zero cut out vertices >= maxID
  * @param smart: try to reduce complexity of covariance to cheapest model
  */
-std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D(
+std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
 		std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
 		int maxID = 0, bool addNoise=false, bool smart=true);
-std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D(
+std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
 		const std::string& filename,
 		boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
 		int maxID = 0, bool addNoise=false, bool smart=true);
 
 /** save 2d graph */
-void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Values& config, const gtsam::SharedDiagonal model,
+void save2D(const gtsam::Pose2Graph& graph, const DynamicValues& config, const gtsam::SharedDiagonal model,
 		const std::string& filename);
 
 /**
diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp
index 86dd3ea16..090607d72 100644
--- a/gtsam/slam/pose2SLAM.cpp
+++ b/gtsam/slam/pose2SLAM.cpp
@@ -22,16 +22,16 @@
 // Use pose2SLAM namespace for specific SLAM instance
 namespace gtsam {
 
-	template class NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>;
+	template class NonlinearOptimizer<pose2SLAM::Graph, GaussianFactorGraph, GaussianSequentialSolver>;
 
 	namespace pose2SLAM {
 
 		/* ************************************************************************* */
-		Values circle(size_t n, double R) {
-			Values x;
+		DynamicValues circle(size_t n, double R) {
+			DynamicValues x;
 			double theta = 0, dtheta = 2 * M_PI / n;
 			for (size_t i = 0; i < n; i++, theta += dtheta)
-				x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
+				x.insert(Key(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
 			return x;
 		}
 
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 45017a81b..714bcce51 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -37,9 +37,6 @@ namespace gtsam {
 		/// Keys with Pose2 and symbol 'x'
 		typedef TypedSymbol<Pose2, 'x'> Key;
 
-		/// Values with type 'Key'
-		typedef Values<Key> Values;
-
 		/**
 		 * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
 		 * @param n number of poses
@@ -47,22 +44,22 @@ namespace gtsam {
 		 * @param c character to use for keys
 		 * @return circle of n 2D poses
 		 */
-		Values circle(size_t n, double R);
+		DynamicValues circle(size_t n, double R);
 
 		/// A prior factor on Key with Pose2 data type.
-		typedef PriorFactor<Values, Key> Prior;
+		typedef PriorFactor<Key> Prior;
 
 		/// A factor to put constraints between two factors.
-		typedef BetweenFactor<Values, Key> Constraint;
+		typedef BetweenFactor<Key> Constraint;
 
 		/// A hard constraint would enforce that the given key would have the input value in the results.
-		typedef NonlinearEquality<Values, Key> HardConstraint;
+		typedef NonlinearEquality<Key> HardConstraint;
 
 		/// Graph
-		struct Graph: public NonlinearFactorGraph<Values> {
+		struct Graph: public NonlinearFactorGraph {
 
 			/// Adds a factor between keys of the same type
-			typedef BetweenFactor<Values, Key> Constraint;
+			typedef BetweenFactor<Key> Constraint;
 
 			/// A simple typedef from Pose2 to Pose
 			typedef Pose2 Pose;
@@ -78,17 +75,16 @@ namespace gtsam {
 		};
 
 		/// The sequential optimizer
-		typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
+		typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
 
 		/// The multifrontal optimizer
-		typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
+		typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 
 	} // pose2SLAM
 
 	/**
 	 * Backwards compatibility
 	 */
-	typedef pose2SLAM::Values Pose2Values;			///< Typedef for Values class for backwards compatibility
 	typedef pose2SLAM::Prior Pose2Prior;			///< Typedef for Prior class for backwards compatibility
 	typedef pose2SLAM::Constraint Pose2Factor;		///< Typedef for Constraint class for backwards compatibility
 	typedef pose2SLAM::Graph Pose2Graph;			///< Typedef for Graph class for backwards compatibility
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index cae60bb80..4cb23f76a 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -32,44 +32,46 @@ namespace gtsam {
 		// Simulated2D robots have no orientation, just a position
 		typedef TypedSymbol<Point2, 'x'> PoseKey;
 		typedef TypedSymbol<Point2, 'l'> PointKey;
-		typedef Values<PoseKey> PoseValues;
-		typedef Values<PointKey> PointValues;
 
 		/**
 		 *  Custom Values class that holds poses and points
 		 */
-		class Values: public TupleValues2<PoseValues, PointValues> {
+		class Values: public DynamicValues {
+			size_t nrPoses_;
+			size_t nrPoints_;
 		public:
-			typedef TupleValues2<PoseValues, PointValues> Base;  ///< base class
+			typedef DynamicValues Base;  ///< base class
 			typedef boost::shared_ptr<Point2> sharedPoint;  ///< shortcut to shared Point type
 
 			/// Constructor
-			Values() {
+			Values() : nrPoses_(0), nrPoints_(0) {
 			}
 
 			/// Copy constructor
 			Values(const Base& base) :
-					Base(base) {
+					Base(base), nrPoses_(0), nrPoints_(0) {
 			}
 
 			/// Insert a pose
 			void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
 				insert(i, p);
+				nrPoses_++;
 			}
 
 			/// Insert a point
 			void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
 				insert(j, p);
+				nrPoints_++;
 			}
 
 			/// Number of poses
 			int nrPoses() const {
-				return this->first_.size();
+				return nrPoses_;
 			}
 
 			/// Number of points
 			int nrPoints() const {
-				return this->second_.size();
+				return nrPoints_;
 			}
 
 			/// Return pose i
@@ -112,18 +114,18 @@ namespace gtsam {
 		/**
 		 *  Unary factor encoding a soft prior on a vector
 		 */
-		template<class VALUES = Values, class KEY = PoseKey>
-		class GenericPrior: public NonlinearFactor1<VALUES, KEY> {
+		template<class KEY = PoseKey>
+		class GenericPrior: public NonlinearFactor1<KEY> {
 		public:
-			typedef NonlinearFactor1<VALUES, KEY> Base;  ///< base class
-			typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr;
+			typedef NonlinearFactor1<KEY> Base;  ///< base class
+			typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
 			typedef typename KEY::Value Pose; ///< shortcut to Pose type
 
 			Pose z_; ///< prior mean
 
 			/// Create generic prior
 			GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
-					NonlinearFactor1<VALUES, KEY>(model, key), z_(z) {
+					NonlinearFactor1<KEY>(model, key), z_(z) {
 			}
 
 			/// Return error and optional derivative
@@ -150,11 +152,11 @@ namespace gtsam {
 		/**
 		 * Binary factor simulating "odometry" between two Vectors
 		 */
-		template<class VALUES = Values, class KEY = PoseKey>
-		class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
+		template<class KEY = PoseKey>
+		class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
 		public:
-			typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class
-			typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr;
+			typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
+			typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
 			typedef typename KEY::Value Pose; ///< shortcut to Pose type
 
 			Pose z_; ///< odometry measurement
@@ -162,7 +164,7 @@ namespace gtsam {
 			/// Create odometry
 			GenericOdometry(const Pose& z, const SharedNoiseModel& model,
 					const KEY& i1, const KEY& i2) :
-					NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
+					NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
 			}
 
 			/// Evaluate error and optionally return derivatives
@@ -190,11 +192,11 @@ namespace gtsam {
 		/**
 		 * Binary factor simulating "measurement" between two Vectors
 		 */
-		template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey>
-		class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> {
+		template<class XKEY = PoseKey, class LKEY = PointKey>
+		class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
 		public:
-			typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base;  ///< base class
-			typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr;
+			typedef NonlinearFactor2<XKEY, LKEY> Base;  ///< base class
+			typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
 			typedef typename XKEY::Value Pose; ///< shortcut to Pose type
 			typedef typename LKEY::Value Point; ///< shortcut to Point type
 
@@ -203,7 +205,7 @@ namespace gtsam {
 			/// Create measurement factor
 			GenericMeasurement(const Point& z, const SharedNoiseModel& model,
 					const XKEY& i, const LKEY& j) :
-					NonlinearFactor2<VALUES, XKEY, LKEY>(model, i, j), z_(z) {
+					NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
 			}
 
 			/// Evaluate error and optionally return derivatives
@@ -229,13 +231,13 @@ namespace gtsam {
 		};
 
 		/** Typedefs for regular use */
-		typedef GenericPrior<Values, PoseKey> Prior;
-		typedef GenericOdometry<Values, PoseKey> Odometry;
-		typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement;
+		typedef GenericPrior<PoseKey> Prior;
+		typedef GenericOdometry<PoseKey> Odometry;
+		typedef GenericMeasurement<PoseKey, PointKey> Measurement;
 
 		// Specialization of a graph for this example domain
 		// TODO: add functions to add factor types
-		class Graph : public NonlinearFactorGraph<Values> {
+		class Graph : public NonlinearFactorGraph {
 		public:
 			Graph() {}
 		};
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index b9cf150ef..f44c92118 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -76,15 +76,15 @@ namespace gtsam {
 				boost::none, boost::optional<Matrix&> H2 = boost::none);
 
 		/// Unary factor encoding a soft prior on a vector
-		template<class VALUES = Values, class Key = PoseKey>
-		struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
+		template<class Key = PoseKey>
+		struct GenericPosePrior: public NonlinearFactor1<Key> {
 
 			Pose2 z_; ///< measurement
 
 			/// Create generic pose prior
 			GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
 					const Key& key) :
-					NonlinearFactor1<VALUES, Key>(model, key), z_(z) {
+					NonlinearFactor1<Key>(model, key), z_(z) {
 			}
 
 			/// Evaluate error and optionally derivative
@@ -98,8 +98,8 @@ namespace gtsam {
 		/**
 		 * Binary factor simulating "odometry" between two Vectors
 		 */
-		template<class VALUES = Values, class KEY = PoseKey>
-		struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
+		template<class KEY = PoseKey>
+		struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
 			Pose2 z_;   ///< Between measurement for odometry factor
 
 			/**
@@ -107,7 +107,7 @@ namespace gtsam {
 			 */
 			GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
 					const KEY& i1, const KEY& i2) :
-					NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
+					NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
 			}
 
 			/// Evaluate error and optionally derivative
@@ -119,10 +119,10 @@ namespace gtsam {
 
 		};
 
-		typedef GenericOdometry<Values, PoseKey> Odometry;
+		typedef GenericOdometry<PoseKey> Odometry;
 
 		/// Graph specialization for syntactic sugar use with matlab
-		class Graph : public NonlinearFactorGraph<Values> {
+		class Graph : public NonlinearFactorGraph {
 		public:
 			Graph() {}
 			// TODO: add functions to add factors
diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index 32f05e967..e995308f8 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -40,10 +40,6 @@ namespace simulated3D {
 typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
 typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
 
-typedef Values<PoseKey> PoseValues;
-typedef Values<PointKey> PointValues;
-typedef TupleValues2<PoseValues, PointValues> Values;
-
 /**
  * Prior on a single pose
  */
@@ -66,7 +62,7 @@ Point3 mea(const Point3& x, const Point3& l,
 /**
  * A prior factor on a single linear robot pose
  */
-struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
+struct PointPrior3D: public NonlinearFactor1<PoseKey> {
 
 	Point3 z_; ///< The prior pose value for the variable attached to this factor
 
@@ -78,7 +74,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
 	 */
 	PointPrior3D(const Point3& z,
 			const SharedNoiseModel& model, const PoseKey& j) :
-				NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
+				NonlinearFactor1<PoseKey> (model, j), z_(z) {
 	}
 
 	/**
@@ -97,8 +93,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
 /**
  * Models a linear 3D measurement between 3D points
  */
-struct Simulated3DMeasurement: public NonlinearFactor2<Values,
-PoseKey, PointKey> {
+struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
 
 	Point3 z_; ///< Linear displacement between a pose and landmark
 
@@ -111,7 +106,7 @@ PoseKey, PointKey> {
 	 */
 	Simulated3DMeasurement(const Point3& z,
 			const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
-				NonlinearFactor2<Values, PoseKey, PointKey> (
+				NonlinearFactor2<PoseKey, PointKey> (
 						model, j1, j2), z_(z) {
 	}
 
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 40d3e98ff..948616ea6 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -39,7 +39,7 @@ using namespace std;
 namespace gtsam {
 namespace example {
 
-	typedef boost::shared_ptr<NonlinearFactor<Values> > shared;
+	typedef boost::shared_ptr<NonlinearFactor > shared;
 
 	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
 	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
@@ -195,14 +195,13 @@ namespace example {
 					 0.0, cos(v.y()));
 		}
 
-		struct UnaryFactor: public gtsam::NonlinearFactor1<Values,
-		simulated2D::PoseKey> {
+		struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
 
 			Point2 z_;
 
 			UnaryFactor(const Point2& z, const SharedNoiseModel& model,
 					const simulated2D::PoseKey& key) :
-				gtsam::NonlinearFactor1<Values, simulated2D::PoseKey>(model, key), z_(z) {
+				gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
 			}
 
 			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
@@ -423,7 +422,7 @@ namespace example {
 	boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
 
 		// create empty graph
-		NonlinearFactorGraph<Values> nlfg;
+		NonlinearFactorGraph nlfg;
 
 		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
 		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1)));
diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h
index 1889ee1b5..2380d86f2 100644
--- a/gtsam/slam/smallExample.h
+++ b/gtsam/slam/smallExample.h
@@ -29,7 +29,7 @@ namespace gtsam {
 	namespace example {
 
 		typedef simulated2D::Values Values;
-		typedef NonlinearFactorGraph<Values> Graph;
+		typedef NonlinearFactorGraph Graph;
 
 		/**
 		 * Create small example for non-linear factor graph

From 5b5bbfdfff048d5f50545eab1c0972d2317cf6b0 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Sun, 29 Jan 2012 21:12:58 +0000
Subject: [PATCH 18/88] testSimmulated2D passes. Too many warnings in boost
 from clang!

---
 gtsam/nonlinear/DoglegOptimizerImpl.h         |  2 +-
 gtsam/nonlinear/Makefile.am                   |  4 +-
 ...orGraph-inl.h => NonlinearFactorGraph.cpp} |  9 ++--
 gtsam/nonlinear/NonlinearFactorGraph.h        |  1 -
 .../{Values-inl.h => ValuesOld-inl.h}         | 48 +++++++++----------
 gtsam/nonlinear/{Values.h => ValuesOld.h}     | 32 ++++++-------
 gtsam/slam/PartialPriorFactor.h               |  8 ++--
 gtsam/slam/ProjectionFactor.h                 | 10 ++--
 gtsam/slam/RangeFactor.h                      | 10 ++--
 gtsam/slam/StereoFactor.h                     |  6 +--
 gtsam/slam/dataset.cpp                        | 10 ++--
 gtsam/slam/planarSLAM.cpp                     |  4 +-
 gtsam/slam/planarSLAM.h                       | 46 +++++++-----------
 gtsam/slam/pose2SLAM.h                        |  1 -
 gtsam/slam/pose3SLAM.cpp                      |  6 +--
 gtsam/slam/pose3SLAM.h                        | 16 +++----
 gtsam/slam/simulated2D.h                      |  1 -
 gtsam/slam/simulated2DOriented.h              | 14 +++---
 gtsam/slam/simulated3D.h                      |  1 -
 gtsam/slam/visualSLAM.h                       | 28 +++++------
 tests/testNonlinearFactor.cpp                 |  8 ++--
 21 files changed, 120 insertions(+), 145 deletions(-)
 rename gtsam/nonlinear/{NonlinearFactorGraph-inl.h => NonlinearFactorGraph.cpp} (98%)
 rename gtsam/nonlinear/{Values-inl.h => ValuesOld-inl.h} (81%)
 rename gtsam/nonlinear/{Values.h => ValuesOld.h} (90%)

diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
index daab0c66e..5923baa18 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.h
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -7,7 +7,7 @@
 
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 40a96ba5f..f641ab9e7 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -22,10 +22,10 @@ check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/t
 
 # Nonlinear nonlinear
 headers += Key.h 
-headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
+headers += NonlinearFactorGraph.h
 headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h
 headers += NonlinearFactor.h
-sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp
+sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp
 headers += DoglegOptimizer.h DoglegOptimizer-inl.h
 
 # Nonlinear iSAM(2)
diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph.cpp
similarity index 98%
rename from gtsam/nonlinear/NonlinearFactorGraph-inl.h
rename to gtsam/nonlinear/NonlinearFactorGraph.cpp
index 445dbf20e..880cb2fb9 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -10,19 +10,18 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    NonlinearFactorGraph-inl.h
+ * @file    NonlinearFactorGraph.cpp
  * @brief   Factor Graph Consisting of non-linear factors
  * @author  Frank Dellaert
  * @author  Carlos Nieto
  * @author  Christian Potthast
  */
 
-#pragma once
-
+#include <cmath>
+#include <boost/foreach.hpp>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/inference.h>
-#include <boost/foreach.hpp>
-#include <cmath>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 64b035bc0..96beadf3d 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -100,4 +100,3 @@ namespace gtsam {
 
 } // namespace
 
-#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/ValuesOld-inl.h
similarity index 81%
rename from gtsam/nonlinear/Values-inl.h
rename to gtsam/nonlinear/ValuesOld-inl.h
index 28f08bfb5..5f53e32e8 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/ValuesOld-inl.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Values.cpp
+ * @file ValuesOld.cpp
  * @author Richard Roberts
  */
 
@@ -34,8 +34,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
   template<class J>
-  void Values<J>::print(const string &s) const {
-       cout << "Values " << s << ", size " << values_.size() << "\n";
+  void ValuesOld<J>::print(const string &s) const {
+       cout << "ValuesOld " << s << ", size " << values_.size() << "\n";
        BOOST_FOREACH(const KeyValuePair& v, values_) {
          gtsam::print(v.second, (string)(v.first));
        }
@@ -43,7 +43,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  bool Values<J>::equals(const Values<J>& expected, double tol) const {
+  bool ValuesOld<J>::equals(const ValuesOld<J>& expected, double tol) const {
     if (values_.size() != expected.values_.size()) return false;
     BOOST_FOREACH(const KeyValuePair& v, values_) {
     	if (!expected.exists(v.first)) return false;
@@ -55,7 +55,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  const typename J::Value& Values<J>::at(const J& j) const {
+  const typename J::Value& ValuesOld<J>::at(const J& j) const {
     const_iterator it = values_.find(j);
     if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
     else return it->second;
@@ -63,7 +63,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  size_t Values<J>::dim() const {
+  size_t ValuesOld<J>::dim() const {
   	size_t n = 0;
   	BOOST_FOREACH(const KeyValuePair& value, values_)
   		n += value.second.dim();
@@ -72,27 +72,27 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  VectorValues Values<J>::zero(const Ordering& ordering) const {
+  VectorValues ValuesOld<J>::zero(const Ordering& ordering) const {
   	return VectorValues::Zero(this->dims(ordering));
   }
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::insert(const J& name, const typename J::Value& val) {
+  void ValuesOld<J>::insert(const J& name, const typename J::Value& val) {
     if(!values_.insert(make_pair(name, val)).second)
       throw KeyAlreadyExists<J>(name, val);
   }
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::insert(const Values<J>& cfg) {
+  void ValuesOld<J>::insert(const ValuesOld<J>& cfg) {
 	  BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
 		 insert(v.first, v.second);
   }
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::update(const Values<J>& cfg) {
+  void ValuesOld<J>::update(const ValuesOld<J>& cfg) {
 	  BOOST_FOREACH(const KeyValuePair& v, values_) {
 	  	boost::optional<typename J::Value> t = cfg.exists_(v.first);
 	  	if (t) values_[v.first] = *t;
@@ -101,13 +101,13 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::update(const J& j, const typename J::Value& val) {
+  void ValuesOld<J>::update(const J& j, const typename J::Value& val) {
 	  	values_[j] = val;
   }
 
   /* ************************************************************************* */
   template<class J>
-  std::list<J> Values<J>::keys() const {
+  std::list<J> ValuesOld<J>::keys() const {
 	  std::list<J> ret;
 	  BOOST_FOREACH(const KeyValuePair& v, values_)
 		  ret.push_back(v.first);
@@ -116,14 +116,14 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::erase(const J& j) {
+  void ValuesOld<J>::erase(const J& j) {
     size_t dim; // unused
     erase(j, dim);
   }
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::erase(const J& j, size_t& dim) {
+  void ValuesOld<J>::erase(const J& j, size_t& dim) {
     iterator it = values_.find(j);
     if (it == values_.end())
       throw KeyDoesNotExist<J>("erase", j);
@@ -134,8 +134,8 @@ namespace gtsam {
   /* ************************************************************************* */
   // todo: insert for every element is inefficient
   template<class J>
-  Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
-		Values<J> newValues;
+  ValuesOld<J> ValuesOld<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
+		ValuesOld<J> newValues;
 		typedef pair<J,typename J::Value> KeyValue;
 		BOOST_FOREACH(const KeyValue& value, this->values_) {
 			const J& j = value.first;
@@ -151,7 +151,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  std::vector<size_t> Values<J>::dims(const Ordering& ordering) const {
+  std::vector<size_t> ValuesOld<J>::dims(const Ordering& ordering) const {
     _ValuesDimensionCollector dimCollector(ordering);
     this->apply(dimCollector);
     return dimCollector.dimensions;
@@ -159,7 +159,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  Ordering::shared_ptr Values<J>::orderingArbitrary(Index firstVar) const {
+  Ordering::shared_ptr ValuesOld<J>::orderingArbitrary(Index firstVar) const {
     // Generate an initial key ordering in iterator order
     _ValuesKeyOrderer keyOrderer(firstVar);
     this->apply(keyOrderer);
@@ -169,12 +169,12 @@ namespace gtsam {
 //  /* ************************************************************************* */
 //  // todo: insert for every element is inefficient
 //  template<class J>
-//  Values<J> Values<J>::retract(const Vector& delta) const {
+//  ValuesOld<J> ValuesOld<J>::retract(const Vector& delta) const {
 //    if(delta.size() != dim()) {
-//    	cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
+//    	cout << "ValuesOld::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
 //      throw invalid_argument("Delta vector length does not match config dimensionality.");
 //    }
-//    Values<J> newValues;
+//    ValuesOld<J> newValues;
 //    int delta_offset = 0;
 //		typedef pair<J,typename J::Value> KeyValue;
 //		BOOST_FOREACH(const KeyValue& value, this->values_) {
@@ -191,7 +191,7 @@ namespace gtsam {
   // todo: insert for every element is inefficient
   // todo: currently only logmaps elements in both configs
   template<class J>
-  inline VectorValues Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering) const {
+  inline VectorValues ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering) const {
   	VectorValues delta(this->dims(ordering));
   	localCoordinates(cp, ordering, delta);
   	return delta;
@@ -199,7 +199,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class J>
-  void Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering, VectorValues& delta) const {
+  void ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering, VectorValues& delta) const {
     typedef pair<J,typename J::Value> KeyValue;
     BOOST_FOREACH(const KeyValue& value, cp) {
       assert(this->exists(value.first));
@@ -213,7 +213,7 @@ namespace gtsam {
     if(message_.empty())
       message_ = 
           "Attempting to " + std::string(operation_) + " the key \"" +
-          (std::string)key_ + "\", which does not exist in the Values.";
+          (std::string)key_ + "\", which does not exist in the ValuesOld.";
     return message_.c_str();
   }
 
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/ValuesOld.h
similarity index 90%
rename from gtsam/nonlinear/Values.h
rename to gtsam/nonlinear/ValuesOld.h
index a4bc57c0c..88595a5dd 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/ValuesOld.h
@@ -10,14 +10,14 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Values.h
+ * @file ValuesOld.h
  * @author Richard Roberts
  *
  * @brief A templated config for Manifold-group elements
  *
  *  Detailed story:
  *  A values structure is a map from keys to values. It is used to specify the value of a bunch
- *  of variables in a factor graph. A Values is a values structure which can hold variables that
+ *  of variables in a factor graph. A ValuesOld is a values structure which can hold variables that
  *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
  *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
  */
@@ -52,7 +52,7 @@ namespace gtsam {
 	 *  labels (example: Pose2, Point2, etc)
 	 */
   template<class J>
-  class Values {
+  class ValuesOld {
 
   public:
 
@@ -77,12 +77,12 @@ namespace gtsam {
 
   public:
 
-    Values() {}
-    Values(const Values& config) :
+    ValuesOld() {}
+    ValuesOld(const ValuesOld& config) :
       values_(config.values_) {}
     template<class J_ALT>
-    Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
-    virtual ~Values() {}
+    ValuesOld(const ValuesOld<J_ALT>& other) {} // do nothing when initializing with wrong type
+    virtual ~ValuesOld() {}
 
     /// @name Testable
     /// @{
@@ -91,7 +91,7 @@ namespace gtsam {
     void print(const std::string &s="") const;
 
     /** Test whether configs are identical in keys and values */
-    bool equals(const Values& expected, double tol=1e-9) const;
+    bool equals(const ValuesOld& expected, double tol=1e-9) const;
 
     /// @}
 
@@ -132,13 +132,13 @@ namespace gtsam {
     size_t dim() const;
 
     /** Add a delta config to current config and returns a new config */
-    Values retract(const VectorValues& delta, const Ordering& ordering) const;
+    ValuesOld retract(const VectorValues& delta, const Ordering& ordering) const;
 
     /** Get a delta config about a linearization point c0 (*this) */
-    VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
+    VectorValues localCoordinates(const ValuesOld& cp, const Ordering& ordering) const;
 
     /** Get a delta config about a linearization point c0 (*this) */
-    void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
+    void localCoordinates(const ValuesOld& cp, const Ordering& ordering, VectorValues& delta) const;
 
     /// @}
 
@@ -148,10 +148,10 @@ namespace gtsam {
     void insert(const J& j, const Value& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
-    void insert(const Values& cfg);
+    void insert(const ValuesOld& cfg);
 
     /** update the current available values without adding new ones */
-    void update(const Values& cfg);
+    void update(const ValuesOld& cfg);
 
     /** single element change of existing element */
     void update(const J& j, const Value& val);
@@ -172,7 +172,7 @@ namespace gtsam {
     std::list<J> keys() const;
 
     /** Replace all keys and variables */
-    Values& operator=(const Values& rhs) {
+    ValuesOld& operator=(const ValuesOld& rhs) {
       values_ = rhs.values_;
       return (*this);
     }
@@ -185,7 +185,7 @@ namespace gtsam {
     /**
      * Apply a class with an application operator() to a const_iterator over
      * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
+     * iterator for every type in the ValuesOld, (i.e. through templating).
      */
     template<typename A>
     void apply(A& operation) {
@@ -297,5 +297,5 @@ public:
 
 } // \namespace gtsam
 
-#include <gtsam/nonlinear/Values-inl.h>
+#include <gtsam/nonlinear/ValuesOld-inl.h>
 
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index eb1c4614c..4d66620dd 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -38,16 +38,16 @@ namespace gtsam {
 	 * For practical use, it would be good to subclass this factor and have the class type
 	 * construct the mask.
 	 */
-	template<class VALUES, class KEY>
-	class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> {
+	template<class KEY>
+	class PartialPriorFactor: public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value T;
 
 	protected:
 
-		typedef NonlinearFactor1<VALUES, KEY> Base;
-		typedef PartialPriorFactor<VALUES, KEY> This;
+		typedef NonlinearFactor1<KEY> Base;
+		typedef PartialPriorFactor<KEY> This;
 
 		Vector prior_;             ///< measurement on logmap parameters, in compressed form
 		std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 73d18a7e6..dcf425aaa 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -29,8 +29,8 @@ namespace gtsam {
 	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
 	 * i.e. the main building block for visual SLAM.
 	 */
-	template<class VALUES, class LMK, class POSK>
-	class GenericProjectionFactor: public NonlinearFactor2<VALUES, POSK, LMK> {
+	template<class LMK, class POSK>
+	class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
 	protected:
 
 		// Keep a copy of measurement and calibration for I/O
@@ -40,10 +40,10 @@ namespace gtsam {
 	public:
 
 		/// shorthand for base class type
-		typedef NonlinearFactor2<VALUES, POSK, LMK> Base;
+		typedef NonlinearFactor2<POSK, LMK> Base;
 
 		/// shorthand for a smart pointer to a factor
-		typedef boost::shared_ptr<GenericProjectionFactor<VALUES, LMK, POSK> > shared_ptr;
+		typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
 
 		/// Default constructor
 		GenericProjectionFactor() :
@@ -73,7 +73,7 @@ namespace gtsam {
 		}
 
 		/// equals
-		bool equals(const GenericProjectionFactor<VALUES, LMK, POSK>& p
+		bool equals(const GenericProjectionFactor<LMK, POSK>& p
 				, double tol = 1e-9) const {
 			return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
 					&& this->K_->equals(*p.K_, tol);
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index 93cef86f1..b762c5690 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -25,14 +25,14 @@ namespace gtsam {
 	/**
 	 * Binary factor for a range measurement
 	 */
-	template<class VALUES, class POSEKEY, class POINTKEY>
-	class RangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
+	template<class POSEKEY, class POINTKEY>
+	class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
 	private:
 
 		double z_; /** measurement */
 
-		typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
+		typedef RangeFactor<POSEKEY, POINTKEY> This;
+		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
 
 		typedef typename POSEKEY::Value Pose;
 		typedef typename POINTKEY::Value Point;
@@ -64,7 +64,7 @@ namespace gtsam {
 		}
 
 		/** equals specialized to this factor */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e = dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol;
 		}
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 729b9fff1..7f1e8077d 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -22,8 +22,8 @@
 
 namespace gtsam {
 
-template<class VALUES, class KEY1, class KEY2>
-class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
+template<class KEY1, class KEY2>
+class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
 private:
 
 	// Keep a copy of measurement and calibration for I/O
@@ -33,7 +33,7 @@ private:
 public:
 
 	// shorthand for base class type
-	typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;					///< typedef for base class
+	typedef NonlinearFactor2<KEY1, KEY2> Base;					///< typedef for base class
 	typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
 	typedef typename KEY1::Value CamPose;												///< typedef for Pose Lie Value type
 
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 712125588..d94749b7f 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -156,10 +156,10 @@ void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDi
 	fstream stream(filename.c_str(), fstream::out);
 
 	// save poses
-	pose2SLAM::Key key;
-	Pose2 pose;
-	BOOST_FOREACH(boost::tie(key, pose), config)
-		stream << "VERTEX2 " << key.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
+	for (DynamicValues::const_iterator it = config.begin(); it != config.end(); ++it) {
+		Pose2 pose = config.at<Pose2>(it->first);
+		stream << "VERTEX2 " << it->first.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
+	}
 
 	// save edges
 	Matrix R = model->R();
@@ -168,7 +168,7 @@ void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDi
 		boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
 		if (!factor) continue;
 
-		pose = factor->measured().inverse();
+		Pose2 pose = factor->measured().inverse();
 		stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
 				<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
 				<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp
index c90e56be9..d4f48e15d 100644
--- a/gtsam/slam/planarSLAM.cpp
+++ b/gtsam/slam/planarSLAM.cpp
@@ -25,8 +25,8 @@ namespace gtsam {
 
 	namespace planarSLAM {
 
-		Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
-				NonlinearFactorGraph<Values>(graph) {}
+		Graph::Graph(const NonlinearFactorGraph& graph) :
+				NonlinearFactorGraph(graph) {}
 
 		void Graph::addPrior(const PoseKey& i, const Pose2& p,
 				const SharedNoiseModel& model) {
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 5776e989c..15f9ff61f 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -22,7 +22,6 @@
 #include <gtsam/slam/RangeFactor.h>
 #include <gtsam/slam/BearingFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
@@ -41,32 +40,21 @@ namespace gtsam {
 		/// Typedef for a PointKey with Point2 data and 'l' symbol
 		typedef TypedSymbol<Point2, 'l'> PointKey;
 
-		/// Typedef for Values structure with PoseKey type
-		typedef Values<PoseKey> PoseValues;
-
-		/// Typedef for Values structure with PointKey type
-		typedef Values<PointKey> PointValues;
-
 		/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
-		struct Values: public TupleValues2<PoseValues, PointValues> {
+		struct Values: public DynamicValues {
 
 			/// Default constructor
 			Values() {}
 
 			/// Copy constructor
-			Values(const TupleValues2<PoseValues, PointValues>& values) :
-				TupleValues2<PoseValues, PointValues>(values) {
-			}
-
-			/// Copy constructor
-			Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
-				TupleValues2<PoseValues, PointValues>(values) {
+			Values(const DynamicValues& values) :
+				DynamicValues(values) {
 			}
 
 			/// From sub-values
-			Values(const PoseValues& poses, const PointValues& points) :
-				TupleValues2<PoseValues, PointValues>(poses, points) {
-			}
+//			Values(const DynamicValues& poses, const DynamicValues& points) :
+//				DynamicValues(poses, points) {
+//			}
 
 			// Convenience for MATLAB wrapper, which does not allow for identically named methods
 
@@ -88,26 +76,26 @@ namespace gtsam {
 		 */
 
 		/// A hard constraint for PoseKeys to enforce particular values
-		typedef NonlinearEquality<Values, PoseKey> Constraint;
+		typedef NonlinearEquality<PoseKey> Constraint;
 		/// A prior factor to bias the value of a PoseKey
-		typedef PriorFactor<Values, PoseKey> Prior;
+		typedef PriorFactor<PoseKey> Prior;
 		/// A factor between two PoseKeys set with a Pose2
-		typedef BetweenFactor<Values, PoseKey> Odometry;
+		typedef BetweenFactor<PoseKey> Odometry;
 		/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
-		typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
+		typedef BearingFactor<PoseKey, PointKey> Bearing;
 		/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
-		typedef RangeFactor<Values, PoseKey, PointKey> Range;
+		typedef RangeFactor<PoseKey, PointKey> Range;
 		/// A factor between a PoseKey and a PointKey to express difference in rotation and location
-		typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
+		typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
 
 		/// Creates a NonlinearFactorGraph with the Values type
-		struct Graph: public NonlinearFactorGraph<Values> {
+		struct Graph: public NonlinearFactorGraph {
 
 			/// Default constructor for a NonlinearFactorGraph
 			Graph(){}
 
 			/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
-			Graph(const NonlinearFactorGraph<Values>& graph);
+			Graph(const NonlinearFactorGraph& graph);
 
 			/// Biases the value of PoseKey key with Pose2 p given a noise model
 			void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
@@ -132,15 +120,15 @@ namespace gtsam {
 					const Rot2& bearing, double range, const SharedNoiseModel& model);
 
 			/// Optimize
-			Values optimize(const Values& initialEstimate) {
-				typedef NonlinearOptimizer<Graph, Values> Optimizer;
+			Values optimize(const DynamicValues& initialEstimate) {
+				typedef NonlinearOptimizer<Graph> Optimizer;
 				return *Optimizer::optimizeLM(*this, initialEstimate,
 										NonlinearOptimizationParameters::LAMBDA);
 			}
 		};
 
 		/// Optimizer
-		typedef NonlinearOptimizer<Graph, Values> Optimizer;
+		typedef NonlinearOptimizer<Graph> Optimizer;
 
 	} // planarSLAM
 
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 714bcce51..88110c40e 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -19,7 +19,6 @@
 
 #include <gtsam/base/LieVector.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/Key.h>
diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp
index 102652813..29eb14ba9 100644
--- a/gtsam/slam/pose3SLAM.cpp
+++ b/gtsam/slam/pose3SLAM.cpp
@@ -25,8 +25,8 @@ namespace gtsam {
 	namespace pose3SLAM {
 
 		/* ************************************************************************* */
-		Values circle(size_t n, double radius) {
-			Values x;
+		DynamicValues circle(size_t n, double radius) {
+			DynamicValues x;
 			double theta = 0, dtheta = 2 * M_PI / n;
 			// We use aerospace/navlab convention, X forward, Y right, Z down
 			// First pose will be at (R,0,0)
@@ -39,7 +39,7 @@ namespace gtsam {
 				Point3 gti(radius*cos(theta), radius*sin(theta), 0);
 				Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
 				Pose3 gTi(gR0 * _0Ri, gti);
-				x.insert(i, gTi);
+				x.insert(Key(i), gTi);
 			}
 			return x;
 		}
diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h
index f4516e647..544dcb5f0 100644
--- a/gtsam/slam/pose3SLAM.h
+++ b/gtsam/slam/pose3SLAM.h
@@ -18,7 +18,6 @@
 #pragma once
 
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/Key.h>
@@ -33,8 +32,6 @@ namespace gtsam {
 
 		/// Creates a Key with data Pose3 and symbol 'x'
 		typedef TypedSymbol<Pose3, 'x'> Key;
-		/// Creates a Values structure with type 'Key'
-		typedef Values<Key> Values;
 
 		/**
 		 * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@@ -42,17 +39,17 @@ namespace gtsam {
 		 * @param R radius of circle
 		 * @return circle of n 3D poses
 		 */
-		Values circle(size_t n, double R);
+		DynamicValues circle(size_t n, double R);
 
 		/// A prior factor on Key with Pose3 data type.
-		typedef PriorFactor<Values, Key> Prior;
+		typedef PriorFactor<Key> Prior;
 		/// A factor to put constraints between two factors.
-		typedef BetweenFactor<Values, Key> Constraint;
+		typedef BetweenFactor<Key> Constraint;
 		/// A hard constraint would enforce that the given key would have the input value in the results.
-		typedef NonlinearEquality<Values, Key> HardConstraint;
+		typedef NonlinearEquality<Key> HardConstraint;
 
 		/// Graph
-		struct Graph: public NonlinearFactorGraph<Values> {
+		struct Graph: public NonlinearFactorGraph {
 
 			/// Adds a factor between keys of the same type
 			void addPrior(const Key& i, const Pose3& p,
@@ -67,14 +64,13 @@ namespace gtsam {
 		};
 
 		/// Optimizer
-		typedef NonlinearOptimizer<Graph, Values> Optimizer;
+		typedef NonlinearOptimizer<Graph> Optimizer;
 
 	} // pose3SLAM
 
 	/**
 	 * Backwards compatibility
 	 */
-	typedef pose3SLAM::Values Pose3Values;			///< Typedef for Values class for backwards compatibility
 	typedef pose3SLAM::Prior Pose3Prior;			///< Typedef for Prior class for backwards compatibility
 	typedef pose3SLAM::Constraint Pose3Factor;		///< Typedef for Constraint class for backwards compatibility
 	typedef pose3SLAM::Graph Pose3Graph;			///< Typedef for Graph class for backwards compatibility
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index 4cb23f76a..291bde25e 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -19,7 +19,6 @@
 #pragma once
 
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index f44c92118..d54924c12 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -19,7 +19,6 @@
 #pragma once
 
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
@@ -32,25 +31,26 @@ namespace gtsam {
 		// The types that take an oriented pose2 rather than point2
 		typedef TypedSymbol<Point2, 'l'> PointKey;
 		typedef TypedSymbol<Pose2, 'x'> PoseKey;
-		typedef Values<PoseKey> PoseValues;
-		typedef Values<PointKey> PointValues;
 
 		/// Specialized Values structure with syntactic sugar for
 		/// compatibility with matlab
-		class Values: public TupleValues2<PoseValues, PointValues> {
+		class Values: public DynamicValues {
+			int nrPoses_, nrPoints_;
 		public:
-			Values() {}
+			Values() : nrPoses_(0), nrPoints_(0) {}
 
 			void insertPose(const PoseKey& i, const Pose2& p) {
 				insert(i, p);
+				nrPoses_++;
 			}
 
 			void insertPoint(const PointKey& j, const Point2& p) {
 				insert(j, p);
+				nrPoints_++;
 			}
 
-			int nrPoses() const {	return this->first_.size();	}
-			int nrPoints() const { return this->second_.size();	}
+			int nrPoses() const {	return nrPoses_;	}
+			int nrPoints() const { return nrPoints_;	}
 
 			Pose2 pose(const PoseKey& i) const { return (*this)[i];	}
 			Point2 point(const PointKey& j) const { return (*this)[j]; }
diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index e995308f8..4a24bcaa6 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -24,7 +24,6 @@
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/Key.h>
-#include <gtsam/nonlinear/TupleValues.h>
 
 // \namespace
 
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index 16e424344..958337677 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -22,7 +22,6 @@
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/nonlinear/Key.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/slam/PriorFactor.h>
@@ -39,25 +38,22 @@ namespace gtsam {
    */
   typedef TypedSymbol<Pose3,'x'> PoseKey;									///< The key type used for poses
   typedef TypedSymbol<Point3,'l'> PointKey;								///< The key type used for points
-  typedef Values<PoseKey> PoseValues;									///< Values used for poses
-  typedef Values<PointKey> PointValues;								///< Values used for points
-  typedef TupleValues2<PoseValues, PointValues> Values;		///< Values data structure
-  typedef boost::shared_ptr<Values> shared_values;				///< shared pointer to values data structure
+  typedef boost::shared_ptr<DynamicValues> shared_values;				///< shared pointer to values data structure
 
-  typedef NonlinearEquality<Values, PoseKey> PoseConstraint;	 ///< put a hard constraint on a pose
-  typedef NonlinearEquality<Values, PointKey> PointConstraint; ///< put a hard constraint on a point
-  typedef PriorFactor<Values, PoseKey> PosePrior;							 ///< put a soft prior on a Pose
-  typedef PriorFactor<Values, PointKey> PointPrior;						 ///< put a soft prior on a point
-  typedef RangeFactor<Values, PoseKey, PointKey> RangeFactor;  ///< put a factor on the range from a pose to a point
+  typedef NonlinearEquality<PoseKey> PoseConstraint;	 ///< put a hard constraint on a pose
+  typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
+  typedef PriorFactor<PoseKey> PosePrior;							 ///< put a soft prior on a Pose
+  typedef PriorFactor<PointKey> PointPrior;						 ///< put a soft prior on a point
+  typedef RangeFactor<PoseKey, PointKey> RangeFactor;  ///< put a factor on the range from a pose to a point
   
   /// monocular and stereo camera typedefs for general use
-  typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
-  typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
+  typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
+  typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
 
   /**
    * Non-linear factor graph for vanilla visual SLAM
    */
-  class Graph: public NonlinearFactorGraph<Values> {
+  class Graph: public NonlinearFactorGraph {
 
   public:
   	/// shared pointer to this type of graph
@@ -69,12 +65,12 @@ namespace gtsam {
 
     /// print out graph
     void print(const std::string& s = "") const {
-      NonlinearFactorGraph<Values>::print(s);
+      NonlinearFactorGraph::print(s);
     }
 
     /// check if two graphs are equal
     bool equals(const Graph& p, double tol = 1e-9) const {
-      return NonlinearFactorGraph<Values>::equals(p, tol);
+      return NonlinearFactorGraph::equals(p, tol);
     }
 
     /**
@@ -140,6 +136,6 @@ namespace gtsam {
   }; // Graph
 
   /// typedef for Optimizer. The current default will use the multi-frontal solver
-  typedef NonlinearOptimizer<Graph, Values> Optimizer;
+  typedef NonlinearOptimizer<Graph> Optimizer;
 
 } } // namespaces
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 095f562a4..0d5402607 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -93,7 +93,7 @@ TEST( NonlinearFactor, NonlinearFactor )
   CHECK(assert_equal(0.1*ones(2),actual_e));
 
   // error = 0.5 * [1 1] * [1;1] = 1
-  double expected = 1.0; 
+  double expected = 1.0;
 
   // calculate the error from the factor "f1"
   double actual = factor->error(cfg);
@@ -179,14 +179,14 @@ TEST( NonlinearFactor, size )
 {
 	// create a non linear factor graph
 	Graph fg = createNonlinearFactorGraph();
-	
+
 	// create a values structure for the non linear factor graph
 	example::Values cfg = createNoisyValues();
-	
+
 	// get some factors from the graph
 	Graph::sharedFactor factor1 = fg[0], factor2 = fg[1],
 			factor3 = fg[2];
-	
+
 	CHECK(factor1->size() == 1);
 	CHECK(factor2->size() == 2);
 	CHECK(factor3->size() == 2);

From 2db224df3c6846b710a352a87438a8570dfcd3b1 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Sun, 29 Jan 2012 22:10:35 +0000
Subject: [PATCH 19/88] All compiled! Only SPCG and linear/SubgraphSolver are
 not fixed.

---
 examples/CameraResectioning.cpp               | 15 ++++++------
 examples/Makefile.am                          |  4 ++--
 examples/PlanarSLAMExample_easy.cpp           |  2 +-
 examples/PlanarSLAMSelfContained_advanced.cpp | 24 ++++++++-----------
 examples/Pose2SLAMExample_advanced.cpp        |  4 ++--
 examples/Pose2SLAMExample_easy.cpp            |  4 ++--
 examples/Pose2SLAMwSPCG_easy.cpp              |  2 +-
 examples/SimpleRotation.cpp                   | 10 ++++----
 examples/easyPoint2KalmanFilter.cpp           | 16 ++++++-------
 examples/elaboratePoint2KalmanFilter.cpp      | 18 +++++++-------
 examples/vSLAMexample/vISAMexample.cpp        | 14 +++++------
 examples/vSLAMexample/vSFMexample.cpp         | 10 ++++----
 gtsam/linear/SubgraphSolver-inl.h             | 18 +++++++-------
 gtsam/linear/SubgraphSolver.h                 | 11 +++++----
 gtsam/nonlinear/ExtendedKalmanFilter-inl.h    | 22 ++++++++---------
 gtsam/nonlinear/ExtendedKalmanFilter.h        | 10 ++++----
 gtsam/nonlinear/NonlinearISAM-inl.h           | 24 +++++++++----------
 gtsam/nonlinear/NonlinearISAM.h               | 11 ++++-----
 gtsam/nonlinear/NonlinearOptimization-inl.h   | 10 ++++----
 gtsam/slam/pose2SLAM.h                        |  4 ++--
 tests/timeMultifrontalOnDataset.cpp           |  2 +-
 tests/timeSequentialOnDataset.cpp             |  2 +-
 22 files changed, 113 insertions(+), 124 deletions(-)

diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index aee073df7..9727ceacb 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -20,7 +20,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/SimpleCamera.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
@@ -28,13 +27,12 @@
 using namespace gtsam;
 
 typedef TypedSymbol<Pose3, 'x'> PoseKey;
-typedef Values<PoseKey> PoseValues;
 
 /**
  * Unary factor for the pose.
  */
-class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> {
-  typedef NonlinearFactor1<PoseValues, PoseKey> Base;
+class ResectioningFactor: public NonlinearFactor1<PoseKey> {
+  typedef NonlinearFactor1<PoseKey> Base;
 
   shared_ptrK K_; // camera's intrinsic parameters
   Point3 P_; // 3D point on the calibration rig
@@ -46,6 +44,8 @@ public:
     Base(model, key), K_(calib), P_(P), p_(p) {
   }
 
+  virtual ~ResectioningFactor() {}
+
   virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
       boost::none) const {
     SimpleCamera camera(*K_, X);
@@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
   PoseKey X(1);
 
   /* 1. create graph */
-  NonlinearFactorGraph<PoseValues> graph;
+  NonlinearFactorGraph graph;
 
   /* 2. add factors to the graph */
   // add measurement factors
@@ -92,14 +92,13 @@ int main(int argc, char* argv[]) {
   graph.push_back(factor);
 
   /* 3. Create an initial estimate for the camera pose */
-  PoseValues initial;
+  DynamicValues initial;
   initial.insert(X, Pose3(Rot3(1.,0.,0.,
                                0.,-1.,0.,
                                0.,0.,-1.), Point3(0.,0.,2.0)));
 
   /* 4. Optimize the graph using Levenberg-Marquardt*/
-  PoseValues result = optimize<NonlinearFactorGraph<PoseValues> , PoseValues> (
-      graph, initial);
+  DynamicValues result = optimize<NonlinearFactorGraph> (graph, initial);
   result.print("Final result: ");
 
   return 0;
diff --git a/examples/Makefile.am b/examples/Makefile.am
index 14b6fb935..d7ec2aaea 100644
--- a/examples/Makefile.am
+++ b/examples/Makefile.am
@@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy			# Solves SLAM example from tutorial
 noinst_PROGRAMS += PlanarSLAMSelfContained_advanced	# Solves SLAM example from tutorial with all typedefs in the script
 noinst_PROGRAMS += Pose2SLAMExample_easy 			# Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
 noinst_PROGRAMS += Pose2SLAMExample_advanced		# Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
-noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
-noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
+#noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
+#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
 noinst_PROGRAMS += elaboratePoint2KalmanFilter 		# simple linear Kalman filter on a moving 2D point, but done using factor graphs
 noinst_PROGRAMS += easyPoint2KalmanFilter 		    # uses the cool generic templated Kalman filter class to do the same
 noinst_PROGRAMS += CameraResectioning
diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp
index d697ae261..b73a989d4 100644
--- a/examples/PlanarSLAMExample_easy.cpp
+++ b/examples/PlanarSLAMExample_easy.cpp
@@ -86,7 +86,7 @@ int main(int argc, char** argv) {
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
+	planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp
index d915a9ebd..10b1b3576 100644
--- a/examples/PlanarSLAMSelfContained_advanced.cpp
+++ b/examples/PlanarSLAMSelfContained_advanced.cpp
@@ -34,7 +34,6 @@
 #include <gtsam/slam/BearingRangeFactor.h>
 
 // implementations for structures - needed if self-contained, and these should be included last
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
@@ -43,12 +42,9 @@
 // Main typedefs
 typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey;       // Key for poses, with type included
 typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey;      // Key for points, with type included
-typedef gtsam::Values<PoseKey> PoseValues;                // config type for poses
-typedef gtsam::Values<PointKey> PointValues;              // config type for points
-typedef gtsam::TupleValues2<PoseValues, PointValues> PlanarValues; // main config with two variable classes
-typedef gtsam::NonlinearFactorGraph<PlanarValues> Graph;			 // graph structure
-typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
-typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
+typedef gtsam::NonlinearFactorGraph Graph;			 // graph structure
+typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
+typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
 
 using namespace std;
 using namespace gtsam;
@@ -74,7 +70,7 @@ int main(int argc, char** argv) {
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	PriorFactor<PlanarValues, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
+	PriorFactor<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
 	graph->add(posePrior);  // add the factor to the graph
 
 	/* add odometry */
@@ -82,8 +78,8 @@ int main(int argc, char** argv) {
 	SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
 	// create between factors to represent odometry
-	BetweenFactor<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
-	BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
+	BetweenFactor<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
+	BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
 	graph->add(odom12); // add both to graph
 	graph->add(odom23);
 
@@ -100,9 +96,9 @@ int main(int argc, char** argv) {
 		   range32 = 2.0;
 
 	// create bearing/range factors
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
+	BearingRangeFactor<PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
+	BearingRangeFactor<PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
+	BearingRangeFactor<PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
 
 	// add the factors
 	graph->add(meas11);
@@ -112,7 +108,7 @@ int main(int argc, char** argv) {
 	graph->print("Full Graph");
 
 	// initialize to noisy points
-	boost::shared_ptr<PlanarValues> initial(new PlanarValues);
+	boost::shared_ptr<DynamicValues> initial(new DynamicValues);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp
index 703f6a47d..b6bfe7303 100644
--- a/examples/Pose2SLAMExample_advanced.cpp
+++ b/examples/Pose2SLAMExample_advanced.cpp
@@ -57,7 +57,7 @@ int main(int argc, char** argv) {
 
   /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
-	shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
+	shared_ptr<DynamicValues> initial(new DynamicValues);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
 	Optimizer optimizer(graph, initial, ordering, params);
 	Optimizer optimizer_result = optimizer.levenbergMarquardt();
 
-	pose2SLAM::Values result = *optimizer_result.values();
+	DynamicValues result = *optimizer_result.values();
 	result.print("final result");
 
 	/* Get covariances */
diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp
index 68ece651b..fb98ebd97 100644
--- a/examples/Pose2SLAMExample_easy.cpp
+++ b/examples/Pose2SLAMExample_easy.cpp
@@ -55,7 +55,7 @@ int main(int argc, char** argv) {
 
     /* 3. Create the data structure to hold the initial estinmate to the solution
      * initialize to noisy points */
-	pose2SLAM::Values initial;
+	DynamicValues initial;
 	initial.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -63,7 +63,7 @@ int main(int argc, char** argv) {
 
 	/* 4 Single Step Optimization
 	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
-	pose2SLAM::Values result = optimize<Graph, pose2SLAM::Values>(graph, initial);
+	DynamicValues result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 
diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp
index 3cce81d48..51be80841 100644
--- a/examples/Pose2SLAMwSPCG_easy.cpp
+++ b/examples/Pose2SLAMwSPCG_easy.cpp
@@ -27,7 +27,7 @@ using namespace gtsam;
 using namespace pose2SLAM;
 
 Graph graph;
-pose2SLAM::Values initial, result;
+DynamicValues initial, result;
 
 /* ************************************************************************* */
 int main(void) {
diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp
index 9d897dd39..bd00f4a04 100644
--- a/examples/SimpleRotation.cpp
+++ b/examples/SimpleRotation.cpp
@@ -23,7 +23,6 @@
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/Key.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
 
@@ -53,8 +52,7 @@ using namespace gtsam;
  * and 2D poses.
  */
 typedef TypedSymbol<Rot2, 'x'> Key; 								/// Variable labels for a specific type
-typedef Values<Key> RotValues;											/// Class to store values - acts as a state for the system
-typedef NonlinearFactorGraph<RotValues> Graph;					/// Graph container for constraints - needs to know type of variables
+typedef NonlinearFactorGraph Graph;					/// Graph container for constraints - needs to know type of variables
 
 const double degree = M_PI / 180;
 
@@ -83,7 +81,7 @@ int main() {
 	prior.print("goal angle");
 	SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
 	Key key(1);
-	PriorFactor<RotValues, Key> factor(key, prior, model);
+	PriorFactor<Key> factor(key, prior, model);
 
 	/**
 	 *    Step 3: create a graph container and add the factor to it
@@ -114,7 +112,7 @@ int main() {
 	 * on the type of key used to find the appropriate value map if there
 	 * are multiple types of variables.
 	 */
-	RotValues initial;
+	DynamicValues initial;
 	initial.insert(key, Rot2::fromAngle(20 * degree));
 	initial.print("initial estimate");
 
@@ -126,7 +124,7 @@ int main() {
 	 * initial estimate.  This will yield a new RotValues structure
 	 * with the final state of the optimization.
 	 */
-	RotValues result = optimize<Graph, RotValues>(graph, initial);
+	DynamicValues result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp
index f22d65f95..d190c38dc 100644
--- a/examples/easyPoint2KalmanFilter.cpp
+++ b/examples/easyPoint2KalmanFilter.cpp
@@ -24,7 +24,6 @@
 #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/geometry/Point2.h>
 
 using namespace std;
@@ -32,7 +31,6 @@ using namespace gtsam;
 
 // Define Types for Linear System Test
 typedef TypedSymbol<Point2, 'x'> LinearKey;
-typedef Values<LinearKey> LinearValues;
 typedef Point2 LinearMeasurement;
 
 int main() {
@@ -42,7 +40,7 @@ int main() {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<LinearValues,LinearKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
 
 
 
@@ -67,7 +65,7 @@ int main() {
   // Predict delta based on controls
   Point2 difference(1,0);
   // Create Factor
-  BetweenFactor<LinearValues,LinearKey> factor1(x0, x1, difference, Q);
+  BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
 
   // Predict the new value with the EKF class
   Point2 x1_predict = ekf.predict(factor1);
@@ -88,7 +86,7 @@ int main() {
 
   // This simple measurement can be modeled with a PriorFactor
   Point2 z1(1.0, 0.0);
-  PriorFactor<LinearValues,LinearKey> factor2(x1, z1, R);
+  PriorFactor<LinearKey> factor2(x1, z1, R);
 
   // Update the Kalman Filter with the measurement
   Point2 x1_update = ekf.update(factor2);
@@ -100,13 +98,13 @@ int main() {
   // Predict
   LinearKey x2(2);
   difference = Point2(1,0);
-  BetweenFactor<LinearValues,LinearKey> factor3(x1, x2, difference, Q);
+  BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
   Point2 x2_predict = ekf.predict(factor1);
   x2_predict.print("X2 Predict");
 
   // Update
   Point2 z2(2.0, 0.0);
-  PriorFactor<LinearValues,LinearKey> factor4(x2, z2, R);
+  PriorFactor<LinearKey> factor4(x2, z2, R);
   Point2 x2_update = ekf.update(factor4);
   x2_update.print("X2 Update");
 
@@ -116,13 +114,13 @@ int main() {
   // Predict
   LinearKey x3(3);
   difference = Point2(1,0);
-  BetweenFactor<LinearValues,LinearKey> factor5(x2, x3, difference, Q);
+  BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
   Point2 x3_predict = ekf.predict(factor5);
   x3_predict.print("X3 Predict");
 
   // Update
   Point2 z3(3.0, 0.0);
-  PriorFactor<LinearValues,LinearKey> factor6(x3, z3, R);
+  PriorFactor<LinearKey> factor6(x3, z3, R);
   Point2 x3_update = ekf.update(factor6);
   x3_update.print("X3 Update");
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index 8c8cbe147..d001016c4 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -22,7 +22,6 @@
 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Key.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
@@ -35,7 +34,6 @@ using namespace std;
 using namespace gtsam;
 
 typedef TypedSymbol<Point2, 'x'> Key;               /// Variable labels for a specific type
-typedef Values<Key> KalmanValues;                   /// Class to store values - acts as a state for the system
 
 int main() {
 
@@ -48,7 +46,7 @@ int main() {
   Ordering::shared_ptr ordering(new Ordering);
 
   // Create a structure to hold the linearization points
-  KalmanValues linearizationPoints;
+  DynamicValues linearizationPoints;
 
 	// Ground truth example
 	// Start at origin, move to the right (x-axis): 0,0  0,1  0,2
@@ -64,7 +62,7 @@ int main() {
   // This is equivalent to x_0 and P_0
   Point2 x_initial(0,0);
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  PriorFactor<KalmanValues, Key> factor1(x0, x_initial, P_initial);
+  PriorFactor<Key> factor1(x0, x_initial, P_initial);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x0, x_initial);
   linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
@@ -95,7 +93,7 @@ int main() {
 
   Point2 difference(1,0);
   SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<KalmanValues, Key> factor2(x0, x1, difference, Q);
+  BetweenFactor<Key> factor2(x0, x1, difference, Q);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x1, x_initial);
   linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering));
@@ -173,7 +171,7 @@ int main() {
   // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
   Point2 z1(1.0, 0.0);
   SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<KalmanValues, Key> factor4(x1, z1, R1);
+  PriorFactor<Key> factor4(x1, z1, R1);
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
 
@@ -225,7 +223,7 @@ int main() {
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
   Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<KalmanValues, Key> factor6(x1, x2, difference, Q);
+  BetweenFactor<Key> factor6(x1, x2, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x2, x1_update);
@@ -263,7 +261,7 @@ int main() {
   // And update using z2 ...
   Point2 z2(2.0, 0.0);
   SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<KalmanValues, Key> factor8(x2, z2, R2);
+  PriorFactor<Key> factor8(x2, z2, R2);
 
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@@ -314,7 +312,7 @@ int main() {
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
   Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<KalmanValues, Key> factor10(x2, x3, difference, Q);
+  BetweenFactor<Key> factor10(x2, x3, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x3, x2_update);
@@ -352,7 +350,7 @@ int main() {
   // And update using z3 ...
   Point2 z3(3.0, 0.0);
   SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<KalmanValues, Key> factor12(x3, z3, R3);
+  PriorFactor<Key> factor12(x3, z3, R3);
 
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index 6cdc34f17..ce88276c9 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -80,7 +80,7 @@ void readAllDataISAM() {
 /**
  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
  */
-void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
+void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicValues>& initialValues,
     int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
 
   // Create a graph of newFactors with new measurements
@@ -101,10 +101,10 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLA
   }
 
   // Create initial values for all nodes in the newFactors
-  initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
-  initialValues->insert(pose_id, pose);
+  initialValues = shared_ptr<DynamicValues> (new DynamicValues());
+  initialValues->insert(PoseKey(pose_id), pose);
   for (size_t i = 0; i < measurements.size(); i++) {
-    initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
+    initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
   }
 }
 
@@ -124,7 +124,7 @@ int main(int argc, char* argv[]) {
 
   // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
   int relinearizeInterval = 3;
-  NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
+  NonlinearISAM<> isam(relinearizeInterval);
 
   // At each frame (poseId) with new camera pose and set of associated measurements,
   // create a graph of new factors and update ISAM
@@ -132,12 +132,12 @@ int main(int argc, char* argv[]) {
   BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
     const int poseId = features.first;
     shared_ptr<Graph> newFactors;
-    shared_ptr<visualSLAM::Values> initialValues;
+    shared_ptr<DynamicValues> initialValues;
     createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
             features.second, measurementSigma, g_calib);
 
     isam.update(*newFactors, *initialValues);
-    visualSLAM::Values currentEstimate = isam.estimate();
+    DynamicValues currentEstimate = isam.estimate();
     cout << "****************************************************" << endl;
     currentEstimate.print("Current estimate: ");
   }
diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp
index 9dc8bfe34..1edb4a9ae 100644
--- a/examples/vSLAMexample/vSFMexample.cpp
+++ b/examples/vSLAMexample/vSFMexample.cpp
@@ -103,17 +103,17 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
  * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
  * The returned Values structure contains all initial values for all nodes.
  */
-visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
+DynamicValues initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
 
-	visualSLAM::Values initValues;
+	DynamicValues initValues;
 
   // Initialize landmarks 3D positions.
   for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
-    initValues.insert(lmit->first, lmit->second);
+    initValues.insert(PointKey(lmit->first), lmit->second);
 
   // Initialize camera poses.
   for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
-    initValues.insert(poseit->first, poseit->second);
+    initValues.insert(PoseKey(poseit->first), poseit->second);
 
   return initValues;
 }
@@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
   boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
 
   // Create an initial Values structure using groundtruth values as the initial estimates
-  boost::shared_ptr<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses)));
+  boost::shared_ptr<DynamicValues> initialEstimates(new DynamicValues(initialize(g_landmarks, g_poses)));
   cout << "*******************************************************" << endl;
   initialEstimates->print("INITIAL ESTIMATES: ");
 
diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h
index 63a497ac1..78fa4fdfe 100644
--- a/gtsam/linear/SubgraphSolver-inl.h
+++ b/gtsam/linear/SubgraphSolver-inl.h
@@ -20,8 +20,8 @@ using namespace std;
 
 namespace gtsam {
 
-template<class GRAPH, class LINEAR, class VALUES>
-void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
+template<class GRAPH, class LINEAR, class KEY>
+void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
 
 	GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
 	GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
@@ -46,8 +46,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
 			Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
 }
 
-template<class GRAPH, class LINEAR, class VALUES>
-VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
+template<class GRAPH, class LINEAR, class KEY>
+VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
 
 	// preconditioned conjugate gradient
 	VectorValues zeros = pc_->zero();
@@ -59,18 +59,18 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
 	return xbar;
 }
 
-template<class GRAPH, class LINEAR, class VALUES>
-void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
+template<class GRAPH, class LINEAR, class KEY>
+void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const DynamicValues& theta0) {
 	// generate spanning tree
-	PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
+	PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
 
 	// make the ordering
-	list<Key> keys = predecessorMap2Keys(tree_);
+	list<KEY> keys = predecessorMap2Keys(tree_);
 	ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
 
 	// build factor pairs
 	pairs_.clear();
-	typedef pair<Key,Key> EG ;
+	typedef pair<KEY,KEY> EG ;
 	BOOST_FOREACH( const EG &eg, tree_ ) {
 		Symbol key1 = Symbol(eg.first),
 				key2 = Symbol(eg.second) ;
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 539f102e6..6bb592005 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -16,6 +16,7 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/nonlinear/DynamicValues.h>
 
 namespace gtsam {
 
@@ -31,11 +32,11 @@ bool split(const std::map<Index, Index> &M,
  *   linearize: G * T -> L
  *   solve : L -> VectorValues
  */
-template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class KEY>
 class SubgraphSolver : public IterativeSolver {
 
 private:
-	typedef typename VALUES::Key Key;
+//	typedef typename VALUES::Key Key;
 	typedef typename GRAPH::Pose Pose;
 	typedef typename GRAPH::Constraint Constraint;
 
@@ -43,7 +44,7 @@ private:
 	typedef boost::shared_ptr<Ordering> shared_ordering ;
 	typedef boost::shared_ptr<GRAPH> shared_graph ;
 	typedef boost::shared_ptr<LINEAR> shared_linear ;
-	typedef boost::shared_ptr<VALUES> shared_values ;
+	typedef boost::shared_ptr<DynamicValues> shared_values ;
 	typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
 	typedef std::map<Index,Index> mapPairIndex ;
 
@@ -61,7 +62,7 @@ private:
 
 public:
 
-	SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
+	SubgraphSolver(const GRAPH& G, const DynamicValues& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
 		IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
 
 	SubgraphSolver(const LINEAR& GFG) {
@@ -89,7 +90,7 @@ public:
 	shared_ordering ordering() const { return ordering_; }
 
 protected:
-	void initialize(const GRAPH& G, const VALUES& theta0);
+	void initialize(const GRAPH& G, const DynamicValues& theta0);
 
 private:
 	SubgraphSolver():IterativeSolver(){}
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 823853f6d..7d3314d52 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -27,10 +27,10 @@
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_(
+	template<class KEY>
+	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
 			const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
-			const VALUES& linearizationPoints, const KEY& lastKey,
+			const DynamicValues& linearizationPoints, const KEY& lastKey,
 			JacobianFactor::shared_ptr& newPrior) const {
 
 		// Extract the Index of the provided last key
@@ -66,8 +66,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
-	ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial,
+	template<class KEY>
+	ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
 			noiseModel::Gaussian::shared_ptr P_initial) {
 
 		// Set the initial linearization point to the provided mean
@@ -82,8 +82,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict(
+	template<class KEY>
+	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
 			const MotionFactor& motionFactor) {
 
 		// TODO: This implementation largely ignores the actual factor symbols.
@@ -100,7 +100,7 @@ namespace gtsam {
 		ordering.insert(x1, 1);
 
 		// Create a set of linearization points
-		VALUES linearizationPoints;
+		DynamicValues linearizationPoints;
 		linearizationPoints.insert(x0, x_);
 		linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
 
@@ -122,8 +122,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update(
+	template<class KEY>
+	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
 			const MeasurementFactor& measurementFactor) {
 
 		// TODO: This implementation largely ignores the actual factor symbols.
@@ -138,7 +138,7 @@ namespace gtsam {
 		ordering.insert(x0, 0);
 
 		// Create a set of linearization points
-		VALUES linearizationPoints;
+		DynamicValues linearizationPoints;
 		linearizationPoints.insert(x0, x_);
 
 		// Create a Gaussian Factor Graph
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h
index 9717307bf..3bf8c1887 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter.h
@@ -39,21 +39,21 @@ namespace gtsam {
 	 * TODO: a "predictAndUpdate" that combines both steps for some computational savings.
 	 */
 
-	template<class VALUES, class KEY>
+	template<class KEY>
 	class ExtendedKalmanFilter {
 	public:
 
-		typedef boost::shared_ptr<ExtendedKalmanFilter<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<ExtendedKalmanFilter<KEY> > shared_ptr;
 		typedef typename KEY::Value T;
-		typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor;
-		typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor;
+		typedef NonlinearFactor2<KEY, KEY> MotionFactor;
+		typedef NonlinearFactor1<KEY> MeasurementFactor;
 
 	protected:
 		T x_; // linearization point
 		JacobianFactor::shared_ptr priorFactor_; // density
 
 		T solve_(const GaussianFactorGraph& linearFactorGraph,
-				const Ordering& ordering, const VALUES& linearizationPoints,
+				const Ordering& ordering, const DynamicValues& linearizationPoints,
 				const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
 
 	public:
diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h
index 16ec47247..b98a0a958 100644
--- a/gtsam/nonlinear/NonlinearISAM-inl.h
+++ b/gtsam/nonlinear/NonlinearISAM-inl.h
@@ -29,9 +29,9 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
-		const Values& initialValues) {
+template<class GRAPH>
+void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
+		const DynamicValues& initialValues) {
 
   if(newFactors.size() > 0) {
 
@@ -62,14 +62,14 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
+template<class GRAPH>
+void NonlinearISAM<GRAPH>::reorder_relinearize() {
 
 //  cout << "Reordering, relinearizing..." << endl;
 
   if(factors_.size() > 0) {
     // Obtain the new linearization point
-    const Values newLinPoint = estimate();
+    const DynamicValues newLinPoint = estimate();
 
     isam_.clear();
 
@@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
-VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
+template<class GRAPH>
+DynamicValues NonlinearISAM<GRAPH>::estimate() const {
   if(isam_.size() > 0)
     return linPoint_.retract(optimize(isam_), ordering_);
   else
@@ -98,14 +98,14 @@ VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
-Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
+template<class GRAPH>
+Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
 	return isam_.marginalCovariance(ordering_[key]);
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
+template<class GRAPH>
+void NonlinearISAM<GRAPH>::print(const std::string& s) const {
 	cout << "ISAM - " << s << ":" << endl;
 	cout << "  ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
 	isam_.print("GaussianISAM");
diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h
index 8c6d2499d..827bb30e1 100644
--- a/gtsam/nonlinear/NonlinearISAM.h
+++ b/gtsam/nonlinear/NonlinearISAM.h
@@ -24,11 +24,10 @@ namespace gtsam {
 /**
  * Wrapper class to manage ISAM in a nonlinear context
  */
-template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
+template<class GRAPH = gtsam::NonlinearFactorGraph >
 class NonlinearISAM {
 public:
 
-	typedef VALUES Values;
 	typedef GRAPH Factors;
 
 protected:
@@ -37,7 +36,7 @@ protected:
 	gtsam::GaussianISAM isam_;
 
 	/** The current linearization point */
-	Values linPoint_;
+	DynamicValues linPoint_;
 
 	/** The ordering */
 	gtsam::Ordering ordering_;
@@ -61,10 +60,10 @@ public:
 	NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
 
 	/** Add new factors along with their initial linearization points */
-	void update(const Factors& newFactors, const Values& initialValues);
+	void update(const Factors& newFactors, const DynamicValues& initialValues);
 
 	/** Return the current solution estimate */
-	Values estimate() const;
+	DynamicValues estimate() const;
 
 	/** Relinearization and reordering of variables */
 	void reorder_relinearize();
@@ -84,7 +83,7 @@ public:
 	const GaussianISAM& bayesTree() const { return isam_; }
 
 	/** Return the current linearization point */
-	const Values& getLinearizationPoint() const { return linPoint_; }
+	const DynamicValues& getLinearizationPoint() const { return linPoint_; }
 
 	/** Get the ordering */
 	const gtsam::Ordering& getOrdering() const { return ordering_; }
diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h
index 97da0c367..e1896557b 100644
--- a/gtsam/nonlinear/NonlinearOptimization-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimization-inl.h
@@ -39,7 +39,7 @@ namespace gtsam {
 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
 
 		// Create an nonlinear Optimizer that uses a Sequential Solver
-	  typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
+	  typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
 	  		boost::make_shared<const DynamicValues>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
@@ -62,7 +62,7 @@ namespace gtsam {
 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
 
 		// Create an nonlinear Optimizer that uses a Multifrontal Solver
-	  typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
+	  typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
 	  		boost::make_shared<const DynamicValues>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
@@ -85,7 +85,7 @@ namespace gtsam {
 		// initial optimization state is the same in both cases tested
 		typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver;
 		typedef boost::shared_ptr<Solver> shared_Solver;
-		typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, Solver> SPCGOptimizer;
+		typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
 		shared_Solver solver = boost::make_shared<Solver>(
 				graph, initialEstimate, IterativeOptimizationParameters());
 		SPCGOptimizer optimizer(
@@ -111,10 +111,10 @@ namespace gtsam {
 			const NonlinearOptimizationMethod& nonlinear_method) {
 		switch (solver) {
 		case SEQUENTIAL:
-			return optimizeSequential<G,DynamicValues>(graph, initialEstimate, parameters,
+			return optimizeSequential<G>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 		case MULTIFRONTAL:
-			return optimizeMultiFrontal<G,DynamicValues>(graph, initialEstimate, parameters,
+			return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 		case SPCG:
 //			return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 88110c40e..cc688c597 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -74,10 +74,10 @@ namespace gtsam {
 		};
 
 		/// The sequential optimizer
-		typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
+		typedef NonlinearOptimizer<Graph, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
 
 		/// The multifrontal optimizer
-		typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
+		typedef NonlinearOptimizer<Graph, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 
 	} // pose2SLAM
 
diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp
index c4d23ad73..a21096051 100644
--- a/tests/timeMultifrontalOnDataset.cpp
+++ b/tests/timeMultifrontalOnDataset.cpp
@@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
   else if (argc == 4)
   	nrTrials = strtoul(argv[3], NULL, 10);
 
-  pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
+  pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
 
   // Add a prior on the first pose
   if (soft_prior)
diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp
index 063ecd651..8845b901e 100644
--- a/tests/timeSequentialOnDataset.cpp
+++ b/tests/timeSequentialOnDataset.cpp
@@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
   else if (argc == 4)
   	nrTrials = strtoul(argv[3], NULL, 10);
 
-  pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
+  pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
 
   // Add a prior on the first pose
   if (soft_prior)

From 98f2d47f5802c5929aa97717c4b7e3559ed2f3df Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Mon, 30 Jan 2012 04:34:46 +0000
Subject: [PATCH 20/88] most tests passed, except testPose2SLAMwSPCG,
 testGaussianISAM2, testNonlinearEquality, testNonlinearFactorGraph,
 testProjectionFactor, testVSLAM

---
 gtsam/inference/graph-inl.h                   |  30 ++---
 gtsam/inference/graph.h                       |   7 +-
 gtsam/nonlinear/Makefile.am                   |   4 +-
 gtsam/slam/BetweenFactor.h                    |  10 +-
 gtsam/slam/GeneralSFMFactor.h                 |  12 +-
 gtsam/slam/Makefile.am                        |   2 +-
 gtsam/slam/PartialPriorFactor.h               |   2 +-
 gtsam/slam/simulated2DConstraints.h           |  38 +++---
 gtsam/slam/tests/testGeneralSFMFactor.cpp     |  54 ++++----
 .../testGeneralSFMFactor_Cal3Bundler.cpp      |  54 ++++----
 gtsam/slam/tests/testPlanarSLAM.cpp           |  31 ++---
 gtsam/slam/tests/testPose2SLAM.cpp            | 119 +++++++++---------
 gtsam/slam/tests/testPose3SLAM.cpp            |  69 +++++-----
 gtsam/slam/tests/testProjectionFactor.cpp     |  15 +--
 gtsam/slam/tests/testSimulated3D.cpp          |   2 +-
 gtsam/slam/tests/testStereoFactor.cpp         |   9 +-
 gtsam/slam/tests/testVSLAM.cpp                |  61 ++++-----
 tests/Makefile.am                             |   2 +-
 tests/testBoundingConstraint.cpp              |   8 +-
 tests/testExtendedKalmanFilter.cpp            |  42 +++----
 tests/testGaussianISAM2.cpp                   | 110 ++++++++--------
 tests/testGaussianJunctionTree.cpp            |   6 +-
 tests/testGraph.cpp                           |  24 ++--
 tests/testNonlinearEquality.cpp               |  56 ++++-----
 tests/testNonlinearFactor.cpp                 |  53 ++++----
 tests/testNonlinearISAM.cpp                   |   6 +-
 tests/testNonlinearOptimizer.cpp              |  24 ++--
 tests/testPose2SLAMwSPCG.cpp                  |   6 +-
 tests/testRot3Optimization.cpp                |  20 ++-
 29 files changed, 434 insertions(+), 442 deletions(-)

diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h
index 5692d9a92..5fc784a3e 100644
--- a/gtsam/inference/graph-inl.h
+++ b/gtsam/inference/graph-inl.h
@@ -139,19 +139,19 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
 }
 
 /* ************************************************************************* */
-template <class V,class POSE, class VALUES>
+template <class V, class POSE, class KEY>
 class compose_key_visitor : public boost::default_bfs_visitor {
 
 private:
-	boost::shared_ptr<VALUES> config_;
+	boost::shared_ptr<DynamicValues> config_;
 
 public:
 
-	compose_key_visitor(boost::shared_ptr<VALUES> config_in) {config_ = config_in;}
+	compose_key_visitor(boost::shared_ptr<DynamicValues> config_in) {config_ = config_in;}
 
 	template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
-		typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
-		typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
+		KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
+		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, (*config_)[key_from].compose(relativePose));
 	}
@@ -159,23 +159,23 @@ public:
 };
 
 /* ************************************************************************* */
-template<class G, class Factor, class POSE, class VALUES>
-boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
+template<class G, class Factor, class POSE, class KEY>
+boost::shared_ptr<DynamicValues> composePoses(const G& graph, const PredecessorMap<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, typename VALUES::Key>,
+		boost::property<boost::vertex_name_t, 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<typename VALUES::Key, PoseVertex> key2vertex;
+	map<KEY, PoseVertex> key2vertex;
 	boost::tie(g, root, key2vertex) =
-			predecessorMap2Graph<PoseGraph, PoseVertex, typename VALUES::Key>(tree);
+			predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
 
 	// attach the relative poses to the edges
 	PoseEdge edge12, edge21;
@@ -189,8 +189,8 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
 		boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
 		if (!factor) continue;
 
-		typename VALUES::Key key1 = factor->key1();
-		typename VALUES::Key key2 = factor->key2();
+		KEY key1 = factor->key1();
+		KEY key2 = factor->key2();
 
 		PoseVertex v1 = key2vertex.find(key1)->second;
 		PoseVertex v2 = key2vertex.find(key2)->second;
@@ -207,10 +207,10 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
 	}
 
 	// compose poses
-	boost::shared_ptr<VALUES> config(new VALUES);
-	typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
+	boost::shared_ptr<DynamicValues> config(new DynamicValues);
+	KEY rootKey = boost::get(boost::vertex_name, g, root);
 	config->insert(rootKey, rootPose);
-	compose_key_visitor<PoseVertex, POSE, VALUES> vis(config);
+	compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
 	boost::breadth_first_search(g, root, boost::visitor(vis));
 
 	return config;
diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h
index 7a34159ca..6e51a9f6c 100644
--- a/gtsam/inference/graph.h
+++ b/gtsam/inference/graph.h
@@ -25,6 +25,7 @@
 #include <boost/graph/graph_traits.hpp>
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/shared_ptr.hpp>
+#include <gtsam/nonlinear/DynamicValues.h>
 
 namespace gtsam {
 
@@ -87,9 +88,9 @@ namespace gtsam {
 	/**
 	 * Compose the poses by following the chain specified by the spanning tree
 	 */
-	template<class G, class Factor, class POSE, class VALUES>
-	boost::shared_ptr<VALUES>
-		composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
+	template<class G, class Factor, class POSE, class KEY>
+	boost::shared_ptr<DynamicValues>
+		composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
 
 
 	/**
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index f641ab9e7..42eb2e795 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -16,9 +16,9 @@ check_PROGRAMS =
 #----------------------------------------------------------------------------------------------------
 
 # Lie Groups
-headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
+headers += DynamicValues-inl.h
 sources += DynamicValues.cpp
-check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
+check_PROGRAMS += tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
 
 # Nonlinear nonlinear
 headers += Key.h 
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 09d54d524..707dc3350 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -114,15 +114,15 @@ namespace gtsam {
 	 * This constraint requires the underlying type to a Lie type
 	 *
 	 */
-	template<class VALUES, class KEY>
-	class BetweenConstraint : public BetweenFactor<VALUES, KEY> {
+	template<class KEY>
+	class BetweenConstraint : public BetweenFactor<KEY> {
 	public:
-		typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr;
 
 		/** Syntactic sugar for constrained version */
 		BetweenConstraint(const typename KEY::Value& measured,
 				const KEY& key1, const KEY& key2, double mu = 1000.0)
-			: BetweenFactor<VALUES, KEY>(key1, key2, measured,
+			: BetweenFactor<KEY>(key1, key2, measured,
 					noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
 
 	private:
@@ -132,7 +132,7 @@ namespace gtsam {
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & boost::serialization::make_nvp("BetweenFactor",
-					boost::serialization::base_object<BetweenFactor<VALUES, KEY> >(*this));
+					boost::serialization::base_object<BetweenFactor<KEY> >(*this));
 		}
 	}; // \class BetweenConstraint
 
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 66ec820c4..043999b15 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -28,21 +28,21 @@ namespace gtsam {
 	/**
 	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
 	 */
-	template <class VALUES, class CamK, class LmK>
+	template <class CamK, class LmK>
 	class GeneralSFMFactor:
-	public NonlinearFactor2<VALUES, CamK, LmK> {
+	public NonlinearFactor2<CamK, LmK> {
 	protected:
 		Point2 z_;			///< the 2D measurement
 
 	public:
 
 		typedef typename CamK::Value Cam;										///< typedef for camera type
-		typedef GeneralSFMFactor<VALUES, CamK, LmK> Self ;	///< typedef for this object
-		typedef NonlinearFactor2<VALUES, CamK, LmK> Base;		///< typedef for the base class
+		typedef GeneralSFMFactor<CamK, LmK> Self ;	///< typedef for this object
+		typedef NonlinearFactor2<CamK, LmK> Base;		///< typedef for the base class
 		typedef Point2 Measurement;													///< typedef for the measurement
 
 		// shorthand for a smart pointer to a factor
-		typedef boost::shared_ptr<GeneralSFMFactor<VALUES, LmK, CamK> > shared_ptr;
+		typedef boost::shared_ptr<GeneralSFMFactor<LmK, CamK> > shared_ptr;
 
 		/**
 		 * Constructor
@@ -71,7 +71,7 @@ namespace gtsam {
 		/**
 		 * equals
 		 */
-		bool equals(const GeneralSFMFactor<VALUES, CamK, LmK> &p, double tol = 1e-9) const	{
+		bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const	{
 			return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
 		}
 
diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am
index fa51a9512..43e5c970e 100644
--- a/gtsam/slam/Makefile.am
+++ b/gtsam/slam/Makefile.am
@@ -45,7 +45,7 @@ check_PROGRAMS += tests/testPose3SLAM
 # Visual SLAM
 headers += GeneralSFMFactor.h ProjectionFactor.h
 sources += visualSLAM.cpp
-check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM 
+#check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM 
 check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler
 
 # StereoFactor
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index 4d66620dd..651e55500 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -95,7 +95,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e = dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) &&
 					gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h
index b09a5c638..ca7a5da6c 100644
--- a/gtsam/slam/simulated2DConstraints.h
+++ b/gtsam/slam/simulated2DConstraints.h
@@ -35,13 +35,13 @@ namespace gtsam {
 		namespace equality_constraints {
 
 			/** Typedefs for regular use */
-			typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
-			typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
-			typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
+			typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint;
+			typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint;
+			typedef BetweenConstraint<PoseKey> OdoEqualityConstraint;
 
 			/** Equality between variables */
-			typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
-			typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
+			typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint;
+			typedef NonlinearEquality2<PointKey> PointEqualityConstraint;
 
 		} // \namespace equality_constraints
 
@@ -53,10 +53,10 @@ namespace gtsam {
 			 * @tparam KEY is the key type for the variable constrained
 			 * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
 			 */
-			template<class VALUES, class KEY, unsigned int IDX>
-			struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
-				typedef BoundingConstraint1<VALUES, KEY> Base;  ///< Base class convenience typedef
-				typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
+			template<class KEY, unsigned int IDX>
+			struct ScalarCoordConstraint1: public BoundingConstraint1<KEY> {
+				typedef BoundingConstraint1<KEY> Base;  ///< Base class convenience typedef
+				typedef boost::shared_ptr<ScalarCoordConstraint1<KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
 				typedef typename KEY::Value Point; ///< Constrained variable type
 
 				virtual ~ScalarCoordConstraint1() {}
@@ -96,8 +96,8 @@ namespace gtsam {
 			};
 
 			/** typedefs for use with simulated2D systems */
-			typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
-			typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
+			typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
+			typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
 
 			/**
 			 * Trait for distance constraints to provide distance
@@ -116,9 +116,9 @@ namespace gtsam {
 			 * @tparam VALUES is the variable set for the graph
 			 * @tparam KEY is the type of the keys for the variables constrained
 			 */
-			template<class VALUES, class KEY>
-			struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
-				typedef BoundingConstraint2<VALUES, KEY, KEY> Base;  ///< Base class for factor
+			template<class KEY>
+			struct MaxDistanceConstraint : public BoundingConstraint2<KEY, KEY> {
+				typedef BoundingConstraint2<KEY, KEY> Base;  ///< Base class for factor
 				typedef typename KEY::Value Point; ///< Type of variable constrained
 
 				virtual ~MaxDistanceConstraint() {}
@@ -150,7 +150,7 @@ namespace gtsam {
 				}
 			};
 
-			typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
+			typedef MaxDistanceConstraint<PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
 
 			/**
 			 * Binary inequality constraint forcing a minimum range
@@ -159,9 +159,9 @@ namespace gtsam {
 			 * @tparam XKEY is the type of the pose key constrained
 			 * @tparam PKEY is the type of the point key constrained
 			 */
-			template<class VALUES, class XKEY, class PKEY>
-			struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
-				typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
+			template<class XKEY, class PKEY>
+			struct MinDistanceConstraint : public BoundingConstraint2<XKEY, PKEY> {
+				typedef BoundingConstraint2<XKEY, PKEY> Base; ///< Base class for factor
 				typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
 				typedef typename PKEY::Value Point; ///< Type of point variable constrained
 
@@ -195,7 +195,7 @@ namespace gtsam {
 				}
 			};
 
-			typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
+			typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
 
 
 		} // \namespace inequality_constraints
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 8f9c9d09e..a62122705 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -20,7 +20,6 @@ using namespace boost;
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/inference/graph.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 
 #include <gtsam/geometry/GeneralCameraT.h>
@@ -33,14 +32,11 @@ typedef Cal3_S2Camera GeneralCamera;
 //typedef Cal3BundlerCamera GeneralCamera;
 typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
 typedef TypedSymbol<Point3, 'l'> PointKey;
-typedef Values<CameraKey> CameraConfig;
-typedef Values<PointKey> PointConfig;
-typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
-typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
-typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
-typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
+typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
+typedef NonlinearEquality<CameraKey> CameraConstraint;
+typedef NonlinearEquality<PointKey> Point3Constraint;
 
-class Graph: public NonlinearFactorGraph<VisualValues> {
+class Graph: public NonlinearFactorGraph {
 public:
 	void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
 		push_back(boost::make_shared<Projection>(z, model, i, j));
@@ -73,7 +69,7 @@ double getGaussian()
     return sqrt(-2.0f * (double)log(S) / S) * V1;
 }
 
-typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
 
@@ -101,12 +97,12 @@ TEST( GeneralSFMFactor, error ) {
 	boost::shared_ptr<Projection>
 	factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
 	// For the following configuration, the factor predicts 320,240
-	VisualValues values;
+	DynamicValues values;
 	Rot3 R;
 	Point3 t1(0,0,-6);
 	Pose3 x1(R,t1);
-	values.insert(1, GeneralCamera(x1));
-	Point3 l1;  values.insert(1, l1);
+	values.insert(CameraKey(1), GeneralCamera(x1));
+	Point3 l1;  values.insert(PointKey(1), l1);
 	EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -174,15 +170,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 
 	// add initial
 	const double noise = baseline*0.1;
-	boost::shared_ptr<VisualValues> values(new VisualValues);
+	boost::shared_ptr<DynamicValues> values(new DynamicValues);
 	for ( size_t i = 0 ; i < X.size() ; ++i )
-	  values->insert((int)i, X[i]) ;
+	  values->insert(CameraKey((int)i), X[i]) ;
 
 	for ( size_t i = 0 ; i < L.size() ; ++i ) {
 		Point3 pt(L[i].x()+noise*getGaussian(),
 		          L[i].y()+noise*getGaussian(),
 		          L[i].z()+noise*getGaussian());
-		values->insert(i, pt) ;
+		values->insert(PointKey(i), pt) ;
 	}
 
 	graph->addCameraConstraint(0, X[0]);
@@ -213,9 +209,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
@@ -223,10 +219,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
       Point3 pt(L[i].x()+noise*getGaussian(),
                 L[i].y()+noise*getGaussian(),
                 L[i].z()+noise*getGaussian());
-      values->insert(i, pt) ;
+      values->insert(PointKey(i), pt) ;
     }
     else {
-      values->insert(i, L[i]) ;
+      values->insert(PointKey(i), L[i]) ;
     }
   }
 
@@ -259,16 +255,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
     //Point3 pt(L[i].x(), L[i].y(), L[i].z());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   for ( size_t i = 0 ; i < X.size() ; ++i )
@@ -302,7 +298,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i ) {
     const double
       rot_noise = 1e-5,
@@ -310,7 +306,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
       focal_noise = 1,
       skew_noise = 1e-5;
     if ( i == 0 ) {
-      values->insert((int)i, X[i]) ;
+      values->insert(CameraKey((int)i), X[i]) ;
     }
     else {
 
@@ -321,12 +317,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
           skew_noise, // s
           trans_noise, trans_noise // ux, uy
           ) ;
-      values->insert((int)i, X[i].retract(delta)) ;
+      values->insert(CameraKey((int)i), X[i].retract(delta)) ;
     }
   }
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
-    values->insert(i, L[i]) ;
+    values->insert(PointKey(i), L[i]) ;
   }
 
   // fix X0 and all landmarks, allow only the X[1] to move
@@ -363,16 +359,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 3f4e6df6f..89b5e5f34 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -20,7 +20,6 @@ using namespace boost;
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/inference/graph.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 
 #include <gtsam/geometry/GeneralCameraT.h>
@@ -32,15 +31,12 @@ using namespace gtsam;
 typedef Cal3BundlerCamera GeneralCamera;
 typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
 typedef TypedSymbol<Point3, 'l'> PointKey;
-typedef Values<CameraKey> CameraConfig;
-typedef Values<PointKey> PointConfig;
-typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
-typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
-typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
-typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
+typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
+typedef NonlinearEquality<CameraKey> CameraConstraint;
+typedef NonlinearEquality<PointKey> Point3Constraint;
 
 /* ************************************************************************* */
-class Graph: public NonlinearFactorGraph<VisualValues> {
+class Graph: public NonlinearFactorGraph {
 public:
   void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
     push_back(boost::make_shared<Projection>(z, model, i, j));
@@ -73,7 +69,7 @@ double getGaussian()
     return sqrt(-2.0f * (double)log(S) / S) * V1;
 }
 
-typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
 
@@ -101,12 +97,12 @@ TEST( GeneralSFMFactor, error ) {
   boost::shared_ptr<Projection>
   factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
   // For the following configuration, the factor predicts 320,240
-  VisualValues values;
+  DynamicValues values;
   Rot3 R;
   Point3 t1(0,0,-6);
   Pose3 x1(R,t1);
-  values.insert(1, GeneralCamera(x1));
-  Point3 l1;  values.insert(1, l1);
+  values.insert(CameraKey(1), GeneralCamera(x1));
+  Point3 l1;  values.insert(PointKey(1), l1);
   EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -174,15 +170,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
@@ -213,9 +209,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
@@ -223,10 +219,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
       Point3 pt(L[i].x()+noise*getGaussian(),
                 L[i].y()+noise*getGaussian(),
                 L[i].z()+noise*getGaussian());
-      values->insert(i, pt) ;
+      values->insert(PointKey(i), pt) ;
     }
     else {
-      values->insert(i, L[i]) ;
+      values->insert(PointKey(i), L[i]) ;
     }
   }
 
@@ -259,16 +255,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
     //Point3 pt(L[i].x(), L[i].y(), L[i].z());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   for ( size_t i = 0 ; i < X.size() ; ++i )
@@ -302,13 +298,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i ) {
     const double
       rot_noise = 1e-5, trans_noise = 1e-3,
       focal_noise = 1, distort_noise = 1e-3;
     if ( i == 0 ) {
-      values->insert((int)i, X[i]) ;
+      values->insert(CameraKey((int)i), X[i]) ;
     }
     else {
 
@@ -317,12 +313,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
           trans_noise, trans_noise, trans_noise, // translation
           focal_noise, distort_noise, distort_noise // f, k1, k2
           ) ;
-      values->insert((int)i, X[i].retract(delta)) ;
+      values->insert(CameraKey((int)i), X[i].retract(delta)) ;
     }
   }
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
-    values->insert(i, L[i]) ;
+    values->insert(PointKey(i), L[i]) ;
   }
 
   // fix X0 and all landmarks, allow only the X[1] to move
@@ -359,16 +355,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<DynamicValues> values(new DynamicValues);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp
index 2362e0ae5..3241a1e99 100644
--- a/gtsam/slam/tests/testPlanarSLAM.cpp
+++ b/gtsam/slam/tests/testPlanarSLAM.cpp
@@ -23,6 +23,7 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace planarSLAM;
 
 // some shared test values
 static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
@@ -50,9 +51,9 @@ TEST( planarSLAM, BearingFactor )
 	planarSLAM::Bearing factor(2, 3, z, sigma);
 
 	// create config
-	planarSLAM::Values c;
-	c.insert(2, x2);
-	c.insert(3, l3);
+	DynamicValues c;
+	c.insert(PoseKey(2), x2);
+	c.insert(PointKey(3), l3);
 
 	// Check error
 	Vector actual = factor.unwhitenedError(c);
@@ -78,9 +79,9 @@ TEST( planarSLAM, RangeFactor )
 	planarSLAM::Range factor(2, 3, z, sigma);
 
 	// create config
-	planarSLAM::Values c;
-	c.insert(2, x2);
-	c.insert(3, l3);
+	DynamicValues c;
+	c.insert(PoseKey(2), x2);
+	c.insert(PointKey(3), l3);
 
 	// Check error
 	Vector actual = factor.unwhitenedError(c);
@@ -105,9 +106,9 @@ TEST( planarSLAM, BearingRangeFactor )
 	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
 
 	// create config
-	planarSLAM::Values c;
-	c.insert(2, x2);
-	c.insert(3, l3);
+	DynamicValues c;
+	c.insert(PoseKey(2), x2);
+	c.insert(PointKey(3), l3);
 
 	// Check error
 	Vector actual = factor.unwhitenedError(c);
@@ -138,10 +139,10 @@ TEST( planarSLAM, PoseConstraint_equals )
 TEST( planarSLAM, constructor )
 {
 	// create config
-	planarSLAM::Values c;
-	c.insert(2, x2);
-	c.insert(3, x3);
-	c.insert(3, l3);
+	DynamicValues c;
+	c.insert(PoseKey(2), x2);
+	c.insert(PoseKey(3), x3);
+	c.insert(PointKey(3), l3);
 
 	// create graph
 	planarSLAM::Graph G;
@@ -165,8 +166,8 @@ TEST( planarSLAM, constructor )
 	Vector expected2 = Vector_(1, -0.1);
 	Vector expected3 = Vector_(1, 0.22);
 	// Get NoiseModelFactors
-	FactorGraph<NoiseModelFactor<planarSLAM::Values> > GNM =
-	    *G.dynamicCastFactors<FactorGraph<NoiseModelFactor<planarSLAM::Values> > >();
+	FactorGraph<NoiseModelFactor > GNM =
+	    *G.dynamicCastFactors<FactorGraph<NoiseModelFactor > >();
 	EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
   EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
   EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index c2c11b937..c17c20348 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -32,6 +32,7 @@ using namespace boost::assign;
 
 #include <iostream>
 using namespace std;
+using namespace pose2SLAM;
 
 // common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
@@ -115,9 +116,9 @@ TEST( Pose2SLAM, linearization )
 	// 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)
-	Pose2Values config;
-	config.insert(1,p1);
-	config.insert(2,p2);
+	DynamicValues config;
+	config.insert(Key(1),p1);
+	config.insert(Key(2),p2);
 	// Linearize
 	Ordering ordering(*config.orderingArbitrary());
 	boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
@@ -151,23 +152,23 @@ TEST(Pose2Graph, optimize) {
   fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
-  boost::shared_ptr<Pose2Values> initial(new Pose2Values());
-  initial->insert(0, Pose2(0,0,0));
-  initial->insert(1, Pose2(0,0,0));
+  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  initial->insert(Key(0), Pose2(0,0,0));
+  initial->insert(Key(1), Pose2(0,0,0));
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
   *ordering += "x0","x1";
-  typedef NonlinearOptimizer<Pose2Graph, Pose2Values> Optimizer;
+  typedef NonlinearOptimizer<Pose2Graph> Optimizer;
 
   NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
   Optimizer optimizer0(fg, initial, ordering, params);
   Optimizer optimizer = optimizer0.levenbergMarquardt();
 
   // Check with expected config
-  Pose2Values expected;
-  expected.insert(0, Pose2(0,0,0));
-  expected.insert(1, Pose2(1,2,M_PI_2));
+  DynamicValues expected;
+  expected.insert(Key(0), Pose2(0,0,0));
+  expected.insert(Key(1), Pose2(1,2,M_PI_2));
   CHECK(assert_equal(expected, *optimizer.values()));
 }
 
@@ -176,8 +177,8 @@ TEST(Pose2Graph, optimize) {
 TEST(Pose2Graph, optimizeThreePoses) {
 
 	// Create a hexagon of poses
-	Pose2Values hexagon = pose2SLAM::circle(3,1.0);
-  Pose2 p0 = hexagon[0], p1 = hexagon[1];
+	DynamicValues hexagon = pose2SLAM::circle(3,1.0);
+  Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose2Graph> fg(new Pose2Graph);
@@ -188,10 +189,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
   fg->addConstraint(2, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<Pose2Values> initial(new Pose2Values());
-  initial->insert(0, p0);
-  initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  initial->insert(Key(0), p0);
+  initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
@@ -202,7 +203,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  Pose2Values actual = *optimizer.values();
+  DynamicValues actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual));
@@ -213,8 +214,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
 TEST_UNSAFE(Pose2Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
-	Pose2Values hexagon = pose2SLAM::circle(6,1.0);
-  Pose2 p0 = hexagon[0], p1 = hexagon[1];
+	DynamicValues hexagon = pose2SLAM::circle(6,1.0);
+  Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose2Graph> fg(new Pose2Graph);
@@ -228,13 +229,13 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
   fg->addConstraint(5, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<Pose2Values> initial(new Pose2Values());
-  initial->insert(0, p0);
-  initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  initial->insert(Key(0), p0);
+  initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(3,-0.1, 0.1,-0.1)));
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
@@ -245,13 +246,13 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  Pose2Values actual = *optimizer.values();
+  DynamicValues actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual));
 
   // Check loop closure
-  CHECK(assert_equal(delta,actual[5].between(actual[0])));
+  CHECK(assert_equal(delta,actual[Key(5)].between(actual[Key(0)])));
 
 //  Pose2SLAMOptimizer myOptimizer("3");
 
@@ -279,7 +280,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
 //
 //  myOptimizer.update(x);
 //
-//  Pose2Values expected;
+//  DynamicValues expected;
 //  expected.insert(0, Pose2(0.,0.,0.));
 //  expected.insert(1, Pose2(1.,0.,0.));
 //  expected.insert(2, Pose2(2.,0.,0.));
@@ -340,38 +341,38 @@ TEST(Pose2Graph, optimize2) {
 using namespace pose2SLAM;
 
 /* ************************************************************************* */
-TEST( Pose2Values, pose2Circle )
+TEST( DynamicValues, pose2Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	Pose2Values expected;
-	expected.insert(0, Pose2( 1,  0,   M_PI_2));
-	expected.insert(1, Pose2( 0,  1, - M_PI  ));
-	expected.insert(2, Pose2(-1,  0, - M_PI_2));
-	expected.insert(3, Pose2( 0, -1,   0     ));
+	DynamicValues expected;
+	expected.insert(Key(0), Pose2( 1,  0,   M_PI_2));
+	expected.insert(Key(1), Pose2( 0,  1, - M_PI  ));
+	expected.insert(Key(2), Pose2(-1,  0, - M_PI_2));
+	expected.insert(Key(3), Pose2( 0, -1,   0     ));
 
-	Pose2Values actual = pose2SLAM::circle(4,1.0);
+	DynamicValues actual = pose2SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( Pose2Values, expmap )
+TEST( DynamicValues, expmap )
 {
 	// expected is circle shifted to right
-	Pose2Values expected;
-	expected.insert(0, Pose2( 1.1,  0,   M_PI_2));
-	expected.insert(1, Pose2( 0.1,  1, - M_PI  ));
-	expected.insert(2, Pose2(-0.9,  0, - M_PI_2));
-	expected.insert(3, Pose2( 0.1, -1,   0     ));
+	DynamicValues expected;
+	expected.insert(Key(0), Pose2( 1.1,  0,   M_PI_2));
+	expected.insert(Key(1), Pose2( 0.1,  1, - M_PI  ));
+	expected.insert(Key(2), Pose2(-0.9,  0, - M_PI_2));
+	expected.insert(Key(3), Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
-  Pose2Values circle(pose2SLAM::circle(4,1.0));
+  DynamicValues circle(pose2SLAM::circle(4,1.0));
   Ordering ordering(*circle.orderingArbitrary());
 	VectorValues delta(circle.dims(ordering));
 	delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0);
 	delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0);
 	delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0);
 	delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0);
-	Pose2Values actual = circle.retract(delta, ordering);
+	DynamicValues actual = circle.retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -384,8 +385,8 @@ TEST( Pose2Prior, error )
 {
 	// Choose a linearization point
 	Pose2 p1(1, 0, 0); // robot at (1,0)
-	Pose2Values x0;
-	x0.insert(1, p1);
+	DynamicValues x0;
+	x0.insert(Key(1), p1);
 
 	// Create factor
 	Pose2Prior factor(1, p1, sigmas);
@@ -406,7 +407,7 @@ TEST( Pose2Prior, error )
 	VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
 	addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
 	VectorValues plus = delta + addition;
-	Pose2Values x1 = x0.retract(plus, ordering);
+	DynamicValues x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -428,8 +429,8 @@ LieVector hprior(const Pose2& p1) {
 TEST( Pose2Prior, linearize )
 {
 	// Choose a linearization point at ground truth
-	Pose2Values x0;
-	x0.insert(1,priorVal);
+	DynamicValues x0;
+	x0.insert(Key(1),priorVal);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -448,9 +449,9 @@ TEST( Pose2Factor, error )
 	// Choose a linearization point
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
-	Pose2Values x0;
-	x0.insert(1, p1);
-	x0.insert(2, p2);
+	DynamicValues x0;
+	x0.insert(Key(1), p1);
+	x0.insert(Key(2), p2);
 
 	// Create factor
 	Pose2 z = p1.between(p2);
@@ -472,7 +473,7 @@ TEST( Pose2Factor, error )
 	// Check error after increasing p2
 	VectorValues plus = delta;
 	plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
-	Pose2Values x1 = x0.retract(plus, ordering);
+	DynamicValues x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -489,9 +490,9 @@ TEST( Pose2Factor, rhs )
 	// 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.1) looking at negative (ground truth is at -1,4)
-	Pose2Values x0;
-	x0.insert(1,p1);
-	x0.insert(2,p2);
+	DynamicValues x0;
+	x0.insert(Key(1),p1);
+	x0.insert(Key(2),p2);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -519,9 +520,9 @@ TEST( Pose2Factor, linearize )
 	// Choose a linearization point at ground truth
 	Pose2 p1(1,2,M_PI_2);
 	Pose2 p2(-1,4,M_PI);
-	Pose2Values x0;
-	x0.insert(1,p1);
-	x0.insert(2,p2);
+	DynamicValues x0;
+	x0.insert(Key(1),p1);
+	x0.insert(Key(2),p2);
 
 	// expected linearization
 	Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 949afea0f..574d05b6d 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -36,6 +36,7 @@ using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
+using namespace pose3SLAM;
 
 // common measurement covariance
 static Matrix covariance = eye(6);
@@ -48,8 +49,8 @@ TEST(Pose3Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
 	double radius = 10;
-	Pose3Values hexagon = pose3SLAM::circle(6,radius);
-  Pose3 gT0 = hexagon[0], gT1 = hexagon[1];
+	DynamicValues hexagon = pose3SLAM::circle(6,radius);
+  Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose3Graph> fg(new Pose3Graph);
@@ -65,13 +66,13 @@ TEST(Pose3Graph, optimizeCircle) {
   fg->addConstraint(5,0, _0T1, covariance);
 
   // Create initial config
-  boost::shared_ptr<Pose3Values> initial(new Pose3Values());
-  initial->insert(0, gT0);
-  initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  initial->insert(Key(0), gT0);
+  initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(Key(5), hexagon[Key(5)].retract(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);
@@ -80,18 +81,18 @@ TEST(Pose3Graph, optimizeCircle) {
   pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  Pose3Values actual = *optimizer.values();
+  DynamicValues actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual,1e-4));
 
   // Check loop closure
-  CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
+  CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5));
 }
 
 /* ************************************************************************* */
 TEST(Pose3Graph, partial_prior_height) {
-	typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
+	typedef PartialPriorFactor<pose3SLAM::Key> Partial;
 
 	// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
 
@@ -109,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) {
 	pose3SLAM::Graph graph;
 	graph.add(height);
 
-	pose3SLAM::Values values;
+	DynamicValues values;
 	values.insert(key, init);
 
 	// linearization
 	EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
 
-	pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
+	DynamicValues actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
 	EXPECT(assert_equal(expected, actual[key], tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
@@ -133,9 +134,9 @@ TEST( Pose3Factor, error )
 	Pose3Factor factor(1,2, z, I6);
 
 	// Create config
-	Pose3Values x;
-	x.insert(1,t1);
-	x.insert(2,t2);
+	DynamicValues x;
+	x.insert(Key(1),t1);
+	x.insert(Key(2),t2);
 
 	// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
 	Vector actual = factor.unwhitenedError(x);
@@ -145,7 +146,7 @@ TEST( Pose3Factor, error )
 
 /* ************************************************************************* */
 TEST(Pose3Graph, partial_prior_xy) {
-	typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
+	typedef PartialPriorFactor<pose3SLAM::Key> Partial;
 
 	// XY prior - full mask interface
 	pose3SLAM::Key key(1);
@@ -164,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) {
 	pose3SLAM::Graph graph;
 	graph.add(priorXY);
 
-	pose3SLAM::Values values;
+	DynamicValues values;
 	values.insert(key, init);
 
-	pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
+	DynamicValues actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
 	EXPECT(assert_equal(expected, actual[key], tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
@@ -180,27 +181,27 @@ Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
 Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
 
 /* ************************************************************************* */
-TEST( Pose3Values, pose3Circle )
+TEST( DynamicValues, pose3Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	Pose3Values expected;
-	expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
-	expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
-	expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
-	expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
+	DynamicValues expected;
+	expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0)));
+	expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0)));
+	expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0)));
+	expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0)));
 
-	Pose3Values actual = pose3SLAM::circle(4,1.0);
+	DynamicValues actual = pose3SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( Pose3Values, expmap )
+TEST( DynamicValues, expmap )
 {
-	Pose3Values expected;
-	expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
-	expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
-	expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
-	expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
+	DynamicValues expected;
+	expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
+	expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
+	expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
+	expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
 
 	Ordering ordering(*expected.orderingArbitrary());
 	VectorValues delta(expected.dims(ordering));
@@ -209,7 +210,7 @@ TEST( Pose3Values, 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);
-	Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
+	DynamicValues actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index 09fd612d7..f5f41fd2e 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -25,6 +25,7 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace visualSLAM;
 
 // make cube
 static Point3
@@ -51,9 +52,9 @@ TEST( ProjectionFactor, error )
 	factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
 
 	// For the following values structure, the factor predicts 320,240
-	visualSLAM::Values config;
-	Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
-	Point3 l1;  config.insert(1, l1);
+	DynamicValues config;
+	Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1);
+	Point3 l1;  config.insert(PointKey(1), l1);
 	// Point should project to Point2(320.,240.)
 	CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
 
@@ -80,13 +81,13 @@ TEST( ProjectionFactor, error )
 	CHECK(assert_equal(expected_lfg,*actual_lfg));
 
 	// expmap on a config
-	visualSLAM::Values expected_config;
-  Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
-  Point3 l2(1,2,3); expected_config.insert(1, l2);
+	DynamicValues expected_config;
+  Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
+  Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
 	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
 	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
-	visualSLAM::Values actual_config = config.retract(delta, ordering);
+	DynamicValues actual_config = config.retract(delta, ordering);
 	CHECK(assert_equal(expected_config,actual_config,1e-9));
 }
 
diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp
index 30f41e2fd..5a90f2489 100644
--- a/gtsam/slam/tests/testSimulated3D.cpp
+++ b/gtsam/slam/tests/testSimulated3D.cpp
@@ -28,7 +28,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( simulated3D, Values )
 {
-	simulated3D::Values actual;
+	DynamicValues actual;
 	actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
 	actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
 	EXPECT(assert_equal(actual,actual,1e-9));
diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp
index 50a84d8b6..1fd23b57f 100644
--- a/gtsam/slam/tests/testStereoFactor.cpp
+++ b/gtsam/slam/tests/testStereoFactor.cpp
@@ -30,6 +30,7 @@
 using namespace std;
 using namespace gtsam;
 using namespace boost;
+using namespace visualSLAM;
 
 Pose3 camera1(Matrix_(3,3,
 		       1., 0., 0.,
@@ -59,15 +60,15 @@ TEST( StereoFactor, singlePoint)
 	graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
 
 	// Create a configuration corresponding to the ground truth
-	boost::shared_ptr<visualSLAM::Values> values(new visualSLAM::Values());
-	values->insert(1, camera1); // add camera at z=6.25m looking towards origin
+	boost::shared_ptr<DynamicValues> values(new DynamicValues());
+	values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
 
 	Point3 l1(0, 0, 0);
-	values->insert(1, l1);   // add point at origin;
+	values->insert(PointKey(1), l1);   // add point at origin;
 
 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
 
-	typedef gtsam::NonlinearOptimizer<visualSLAM::Graph,visualSLAM::Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer;   // optimization engine for this domain
+	typedef gtsam::NonlinearOptimizer<visualSLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianMultifrontalSolver> Optimizer;   // optimization engine for this domain
 
 	double absoluteThreshold = 1e-9;
 	double relativeThreshold = 1e-5;
diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp
index c25b4260d..9281e51dc 100644
--- a/gtsam/slam/tests/testVSLAM.cpp
+++ b/gtsam/slam/tests/testVSLAM.cpp
@@ -33,6 +33,7 @@ using namespace boost;
 using namespace std;
 using namespace gtsam;
 using namespace boost;
+using namespace visualSLAM;
 
 static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 
@@ -91,13 +92,13 @@ TEST( Graph, optimizeLM)
   graph->addPointConstraint(3, landmark3);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
-  initialEstimate->insert(1, camera1);
-  initialEstimate->insert(2, camera2);
-  initialEstimate->insert(1, landmark1);
-  initialEstimate->insert(2, landmark2);
-  initialEstimate->insert(3, landmark3);
-  initialEstimate->insert(4, landmark4);
+  boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
+  initialEstimate->insert(PoseKey(1), camera1);
+  initialEstimate->insert(PoseKey(2), camera2);
+  initialEstimate->insert(PointKey(1), landmark1);
+  initialEstimate->insert(PointKey(2), landmark2);
+  initialEstimate->insert(PointKey(3), landmark3);
+  initialEstimate->insert(PointKey(4), landmark4);
 
   // Create an ordering of the variables
   shared_ptr<Ordering> ordering(new Ordering);
@@ -128,13 +129,13 @@ TEST( Graph, optimizeLM2)
   graph->addPoseConstraint(2, camera2);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
-  initialEstimate->insert(1, camera1);
-  initialEstimate->insert(2, camera2);
-  initialEstimate->insert(1, landmark1);
-  initialEstimate->insert(2, landmark2);
-  initialEstimate->insert(3, landmark3);
-  initialEstimate->insert(4, landmark4);
+  boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
+  initialEstimate->insert(PoseKey(1), camera1);
+  initialEstimate->insert(PoseKey(2), camera2);
+  initialEstimate->insert(PointKey(1), landmark1);
+  initialEstimate->insert(PointKey(2), landmark2);
+  initialEstimate->insert(PointKey(3), landmark3);
+  initialEstimate->insert(PointKey(4), landmark4);
 
   // Create an ordering of the variables
   shared_ptr<Ordering> ordering(new Ordering);
@@ -165,13 +166,13 @@ TEST( Graph, CHECK_ORDERING)
   graph->addPoseConstraint(2, camera2);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
-  initialEstimate->insert(1, camera1);
-  initialEstimate->insert(2, camera2);
-  initialEstimate->insert(1, landmark1);
-  initialEstimate->insert(2, landmark2);
-  initialEstimate->insert(3, landmark3);
-  initialEstimate->insert(4, landmark4);
+  boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
+  initialEstimate->insert(PoseKey(1), camera1);
+  initialEstimate->insert(PoseKey(2), camera2);
+  initialEstimate->insert(PointKey(1), landmark1);
+  initialEstimate->insert(PointKey(2), landmark2);
+  initialEstimate->insert(PointKey(3), landmark3);
+  initialEstimate->insert(PointKey(4), landmark4);
 
   Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
 
@@ -193,23 +194,23 @@ TEST( Graph, CHECK_ORDERING)
 TEST( Values, update_with_large_delta) {
 	// this test ensures that if the update for delta is larger than
 	// the size of the config, it only updates existing variables
-	visualSLAM::Values init;
-	init.insert(1, Pose3());
-	init.insert(1, Point3(1.0, 2.0, 3.0));
+	DynamicValues init;
+	init.insert(PoseKey(1), Pose3());
+	init.insert(PointKey(1), Point3(1.0, 2.0, 3.0));
 
-	visualSLAM::Values expected;
-	expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
-	expected.insert(1, Point3(1.1, 2.1, 3.1));
+	DynamicValues expected;
+	expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
+	expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1));
 
 	Ordering largeOrdering;
-	visualSLAM::Values largeValues = init;
-	largeValues.insert(2, Pose3());
+	DynamicValues largeValues = init;
+	largeValues.insert(PoseKey(2), Pose3());
 	largeOrdering += "x1","l1","x2";
 	VectorValues delta(largeValues.dims(largeOrdering));
 	delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
 	delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
 	delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
-	visualSLAM::Values actual = init.retract(delta, largeOrdering);
+	DynamicValues actual = init.retract(delta, largeOrdering);
 
 	CHECK(assert_equal(expected,actual));
 }
diff --git a/tests/Makefile.am b/tests/Makefile.am
index 2ed6139bb..433693bc3 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -16,7 +16,7 @@ check_PROGRAMS += testGaussianJunctionTree
 check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
 check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer
 check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
-check_PROGRAMS += testTupleValues
+#check_PROGRAMS += testTupleValues
 check_PROGRAMS += testNonlinearISAM
 check_PROGRAMS += testBoundingConstraint
 check_PROGRAMS += testPose2SLAMwSPCG
diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp
index 062a04ed6..d6bda8472 100644
--- a/tests/testBoundingConstraint.cpp
+++ b/tests/testBoundingConstraint.cpp
@@ -31,10 +31,10 @@ SharedDiagonal soft_model2 = noiseModel::Unit::Create(2);
 SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
 SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
 
-typedef NonlinearFactorGraph<simulated2D::Values> Graph;
+typedef NonlinearFactorGraph Graph;
 typedef boost::shared_ptr<Graph> shared_graph;
 typedef boost::shared_ptr<simulated2D::Values> shared_values;
-typedef NonlinearOptimizer<Graph, simulated2D::Values> Optimizer;
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 // some simple inequality constraints
 simulated2D::PoseKey key(1);
@@ -160,7 +160,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
 	initValues->insert(x1, start_pt);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
-	simulated2D::Values expected;
+	DynamicValues expected;
 	expected.insert(x1, goal_pt);
 	CHECK(assert_equal(expected, *actual, tol));
 }
@@ -182,7 +182,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 	initValues->insert(x1, start_pt);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
-	simulated2D::Values expected;
+	DynamicValues expected;
 	expected.insert(x1, goal_pt);
 	CHECK(assert_equal(expected, *actual, tol));
 }
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 3a946858a..712c986ae 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -20,14 +20,12 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/geometry/Point2.h>
 
 using namespace gtsam;
 
 // Define Types for System Test
 typedef TypedSymbol<Point2, 'x'> TestKey;
-typedef Values<TestKey> TestValues;
 
 /* ************************************************************************* */
 TEST( ExtendedKalmanFilter, linear ) {
@@ -37,7 +35,7 @@ TEST( ExtendedKalmanFilter, linear ) {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<TestValues,TestKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
 
   // Create the TestKeys for our example
   TestKey x0(0), x1(1), x2(2), x3(3);
@@ -59,30 +57,30 @@ TEST( ExtendedKalmanFilter, linear ) {
 
   // Run iteration 1
   // Create motion factor
-  BetweenFactor<TestValues,TestKey> factor1(x0, x1, difference, Q);
+  BetweenFactor<TestKey> factor1(x0, x1, difference, Q);
   Point2 predict1 = ekf.predict(factor1);
   EXPECT(assert_equal(expected1,predict1));
 
   // Create the measurement factor
-  PriorFactor<TestValues,TestKey> factor2(x1, z1, R);
+  PriorFactor<TestKey> factor2(x1, z1, R);
   Point2 update1 = ekf.update(factor2);
   EXPECT(assert_equal(expected1,update1));
 
   // Run iteration 2
-  BetweenFactor<TestValues,TestKey> factor3(x1, x2, difference, Q);
+  BetweenFactor<TestKey> factor3(x1, x2, difference, Q);
   Point2 predict2 = ekf.predict(factor3);
   EXPECT(assert_equal(expected2,predict2));
 
-  PriorFactor<TestValues,TestKey> factor4(x2, z2, R);
+  PriorFactor<TestKey> factor4(x2, z2, R);
   Point2 update2 = ekf.update(factor4);
   EXPECT(assert_equal(expected2,update2));
 
   // Run iteration 3
-  BetweenFactor<TestValues,TestKey> factor5(x2, x3, difference, Q);
+  BetweenFactor<TestKey> factor5(x2, x3, difference, Q);
   Point2 predict3 = ekf.predict(factor5);
   EXPECT(assert_equal(expected3,predict3));
 
-  PriorFactor<TestValues,TestKey> factor6(x3, z3, R);
+  PriorFactor<TestKey> factor6(x3, z3, R);
   Point2 update3 = ekf.update(factor6);
   EXPECT(assert_equal(expected3,update3));
 
@@ -91,12 +89,12 @@ TEST( ExtendedKalmanFilter, linear ) {
 
 
 // Create Motion Model Factor
-class NonlinearMotionModel : public NonlinearFactor2<TestValues,TestKey,TestKey> {
+class NonlinearMotionModel : public NonlinearFactor2<TestKey,TestKey> {
 public:
   typedef TestKey::Value T;
 
 private:
-  typedef NonlinearFactor2<TestValues,TestKey,TestKey> Base;
+  typedef NonlinearFactor2<TestKey,TestKey> Base;
   typedef NonlinearMotionModel This;
 
 protected:
@@ -163,7 +161,7 @@ public:
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
-  virtual bool equals(const NonlinearFactor2<TestValues,TestKey,TestKey>& f, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor2<TestKey,TestKey>& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
     return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_);
   }
@@ -172,7 +170,7 @@ public:
    * calculate the error of the factor
    * Override for systems with unusual noise models
    */
-  virtual double error(const TestValues& c) const {
+  virtual double error(const DynamicValues& c) const {
     Vector w = whitenedError(c);
     return 0.5 * w.dot(w);
   }
@@ -183,7 +181,7 @@ public:
   }
 
   /** Vector of errors, whitened ! */
-  Vector whitenedError(const TestValues& c) const {
+  Vector whitenedError(const DynamicValues& c) const {
     return QInvSqrt(c[key1_])*unwhitenedError(c);
   }
 
@@ -192,7 +190,7 @@ public:
    * 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 TestValues& c, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
     const X1& x1 = c[key1_];
     const X2& x2 = c[key2_];
     Matrix A1, A2;
@@ -238,13 +236,13 @@ public:
 };
 
 // Create Measurement Model Factor
-class NonlinearMeasurementModel : public NonlinearFactor1<TestValues,TestKey> {
+class NonlinearMeasurementModel : public NonlinearFactor1<TestKey> {
 public:
   typedef TestKey::Value T;
 
 private:
 
-  typedef NonlinearFactor1<TestValues,TestKey> Base;
+  typedef NonlinearFactor1<TestKey> Base;
   typedef NonlinearMeasurementModel This;
 
 protected:
@@ -300,7 +298,7 @@ public:
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
-  virtual bool equals(const NonlinearFactor1<TestValues,TestKey>& f, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor1<TestKey>& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
     return (e != NULL) && (key_ == e->key_);
   }
@@ -309,7 +307,7 @@ public:
    * calculate the error of the factor
    * Override for systems with unusual noise models
    */
-  virtual double error(const TestValues& c) const {
+  virtual double error(const DynamicValues& c) const {
     Vector w = whitenedError(c);
     return 0.5 * w.dot(w);
   }
@@ -320,7 +318,7 @@ public:
   }
 
   /** Vector of errors, whitened ! */
-  Vector whitenedError(const TestValues& c) const {
+  Vector whitenedError(const DynamicValues& c) const {
     return RInvSqrt(c[key_])*unwhitenedError(c);
   }
 
@@ -329,7 +327,7 @@ public:
    * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z
    * Hence b = z - h(x1) = - error_vector(x)
    */
-  boost::shared_ptr<GaussianFactor> linearize(const TestValues& c, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
     const X& x1 = c[key_];
     Matrix A1;
     Vector b = -evaluateError(x1, A1);
@@ -395,7 +393,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<TestValues,TestKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
 
   // Enter Predict-Update Loop
   Point2 x_predict, x_update;
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index d28844cd9..7c39b8c60 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -33,10 +33,10 @@ const double tol = 1e-4;
 TEST(ISAM2, AddVariables) {
 
   // Create initial state
-  planarSLAM::Values theta;
+  DynamicValues theta;
   theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
   theta.insert(planarSLAM::PointKey(0), Point2(.4, .5));
-  planarSLAM::Values newTheta;
+  DynamicValues newTheta;
   newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
 
   VectorValues deltaUnpermuted;
@@ -51,7 +51,7 @@ TEST(ISAM2, AddVariables) {
 
   Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
 
-  GaussianISAM2<planarSLAM::Values>::Nodes nodes(2);
+  GaussianISAM2<>::Nodes nodes(2);
 
   // Verify initial state
   LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]);
@@ -60,7 +60,7 @@ TEST(ISAM2, AddVariables) {
   EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]]));
 
   // Create expected state
-  planarSLAM::Values thetaExpected;
+  DynamicValues thetaExpected;
   thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
   thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5));
   thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
@@ -79,11 +79,11 @@ TEST(ISAM2, AddVariables) {
 
   Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
 
-  GaussianISAM2<planarSLAM::Values>::Nodes nodesExpected(
-          3, GaussianISAM2<planarSLAM::Values>::sharedClique());
+  GaussianISAM2<>::Nodes nodesExpected(
+          3, GaussianISAM2<>::sharedClique());
 
   // Expand initial state
-  GaussianISAM2<planarSLAM::Values>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes);
+  GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes);
 
   EXPECT(assert_equal(thetaExpected, theta));
   EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
@@ -148,7 +148,7 @@ TEST(ISAM2, AddVariables) {
 TEST(ISAM2, optimize2) {
 
   // Create initialization
-  planarSLAM::Values theta;
+  DynamicValues theta;
   theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
   // Create conditional
@@ -168,8 +168,8 @@ TEST(ISAM2, optimize2) {
   conditional->solveInPlace(expected);
 
   // Clique
-  GaussianISAM2<planarSLAM::Values>::sharedClique clique(
-      GaussianISAM2<planarSLAM::Values>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
+  GaussianISAM2<>::sharedClique clique(
+      GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
   VectorValues actual(theta.dims(ordering));
   conditional->rhs(actual);
   optimize2(clique, actual);
@@ -180,15 +180,15 @@ TEST(ISAM2, optimize2) {
 }
 
 /* ************************************************************************* */
-bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fullinit, const GaussianISAM2<planarSLAM::Values>& isam) {
-  planarSLAM::Values actual = isam.calculateEstimate();
+bool isam_check(const planarSLAM::Graph& fullgraph, const DynamicValues& fullinit, const GaussianISAM2<>& isam) {
+  DynamicValues actual = isam.calculateEstimate();
   Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
   GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
 //  linearized.print("Expected linearized: ");
   GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
 //  gbn.print("Expected bayesnet: ");
   VectorValues delta = optimize(gbn);
-  planarSLAM::Values expected = fullinit.retract(delta, ordering);
+  DynamicValues expected = fullinit.retract(delta, ordering);
 
   return assert_equal(expected, actual);
 }
@@ -210,8 +210,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
 
   // These variables will be reused and accumulate factors and values
-  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
-  planarSLAM::Values fullinit;
+  GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
+  DynamicValues fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -223,7 +223,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -238,7 +238,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -253,7 +253,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -271,7 +271,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -286,7 +286,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -298,7 +298,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
   CHECK(isam_check(fullgraph, fullinit, isam));
 
   // Check gradient at each node
-  typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
+  typedef GaussianISAM2<>::sharedClique sharedClique;
   BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
     // Compute expected gradient
     FactorGraph<JacobianFactor> jfg;
@@ -343,8 +343,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
 
   // These variables will be reused and accumulate factors and values
-  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
-  planarSLAM::Values fullinit;
+  GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
+  DynamicValues fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -356,7 +356,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -371,7 +371,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -386,7 +386,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -404,7 +404,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -419,7 +419,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -431,7 +431,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
   CHECK(isam_check(fullgraph, fullinit, isam));
 
   // Check gradient at each node
-  typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
+  typedef GaussianISAM2<>::sharedClique sharedClique;
   BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
     // Compute expected gradient
     FactorGraph<JacobianFactor> jfg;
@@ -471,8 +471,8 @@ TEST(ISAM2, clone) {
   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
 
   // These variables will be reused and accumulate factors and values
-  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
-  planarSLAM::Values fullinit;
+  GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
+  DynamicValues fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -484,7 +484,7 @@ TEST(ISAM2, clone) {
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -499,7 +499,7 @@ TEST(ISAM2, clone) {
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -514,7 +514,7 @@ TEST(ISAM2, clone) {
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -532,7 +532,7 @@ TEST(ISAM2, clone) {
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -547,7 +547,7 @@ TEST(ISAM2, clone) {
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -556,8 +556,8 @@ TEST(ISAM2, clone) {
   }
 
   // CLONING...
-  boost::shared_ptr<GaussianISAM2<planarSLAM::Values> > isam2
-      = boost::shared_ptr<GaussianISAM2<planarSLAM::Values> >(new GaussianISAM2<planarSLAM::Values>());
+  boost::shared_ptr<GaussianISAM2<> > isam2
+      = boost::shared_ptr<GaussianISAM2<> >(new GaussianISAM2<>());
   isam.cloneTo(isam2);
 
   CHECK(assert_equal(isam, *isam2));
@@ -644,8 +644,8 @@ TEST(ISAM2, removeFactors)
   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
 
   // These variables will be reused and accumulate factors and values
-  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
-  planarSLAM::Values fullinit;
+  GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
+  DynamicValues fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -657,7 +657,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -672,7 +672,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -687,7 +687,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -705,7 +705,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -721,7 +721,7 @@ TEST(ISAM2, removeFactors)
     fullgraph.push_back(newfactors[0]);
     fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -731,14 +731,14 @@ TEST(ISAM2, removeFactors)
     // Remove the measurement on landmark 0
     FastVector<size_t> toRemove;
     toRemove.push_back(result.newFactorsIndices[1]);
-    isam.update(planarSLAM::Graph(), planarSLAM::Values(), toRemove);
+    isam.update(planarSLAM::Graph(), DynamicValues(), toRemove);
   }
 
   // Compare solutions
   CHECK(isam_check(fullgraph, fullinit, isam));
 
   // Check gradient at each node
-  typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
+  typedef GaussianISAM2<>::sharedClique sharedClique;
   BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
     // Compute expected gradient
     FactorGraph<JacobianFactor> jfg;
@@ -783,8 +783,8 @@ TEST(ISAM2, constrained_ordering)
   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
 
   // These variables will be reused and accumulate factors and values
-  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
-  planarSLAM::Values fullinit;
+  GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
+  DynamicValues fullinit;
   planarSLAM::Graph fullgraph;
 
   // We will constrain x3 and x4 to the end
@@ -799,7 +799,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -814,7 +814,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -832,7 +832,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -850,7 +850,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -865,7 +865,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    planarSLAM::Values init;
+    DynamicValues init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -881,7 +881,7 @@ TEST(ISAM2, constrained_ordering)
       (isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12));
 
   // Check gradient at each node
-  typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
+  typedef GaussianISAM2<>::sharedClique sharedClique;
   BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
     // Compute expected gradient
     FactorGraph<JacobianFactor> jfg;
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index c7418a673..fc9662519 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -198,9 +198,9 @@ TEST(GaussianJunctionTree, simpleMarginal) {
   fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0));
   fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
 
-  pose2SLAM::Values init;
-  init.insert(0, Pose2());
-  init.insert(1, Pose2(1.0, 0.0, 0.0));
+  DynamicValues init;
+  init.insert(pose2SLAM::Key(0), Pose2());
+  init.insert(pose2SLAM::Key(1), Pose2(1.0, 0.0, 0.0));
 
   Ordering ordering;
   ordering += "x1", "x0";
diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp
index e9698a753..f6d8629bf 100644
--- a/tests/testGraph.cpp
+++ b/tests/testGraph.cpp
@@ -86,22 +86,22 @@ TEST( Graph, composePoses )
 	graph.addConstraint(2,3, p23, cov);
 	graph.addConstraint(4,3, p43, cov);
 
-	PredecessorMap<Pose2Values::Key> tree;
-	tree.insert(1,2);
-	tree.insert(2,2);
-	tree.insert(3,2);
-	tree.insert(4,3);
+	PredecessorMap<pose2SLAM::Key> tree;
+	tree.insert(pose2SLAM::Key(1),2);
+	tree.insert(pose2SLAM::Key(2),2);
+	tree.insert(pose2SLAM::Key(3),2);
+	tree.insert(pose2SLAM::Key(4),3);
 
 	Pose2 rootPose = p2;
 
-	boost::shared_ptr<Pose2Values> actual = composePoses<Pose2Graph, Pose2Factor,
-			Pose2, Pose2Values> (graph, tree, rootPose);
+	boost::shared_ptr<DynamicValues> actual = composePoses<Pose2Graph, Pose2Factor,
+			Pose2, pose2SLAM::Key> (graph, tree, rootPose);
 
-	Pose2Values expected;
-	expected.insert(1, p1);
-	expected.insert(2, p2);
-	expected.insert(3, p3);
-	expected.insert(4, p4);
+	DynamicValues expected;
+	expected.insert(pose2SLAM::Key(1), p1);
+	expected.insert(pose2SLAM::Key(2), p2);
+	expected.insert(pose2SLAM::Key(3), p3);
+	expected.insert(pose2SLAM::Key(4), p4);
 
 	LONGS_EQUAL(4, actual->size());
 	CHECK(assert_equal(expected, *actual));
diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp
index 501bb9db8..b917185e9 100644
--- a/tests/testNonlinearEquality.cpp
+++ b/tests/testNonlinearEquality.cpp
@@ -32,20 +32,19 @@ namespace eq2D = gtsam::simulated2D::equality_constraints;
 static const double tol = 1e-5;
 
 typedef TypedSymbol<Pose2, 'x'> PoseKey;
-typedef Values<PoseKey> PoseValues;
-typedef PriorFactor<PoseValues, PoseKey> PosePrior;
-typedef NonlinearEquality<PoseValues, PoseKey> PoseNLE;
+typedef PriorFactor<PoseKey> PosePrior;
+typedef NonlinearEquality<PoseKey> PoseNLE;
 typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
 
-typedef NonlinearFactorGraph<PoseValues> PoseGraph;
-typedef NonlinearOptimizer<PoseGraph,PoseValues> PoseOptimizer;
+typedef NonlinearFactorGraph PoseGraph;
+typedef NonlinearOptimizer<PoseGraph> PoseOptimizer;
 
 PoseKey key(1);
 
 /* ************************************************************************* */
 TEST ( NonlinearEquality, linearization ) {
 	Pose2 value = Pose2(2.1, 1.0, 2.0);
-	PoseValues linearize;
+	DynamicValues linearize;
 	linearize.insert(key, value);
 
 	// create a nonlinear equality constraint
@@ -63,7 +62,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
 
 	PoseKey key(1);
 	Pose2 value;
-	PoseValues config;
+	DynamicValues config;
 	config.insert(key, value);
 
 	// create a nonlinear equality constraint
@@ -77,7 +76,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
 TEST ( NonlinearEquality, linearization_fail ) {
 	Pose2 value = Pose2(2.1, 1.0, 2.0);
 	Pose2 wrong = Pose2(2.1, 3.0, 4.0);
-	PoseValues bad_linearize;
+	DynamicValues bad_linearize;
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
@@ -93,7 +92,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
 	PoseKey key(1);
 	Pose2 value(2.0, 1.0, 2.0),
 		  wrong(2.0, 3.0, 4.0);
-	PoseValues bad_linearize;
+	DynamicValues bad_linearize;
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
@@ -109,7 +108,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
 	PoseKey key(1);
 	Pose2 value,
 		  wrong(2.0, 3.0, 4.0);
-	PoseValues bad_linearize;
+	DynamicValues bad_linearize;
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
@@ -123,7 +122,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
 TEST ( NonlinearEquality, error ) {
 	Pose2 value = Pose2(2.1, 1.0, 2.0);
 	Pose2 wrong = Pose2(2.1, 3.0, 4.0);
-	PoseValues feasible, bad_linearize;
+	DynamicValues feasible, bad_linearize;
 	feasible.insert(key, value);
 	bad_linearize.insert(key, wrong);
 
@@ -168,7 +167,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
 	EXPECT(assert_equal(expVec, actVec, 1e-5));
 
 	// the actual error should have a gain on it
-	PoseValues config;
+	DynamicValues config;
 	config.insert(key1, badPoint1);
 	double actError = nle.error(config);
 	DOUBLES_EQUAL(500.0, actError, 1e-9);
@@ -195,7 +194,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
 
 	// initialize away from the ideal
 	Pose2 initPose(0.0, 2.0, 3.0);
-	boost::shared_ptr<PoseValues> init(new PoseValues());
+	boost::shared_ptr<DynamicValues> init(new DynamicValues());
 	init->insert(key1, initPose);
 
 	// optimize
@@ -206,7 +205,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
 	PoseOptimizer result = optimizer.levenbergMarquardt();
 
 	// verify
-	PoseValues expected;
+	DynamicValues expected;
 	expected.insert(key1, feasible1);
 	EXPECT(assert_equal(expected, *result.values()));
 }
@@ -219,7 +218,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 	Pose2 feasible1(1.0, 2.0, 3.0);
 
 	// initialize away from the ideal
-	boost::shared_ptr<PoseValues> init(new PoseValues());
+	boost::shared_ptr<DynamicValues> init(new DynamicValues());
 	Pose2 initPose(0.0, 2.0, 3.0);
 	init->insert(key1, initPose);
 
@@ -242,7 +241,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 	PoseOptimizer result = optimizer.levenbergMarquardt();
 
 	// verify
-	PoseValues expected;
+	DynamicValues expected;
 	expected.insert(key1, feasible1);
 	EXPECT(assert_equal(expected, *result.values()));
 }
@@ -251,10 +250,10 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 SharedDiagonal hard_model = noiseModel::Constrained::All(2);
 SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
 
-typedef NonlinearFactorGraph<simulated2D::Values> Graph;
+typedef NonlinearFactorGraph Graph;
 typedef boost::shared_ptr<Graph> shared_graph;
-typedef boost::shared_ptr<simulated2D::Values> shared_values;
-typedef NonlinearOptimizer<Graph, simulated2D::Values> Optimizer;
+typedef boost::shared_ptr<DynamicValues> shared_values;
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 /* ************************************************************************* */
 TEST( testNonlinearEqualityConstraint, unary_basics ) {
@@ -326,7 +325,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
 	// verify error values
 	EXPECT(constraint->active(*initValues));
 
-	simulated2D::Values expected;
+	DynamicValues expected;
 	expected.insert(key, truth_pt);
 	EXPECT(constraint->active(expected));
 	EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
@@ -423,7 +422,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
 	initValues->insert(key2, badPt);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
-	simulated2D::Values expected;
+	DynamicValues expected;
 	expected.insert(key1, truth_pt1);
 	expected.insert(key2, truth_pt2);
 	CHECK(assert_equal(expected, *actual, tol));
@@ -463,7 +462,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
 
-	simulated2D::Values expected;
+	DynamicValues expected;
 	expected.insert(x1, pt_x1);
 	expected.insert(l1, Point2(1.0, 6.0));
 	expected.insert(l2, Point2(1.0, 6.0));
@@ -507,7 +506,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
 	// optimize
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
 
-	simulated2D::Values expected;
+	DynamicValues expected;
 	expected.insert(x1, Point2(1.0, 1.0));
 	expected.insert(l1, Point2(1.0, 6.0));
 	expected.insert(l2, Point2(1.0, 6.0));
@@ -522,13 +521,12 @@ Cal3_S2 K(fov,w,h);
 boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
 
 // typedefs for visual SLAM example
-typedef visualSLAM::Values VValues;
-typedef boost::shared_ptr<VValues> shared_vconfig;
+typedef boost::shared_ptr<DynamicValues> shared_vconfig;
 typedef visualSLAM::Graph VGraph;
-typedef NonlinearOptimizer<VGraph,VValues> VOptimizer;
+typedef NonlinearOptimizer<VGraph> VOptimizer;
 
 // factors for visual slam
-typedef NonlinearEquality2<VValues, visualSLAM::PointKey> Point3Equality;
+typedef NonlinearEquality2<visualSLAM::PointKey> Point3Equality;
 
 /* ********************************************************************* */
 TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
@@ -567,7 +565,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 	Point3 landmark1(0.5, 5.0, 0.0);
 	Point3 landmark2(1.5, 5.0, 0.0);
 
-	shared_vconfig initValues(new VValues());
+	shared_vconfig initValues(new DynamicValues());
 	initValues->insert(x1, pose1);
 	initValues->insert(x2, pose2);
 	initValues->insert(l1, landmark1);
@@ -577,7 +575,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 	VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues);
 
 	// create config
-	VValues truthValues;
+	DynamicValues truthValues;
 	truthValues.insert(x1, camera1.pose());
 	truthValues.insert(x2, camera2.pose());
 	truthValues.insert(l1, landmark);
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 0d5402607..c9fa05c45 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -40,7 +40,7 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
-typedef boost::shared_ptr<NonlinearFactor<VectorValues> > shared_nlf;
+typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
 
 /* ************************************************************************* */
 TEST( NonlinearFactor, equals )
@@ -89,7 +89,7 @@ TEST( NonlinearFactor, NonlinearFactor )
 
   // calculate the error_vector from the factor "f1"
   // error_vector = [0.1 0.1]
-  Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor<example::Values> >(factor)->unwhitenedError(cfg);
+  Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor>(factor)->unwhitenedError(cfg);
   CHECK(assert_equal(0.1*ones(2),actual_e));
 
   // error = 0.5 * [1 1] * [1;1] = 1
@@ -236,12 +236,11 @@ TEST( NonlinearFactor, linearize_constraint2 )
 
 /* ************************************************************************* */
 typedef TypedSymbol<LieVector, 'x'> TestKey;
-typedef gtsam::Values<TestKey> TestValues;
 
 /* ************************************************************************* */
-class TestFactor4 : public NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> {
+class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
 public:
-  typedef NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> Base;
+  typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
   TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
 
   virtual Vector
@@ -263,11 +262,11 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor4) {
   TestFactor4 tf;
-  TestValues tv;
-  tv.insert(1, LieVector(1, 1.0));
-  tv.insert(2, LieVector(1, 2.0));
-  tv.insert(3, LieVector(1, 3.0));
-  tv.insert(4, LieVector(1, 4.0));
+  DynamicValues tv;
+  tv.insert(TestKey(1), LieVector(1, 1.0));
+  tv.insert(TestKey(2), LieVector(1, 2.0));
+  tv.insert(TestKey(3), LieVector(1, 3.0));
+  tv.insert(TestKey(4), LieVector(1, 4.0));
   EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
   Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
@@ -284,9 +283,9 @@ TEST(NonlinearFactor, NonlinearFactor4) {
 }
 
 /* ************************************************************************* */
-class TestFactor5 : public NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> {
+class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> {
 public:
-  typedef NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base;
   TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
 
   virtual Vector
@@ -310,12 +309,12 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor5) {
   TestFactor5 tf;
-  TestValues tv;
-  tv.insert(1, LieVector(1, 1.0));
-  tv.insert(2, LieVector(1, 2.0));
-  tv.insert(3, LieVector(1, 3.0));
-  tv.insert(4, LieVector(1, 4.0));
-  tv.insert(5, LieVector(1, 5.0));
+  DynamicValues tv;
+  tv.insert(TestKey(1), LieVector(1, 1.0));
+  tv.insert(TestKey(2), LieVector(1, 2.0));
+  tv.insert(TestKey(3), LieVector(1, 3.0));
+  tv.insert(TestKey(4), LieVector(1, 4.0));
+  tv.insert(TestKey(5), LieVector(1, 5.0));
   EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
   Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
@@ -334,9 +333,9 @@ TEST(NonlinearFactor, NonlinearFactor5) {
 }
 
 /* ************************************************************************* */
-class TestFactor6 : public NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
+class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
 public:
-  typedef NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
   TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
 
   virtual Vector
@@ -362,13 +361,13 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor6) {
   TestFactor6 tf;
-  TestValues tv;
-  tv.insert(1, LieVector(1, 1.0));
-  tv.insert(2, LieVector(1, 2.0));
-  tv.insert(3, LieVector(1, 3.0));
-  tv.insert(4, LieVector(1, 4.0));
-  tv.insert(5, LieVector(1, 5.0));
-  tv.insert(6, LieVector(1, 6.0));
+  DynamicValues tv;
+  tv.insert(TestKey(1), LieVector(1, 1.0));
+  tv.insert(TestKey(2), LieVector(1, 2.0));
+  tv.insert(TestKey(3), LieVector(1, 3.0));
+  tv.insert(TestKey(4), LieVector(1, 4.0));
+  tv.insert(TestKey(5), LieVector(1, 5.0));
+  tv.insert(TestKey(6), LieVector(1, 6.0));
   EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
   Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index b90e98625..66fd410b6 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -12,7 +12,7 @@
 using namespace gtsam;
 using namespace planarSLAM;
 
-typedef NonlinearISAM<planarSLAM::Values> PlanarISAM;
+typedef NonlinearISAM<> PlanarISAM;
 
 const double tol=1e-5;
 
@@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
 	Graph start_factors;
 	start_factors.addPoseConstraint(key, cur_pose);
 
-	planarSLAM::Values init;
-	planarSLAM::Values expected;
+	DynamicValues init;
+	DynamicValues expected;
 	init.insert(key, cur_pose);
 	expected.insert(key, cur_pose);
 	isam.update(start_factors, init);
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index ccc1748dc..57366500e 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -44,7 +44,7 @@ using namespace gtsam;
 
 const double tol = 1e-5;
 
-typedef NonlinearOptimizer<example::Graph,example::Values> Optimizer;
+typedef NonlinearOptimizer<example::Graph> Optimizer;
 
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
@@ -241,11 +241,11 @@ TEST( NonlinearOptimizer, optimization_method )
 	example::Values c0;
 	c0.insert(simulated2D::PoseKey(1), x0);
 
-	example::Values actualMFQR = optimize<example::Graph,example::Values>(
+	DynamicValues actualMFQR = optimize<example::Graph>(
 			fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM);
 	DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
 
-	example::Values actualMFLDL = optimize<example::Graph,example::Values>(
+	DynamicValues actualMFLDL = optimize<example::Graph>(
 			fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM);
 	DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
 }
@@ -253,26 +253,26 @@ TEST( NonlinearOptimizer, optimization_method )
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, Factorization )
 {
-	typedef NonlinearOptimizer<Pose2Graph, Pose2Values, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
+	typedef NonlinearOptimizer<Pose2Graph, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
 
-	boost::shared_ptr<Pose2Values> config(new Pose2Values);
-	config->insert(1, Pose2(0.,0.,0.));
-	config->insert(2, Pose2(1.5,0.,0.));
+	boost::shared_ptr<DynamicValues> config(new DynamicValues);
+	config->insert(pose2SLAM::Key(1), Pose2(0.,0.,0.));
+	config->insert(pose2SLAM::Key(2), Pose2(1.5,0.,0.));
 
 	boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
 	graph->addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
 	graph->addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
 
 	boost::shared_ptr<Ordering> ordering(new Ordering);
-	ordering->push_back(Pose2Values::Key(1));
-	ordering->push_back(Pose2Values::Key(2));
+	ordering->push_back(pose2SLAM::Key(1));
+	ordering->push_back(pose2SLAM::Key(2));
 
 	Optimizer optimizer(graph, config, ordering);
 	Optimizer optimized = optimizer.iterateLM();
 
-	Pose2Values expected;
-	expected.insert(1, Pose2(0.,0.,0.));
-	expected.insert(2, Pose2(1.,0.,0.));
+	DynamicValues expected;
+	expected.insert(pose2SLAM::Key(1), Pose2(0.,0.,0.));
+	expected.insert(pose2SLAM::Key(2), Pose2(1.,0.,0.));
 	CHECK(assert_equal(expected, *optimized.values(), 1e-5));
 }
 
diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp
index 1b3d9f455..64585ff75 100644
--- a/tests/testPose2SLAMwSPCG.cpp
+++ b/tests/testPose2SLAMwSPCG.cpp
@@ -40,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) {
 	graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
 	graph.addPrior(x1, Pose2(0,0,0), sigma) ;
 
-	pose2SLAM::Values initial;
+	DynamicValues initial;
 	initial.insert(x1, Pose2(  0,  0,   0));
 	initial.insert(x2, Pose2(  0, 2.1, 0.01));
 	initial.insert(x3, Pose2(  0, 3.9,-0.01));
@@ -51,7 +51,7 @@ TEST(testPose2SLAMwSPCG, example1) {
 	initial.insert(x8, Pose2(3.9, 2.1, 0.01));
 	initial.insert(x9, Pose2(4.1, 3.9,-0.01));
 
-	pose2SLAM::Values expected;
+	DynamicValues expected;
 	expected.insert(x1, Pose2(0.0, 0.0, 0.0));
 	expected.insert(x2, Pose2(0.0, 2.0, 0.0));
 	expected.insert(x3, Pose2(0.0, 4.0, 0.0));
@@ -62,7 +62,7 @@ TEST(testPose2SLAMwSPCG, example1) {
 	expected.insert(x8, Pose2(4.0, 2.0, 0.0));
 	expected.insert(x9, Pose2(4.0, 4.0, 0.0));
 
-	pose2SLAM::Values actual = optimizeSPCG(graph, initial);
+	DynamicValues actual = optimizeSPCG(graph, initial);
 
 	EXPECT(assert_equal(expected, actual, tol));
 }
diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp
index a35165273..1e57a0797 100644
--- a/tests/testRot3Optimization.cpp
+++ b/tests/testRot3Optimization.cpp
@@ -23,7 +23,6 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
@@ -31,27 +30,26 @@
 using namespace gtsam;
 
 typedef TypedSymbol<Rot3, 'r'> Key;
-typedef Values<Key> Rot3Values;
-typedef PriorFactor<Rot3Values, Key> Prior;
-typedef BetweenFactor<Rot3Values, Key> Between;
-typedef NonlinearFactorGraph<Rot3Values> Graph;
+typedef PriorFactor<Key> Prior;
+typedef BetweenFactor<Key> Between;
+typedef NonlinearFactorGraph Graph;
 
 /* ************************************************************************* */
 TEST(Rot3, optimize) {
 
   // Optimize a circle
-  Rot3Values truth;
-  Rot3Values initial;
+  DynamicValues truth;
+  DynamicValues initial;
   Graph fg;
   fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01)));
   for(int j=0; j<6; ++j) {
-    truth.insert(j, Rot3::Rz(M_PI/3.0 * double(j)));
-    initial.insert(j, Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
-    fg.add(Between(j, (j+1)%6, Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
+    truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j)));
+    initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
+    fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
   }
 
   NonlinearOptimizationParameters params;
-  Rot3Values final = optimize(fg, initial, params);
+  DynamicValues final = optimize(fg, initial, params);
 
   EXPECT(assert_equal(truth, final, 1e-5));
 }

From f86da6f983dfdcc4467601dbefb3df7b521e7198 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Mon, 30 Jan 2012 16:46:36 +0000
Subject: [PATCH 21/88] testProjectionFactor and testVSLAM passed. Remaining
 unpassed tests: GaussianISAM2, NonlinearFactorGraph (colamd failed), SPCG

---
 gtsam/nonlinear/DynamicValues.cpp         | 19 +++++++++++--------
 gtsam/nonlinear/DynamicValues.h           | 13 +++++++++++++
 gtsam/slam/Makefile.am                    |  2 +-
 gtsam/slam/tests/testProjectionFactor.cpp |  2 ++
 tests/Makefile.am                         |  4 ++--
 5 files changed, 29 insertions(+), 11 deletions(-)

diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp
index 5b8d89a83..57d760b59 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/DynamicValues.cpp
@@ -168,14 +168,17 @@ namespace gtsam {
 
   /* ************************************************************************* */
   vector<size_t> DynamicValues::dims(const Ordering& ordering) const {
-    vector<size_t> result(values_.size());
-    // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
-    result.assign(
-        boost::make_transform_iterator(values_.begin(),
-            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
-        boost::make_transform_iterator(values_.end(),
-            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
-    return result;
+//    vector<size_t> result(values_.size());
+//    // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
+//    result.assign(
+//        boost::make_transform_iterator(values_.begin(),
+//            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
+//        boost::make_transform_iterator(values_.end(),
+//            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
+//    return result;
+    _ValuesDimensionCollector dimCollector(ordering);
+    this->apply(dimCollector);
+    return dimCollector.dimensions;
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h
index d11a23ec9..4f866749e 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/DynamicValues.h
@@ -41,6 +41,19 @@ namespace gtsam {
   // Forward declarations
   class ValueCloneAllocator;
 
+  struct _ValuesDimensionCollector {
+    const Ordering& ordering;
+    std::vector<size_t> dimensions;
+    _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
+    template<typename I> void operator()(const I& key_value) {
+      Index var;
+      if(ordering.tryAt(key_value->first, var)) {
+        assert(var < dimensions.size());
+        dimensions[var] = key_value->second->dim();
+      }
+    }
+  };
+
 /**
   * A non-templated config holding any types of Manifold-group elements.  A
   * values structure is a map from keys to values. It is used to specify the
diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am
index 43e5c970e..fa51a9512 100644
--- a/gtsam/slam/Makefile.am
+++ b/gtsam/slam/Makefile.am
@@ -45,7 +45,7 @@ check_PROGRAMS += tests/testPose3SLAM
 # Visual SLAM
 headers += GeneralSFMFactor.h ProjectionFactor.h
 sources += visualSLAM.cpp
-#check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM 
+check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM 
 check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler
 
 # StereoFactor
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index f5f41fd2e..ae2aee55f 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -85,6 +85,8 @@ TEST( ProjectionFactor, error )
   Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
   Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
+	ordering.print("ordering: ");
+	delta.print("delta: ");
 	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
 	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
 	DynamicValues actual_config = config.retract(delta, ordering);
diff --git a/tests/Makefile.am b/tests/Makefile.am
index 433693bc3..567a1ff0a 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -19,8 +19,8 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
 #check_PROGRAMS += testTupleValues
 check_PROGRAMS += testNonlinearISAM
 check_PROGRAMS += testBoundingConstraint
-check_PROGRAMS += testPose2SLAMwSPCG
-check_PROGRAMS += testGaussianISAM2
+#check_PROGRAMS += testPose2SLAMwSPCG
+#check_PROGRAMS += testGaussianISAM2
 check_PROGRAMS += testExtendedKalmanFilter
 check_PROGRAMS += testRot3Optimization
 

From 51dca5b2d0dc944b7d9e0c7630bc07292451abc1 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 30 Jan 2012 21:23:02 +0000
Subject: [PATCH 22/88] (in branch) Fixed Value base class assignment operator,
 ISAM2 expmap function, and unit tests

---
 gtsam/base/DerivedValue.h                   | 17 +++++++++++++++++
 gtsam/base/Value.h                          |  3 +++
 gtsam/linear/VectorValues.h                 |  2 +-
 gtsam/nonlinear/ISAM2-impl-inl.h            |  6 ++++--
 gtsam/nonlinear/tests/testDynamicValues.cpp |  2 +-
 gtsam/slam/tests/testProjectionFactor.cpp   |  2 --
 tests/Makefile.am                           |  2 +-
 tests/testExtendedKalmanFilter.cpp          |  4 ++--
 tests/testNonlinearFactorGraph.cpp          |  6 +++++-
 9 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index c30258919..f59303675 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -73,6 +73,23 @@ public:
     return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
 	}
 
+	/// Assignment operator
+	virtual Value& operator=(const Value& rhs) {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
+
+    // Do the assignment and return the result
+    return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
+	}
+
+protected:
+	/// Assignment operator, protected because only the Value or DERIVED
+	/// assignment operators should be used.
+	DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
+	  // Nothing to do, do not call base class assignment operator
+	  return *this;
+	}
+
 };
 
 } /* namespace gtsam */
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 1e3c809c3..8d0b2dd05 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -136,6 +136,9 @@ namespace gtsam {
      */
     virtual Vector localCoordinates_(const Value& value) const = 0;
 
+    /** Assignment operator */
+    virtual Value& operator=(const Value& rhs) = 0;
+
     /** Virutal destructor */
     virtual ~Value() {}
 
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 33351ceb7..0be2cad19 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -175,7 +175,7 @@ namespace gtsam {
     /// @name Advanced Constructors
     /// @{
 
-    /** Construct from a container of variable dimensions (in variable order). */
+    /** Construct from a container of variable dimensions (in variable order), without initializing any values. */
     template<class CONTAINER>
     VectorValues(const CONTAINER& dimensions) { append(dimensions); }
 
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index 8b0c0b99c..7ddd2f8c6 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -245,10 +245,12 @@ struct _SelectiveExpmapAndClear {
       else
         cout << "       " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
     }
-    assert(delta[var].size() == (int)it_x->second.dim());
+    assert(delta[var].size() == (int)it_x->second->dim());
     assert(delta[var].unaryExpr(&isfinite<double>).all());
     if(mask[var]) {
-      it_x->second = it_x->second.retract(delta[var]);
+      Value* retracted = it_x->second->retract_(delta[var]);
+      *it_x->second = *retracted;
+      retracted->deallocate_();
       if(invalidate)
         (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
     }
diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp
index f7493f1b1..51ffd0320 100644
--- a/gtsam/nonlinear/tests/testDynamicValues.cpp
+++ b/gtsam/nonlinear/tests/testDynamicValues.cpp
@@ -162,7 +162,7 @@ TEST(DynamicValues, expmap_b)
   config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
   Ordering ordering(*config0.orderingArbitrary());
-  VectorValues increment(config0.dims(ordering));
+  VectorValues increment(VectorValues::Zero(config0.dims(ordering)));
   increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
 
   DynamicValues expected;
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index ae2aee55f..f5f41fd2e 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -85,8 +85,6 @@ TEST( ProjectionFactor, error )
   Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
   Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
-	ordering.print("ordering: ");
-	delta.print("delta: ");
 	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
 	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
 	DynamicValues actual_config = config.retract(delta, ordering);
diff --git a/tests/Makefile.am b/tests/Makefile.am
index 567a1ff0a..086904640 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -20,7 +20,7 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
 check_PROGRAMS += testNonlinearISAM
 check_PROGRAMS += testBoundingConstraint
 #check_PROGRAMS += testPose2SLAMwSPCG
-#check_PROGRAMS += testGaussianISAM2
+check_PROGRAMS += testGaussianISAM2
 check_PROGRAMS += testExtendedKalmanFilter
 check_PROGRAMS += testRot3Optimization
 
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 712c986ae..1ff5fe047 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -365,8 +365,8 @@ public:
 TEST( ExtendedKalmanFilter, nonlinear ) {
 
   // Create the set of expected output TestValues (generated using Matlab Kalman Filter)
-  Point2 expected_predict[10];
-  Point2 expected_update[10];
+  Point2 expected_predict[11];
+  Point2 expected_update[11];
   expected_predict[1] = Point2(0.81,0.99);
   expected_update[1] =  Point2(0.824926197027,0.29509808);
   expected_predict[2] = Point2(0.680503230541,0.24343413);
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 6b926c6d7..8606c027f 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -75,8 +75,12 @@ TEST( Graph, keys )
 /* ************************************************************************* */
 TEST( Graph, GET_ORDERING)
 {
-  Ordering expected; expected += "x1","l1","x2";
+//  Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1
+  Ordering expected; expected += "l1","x2","x1"; // For starting with l1,x1,x2
   Graph nlfg = createNonlinearFactorGraph();
+  SymbolicFactorGraph::shared_ptr symbolic;
+  Ordering::shared_ptr ordering;
+  boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues());
   Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues());
   CHECK(assert_equal(expected,actual));
 }

From aeeb35f7db6d973be9708d8228a53ae6e81bb7e4 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Mon, 30 Jan 2012 23:36:40 +0000
Subject: [PATCH 23/88] added missing headers

---
 gtsam/base/Makefile.am      | 2 +-
 gtsam/nonlinear/Makefile.am | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am
index ced6530f8..0864f7781 100644
--- a/gtsam/base/Makefile.am
+++ b/gtsam/base/Makefile.am
@@ -26,7 +26,7 @@ sources += timing.cpp debug.cpp
 check_PROGRAMS += tests/testDebug tests/testTestableAssertions
 
 # Manifolds and Lie Groups
-headers += Value.h Manifold.h Group.h
+headers += DerivedValue.h Value.h Manifold.h Group.h
 headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
 sources += LieVector.cpp
 check_PROGRAMS += tests/testLieVector tests/testLieScalar
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 42eb2e795..1ad683047 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -16,7 +16,7 @@ check_PROGRAMS =
 #----------------------------------------------------------------------------------------------------
 
 # Lie Groups
-headers += DynamicValues-inl.h
+headers += DynamicValues.h DynamicValues-inl.h
 sources += DynamicValues.cpp
 check_PROGRAMS += tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
 

From f3a3f8ebf63805da82c0aa6bd407195dd6ea9b49 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 2 Feb 2012 15:41:18 +0000
Subject: [PATCH 24/88] Comment fix

---
 gtsam/nonlinear/NonlinearFactorGraph.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 96beadf3d..1b1bba892 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -28,8 +28,8 @@
 namespace gtsam {
 
 	/**
-	 * A non-linear factor graph is templated on a values structure, but the factor type
-	 * is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
+	 * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
+	 * which derive from NonlinearFactor. The values structures are typically (in SAM) more general
 	 * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
 	 * Linearizing the non-linear factor graph creates a linear factor graph on the 
 	 * tangent vector space at the linearization point. Because the tangent space is a true

From 26cdf28421c8cd6e97700761ba70356d0880c510 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 2 Feb 2012 16:16:46 +0000
Subject: [PATCH 25/88] Renamed DynamicValues to Values and removed specialized
 derived Values classes

---
 examples/CameraResectioning.cpp               |   4 +-
 examples/PlanarSLAMExample_easy.cpp           |   4 +-
 examples/PlanarSLAMSelfContained_advanced.cpp |   2 +-
 examples/Pose2SLAMExample_advanced.cpp        |   4 +-
 examples/Pose2SLAMExample_easy.cpp            |   4 +-
 examples/Pose2SLAMwSPCG_advanced.cpp          |  10 +-
 examples/Pose2SLAMwSPCG_easy.cpp              |   2 +-
 examples/SimpleRotation.cpp                   |   4 +-
 examples/elaboratePoint2KalmanFilter.cpp      |   2 +-
 examples/vSLAMexample/vISAMexample.cpp        |   8 +-
 examples/vSLAMexample/vSFMexample.cpp         |   6 +-
 gtsam.h                                       |  10 +-
 gtsam/inference/graph-inl.h                   |   8 +-
 gtsam/inference/graph.h                       |   4 +-
 gtsam/linear/SubgraphSolver-inl.h             |   2 +-
 gtsam/linear/SubgraphSolver.h                 |   8 +-
 gtsam/nonlinear/ExtendedKalmanFilter-inl.h    |   6 +-
 gtsam/nonlinear/ExtendedKalmanFilter.h        |   2 +-
 gtsam/nonlinear/ISAM2-impl-inl.h              |   8 +-
 gtsam/nonlinear/ISAM2-inl.h                   |   6 +-
 gtsam/nonlinear/ISAM2.h                       |  12 +-
 gtsam/nonlinear/Makefile.am                   |   6 +-
 gtsam/nonlinear/NonlinearEquality.h           |   4 +-
 gtsam/nonlinear/NonlinearFactor.h             |  28 +-
 gtsam/nonlinear/NonlinearFactorGraph.cpp      |  10 +-
 gtsam/nonlinear/NonlinearFactorGraph.h        |  10 +-
 gtsam/nonlinear/NonlinearISAM-inl.h           |   6 +-
 gtsam/nonlinear/NonlinearISAM.h               |   8 +-
 gtsam/nonlinear/NonlinearOptimization-inl.h   |  18 +-
 gtsam/nonlinear/NonlinearOptimization.h       |   2 +-
 gtsam/nonlinear/NonlinearOptimizer-inl.h      |   6 +-
 gtsam/nonlinear/NonlinearOptimizer.h          |  36 +-
 gtsam/nonlinear/TupleValues-inl.h             | 165 ------
 gtsam/nonlinear/TupleValues.h                 | 524 ------------------
 .../{DynamicValues-inl.h => Values-inl.h}     |  18 +-
 .../{DynamicValues.cpp => Values.cpp}         |  56 +-
 gtsam/nonlinear/{DynamicValues.h => Values.h} |  48 +-
 gtsam/nonlinear/ValuesOld-inl.h               | 231 --------
 gtsam/nonlinear/ValuesOld.h                   | 301 ----------
 gtsam/nonlinear/tests/testDynamicValues.cpp   | 265 ---------
 gtsam/nonlinear/tests/testNonlinearFactor.cpp |   6 +-
 gtsam/nonlinear/tests/testValues.cpp          | 221 ++++----
 gtsam/slam/BoundingConstraint.h               |   4 +-
 gtsam/slam/dataset.cpp                        |  10 +-
 gtsam/slam/dataset.h                          |   6 +-
 gtsam/slam/planarSLAM.h                       |  33 +-
 gtsam/slam/pose2SLAM.cpp                      |   4 +-
 gtsam/slam/pose2SLAM.h                        |   2 +-
 gtsam/slam/pose3SLAM.cpp                      |   4 +-
 gtsam/slam/pose3SLAM.h                        |   2 +-
 gtsam/slam/simulated2D.h                      |  52 --
 gtsam/slam/simulated2DOriented.h              |  24 -
 gtsam/slam/smallExample.h                     |   1 -
 gtsam/slam/tests/testGeneralSFMFactor.cpp     |  12 +-
 .../testGeneralSFMFactor_Cal3Bundler.cpp      |  12 +-
 gtsam/slam/tests/testPlanarSLAM.cpp           |   8 +-
 gtsam/slam/tests/testPose2SLAM.cpp            |  48 +-
 gtsam/slam/tests/testPose3SLAM.cpp            |  28 +-
 gtsam/slam/tests/testProjectionFactor.cpp     |   6 +-
 gtsam/slam/tests/testSimulated2D.cpp          |   7 +-
 gtsam/slam/tests/testSimulated2DOriented.cpp  |   2 +-
 gtsam/slam/tests/testSimulated3D.cpp          |   2 +-
 gtsam/slam/tests/testStereoFactor.cpp         |   2 +-
 gtsam/slam/tests/testVSLAM.cpp                |  14 +-
 gtsam/slam/visualSLAM.h                       |   2 +-
 tests/testBoundingConstraint.cpp              |  30 +-
 tests/testDoglegOptimizer.cpp                 |   4 +-
 tests/testExtendedKalmanFilter.cpp            |  12 +-
 tests/testGaussianISAM2.cpp                   |  80 +--
 tests/testGaussianJunctionTree.cpp            |  10 +-
 tests/testGraph.cpp                           |   4 +-
 tests/testInference.cpp                       |   2 +-
 tests/testNonlinearEquality.cpp               |  62 +--
 tests/testNonlinearFactor.cpp                 |  22 +-
 tests/testNonlinearFactorGraph.cpp            |   8 +-
 tests/testNonlinearISAM.cpp                   |   8 +-
 tests/testNonlinearOptimizer.cpp              |  28 +-
 tests/testPose2SLAMwSPCG.cpp                  |   6 +-
 tests/testRot3Optimization.cpp                |   6 +-
 tests/testSerialization.cpp                   |  10 +-
 tests/timeMultifrontalOnDataset.cpp           |   2 +-
 tests/timeSequentialOnDataset.cpp             |   2 +-
 82 files changed, 526 insertions(+), 2140 deletions(-)
 delete mode 100644 gtsam/nonlinear/TupleValues-inl.h
 delete mode 100644 gtsam/nonlinear/TupleValues.h
 rename gtsam/nonlinear/{DynamicValues-inl.h => Values-inl.h} (81%)
 rename gtsam/nonlinear/{DynamicValues.cpp => Values.cpp} (80%)
 rename gtsam/nonlinear/{DynamicValues.h => Values.h} (88%)
 delete mode 100644 gtsam/nonlinear/ValuesOld-inl.h
 delete mode 100644 gtsam/nonlinear/ValuesOld.h
 delete mode 100644 gtsam/nonlinear/tests/testDynamicValues.cpp

diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 9727ceacb..511e7eb3f 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -92,13 +92,13 @@ int main(int argc, char* argv[]) {
   graph.push_back(factor);
 
   /* 3. Create an initial estimate for the camera pose */
-  DynamicValues initial;
+  Values initial;
   initial.insert(X, Pose3(Rot3(1.,0.,0.,
                                0.,-1.,0.,
                                0.,0.,-1.), Point3(0.,0.,2.0)));
 
   /* 4. Optimize the graph using Levenberg-Marquardt*/
-  DynamicValues result = optimize<NonlinearFactorGraph> (graph, initial);
+  Values result = optimize<NonlinearFactorGraph> (graph, initial);
   result.print("Final result: ");
 
   return 0;
diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp
index b73a989d4..c84924816 100644
--- a/examples/PlanarSLAMExample_easy.cpp
+++ b/examples/PlanarSLAMExample_easy.cpp
@@ -76,7 +76,7 @@ int main(int argc, char** argv) {
 	graph.print("full graph");
 
 	// initialize to noisy points
-	planarSLAM::Values initialEstimate;
+	Values initialEstimate;
 	initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -86,7 +86,7 @@ int main(int argc, char** argv) {
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
+	Values result = optimize<Graph>(graph, initialEstimate);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp
index 10b1b3576..fd6d2cbfe 100644
--- a/examples/PlanarSLAMSelfContained_advanced.cpp
+++ b/examples/PlanarSLAMSelfContained_advanced.cpp
@@ -108,7 +108,7 @@ int main(int argc, char** argv) {
 	graph->print("Full Graph");
 
 	// initialize to noisy points
-	boost::shared_ptr<DynamicValues> initial(new DynamicValues);
+	boost::shared_ptr<Values> initial(new Values);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp
index b6bfe7303..0e60c46df 100644
--- a/examples/Pose2SLAMExample_advanced.cpp
+++ b/examples/Pose2SLAMExample_advanced.cpp
@@ -57,7 +57,7 @@ int main(int argc, char** argv) {
 
   /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
-	shared_ptr<DynamicValues> initial(new DynamicValues);
+	shared_ptr<Values> initial(new Values);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
 	Optimizer optimizer(graph, initial, ordering, params);
 	Optimizer optimizer_result = optimizer.levenbergMarquardt();
 
-	DynamicValues result = *optimizer_result.values();
+	Values result = *optimizer_result.values();
 	result.print("final result");
 
 	/* Get covariances */
diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp
index fb98ebd97..e4281aa09 100644
--- a/examples/Pose2SLAMExample_easy.cpp
+++ b/examples/Pose2SLAMExample_easy.cpp
@@ -55,7 +55,7 @@ int main(int argc, char** argv) {
 
     /* 3. Create the data structure to hold the initial estinmate to the solution
      * initialize to noisy points */
-	DynamicValues initial;
+	Values initial;
 	initial.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -63,7 +63,7 @@ int main(int argc, char** argv) {
 
 	/* 4 Single Step Optimization
 	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
-	DynamicValues result = optimize<Graph>(graph, initial);
+	Values result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 
diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp
index b298ed92a..adc9f6780 100644
--- a/examples/Pose2SLAMwSPCG_advanced.cpp
+++ b/examples/Pose2SLAMwSPCG_advanced.cpp
@@ -27,17 +27,17 @@ using namespace gtsam;
 using namespace pose2SLAM;
 
 typedef boost::shared_ptr<Graph> sharedGraph ;
-typedef boost::shared_ptr<pose2SLAM::Values> sharedValue ;
+typedef boost::shared_ptr<Values> sharedValue ;
 //typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
 
 
-typedef SubgraphSolver<Graph, GaussianFactorGraph, pose2SLAM::Values> Solver;
+typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
 typedef boost::shared_ptr<Solver> sharedSolver ;
-typedef NonlinearOptimizer<Graph, pose2SLAM::Values, GaussianFactorGraph, Solver> SPCGOptimizer;
+typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
 
 sharedGraph graph;
 sharedValue initial;
-pose2SLAM::Values result;
+Values result;
 
 /* ************************************************************************* */
 int main(void) {
@@ -47,7 +47,7 @@ int main(void) {
 	Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
 
 	graph = boost::make_shared<Graph>() ;
-	initial = boost::make_shared<pose2SLAM::Values>() ;
+	initial = boost::make_shared<Values>() ;
 
 	// create a 3 by 3 grid
 	// x3 x6 x9
diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp
index 51be80841..aa11fd959 100644
--- a/examples/Pose2SLAMwSPCG_easy.cpp
+++ b/examples/Pose2SLAMwSPCG_easy.cpp
@@ -27,7 +27,7 @@ using namespace gtsam;
 using namespace pose2SLAM;
 
 Graph graph;
-DynamicValues initial, result;
+Values initial, result;
 
 /* ************************************************************************* */
 int main(void) {
diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp
index bd00f4a04..821b177dc 100644
--- a/examples/SimpleRotation.cpp
+++ b/examples/SimpleRotation.cpp
@@ -112,7 +112,7 @@ int main() {
 	 * on the type of key used to find the appropriate value map if there
 	 * are multiple types of variables.
 	 */
-	DynamicValues initial;
+	Values initial;
 	initial.insert(key, Rot2::fromAngle(20 * degree));
 	initial.print("initial estimate");
 
@@ -124,7 +124,7 @@ int main() {
 	 * initial estimate.  This will yield a new RotValues structure
 	 * with the final state of the optimization.
 	 */
-	DynamicValues result = optimize<Graph>(graph, initial);
+	Values result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index d001016c4..30d77aedc 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -46,7 +46,7 @@ int main() {
   Ordering::shared_ptr ordering(new Ordering);
 
   // Create a structure to hold the linearization points
-  DynamicValues linearizationPoints;
+  Values linearizationPoints;
 
 	// Ground truth example
 	// Start at origin, move to the right (x-axis): 0,0  0,1  0,2
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index ce88276c9..f197399df 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -80,7 +80,7 @@ void readAllDataISAM() {
 /**
  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
  */
-void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicValues>& initialValues,
+void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
     int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
 
   // Create a graph of newFactors with new measurements
@@ -101,7 +101,7 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicVa
   }
 
   // Create initial values for all nodes in the newFactors
-  initialValues = shared_ptr<DynamicValues> (new DynamicValues());
+  initialValues = shared_ptr<Values> (new Values());
   initialValues->insert(PoseKey(pose_id), pose);
   for (size_t i = 0; i < measurements.size(); i++) {
     initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
@@ -132,12 +132,12 @@ int main(int argc, char* argv[]) {
   BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
     const int poseId = features.first;
     shared_ptr<Graph> newFactors;
-    shared_ptr<DynamicValues> initialValues;
+    shared_ptr<Values> initialValues;
     createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
             features.second, measurementSigma, g_calib);
 
     isam.update(*newFactors, *initialValues);
-    DynamicValues currentEstimate = isam.estimate();
+    Values currentEstimate = isam.estimate();
     cout << "****************************************************" << endl;
     currentEstimate.print("Current estimate: ");
   }
diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp
index 1edb4a9ae..70ffaf05a 100644
--- a/examples/vSLAMexample/vSFMexample.cpp
+++ b/examples/vSLAMexample/vSFMexample.cpp
@@ -103,9 +103,9 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
  * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
  * The returned Values structure contains all initial values for all nodes.
  */
-DynamicValues initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
+Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
 
-	DynamicValues initValues;
+	Values initValues;
 
   // Initialize landmarks 3D positions.
   for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
@@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
   boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
 
   // Create an initial Values structure using groundtruth values as the initial estimates
-  boost::shared_ptr<DynamicValues> initialEstimates(new DynamicValues(initialize(g_landmarks, g_poses)));
+  boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
   cout << "*******************************************************" << endl;
   initialEstimates->print("INITIAL ESTIMATES: ");
 
diff --git a/gtsam.h b/gtsam.h
index 08727f9ef..0d6c3db12 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -321,9 +321,9 @@ class Graph {
 
 	void print(string s) const;
 
-	double error(const planarSLAM::Values& values) const;
-	Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
-	GaussianFactorGraph* linearize(const planarSLAM::Values& values,
+	double error(const Values& values) const;
+	Ordering* orderingCOLAMD(const Values& values) const;
+	GaussianFactorGraph* linearize(const Values& values,
 			const Ordering& ordering) const;
 
 	void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel);
@@ -333,14 +333,14 @@ class Graph {
 	void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel);
 	void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range,
 			const SharedNoiseModel& noiseModel);
-	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
+	Values optimize(const Values& initialEstimate);
 };
 
 class Odometry {
 	Odometry(int key1, int key2, const Pose2& measured,
 			const SharedNoiseModel& model);
 	void print(string s) const;
-	GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const;
+	GaussianFactor* linearize(const Values& center, const Ordering& ordering) const;
 };
 
 }///\namespace planarSLAM
diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h
index 5fc784a3e..be4ddf840 100644
--- a/gtsam/inference/graph-inl.h
+++ b/gtsam/inference/graph-inl.h
@@ -143,11 +143,11 @@ template <class V, class POSE, class KEY>
 class compose_key_visitor : public boost::default_bfs_visitor {
 
 private:
-	boost::shared_ptr<DynamicValues> config_;
+	boost::shared_ptr<Values> config_;
 
 public:
 
-	compose_key_visitor(boost::shared_ptr<DynamicValues> config_in) {config_ = config_in;}
+	compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
 
 	template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
 		KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
@@ -160,7 +160,7 @@ public:
 
 /* ************************************************************************* */
 template<class G, class Factor, class POSE, class KEY>
-boost::shared_ptr<DynamicValues> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
+boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
 		const POSE& rootPose) {
 
 	//TODO: change edge_weight_t to edge_pose_t
@@ -207,7 +207,7 @@ boost::shared_ptr<DynamicValues> composePoses(const G& graph, const PredecessorM
 	}
 
 	// compose poses
-	boost::shared_ptr<DynamicValues> config(new DynamicValues);
+	boost::shared_ptr<Values> config(new Values);
 	KEY rootKey = boost::get(boost::vertex_name, g, root);
 	config->insert(rootKey, rootPose);
 	compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h
index 6e51a9f6c..98d41940b 100644
--- a/gtsam/inference/graph.h
+++ b/gtsam/inference/graph.h
@@ -25,7 +25,7 @@
 #include <boost/graph/graph_traits.hpp>
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/shared_ptr.hpp>
-#include <gtsam/nonlinear/DynamicValues.h>
+#include <gtsam/nonlinear/Values.h>
 
 namespace gtsam {
 
@@ -89,7 +89,7 @@ namespace gtsam {
 	 * Compose the poses by following the chain specified by the spanning tree
 	 */
 	template<class G, class Factor, class POSE, class KEY>
-	boost::shared_ptr<DynamicValues>
+	boost::shared_ptr<Values>
 		composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
 
 
diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h
index 78fa4fdfe..573d97f04 100644
--- a/gtsam/linear/SubgraphSolver-inl.h
+++ b/gtsam/linear/SubgraphSolver-inl.h
@@ -60,7 +60,7 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
 }
 
 template<class GRAPH, class LINEAR, class KEY>
-void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const DynamicValues& theta0) {
+void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
 	// generate spanning tree
 	PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
 
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 6bb592005..30acb8fc4 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -16,7 +16,7 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/nonlinear/DynamicValues.h>
+#include <gtsam/nonlinear/Values.h>
 
 namespace gtsam {
 
@@ -44,7 +44,7 @@ private:
 	typedef boost::shared_ptr<Ordering> shared_ordering ;
 	typedef boost::shared_ptr<GRAPH> shared_graph ;
 	typedef boost::shared_ptr<LINEAR> shared_linear ;
-	typedef boost::shared_ptr<DynamicValues> shared_values ;
+	typedef boost::shared_ptr<Values> shared_values ;
 	typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
 	typedef std::map<Index,Index> mapPairIndex ;
 
@@ -62,7 +62,7 @@ private:
 
 public:
 
-	SubgraphSolver(const GRAPH& G, const DynamicValues& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
+	SubgraphSolver(const GRAPH& G, const Values& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
 		IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
 
 	SubgraphSolver(const LINEAR& GFG) {
@@ -90,7 +90,7 @@ public:
 	shared_ordering ordering() const { return ordering_; }
 
 protected:
-	void initialize(const GRAPH& G, const DynamicValues& theta0);
+	void initialize(const GRAPH& G, const Values& theta0);
 
 private:
 	SubgraphSolver():IterativeSolver(){}
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 7d3314d52..2c4e382a1 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -30,7 +30,7 @@ namespace gtsam {
 	template<class KEY>
 	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
 			const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
-			const DynamicValues& linearizationPoints, const KEY& lastKey,
+			const Values& linearizationPoints, const KEY& lastKey,
 			JacobianFactor::shared_ptr& newPrior) const {
 
 		// Extract the Index of the provided last key
@@ -100,7 +100,7 @@ namespace gtsam {
 		ordering.insert(x1, 1);
 
 		// Create a set of linearization points
-		DynamicValues linearizationPoints;
+		Values linearizationPoints;
 		linearizationPoints.insert(x0, x_);
 		linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
 
@@ -138,7 +138,7 @@ namespace gtsam {
 		ordering.insert(x0, 0);
 
 		// Create a set of linearization points
-		DynamicValues linearizationPoints;
+		Values linearizationPoints;
 		linearizationPoints.insert(x0, x_);
 
 		// Create a Gaussian Factor Graph
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h
index 3bf8c1887..2963638bf 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter.h
@@ -53,7 +53,7 @@ namespace gtsam {
 		JacobianFactor::shared_ptr priorFactor_; // density
 
 		T solve_(const GaussianFactorGraph& linearFactorGraph,
-				const Ordering& ordering, const DynamicValues& linearizationPoints,
+				const Ordering& ordering, const Values& linearizationPoints,
 				const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
 
 	public:
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index 7ddd2f8c6..c37892654 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -45,7 +45,7 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
    * @param ordering Current ordering to be augmented with new variables
    * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
    */
-  static void AddVariables(const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
+  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
 
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
@@ -95,7 +95,7 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
    * where we might expmap something twice, or expmap it but then not
    * recalculate its delta.
    */
-  static void ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
+  static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
       const Ordering& ordering, const std::vector<bool>& mask,
       boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
 
@@ -136,7 +136,7 @@ struct _VariableAdder {
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
-    const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
+    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
   theta.insert(newTheta);
@@ -259,7 +259,7 @@ struct _SelectiveExpmapAndClear {
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
-void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
+void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
     const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
   // If debugging, invalidate if requested, otherwise do not invalidate.
   // Invalidating means setting expmapped entries to Inf, to trigger assertions
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 231fbdea7..6435331ba 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -580,10 +580,10 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
-DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
+Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
   // We use ExpmapMasked here instead of regular expmap because the former
   // handles Permuted<VectorValues>
-	DynamicValues ret(theta_);
+	Values ret(theta_);
   vector<bool> mask(ordering_.nVars(), true);
   Impl::ExpmapMasked(ret, delta_, ordering_, mask);
   return ret;
@@ -600,7 +600,7 @@ typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key)
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
-DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
+Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
   VectorValues delta(theta_.dims(ordering_));
   optimize2(this->root(), delta);
   return theta_.retract(delta, ordering_);
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 85db8b691..62a86cf64 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -271,7 +271,7 @@ class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
 protected:
 
   /** The current linearization point */
-  DynamicValues theta_;
+  Values theta_;
 
   /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
   VariableIndex variableIndex_;
@@ -314,7 +314,7 @@ public:
 
   typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
   typedef ISAM2<CONDITIONAL> This; ///< This class
-  typedef DynamicValues Values;
+  typedef Values Values;
   typedef GRAPH Graph;
 
   /** Create an empty ISAM2 instance */
@@ -367,19 +367,19 @@ public:
    * (Params::relinearizeSkip).
    * @return An ISAM2Result struct containing information about the update
    */
-  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const DynamicValues& newTheta = DynamicValues(),
+  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
       const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
       const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
       bool force_relinearize = false);
 
   /** Access the current linearization point */
-  const DynamicValues& getLinearizationPoint() const {return theta_;}
+  const Values& getLinearizationPoint() const {return theta_;}
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only
    * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
    */
-  DynamicValues calculateEstimate() const;
+  Values calculateEstimate() const;
 
   /** Compute an estimate for a single variable using its incomplete linear delta computed
    * during the last update.  This is faster than calling the no-argument version of
@@ -398,7 +398,7 @@ public:
 
   /** Compute an estimate using a complete delta computed by a full back-substitution.
    */
-  DynamicValues calculateBestEstimate() const;
+  Values calculateBestEstimate() const;
 
   /** Access the current delta, computed during the last call to update */
   const Permuted<VectorValues>& getDelta() const { return delta_; }
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 1ad683047..923fbda8a 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -16,9 +16,9 @@ check_PROGRAMS =
 #----------------------------------------------------------------------------------------------------
 
 # Lie Groups
-headers += DynamicValues.h DynamicValues-inl.h
-sources += DynamicValues.cpp
-check_PROGRAMS += tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
+headers += Values-inl.h
+sources += Values.cpp
+check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering tests/testNonlinearFactor
 
 # Nonlinear nonlinear
 headers += Key.h 
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index e0066f21a..82e830990 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -108,7 +108,7 @@ namespace gtsam {
 		}
 
 		/** actual error function calculation */
-		virtual double error(const DynamicValues& c) const {
+		virtual double error(const Values& c) const {
 			const T& xj = c[this->key_];
 			Vector e = this->unwhitenedError(c);
 			if (allow_error_ || !compare_(xj, feasible_)) {
@@ -135,7 +135,7 @@ namespace gtsam {
 		}
 
 		// Linearize is over-written, because base linearization tries to whiten
-		virtual GaussianFactor::shared_ptr linearize(const DynamicValues& x, const Ordering& ordering) const {
+		virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
 			const T& xj = x[this->key_];
 			Matrix A;
 			Vector b = evaluateError(xj, A);
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index c392c05d7..2e35b97c1 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -31,7 +31,7 @@
 #include <gtsam/linear/SharedNoiseModel.h>
 #include <gtsam/linear/JacobianFactor.h>
 
-#include <gtsam/nonlinear/DynamicValues.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
@@ -107,7 +107,7 @@ public:
    * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
    * You can override this for systems with unusual noise models.
    */
-  virtual double error(const DynamicValues& c) const = 0;
+  virtual double error(const Values& c) const = 0;
 
   /** get the dimension of the factor (number of rows on linearization) */
   virtual size_t dim() const = 0;
@@ -122,11 +122,11 @@ public:
 	 * when the constraint is *NOT* fulfilled.
 	 * @return true if the constraint is active
 	 */
-	virtual bool active(const DynamicValues& c) const { return true; }
+	virtual bool active(const Values& c) const { return true; }
 
   /** linearize to a GaussianFactor */
   virtual boost::shared_ptr<GaussianFactor>
-  linearize(const DynamicValues& c, const Ordering& ordering) const = 0;
+  linearize(const Values& c, const Ordering& ordering) const = 0;
 
   /**
    * Create a symbolic factor using the given ordering to determine the
@@ -231,13 +231,13 @@ public:
    * If any of the optional Matrix reference arguments are specified, it should compute
    * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
    */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
 
   /**
    * Vector of errors, whitened
    * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
    */
-  Vector whitenedError(const DynamicValues& c) const {
+  Vector whitenedError(const Values& c) const {
     return noiseModel_->whiten(unwhitenedError(c));
   }
 
@@ -247,7 +247,7 @@ public:
    * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
    * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
    */
-  virtual double error(const DynamicValues& c) const {
+  virtual double error(const Values& c) const {
   	if (this->active(c))
   		return 0.5 * noiseModel_->distance(unwhitenedError(c));
   	else
@@ -259,7 +259,7 @@ public:
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
-  boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& x, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
   	// Only linearize if the factor is active
 		if (!this->active(x))
 			return boost::shared_ptr<JacobianFactor>();
@@ -343,7 +343,7 @@ public:
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X& x1 = x[key_];
       if(H) {
@@ -427,7 +427,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X1& x1 = x[key1_];
       const X2& x2 = x[key2_];
@@ -519,7 +519,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
@@ -617,7 +617,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
   			return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -721,7 +721,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -832,7 +832,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 880cb2fb9..11f8e84d9 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -28,7 +28,7 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	double NonlinearFactorGraph::probPrime(const DynamicValues& c) const {
+	double NonlinearFactorGraph::probPrime(const Values& c) const {
 		return exp(-0.5 * error(c));
 	}
 
@@ -38,7 +38,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	double NonlinearFactorGraph::error(const DynamicValues& c) const {
+	double NonlinearFactorGraph::error(const Values& c) const {
 		double total_error = 0.;
 		// iterate over all the factors_ to accumulate the log probabilities
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
@@ -60,7 +60,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
-			const DynamicValues& config) const {
+			const Values& config) const {
 
 		// Create symbolic graph and initial (iterator) ordering
 		SymbolicFactorGraph::shared_ptr symbolic;
@@ -102,7 +102,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
-		const DynamicValues& config) const {
+		const Values& config) const {
 		// Generate an initial key ordering in iterator order
 		Ordering::shared_ptr ordering(config.orderingArbitrary());
 		return make_pair(symbolic(*ordering), ordering);
@@ -110,7 +110,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
-			const DynamicValues& config, const Ordering& ordering) const {
+			const Values& config, const Ordering& ordering) const {
 
 		// create an empty linear FG
 		GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 1b1bba892..fa87baedf 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -50,10 +50,10 @@ namespace gtsam {
     std::set<Symbol> keys() const;
 
 		/** unnormalized error */
-		double error(const DynamicValues& c) const;
+		double error(const Values& c) const;
 
 		/** Unnormalized probability. O(n) */
-		double probPrime(const DynamicValues& c) const;
+		double probPrime(const Values& c) const;
 
 		template<class F>
 		void add(const F& factor) {
@@ -72,20 +72,20 @@ namespace gtsam {
 		 * ordering is found.
 		 */
 		std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
-		symbolic(const DynamicValues& config) const;
+		symbolic(const Values& config) const;
 
     /**
      * Compute a fill-reducing ordering using COLAMD.  This returns the
      * ordering and a VariableIndex, which can later be re-used to save
      * computation.
      */
-		Ordering::shared_ptr orderingCOLAMD(const DynamicValues& config) const;
+		Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
 
 		/**
 		 * linearize a nonlinear factor graph
 		 */
 		boost::shared_ptr<GaussianFactorGraph >
-				linearize(const DynamicValues& config, const Ordering& ordering) const;
+				linearize(const Values& config, const Ordering& ordering) const;
 
 	private:
 
diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h
index b98a0a958..03d2832ce 100644
--- a/gtsam/nonlinear/NonlinearISAM-inl.h
+++ b/gtsam/nonlinear/NonlinearISAM-inl.h
@@ -31,7 +31,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 template<class GRAPH>
 void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
-		const DynamicValues& initialValues) {
+		const Values& initialValues) {
 
   if(newFactors.size() > 0) {
 
@@ -69,7 +69,7 @@ void NonlinearISAM<GRAPH>::reorder_relinearize() {
 
   if(factors_.size() > 0) {
     // Obtain the new linearization point
-    const DynamicValues newLinPoint = estimate();
+    const Values newLinPoint = estimate();
 
     isam_.clear();
 
@@ -90,7 +90,7 @@ void NonlinearISAM<GRAPH>::reorder_relinearize() {
 
 /* ************************************************************************* */
 template<class GRAPH>
-DynamicValues NonlinearISAM<GRAPH>::estimate() const {
+Values NonlinearISAM<GRAPH>::estimate() const {
   if(isam_.size() > 0)
     return linPoint_.retract(optimize(isam_), ordering_);
   else
diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h
index 827bb30e1..a8b34b14c 100644
--- a/gtsam/nonlinear/NonlinearISAM.h
+++ b/gtsam/nonlinear/NonlinearISAM.h
@@ -36,7 +36,7 @@ protected:
 	gtsam::GaussianISAM isam_;
 
 	/** The current linearization point */
-	DynamicValues linPoint_;
+	Values linPoint_;
 
 	/** The ordering */
 	gtsam::Ordering ordering_;
@@ -60,10 +60,10 @@ public:
 	NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
 
 	/** Add new factors along with their initial linearization points */
-	void update(const Factors& newFactors, const DynamicValues& initialValues);
+	void update(const Factors& newFactors, const Values& initialValues);
 
 	/** Return the current solution estimate */
-	DynamicValues estimate() const;
+	Values estimate() const;
 
 	/** Relinearization and reordering of variables */
 	void reorder_relinearize();
@@ -83,7 +83,7 @@ public:
 	const GaussianISAM& bayesTree() const { return isam_; }
 
 	/** Return the current linearization point */
-	const DynamicValues& getLinearizationPoint() const { return linPoint_; }
+	const Values& getLinearizationPoint() const { return linPoint_; }
 
 	/** Get the ordering */
 	const gtsam::Ordering& getOrdering() const { return ordering_; }
diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h
index e1896557b..450626293 100644
--- a/gtsam/nonlinear/NonlinearOptimization-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimization-inl.h
@@ -32,7 +32,7 @@ namespace gtsam {
 	 * The Elimination solver
 	 */
 	template<class G>
-	DynamicValues	optimizeSequential(const G& graph, const DynamicValues& initialEstimate,
+	Values	optimizeSequential(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters, bool useLM) {
 
 		// Use a variable ordering from COLAMD
@@ -41,7 +41,7 @@ namespace gtsam {
 		// Create an nonlinear Optimizer that uses a Sequential Solver
 	  typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
-	  		boost::make_shared<const DynamicValues>(initialEstimate), ordering,
+	  		boost::make_shared<const Values>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
 
 	  // Now optimize using either LM or GN methods.
@@ -55,7 +55,7 @@ namespace gtsam {
 	 * The multifrontal solver
 	 */
 	template<class G>
-	DynamicValues	optimizeMultiFrontal(const G& graph, const DynamicValues& initialEstimate,
+	Values	optimizeMultiFrontal(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters, bool useLM) {
 
 		// Use a variable ordering from COLAMD
@@ -64,7 +64,7 @@ namespace gtsam {
 		// Create an nonlinear Optimizer that uses a Multifrontal Solver
 	  typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
-	  		boost::make_shared<const DynamicValues>(initialEstimate), ordering,
+	  		boost::make_shared<const Values>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
 
 	  // now optimize using either LM or GN methods
@@ -78,19 +78,19 @@ namespace gtsam {
 	 * The sparse preconditioned conjugate gradient solver
 	 */
 	template<class G>
-	DynamicValues	optimizeSPCG(const G& graph, const DynamicValues& initialEstimate,
+	Values	optimizeSPCG(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			bool useLM = true) {
 
 		// initial optimization state is the same in both cases tested
-		typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver;
+		typedef SubgraphSolver<G,GaussianFactorGraph,Values> Solver;
 		typedef boost::shared_ptr<Solver> shared_Solver;
 		typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
 		shared_Solver solver = boost::make_shared<Solver>(
 				graph, initialEstimate, IterativeOptimizationParameters());
 		SPCGOptimizer optimizer(
 				boost::make_shared<const G>(graph),
-				boost::make_shared<const DynamicValues>(initialEstimate),
+				boost::make_shared<const Values>(initialEstimate),
 				solver->ordering(),
 				solver,
 				boost::make_shared<NonlinearOptimizationParameters>(parameters));
@@ -106,7 +106,7 @@ namespace gtsam {
 	 * optimization that returns the values
 	 */
 	template<class G>
-	DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters,
+	Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters,
 			const LinearSolver& solver,
 			const NonlinearOptimizationMethod& nonlinear_method) {
 		switch (solver) {
@@ -117,7 +117,7 @@ namespace gtsam {
 			return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 		case SPCG:
-//			return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
+//			return optimizeSPCG<G,Values>(graph, initialEstimate, parameters,
 //								nonlinear_method == LM);
 			throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
 		}
diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h
index 86ab7a3c8..4a8ab15d2 100644
--- a/gtsam/nonlinear/NonlinearOptimization.h
+++ b/gtsam/nonlinear/NonlinearOptimization.h
@@ -43,7 +43,7 @@ namespace gtsam {
 	 * optimization that returns the values
 	 */
 	template<class G>
-	DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate,
+	Values optimize(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			const LinearSolver& solver = MULTIFRONTAL,
 			const NonlinearOptimizationMethod& nonlinear_method = LM);
diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h
index 2e7a83f76..213ab8daf 100644
--- a/gtsam/nonlinear/NonlinearOptimizer-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h
@@ -86,7 +86,7 @@ namespace gtsam {
 		if (verbosity >= Parameters::DELTA) delta.print("delta");
 
 		// take old values and update it
-		shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_)));
+		shared_values newValues(new Values(values_->retract(delta, *ordering_)));
 
 		// maybe show output
 		if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@@ -178,7 +178,7 @@ namespace gtsam {
 		    if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
 
 		    // update values
-		    shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_)));
+		    shared_values newValues(new Values(values_->retract(delta, *ordering_)));
 
 		    // create new optimization state with more adventurous lambda
 		    double error = graph_->error(*newValues);
@@ -306,7 +306,7 @@ namespace gtsam {
     DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
         parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
         *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR);
-    shared_values newValues(new DynamicValues(values_->retract(result.dx_d, *ordering_)));
+    shared_values newValues(new Values(values_->retract(result.dx_d, *ordering_)));
     return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
   }
 
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index e87b44a5d..d57477653 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -37,19 +37,19 @@ public:
  * and then one of the optimization routines is called. These iterate
  * until convergence. All methods are functional and return a new state.
  *
- * The class is parameterized by the Graph type $G$, Values class type $DynamicValues$,
+ * The class is parameterized by the Graph type $G$, Values class type $Values$,
  * linear system class $L$, the non linear solver type $S$, and the writer type $W$
  *
- * The values class type $DynamicValues$ is in order to be able to optimize over non-vector values structures.
+ * The values class type $Values$ is in order to be able to optimize over non-vector values structures.
  *
  * A nonlinear system solver $S$
- * Concept NonLinearSolver<G,DynamicValues,L> implements
- *   linearize: G * DynamicValues -> L
- *   solve : L -> DynamicValues
+ * Concept NonLinearSolver<G,Values,L> implements
+ *   linearize: G * Values -> L
+ *   solve : L -> Values
  *
  * The writer $W$ generates output to disk or the screen.
  *
- * For example, in a 2D case, $G$ can be Pose2Graph, $DynamicValues$ can be Pose2Values,
+ * For example, in a 2D case, $G$ can be Pose2Graph, $Values$ can be Pose2Values,
  * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
  * The solver class has two main functions: linearize and optimize. The first one
  * linearizes the nonlinear cost function around the current estimate, and the second
@@ -62,7 +62,7 @@ class NonlinearOptimizer {
 public:
 
 	// For performance reasons in recursion, we store values in a shared_ptr
-	typedef boost::shared_ptr<const DynamicValues> shared_values; ///Prevent memory leaks in Values
+	typedef boost::shared_ptr<const Values> shared_values; ///Prevent memory leaks in Values
 	typedef boost::shared_ptr<const G> shared_graph;  /// Prevent memory leaks in Graph
 	typedef boost::shared_ptr<L> shared_linear;    /// Not sure
 	typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
@@ -222,7 +222,7 @@ public:
 
 	/**
 	 *  linearize and optimize
-	 *  This returns an VectorValues, i.e., vectors in tangent space of DynamicValues
+	 *  This returns an VectorValues, i.e., vectors in tangent space of Values
 	 */
 	VectorValues linearizeAndOptimizeForDelta() const {
 		return *createSolver()->optimize();
@@ -309,18 +309,18 @@ public:
 	 * Static interface to LM optimization (no shared_ptr arguments) - see above
 	 */
 	static shared_values optimizeLM(const G& graph,
-			const DynamicValues& values,
+			const Values& values,
 			const Parameters parameters = Parameters()) {
 		return optimizeLM(boost::make_shared<const G>(graph),
-				boost::make_shared<const DynamicValues>(values),
+				boost::make_shared<const Values>(values),
 				boost::make_shared<Parameters>(parameters));
 	}
 
 	static shared_values optimizeLM(const G& graph,
-			const DynamicValues& values,
+			const Values& values,
 			Parameters::verbosityLevel verbosity) {
 		return optimizeLM(boost::make_shared<const G>(graph),
-				boost::make_shared<const DynamicValues>(values),
+				boost::make_shared<const Values>(values),
 				verbosity);
 	}
 
@@ -360,18 +360,18 @@ public:
    * Static interface to Dogleg optimization (no shared_ptr arguments) - see above
    */
   static shared_values optimizeDogLeg(const G& graph,
-      const DynamicValues& values,
+      const Values& values,
       const Parameters parameters = Parameters()) {
     return optimizeDogLeg(boost::make_shared<const G>(graph),
-        boost::make_shared<const DynamicValues>(values),
+        boost::make_shared<const Values>(values),
         boost::make_shared<Parameters>(parameters));
   }
 
   static shared_values optimizeDogLeg(const G& graph,
-      const DynamicValues& values,
+      const Values& values,
       Parameters::verbosityLevel verbosity) {
     return optimizeDogLeg(boost::make_shared<const G>(graph),
-        boost::make_shared<const DynamicValues>(values),
+        boost::make_shared<const Values>(values),
         verbosity);
   }
 
@@ -398,9 +398,9 @@ public:
 	/**
 	 * Static interface to GN optimization (no shared_ptr arguments) - see above
 	 */
-	static shared_values optimizeGN(const G& graph, const DynamicValues& values, const Parameters parameters = Parameters()) {
+	static shared_values optimizeGN(const G& graph, const Values& values, const Parameters parameters = Parameters()) {
 		return optimizeGN(boost::make_shared<const G>(graph),
-				boost::make_shared<const DynamicValues>(values),
+				boost::make_shared<const Values>(values),
 				boost::make_shared<Parameters>(parameters));
 	}
 };
diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h
deleted file mode 100644
index a9d6300c1..000000000
--- a/gtsam/nonlinear/TupleValues-inl.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file TupleValues-inl.h
- * @author Richard Roberts
- * @author Manohar Paluri
- * @author Alex Cunningham
- */
-
-#pragma once
-
-namespace gtsam {
-
-/* ************************************************************************* */
-/** TupleValuesN Implementations */
-/* ************************************************************************* */
-
-/* ************************************************************************* */
-/** TupleValues 1 */
-template<class VALUES1>
-TupleValues1<VALUES1>::TupleValues1(const TupleValues1<VALUES1>& values) :
-		  TupleValuesEnd<VALUES1> (values) {}
-
-template<class VALUES1>
-TupleValues1<VALUES1>::TupleValues1(const VALUES1& cfg1) :
-			  TupleValuesEnd<VALUES1> (cfg1) {}
-
-template<class VALUES1>
-TupleValues1<VALUES1>::TupleValues1(const TupleValuesEnd<VALUES1>& values) :
-	TupleValuesEnd<VALUES1>(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 2 */
-template<class VALUES1, class VALUES2>
-TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues2<VALUES1, VALUES2>& values) :
-		  TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
-
-template<class VALUES1, class VALUES2>
-TupleValues2<VALUES1, VALUES2>::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) :
-			  TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(
-					  cfg1, TupleValuesEnd<VALUES2>(cfg2)) {}
-
-template<class VALUES1, class VALUES2>
-TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues<VALUES1, TupleValuesEnd<VALUES2> >& values) :
-	TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 3 */
-template<class VALUES1, class VALUES2, class VALUES3>
-TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
-		const TupleValues3<VALUES1, VALUES2, VALUES3>& values) :
-		  TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3>
-TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
-		const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3) :
-			  TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(
-					  cfg1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> >(
-							  cfg2, TupleValuesEnd<VALUES3>(cfg3))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3>
-TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
-		const TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >& values) :
-		  TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 4 */
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
-TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
-		const TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2,
-		TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
-TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
-		const VALUES1& cfg1, const VALUES2& cfg2,
-		const VALUES3& cfg3,const VALUES4& cfg4) :
-		  TupleValues<VALUES1, TupleValues<VALUES2,
-			  TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(
-				  cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> > >(
-						  cfg2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> >(
-								  cfg3, TupleValuesEnd<VALUES4>(cfg4)))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
-TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
-		const TupleValues<VALUES1, TupleValues<VALUES2,
-				TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2,TupleValues<VALUES3,
-		TupleValuesEnd<VALUES4> > > >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 5 */
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
-TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
-		const TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>& values) :
-		  TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-			  TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
-TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
-		const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
-				   const VALUES4& cfg4, const VALUES5& cfg5) :
-						   TupleValues<VALUES1, TupleValues<VALUES2,
-						   TupleValues<VALUES3, TupleValues<VALUES4,
-						   TupleValuesEnd<VALUES5> > > > >(
-								   cfg1, TupleValues<VALUES2, TupleValues<VALUES3,
-										 TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > >(
-										   cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> > >(
-												   cfg3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> >(
-														   cfg4, TupleValuesEnd<VALUES5>(cfg5))))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
-TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
-		const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-			  TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-	TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 6 */
-template<class VALUES1, class VALUES2, class VALUES3,
-		 class VALUES4, class VALUES5, class VALUES6>
-TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
-		const TupleValues6<VALUES1, VALUES2, VALUES3,
-						   VALUES4, VALUES5, VALUES6>& values) :
-			  TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-			  TupleValues<VALUES4, TupleValues<VALUES5,
-			  TupleValuesEnd<VALUES6> > > > > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3,
-		 class VALUES4, class VALUES5, class VALUES6>
-TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
-		const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
-		const VALUES4& cfg4, const VALUES5& cfg5, const VALUES6& cfg6) :
-		TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-		TupleValues<VALUES4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > > >(
-				cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValues<VALUES4,
-				      TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > >(
-						cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValues<VALUES5,
-							  TupleValuesEnd<VALUES6> > > >(
-								cfg3, TupleValues<VALUES4, TupleValues<VALUES5,
-								      TupleValuesEnd<VALUES6> > >(
-										cfg4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> >(
-												cfg5, TupleValuesEnd<VALUES6>(cfg6)))))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3,
-	     class VALUES4, class VALUES5, class VALUES6>
-TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
-		const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-		      TupleValues<VALUES4, TupleValues<VALUES5,
-		      TupleValuesEnd<VALUES6> > > > > >& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-	TupleValues<VALUES4, TupleValues<VALUES5,
-	TupleValuesEnd<VALUES6> > > > > >(values) {}
-
-}
diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h
deleted file mode 100644
index 9f1cbebae..000000000
--- a/gtsam/nonlinear/TupleValues.h
+++ /dev/null
@@ -1,524 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file TupleValues.h
- * @author Richard Roberts
- * @author Manohar Paluri
- * @author Alex Cunningham
- */
-
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/linear/VectorValues.h>
-
-#pragma once
-
-namespace gtsam {
-
- /**
-   *  TupleValues are a structure to manage heterogenous Values, so as to
-   *  enable different types of variables/keys to be used simultaneously.  We
-   *  do this with recursive templates (instead of blind pointer casting) to
-   *  reduce run-time overhead and keep static type checking.  The interface
-   *  mimics that of a single Values.
-   *
-   *  This uses a recursive structure of values pairs to form a lisp-like
-   *  list, with a special case (TupleValuesEnd) that contains only one values
-   *  at the end.  Because this recursion is done at compile time, there is no
-   *  runtime performance hit to using this structure.
-   *
-   *  For an easy interface, there are TupleValuesN classes, which wrap
-   *  the recursive TupleValues together as a single class, so you can have
-   *  mixed-type classes from 2-6 types.  Note that a TupleValues2 is equivalent
-   *  to the previously used PairValues.
-   *
-   *  Design and extension note:
-   *  To implement a recursively templated data structure, note that most operations
-   *  have two versions: one with templates and one without.  The templated one allows
-   *  for the arguments to be passed to the next values, while the specialized one
-   *  operates on the "first" values. TupleValuesEnd contains only the specialized version.
-   */
-  template<class VALUES1, class VALUES2>
-  class TupleValues {
-
-  protected:
-	  // Data for internal valuess
-	  VALUES1 first_;	/// Arbitrary values
-	  VALUES2 second_;	/// A TupleValues or TupleValuesEnd, which wraps an arbitrary values
-
-	  /** concept checks */
-	  GTSAM_CONCEPT_TESTABLE_TYPE(VALUES1)
-	  GTSAM_CONCEPT_TESTABLE_TYPE(VALUES2)
-
-  public:
-	  // typedefs for values subtypes
-	  typedef typename VALUES1::Key Key1;
-	  typedef typename VALUES1::Value Value1;
-
-	  /** default constructor */
-	  TupleValues() {}
-
-	  /** Copy constructor */
-	  TupleValues(const TupleValues<VALUES1, VALUES2>& values) :
-		  first_(values.first_), second_(values.second_) {}
-
-	  /** Construct from valuess */
-	  TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) :
-		  first_(cfg1), second_(cfg2) {}
-
-	  /** Print */
-	  void print(const std::string& s = "") const {
-			first_.print(s);
-			second_.print();
-	  }
-
-	  /** Equality with tolerance for keys and values */
-	  bool equals(const TupleValues<VALUES1, VALUES2>& c, double tol=1e-9) const {
-		  return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
-	  }
-
-	  /**
-	   * Insert a key/value pair to the values.
-	   * Note: if the key is already in the values, the values will not be changed.
-	   * Use update() to allow for changing existing values.
-	   * @param key is the key - can be an int (second version) if the can can be initialized from an int
-	   * @param value is the value to insert
-	   */
-	  template<class KEY, class VALUE>
-	  void insert(const KEY& key, const VALUE& value) {second_.insert(key, value);}
-	  void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
-	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value);}
-
-	  /**
-	   * Insert a complete values at a time.
-	   * Note: if the key is already in the values, the values will not be changed.
-	   * Use update() to allow for changing existing values.
-	   * @param values is a full values to add
-	   */
-	  template<class CFG1, class CFG2>
-	  void insert(const TupleValues<CFG1, CFG2>& values) { second_.insert(values); }
-	  void insert(const TupleValues<VALUES1, VALUES2>& values) {
-		  first_.insert(values.first_);
-		  second_.insert(values.second_);
-	  }
-
-	  /**
-	   * Update function for whole valuess - this will change existing values
-	   * @param values is a values to add
-	   */
-	  template<class CFG1, class CFG2>
-	  void update(const TupleValues<CFG1, CFG2>& values) { second_.update(values); }
-	  void update(const TupleValues<VALUES1, VALUES2>& values) {
-	  	first_.update(values.first_);
-	  	second_.update(values.second_);
-	  }
-
-	  /**
-	   * Update function for single key/value pairs - will change existing values
-	   * @param key is the variable identifier
-	   * @param value is the variable value to update
-	   */
-	  template<class KEY, class VALUE>
-	  void update(const KEY& key, const VALUE& value) { second_.update(key, value); }
-	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }
-
-	  /**
-	   * Insert a subvalues
-	   * @param values is the values to insert
-	   */
-	  template<class CFG>
-	  void insertSub(const CFG& values) { second_.insertSub(values); }
-	  void insertSub(const VALUES1& values) { first_.insert(values);  }
-
-	  /** erase an element by key */
-	  template<class KEY>
-	  void erase(const KEY& j)  { second_.erase(j); }
-	  void erase(const Key1& j)  { first_.erase(j); }
-
-	  /** clears the values */
-	  void clear() { first_.clear(); second_.clear(); }
-
-	  /** determine whether an element exists */
-	  template<class KEY>
-	  bool exists(const KEY& j) const { return second_.exists(j); }
-	  bool exists(const Key1& j) const { return first_.exists(j); }
-
-	  /** a variant of exists */
-	  template<class KEY>
-	  boost::optional<typename KEY::Value> exists_(const KEY& j)  const { return second_.exists_(j); }
-	  boost::optional<Value1>                exists_(const Key1& j) const { return first_.exists_(j); }
-
-	  /** access operator */
-	  template<class KEY>
-	  const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; }
-	  const Value1& operator[](const Key1& j) const { return first_[j]; }
-
-	  /** at access function */
-	  template<class KEY>
-	  const typename KEY::Value & at(const KEY& j) const { return second_.at(j); }
-	  const Value1& at(const Key1& j) const { return first_.at(j); }
-
-	  /** direct values access */
-	  const VALUES1& values() const { return first_; }
-	  const VALUES2& rest() const { return second_; }
-
-	  /** zero: create VectorValues of appropriate structure */
-	  VectorValues zero(const Ordering& ordering) const {
-		  return VectorValues::Zero(this->dims(ordering));
-	  }
-
-	  /** @return number of key/value pairs stored */
-	  size_t size() const { return first_.size() + second_.size(); }
-
-	  /** @return true if values is empty */
-	  bool empty() const { return first_.empty() && second_.empty(); }
-
-	  /** @return The dimensionality of the tangent space */
-	  size_t dim() const { return first_.dim() + second_.dim(); }
-
-	  /** Create an array of variable dimensions using the given ordering */
-	  std::vector<size_t> dims(const Ordering& ordering) const {
-	    _ValuesDimensionCollector dimCollector(ordering);
-	    this->apply(dimCollector);
-	    return dimCollector.dimensions;
-	  }
-
-    /**
-     * Generate a default ordering, simply in key sort order.  To instead
-     * create a fill-reducing ordering, use
-     * NonlinearFactorGraph::orderingCOLAMD().  Alternatively, you may permute
-     * this ordering yourself (as orderingCOLAMD() does internally).
-     */
-    Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
-      // Generate an initial key ordering in iterator order
-      _ValuesKeyOrderer keyOrderer(firstVar);
-      this->apply(keyOrderer);
-      return keyOrderer.ordering;
-    }
-
-	  /** Expmap */
-	  TupleValues<VALUES1, VALUES2> retract(const VectorValues& delta, const Ordering& ordering) const {
-	    return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering));
-	  }
-
-	  /** logmap each element */
-	  VectorValues localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
-		  VectorValues delta(this->dims(ordering));
-		  localCoordinates(cp, ordering, delta);
-		  return delta;
-	  }
-
-    /** logmap each element */
-    void localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
-      first_.localCoordinates(cp.first_, ordering, delta);
-      second_.localCoordinates(cp.second_, ordering, delta);
-    }
-
-	  /**
-	   * Apply a class with an application operator() to a const_iterator over
-	   * every <key,value> pair.  The operator must be able to handle such an
-	   * iterator for every type in the Values, (i.e. through templating).
-	   */
-    template<typename A>
-    void apply(A& operation) {
-    	first_.apply(operation);
-    	second_.apply(operation);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-    	first_.apply(operation);
-    	second_.apply(operation);
-    }
-
-  private:
-	  /** Serialization function */
-	  friend class boost::serialization::access;
-	  template<class ARCHIVE>
-	  void serialize(ARCHIVE & ar, const unsigned int version) {
-		  ar & BOOST_SERIALIZATION_NVP(first_);
-		  ar & BOOST_SERIALIZATION_NVP(second_);
-	  }
-
-  };
-
-  /**
-   * End of a recursive TupleValues - contains only one values
-   *
-   * Do not use this class directly - it should only be used as a part
-   * of a recursive structure
-   */
-  template<class VALUES>
-  class TupleValuesEnd {
-
-  protected:
-	  // Data for internal valuess
-	  VALUES first_;
-
-  public:
-	  // typedefs
-	  typedef typename VALUES::Key Key1;
-	  typedef typename VALUES::Value Value1;
-
-	  TupleValuesEnd() {}
-
-	  TupleValuesEnd(const TupleValuesEnd<VALUES>& values) :
-		  first_(values.first_) {}
-
-	  TupleValuesEnd(const VALUES& cfg) :
-		  first_(cfg) {}
-
-	  void print(const std::string& s = "") const {
-			first_.print();
-	  }
-
-	  bool equals(const TupleValuesEnd<VALUES>& c, double tol=1e-9) const {
-		  return first_.equals(c.first_, tol);
-	  }
-
-	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
-	  void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
-
-	  void insert(const TupleValuesEnd<VALUES>& values) {first_.insert(values.first_); }
-
-	  void update(const TupleValuesEnd<VALUES>& values) {first_.update(values.first_); }
-
-	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }
-
-	  void insertSub(const VALUES& values) {first_.insert(values); }
-
-	  const Value1& operator[](const Key1& j) const { return first_[j]; }
-
-	  const VALUES& values() const { return first_; }
-
-	  void erase(const Key1& j) { first_.erase(j); }
-
-	  void clear() { first_.clear(); }
-
-	  bool empty() const { return first_.empty(); }
-
-	  bool exists(const Key1& j) const { return first_.exists(j); }
-
-	  boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }
-
-	  const Value1& at(const Key1& j) const { return first_.at(j); }
-
-	  VectorValues zero(const Ordering& ordering) const {
-		  return VectorValues::Zero(this->dims(ordering));
-	  }
-
-	  size_t size() const { return first_.size(); }
-
-	  size_t dim() const { return first_.dim(); }
-
-	  TupleValuesEnd<VALUES> retract(const VectorValues& delta, const Ordering& ordering) const {
-	        return TupleValuesEnd(first_.retract(delta, ordering));
-	  }
-
-    VectorValues localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
-      VectorValues delta(this->dims(ordering));
-      localCoordinates(cp, ordering, delta);
-      return delta;
-    }
-
-    void localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
-      first_.localCoordinates(cp.first_, ordering, delta);
-    }
-
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      first_.apply(operation);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-      first_.apply(operation);
-    }
-
-    /** Create an array of variable dimensions using the given ordering */
-    std::vector<size_t> dims(const Ordering& ordering) const {
-    	_ValuesDimensionCollector dimCollector(ordering);
-    	this->apply(dimCollector);
-    	return dimCollector.dimensions;
-    }
-
-    /**
-     * Generate a default ordering, simply in key sort order.  To instead
-     * create a fill-reducing ordering, use
-     * NonlinearFactorGraph::orderingCOLAMD().  Alternatively, you may permute
-     * this ordering yourself (as orderingCOLAMD() does internally).
-     */
-    Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
-      // Generate an initial key ordering in iterator order
-      _ValuesKeyOrderer keyOrderer(firstVar);
-      this->apply(keyOrderer);
-      return keyOrderer.ordering;
-    }
-
-  private:
-	  friend class boost::serialization::access;
-	  template<class ARCHIVE>
-	  void serialize(ARCHIVE & ar, const unsigned int version) {
-		  ar & BOOST_SERIALIZATION_NVP(first_);
-	  }
-  };
-
-  /**
-   * Wrapper classes to act as containers for valuess.  Note that these can be cascaded
-   * recursively, as they are TupleValues, and are primarily a short form of the values
-   * structure to make use of the TupleValues easier.
-   *
-   * The interface is designed to mimic PairValues, but for 2-6 values types.
-   */
-
-  template<class C1>
-  class TupleValues1 : public TupleValuesEnd<C1> {
-  public:
- 	  // typedefs
- 	  typedef C1 Values1;
-
- 	  typedef TupleValuesEnd<C1> Base;
- 	  typedef TupleValues1<C1> This;
-
- 	  TupleValues1() {}
- 	  TupleValues1(const This& values);
- 	  TupleValues1(const Base& values);
- 	  TupleValues1(const Values1& cfg1);
-
- 	  // access functions
- 	  inline const Values1& first() const { return this->values(); }
-  };
-
-  template<class C1, class C2>
-  class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
-  public:
-	  // typedefs
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-
-	  typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
-	  typedef TupleValues2<C1, C2> This;
-
-	  TupleValues2() {}
-	  TupleValues2(const This& values);
-	  TupleValues2(const Base& values);
-	  TupleValues2(const Values1& cfg1, const Values2& cfg2);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-  };
-
-  template<class C1, class C2, class C3>
-  class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
-  public:
-	  // typedefs
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-
-	  typedef TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > Base;
-	  typedef TupleValues3<C1, C2, C3> This;
-
-	  TupleValues3() {}
-	  TupleValues3(const Base& values);
-	  TupleValues3(const This& values);
-	  TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-  };
-
-  template<class C1, class C2, class C3, class C4>
-  class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
-  public:
-	  // typedefs
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-	  typedef C4 Values4;
-
-	  typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
-	  typedef TupleValues4<C1, C2, C3, C4> This;
-
-	  TupleValues4() {}
-	  TupleValues4(const This& values);
-	  TupleValues4(const Base& values);
-	  TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-	  inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
-  };
-
-  template<class C1, class C2, class C3, class C4, class C5>
-  class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
-  public:
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-	  typedef C4 Values4;
-	  typedef C5 Values5;
-
-	  typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > Base;
-	  typedef TupleValues5<C1, C2, C3, C4, C5> This;
-
-	  TupleValues5() {}
-	  TupleValues5(const This& values);
-	  TupleValues5(const Base& values);
-	  TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
-				   const Values4& cfg4, const Values5& cfg5);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-	  inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
-	  inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); }
-  };
-
-  template<class C1, class C2, class C3, class C4, class C5, class C6>
-  class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
-  public:
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-	  typedef C4 Values4;
-	  typedef C5 Values5;
-	  typedef C6 Values6;
-
-	  typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > Base;
-	  typedef TupleValues6<C1, C2, C3, C4, C5, C6> This;
-
-	  TupleValues6() {}
-	  TupleValues6(const This& values);
-	  TupleValues6(const Base& values);
-	  TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
-				   const Values4& cfg4, const Values5& cfg5, const Values6& cfg6);
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-	  inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
-	  inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); }
-	  inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().values(); }
-  };
-
-}
-
-#include <gtsam/nonlinear/TupleValues-inl.h>
diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/Values-inl.h
similarity index 81%
rename from gtsam/nonlinear/DynamicValues-inl.h
rename to gtsam/nonlinear/Values-inl.h
index 7686c2293..b49049a02 100644
--- a/gtsam/nonlinear/DynamicValues-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file DynamicValues.h
+ * @file Values.h
  * @author Richard Roberts
  *
  * @brief A non-templated config holding any types of Manifold-group elements
@@ -24,7 +24,7 @@
 
 #include <utility>
 
-#include <gtsam/nonlinear/DynamicValues.h> // Only so Eclipse finds class definition
+#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
 
 namespace gtsam {
 
@@ -39,17 +39,17 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<typename ValueType>
-  const ValueType& DynamicValues::at(const Symbol& j) const {
+  const ValueType& Values::at(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
     // Throw exception if it does not exist
     if(item == values_.end())
-      throw DynamicValuesKeyDoesNotExist("retrieve", j);
+      throw ValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
     if(typeid(*item->second) != typeid(ValueType))
-      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
+      throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
 
     // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
     return static_cast<const ValueType&>(*item->second);
@@ -57,7 +57,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<typename TypedKey>
-  const typename TypedKey::Value& DynamicValues::at(const TypedKey& j) const {
+  const typename TypedKey::Value& Values::at(const TypedKey& j) const {
     // Convert to Symbol
     const Symbol symbol(j.symbol());
 
@@ -67,14 +67,14 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<typename ValueType>
-  boost::optional<const ValueType&> DynamicValues::exists(const Symbol& j) const {
+  boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
     if(item != values_.end()) {
       // Check the type and throw exception if incorrect
       if(typeid(*item->second) != typeid(ValueType))
-        throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
+        throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
 
       // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
       return static_cast<const ValueType&>(*item->second);
@@ -85,7 +85,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class TypedKey>
-  boost::optional<const typename TypedKey::Value&> DynamicValues::exists(const TypedKey& j) const {
+  boost::optional<const typename TypedKey::Value&> Values::exists(const TypedKey& j) const {
     // Convert to Symbol
     const Symbol symbol(j.symbol());
 
diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/Values.cpp
similarity index 80%
rename from gtsam/nonlinear/DynamicValues.cpp
rename to gtsam/nonlinear/Values.cpp
index 57d760b59..c5190cbc8 100644
--- a/gtsam/nonlinear/DynamicValues.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file DynamicValues.h
+ * @file Values.h
  * @author Richard Roberts
  *
  * @brief A non-templated config holding any types of Manifold-group elements
@@ -22,7 +22,7 @@
  *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
  */
 
-#include <gtsam/nonlinear/DynamicValues.h>
+#include <gtsam/nonlinear/Values.h>
 
 #include <list>
 
@@ -35,12 +35,12 @@ using namespace std;
 namespace gtsam {
 
   /* ************************************************************************* */
-  DynamicValues::DynamicValues(const DynamicValues& other) {
+  Values::Values(const Values& other) {
     this->insert(other);
   }
 
   /* ************************************************************************* */
-  void DynamicValues::print(const string& str) const {
+  void Values::print(const string& str) const {
     cout << str << "Values with " << size() << " values:\n" << endl;
     for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
       cout << "  " << (string)key_value->first << ": ";
@@ -49,7 +49,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool DynamicValues::equals(const DynamicValues& other, double tol) const {
+  bool Values::equals(const Values& other, double tol) const {
     if(this->size() != other.size())
       return false;
     for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
@@ -64,18 +64,18 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool DynamicValues::exists(const Symbol& j) const {
+  bool Values::exists(const Symbol& j) const {
     return values_.find(j) != values_.end();
   }
 
   /* ************************************************************************* */
-  VectorValues DynamicValues::zeroVectors(const Ordering& ordering) const {
+  VectorValues Values::zeroVectors(const Ordering& ordering) const {
     return VectorValues::Zero(this->dims(ordering));
   }
 
   /* ************************************************************************* */
-  DynamicValues DynamicValues::retract(const VectorValues& delta, const Ordering& ordering) const {
-    DynamicValues result;
+  Values Values::retract(const VectorValues& delta, const Ordering& ordering) const {
+    Values result;
 
     for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
       const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
@@ -88,14 +88,14 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  VectorValues DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering) const {
+  VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const {
     VectorValues result(this->dims(ordering));
     localCoordinates(cp, ordering, result);
     return result;
   }
 
   /* ************************************************************************* */
-  void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const {
+  void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& result) const {
     if(this->size() != cp.size())
       throw DynamicValuesMismatched();
     for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
@@ -107,15 +107,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void DynamicValues::insert(const Symbol& j, const Value& val) {
+  void Values::insert(const Symbol& j, const Value& val) {
   	Symbol key = j; // Non-const duplicate to deal with non-const insert argument
   	std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
   	if(!insertResult.second)
-  		throw DynamicValuesKeyAlreadyExists(j);
+  		throw ValuesKeyAlreadyExists(j);
   }
 
   /* ************************************************************************* */
-  void DynamicValues::insert(const DynamicValues& values) {
+  void Values::insert(const Values& values) {
     for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
       Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
       insert(key, *key_value->second);
@@ -123,36 +123,36 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void DynamicValues::update(const Symbol& j, const Value& val) {
+  void Values::update(const Symbol& j, const Value& val) {
   	// Find the value to update
   	iterator item = values_.find(j);
   	if(item == values_.end())
-  		throw DynamicValuesKeyDoesNotExist("update", j);
+  		throw ValuesKeyDoesNotExist("update", j);
 
   	// Cast to the derived type
   	if(typeid(*item->second) != typeid(val))
-  		throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(val));
+  		throw ValuesIncorrectType(j, typeid(*item->second), typeid(val));
 
   	values_.replace(item, val.clone_());
   }
 
   /* ************************************************************************* */
-  void DynamicValues::update(const DynamicValues& values) {
+  void Values::update(const Values& values) {
     for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
       this->update(key_value->first, *key_value->second);
     }
   }
 
   /* ************************************************************************* */
-  void DynamicValues::erase(const Symbol& j) {
+  void Values::erase(const Symbol& j) {
     iterator item = values_.find(j);
     if(item == values_.end())
-      throw DynamicValuesKeyDoesNotExist("erase", j);
+      throw ValuesKeyDoesNotExist("erase", j);
     values_.erase(item);
   }
 
   /* ************************************************************************* */
-  FastList<Symbol> DynamicValues::keys() const {
+  FastList<Symbol> Values::keys() const {
     FastList<Symbol> result;
     for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value)
       result.push_back(key_value->first);
@@ -160,14 +160,14 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  DynamicValues& DynamicValues::operator=(const DynamicValues& rhs) {
+  Values& Values::operator=(const Values& rhs) {
     this->clear();
     this->insert(rhs);
     return *this;
   }
 
   /* ************************************************************************* */
-  vector<size_t> DynamicValues::dims(const Ordering& ordering) const {
+  vector<size_t> Values::dims(const Ordering& ordering) const {
 //    vector<size_t> result(values_.size());
 //    // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
 //    result.assign(
@@ -182,7 +182,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar) const {
+  Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
     Ordering::shared_ptr ordering(new Ordering);
     for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
       ordering->insert(key_value->first, firstVar++);
@@ -191,7 +191,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* DynamicValuesKeyAlreadyExists::what() const throw() {
+  const char* ValuesKeyAlreadyExists::what() const throw() {
     if(message_.empty())
       message_ =
           "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
@@ -199,7 +199,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* DynamicValuesKeyDoesNotExist::what() const throw() {
+  const char* ValuesKeyDoesNotExist::what() const throw() {
     if(message_.empty())
       message_ =
           "Attempting to " + std::string(operation_) + " the key \"" +
@@ -208,10 +208,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* DynamicValuesIncorrectType::what() const throw() {
+  const char* ValuesIncorrectType::what() const throw() {
     if(message_.empty())
       message_ =
-          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
+          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in Values is " +
           std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
     return message_.c_str();
   }
diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/Values.h
similarity index 88%
rename from gtsam/nonlinear/DynamicValues.h
rename to gtsam/nonlinear/Values.h
index 4f866749e..27da4d7e4 100644
--- a/gtsam/nonlinear/DynamicValues.h
+++ b/gtsam/nonlinear/Values.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file DynamicValues.h
+ * @file Values.h
  * @author Richard Roberts
  *
  * @brief A non-templated config holding any types of Manifold-group elements
@@ -63,7 +63,7 @@ namespace gtsam {
   * manifold element, and hence supports operations dim, retract, and
   * localCoordinates.
   */
-  class DynamicValues {
+  class Values {
 
   private:
 
@@ -91,11 +91,11 @@ namespace gtsam {
     typedef KeyValueMap::reverse_iterator reverse_iterator;
     typedef KeyValueMap::const_reverse_iterator const_reverse_iterator;
 
-    /** Default constructor creates an empty DynamicValues class */
-    DynamicValues() {}
+    /** Default constructor creates an empty Values class */
+    Values() {}
 
     /** Copy constructor duplicates all keys and values */
-    DynamicValues(const DynamicValues& other);
+    Values(const Values& other);
 
     /// @name Testable
     /// @{
@@ -104,7 +104,7 @@ namespace gtsam {
     void print(const std::string& str = "") const;
 
     /** Test whether the sets of keys and values are identical */
-    bool equals(const DynamicValues& other, double tol=1e-9) const;
+    bool equals(const Values& other, double tol=1e-9) const;
 
     /// @}
 
@@ -178,13 +178,13 @@ namespace gtsam {
     /// @{
 
     /** Add a delta config to current config and returns a new config */
-    DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const;
+    Values retract(const VectorValues& delta, const Ordering& ordering) const;
 
     /** Get a delta config about a linearization point c0 (*this) */
-    VectorValues localCoordinates(const DynamicValues& cp, const Ordering& ordering) const;
+    VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
 
     /** Get a delta config about a linearization point c0 (*this) */
-    void localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& delta) const;
+    void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
 
     ///@}
 
@@ -194,13 +194,13 @@ namespace gtsam {
     void insert(const Symbol& j, const Value& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
-    void insert(const DynamicValues& values);
+    void insert(const Values& values);
 
     /** single element change of existing element */
     void update(const Symbol& j, const Value& val);
 
     /** update the current available values without adding new ones */
-    void update(const DynamicValues& values);
+    void update(const Values& values);
 
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(const Symbol& j);
@@ -218,7 +218,7 @@ namespace gtsam {
     FastList<Symbol> keys() const;
 
     /** Replace all keys and variables */
-    DynamicValues& operator=(const DynamicValues& rhs);
+    Values& operator=(const Values& rhs);
 
     /** Remove all variables from the config */
     void clear() { values_.clear(); }
@@ -254,7 +254,7 @@ namespace gtsam {
   };
 
   /* ************************************************************************* */
-  class DynamicValuesKeyAlreadyExists : public std::exception {
+  class ValuesKeyAlreadyExists : public std::exception {
   protected:
     const Symbol key_; ///< The key that already existed
 
@@ -263,10 +263,10 @@ namespace gtsam {
 
   public:
     /// Construct with the key-value pair attemped to be added
-    DynamicValuesKeyAlreadyExists(const Symbol& key) throw() :
+    ValuesKeyAlreadyExists(const Symbol& key) throw() :
       key_(key) {}
 
-    virtual ~DynamicValuesKeyAlreadyExists() throw() {}
+    virtual ~ValuesKeyAlreadyExists() throw() {}
 
     /// The duplicate key that was attemped to be added
     const Symbol& key() const throw() { return key_; }
@@ -276,7 +276,7 @@ namespace gtsam {
   };
 
   /* ************************************************************************* */
-  class DynamicValuesKeyDoesNotExist : public std::exception {
+  class ValuesKeyDoesNotExist : public std::exception {
   protected:
     const char* operation_; ///< The operation that attempted to access the key
     const Symbol key_; ///< The key that does not exist
@@ -286,10 +286,10 @@ namespace gtsam {
 
   public:
     /// Construct with the key that does not exist in the values
-    DynamicValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
+    ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
       operation_(operation), key_(key) {}
 
-    virtual ~DynamicValuesKeyDoesNotExist() throw() {}
+    virtual ~ValuesKeyDoesNotExist() throw() {}
 
     /// The key that was attempted to be accessed that does not exist
     const Symbol& key() const throw() { return key_; }
@@ -299,7 +299,7 @@ namespace gtsam {
   };
 
   /* ************************************************************************* */
-  class DynamicValuesIncorrectType : public std::exception {
+  class ValuesIncorrectType : public std::exception {
   protected:
     const Symbol key_; ///< The key requested
     const std::type_info& storedTypeId_;
@@ -310,16 +310,16 @@ namespace gtsam {
 
   public:
     /// Construct with the key that does not exist in the values
-    DynamicValuesIncorrectType(const Symbol& key,
+    ValuesIncorrectType(const Symbol& key,
         const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
       key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
 
-    virtual ~DynamicValuesIncorrectType() throw() {}
+    virtual ~ValuesIncorrectType() throw() {}
 
     /// The key that was attempted to be accessed that does not exist
     const Symbol& key() const throw() { return key_; }
 
-    /// The typeid of the value stores in the DynamicValues
+    /// The typeid of the value stores in the Values
     const std::type_info& storedTypeId() const { return storedTypeId_; }
 
     /// The requested typeid
@@ -338,10 +338,10 @@ namespace gtsam {
     virtual ~DynamicValuesMismatched() throw() {}
 
     virtual const char* what() const throw() {
-      return "The Values 'this' and the argument passed to DynamicValues::localCoordinates have mismatched keys and values";
+      return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
     }
   };
 
 }
 
-#include <gtsam/nonlinear/DynamicValues-inl.h>
+#include <gtsam/nonlinear/Values-inl.h>
diff --git a/gtsam/nonlinear/ValuesOld-inl.h b/gtsam/nonlinear/ValuesOld-inl.h
deleted file mode 100644
index 5f53e32e8..000000000
--- a/gtsam/nonlinear/ValuesOld-inl.h
+++ /dev/null
@@ -1,231 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file ValuesOld.cpp
- * @author Richard Roberts
- */
-
-#pragma once
-
-#include <utility>
-#include <iostream>
-#include <stdexcept>
-
-#include <boost/foreach.hpp>
-#include <boost/tuple/tuple.hpp>
-
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/Lie-inl.h>
-#include <gtsam/nonlinear/Ordering.h>
-
-using namespace std;
-
-namespace gtsam {
-
-/* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::print(const string &s) const {
-       cout << "ValuesOld " << s << ", size " << values_.size() << "\n";
-       BOOST_FOREACH(const KeyValuePair& v, values_) {
-         gtsam::print(v.second, (string)(v.first));
-       }
-     }
-
-  /* ************************************************************************* */
-  template<class J>
-  bool ValuesOld<J>::equals(const ValuesOld<J>& expected, double tol) const {
-    if (values_.size() != expected.values_.size()) return false;
-    BOOST_FOREACH(const KeyValuePair& v, values_) {
-    	if (!expected.exists(v.first)) return false;
-      if(!gtsam::equal(v.second, expected[v.first], tol))
-        return false;
-    }
-    return true;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  const typename J::Value& ValuesOld<J>::at(const J& j) const {
-    const_iterator it = values_.find(j);
-    if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
-    else return it->second;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  size_t ValuesOld<J>::dim() const {
-  	size_t n = 0;
-  	BOOST_FOREACH(const KeyValuePair& value, values_)
-  		n += value.second.dim();
-  	return n;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  VectorValues ValuesOld<J>::zero(const Ordering& ordering) const {
-  	return VectorValues::Zero(this->dims(ordering));
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::insert(const J& name, const typename J::Value& val) {
-    if(!values_.insert(make_pair(name, val)).second)
-      throw KeyAlreadyExists<J>(name, val);
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::insert(const ValuesOld<J>& cfg) {
-	  BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
-		 insert(v.first, v.second);
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::update(const ValuesOld<J>& cfg) {
-	  BOOST_FOREACH(const KeyValuePair& v, values_) {
-	  	boost::optional<typename J::Value> t = cfg.exists_(v.first);
-	  	if (t) values_[v.first] = *t;
-	  }
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::update(const J& j, const typename J::Value& val) {
-	  	values_[j] = val;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  std::list<J> ValuesOld<J>::keys() const {
-	  std::list<J> ret;
-	  BOOST_FOREACH(const KeyValuePair& v, values_)
-		  ret.push_back(v.first);
-	  return ret;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::erase(const J& j) {
-    size_t dim; // unused
-    erase(j, dim);
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::erase(const J& j, size_t& dim) {
-    iterator it = values_.find(j);
-    if (it == values_.end())
-      throw KeyDoesNotExist<J>("erase", j);
-    dim = it->second.dim();
-    values_.erase(it);
-  }
-
-  /* ************************************************************************* */
-  // todo: insert for every element is inefficient
-  template<class J>
-  ValuesOld<J> ValuesOld<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
-		ValuesOld<J> newValues;
-		typedef pair<J,typename J::Value> KeyValue;
-		BOOST_FOREACH(const KeyValue& value, this->values_) {
-			const J& j = value.first;
-			const typename J::Value& pj = value.second;
-			Index index;
-			if(ordering.tryAt(j,index)) {
-			  newValues.insert(j, pj.retract(delta[index]));
-			} else
-			  newValues.insert(j, pj);
-		}
-		return newValues;
-	}
-
-  /* ************************************************************************* */
-  template<class J>
-  std::vector<size_t> ValuesOld<J>::dims(const Ordering& ordering) const {
-    _ValuesDimensionCollector dimCollector(ordering);
-    this->apply(dimCollector);
-    return dimCollector.dimensions;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  Ordering::shared_ptr ValuesOld<J>::orderingArbitrary(Index firstVar) const {
-    // Generate an initial key ordering in iterator order
-    _ValuesKeyOrderer keyOrderer(firstVar);
-    this->apply(keyOrderer);
-    return keyOrderer.ordering;
-  }
-
-//  /* ************************************************************************* */
-//  // todo: insert for every element is inefficient
-//  template<class J>
-//  ValuesOld<J> ValuesOld<J>::retract(const Vector& delta) const {
-//    if(delta.size() != dim()) {
-//    	cout << "ValuesOld::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
-//      throw invalid_argument("Delta vector length does not match config dimensionality.");
-//    }
-//    ValuesOld<J> newValues;
-//    int delta_offset = 0;
-//		typedef pair<J,typename J::Value> KeyValue;
-//		BOOST_FOREACH(const KeyValue& value, this->values_) {
-//			const J& j = value.first;
-//			const typename J::Value& pj = value.second;
-//      int cur_dim = pj.dim();
-//      newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim)));
-//      delta_offset += cur_dim;
-//    }
-//    return newValues;
-//  }
-
-  /* ************************************************************************* */
-  // todo: insert for every element is inefficient
-  // todo: currently only logmaps elements in both configs
-  template<class J>
-  inline VectorValues ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering) const {
-  	VectorValues delta(this->dims(ordering));
-  	localCoordinates(cp, ordering, delta);
-  	return delta;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering, VectorValues& delta) const {
-    typedef pair<J,typename J::Value> KeyValue;
-    BOOST_FOREACH(const KeyValue& value, cp) {
-      assert(this->exists(value.first));
-      delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second);
-    }
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  const char* KeyDoesNotExist<J>::what() const throw() {
-    if(message_.empty())
-      message_ = 
-          "Attempting to " + std::string(operation_) + " the key \"" +
-          (std::string)key_ + "\", which does not exist in the ValuesOld.";
-    return message_.c_str();
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  const char* KeyAlreadyExists<J>::what() const throw() {
-    if(message_.empty())
-      message_ = 
-          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
-    return message_.c_str();
-  }
-
-}
-
-
diff --git a/gtsam/nonlinear/ValuesOld.h b/gtsam/nonlinear/ValuesOld.h
deleted file mode 100644
index 88595a5dd..000000000
--- a/gtsam/nonlinear/ValuesOld.h
+++ /dev/null
@@ -1,301 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file ValuesOld.h
- * @author Richard Roberts
- *
- * @brief A templated config for Manifold-group elements
- *
- *  Detailed story:
- *  A values structure is a map from keys to values. It is used to specify the value of a bunch
- *  of variables in a factor graph. A ValuesOld is a values structure which can hold variables that
- *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
- *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
- */
-
-#pragma once
-
-#include <set>
-
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/FastMap.h>
-#include <gtsam/base/Vector.h>
-#include <gtsam/base/Manifold.h>
-#include <gtsam/nonlinear/Ordering.h>
-
-namespace boost { template<class T> class optional; }
-namespace gtsam { class VectorValues; class Ordering; }
-
-namespace gtsam {
-
-  // Forward declarations
-  template<class J> class KeyDoesNotExist;
-  template<class J> class KeyAlreadyExists;
-
-	/**
-	 * Manifold type values structure
-	 * Takes two template types
-	 *  J: a key type to look up values in the values structure (need to be sortable)
-	 *
-	 * Key concept:
-	 *  The key will be assumed to be sortable, and must have a
-	 *  typedef called "Value" with the type of the value the key
-	 *  labels (example: Pose2, Point2, etc)
-	 */
-  template<class J>
-  class ValuesOld {
-
-  public:
-
-    /**
-     * Typedefs
-     */
-  	typedef J Key;
-  	typedef typename J::Value Value;
-  	typedef std::map<Key, Value, std::less<Key>, boost::fast_pool_allocator<std::pair<const Key, Value> > > KeyValueMap;
-	//    typedef FastMap<J,Value> KeyValueMap;
-    typedef typename KeyValueMap::value_type KeyValuePair;
-    typedef typename KeyValueMap::iterator iterator;
-    typedef typename KeyValueMap::const_iterator const_iterator;
-
-  private:
-
-    /** concept check */
-    GTSAM_CONCEPT_TESTABLE_TYPE(Value)
-    GTSAM_CONCEPT_MANIFOLD_TYPE(Value)
-
-    KeyValueMap values_;
-
-  public:
-
-    ValuesOld() {}
-    ValuesOld(const ValuesOld& config) :
-      values_(config.values_) {}
-    template<class J_ALT>
-    ValuesOld(const ValuesOld<J_ALT>& other) {} // do nothing when initializing with wrong type
-    virtual ~ValuesOld() {}
-
-    /// @name Testable
-    /// @{
-
-    /** print */
-    void print(const std::string &s="") const;
-
-    /** Test whether configs are identical in keys and values */
-    bool equals(const ValuesOld& expected, double tol=1e-9) const;
-
-    /// @}
-
-    /** Retrieve a variable by j, throws KeyDoesNotExist<J> if not found */
-    const Value& at(const J& j) const;
-
-    /** operator[] syntax for get, throws KeyDoesNotExist<J> if not found */
-    const Value& operator[](const J& j) const {
-      return at(j); }
-
-    /** Check if a variable exists */
-    bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
-
-    /** Check if a variable exists and return it if so */
-    boost::optional<Value> exists_(const J& i) const {
-    	const_iterator it = values_.find(i);
-    	if (it==values_.end()) return boost::none; else	return it->second;
-    }
-
-    /** The number of variables in this config */
-    size_t size() const { return values_.size(); }
-
-    /** whether the config is empty */
-    bool empty() const { return values_.empty(); }
-
-    /** Get a zero VectorValues of the correct structure */
-    VectorValues zero(const Ordering& ordering) 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(); }
-
-    /// @name Manifold Operations
-    /// @{
-
-    /** The dimensionality of the tangent space */
-    size_t dim() const;
-
-    /** Add a delta config to current config and returns a new config */
-    ValuesOld retract(const VectorValues& delta, const Ordering& ordering) const;
-
-    /** Get a delta config about a linearization point c0 (*this) */
-    VectorValues localCoordinates(const ValuesOld& cp, const Ordering& ordering) const;
-
-    /** Get a delta config about a linearization point c0 (*this) */
-    void localCoordinates(const ValuesOld& cp, const Ordering& ordering, VectorValues& delta) const;
-
-    /// @}
-
-    // imperative methods:
-
-    /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
-    void insert(const J& j, const Value& val);
-
-    /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
-    void insert(const ValuesOld& cfg);
-
-    /** update the current available values without adding new ones */
-    void update(const ValuesOld& cfg);
-
-    /** single element change of existing element */
-    void update(const J& j, const Value& val);
-
-    /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
-    void erase(const J& j);
-
-    /** Remove a variable from the config while returning the dimensionality of
-     * the removed element (normally not needed by user code), throws
-     * KeyDoesNotExist<J> if j is not present.
-     */
-    void erase(const J& j, size_t& dim);
-
-    /**
-     * Returns a set of keys in the config
-     * Note: by construction, the list is ordered
-     */
-    std::list<J> keys() const;
-
-    /** Replace all keys and variables */
-    ValuesOld& operator=(const ValuesOld& rhs) {
-      values_ = rhs.values_;
-      return (*this);
-    }
-
-    /** Remove all variables from the config */
-    void clear() {
-      values_.clear();
-    }
-
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the ValuesOld, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      for(iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-      for(const_iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-
-    /** Create an array of variable dimensions using the given ordering */
-    std::vector<size_t> dims(const Ordering& ordering) const;
-
-    /**
-     * Generate a default ordering, simply in key sort order.  To instead
-     * create a fill-reducing ordering, use
-     * NonlinearFactorGraph::orderingCOLAMD().  Alternatively, you may permute
-     * this ordering yourself (as orderingCOLAMD() does internally).
-     */
-    Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
-
-  private:
-  	/** Serialization function */
-  	friend class boost::serialization::access;
-  	template<class ARCHIVE>
-  	void serialize(ARCHIVE & ar, const unsigned int version) {
-  		ar & BOOST_SERIALIZATION_NVP(values_);
-  	}
-
-  };
-
-  struct _ValuesDimensionCollector {
-    const Ordering& ordering;
-    std::vector<size_t> dimensions;
-    _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
-    template<typename I> void operator()(const I& key_value) {
-      Index var;
-      if(ordering.tryAt(key_value->first, var)) {
-        assert(var < dimensions.size());
-        dimensions[var] = key_value->second.dim();
-      }
-    }
-  };
-
-  /* ************************************************************************* */
-  struct _ValuesKeyOrderer {
-    Index var;
-    Ordering::shared_ptr ordering;
-    _ValuesKeyOrderer(Index firstVar) : var(firstVar), ordering(new Ordering) {}
-    template<typename I> void operator()(const I& key_value) {
-      ordering->insert(key_value->first, var);
-      ++ var;
-    }
-  };
-
-
-/* ************************************************************************* */
-template<class J>
-class KeyAlreadyExists : public std::exception {
-protected:
-  const J key_; ///< The key that already existed
-  const typename J::Value value_; ///< The value attempted to be inserted
-
-private:
-  mutable std::string message_;
-
-public:
-  /// Construct with the key-value pair attemped to be added
-  KeyAlreadyExists(const J& key, const typename J::Value& value) throw() :
-    key_(key), value_(value) {}
-
-  virtual ~KeyAlreadyExists() throw() {}
-
-  /// The duplicate key that was attemped to be added
-  const J& key() const throw() { return key_; }
-
-  /// The value that was attempted to be added
-  const typename J::Value& value() const throw() { return value_; }
-
-  /// The message to be displayed to the user
-  virtual const char* what() const throw();
-};
-
-/* ************************************************************************* */
-template<class J>
-class KeyDoesNotExist : public std::exception {
-protected:
-  const char* operation_; ///< The operation that attempted to access the key
-  const J key_; ///< The key that does not exist
-
-private:
-  mutable std::string message_;
-
-public:
-  /// Construct with the key that does not exist in the values
-  KeyDoesNotExist(const char* operation, const J& key) throw() :
-    operation_(operation), key_(key) {}
-
-  virtual ~KeyDoesNotExist() throw() {}
-
-  /// The key that was attempted to be accessed that does not exist
-  const J& key() const throw() { return key_; }
-
-  /// The message to be displayed to the user
-  virtual const char* what() const throw();
-};
-
-} // \namespace gtsam
-
-#include <gtsam/nonlinear/ValuesOld-inl.h>
-
diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp
deleted file mode 100644
index 51ffd0320..000000000
--- a/gtsam/nonlinear/tests/testDynamicValues.cpp
+++ /dev/null
@@ -1,265 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file testDynamicValues.cpp
- * @author Richard Roberts
- */
-
-#include <CppUnitLite/TestHarness.h>
-#include <stdexcept>
-#include <limits>
-#include <boost/assign/std/list.hpp> // for operator +=
-using namespace boost::assign;
-
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/nonlinear/DynamicValues.h>
-
-using namespace gtsam;
-using namespace std;
-static double inf = std::numeric_limits<double>::infinity();
-
-typedef TypedSymbol<LieVector, 'v'> VecKey;
-
-VecKey key1(1), key2(2), key3(3), key4(4);
-
-/* ************************************************************************* */
-TEST( DynamicValues, equals1 )
-{
-  DynamicValues expected;
-  LieVector v(3, 5.0, 6.0, 7.0);
-  expected.insert(key1,v);
-  DynamicValues actual;
-  actual.insert(key1,v);
-  CHECK(assert_equal(expected,actual));
-}
-
-/* ************************************************************************* */
-TEST( DynamicValues, equals2 )
-{
-  DynamicValues cfg1, cfg2;
-  LieVector v1(3, 5.0, 6.0, 7.0);
-  LieVector v2(3, 5.0, 6.0, 8.0);
-  cfg1.insert(key1, v1);
-  cfg2.insert(key1, v2);
-  CHECK(!cfg1.equals(cfg2));
-  CHECK(!cfg2.equals(cfg1));
-}
-
-/* ************************************************************************* */
-TEST( DynamicValues, equals_nan )
-{
-  DynamicValues cfg1, cfg2;
-  LieVector v1(3, 5.0, 6.0, 7.0);
-  LieVector v2(3, inf, inf, inf);
-  cfg1.insert(key1, v1);
-  cfg2.insert(key1, v2);
-  CHECK(!cfg1.equals(cfg2));
-  CHECK(!cfg2.equals(cfg1));
-}
-
-/* ************************************************************************* */
-TEST( DynamicValues, insert_good )
-{
-  DynamicValues cfg1, cfg2, expected;
-  LieVector v1(3, 5.0, 6.0, 7.0);
-  LieVector v2(3, 8.0, 9.0, 1.0);
-  LieVector v3(3, 2.0, 4.0, 3.0);
-  LieVector v4(3, 8.0, 3.0, 7.0);
-  cfg1.insert(key1, v1);
-  cfg1.insert(key2, v2);
-  cfg2.insert(key3, v4);
-
-  cfg1.insert(cfg2);
-
-  expected.insert(key1, v1);
-  expected.insert(key2, v2);
-  expected.insert(key3, v4);
-
-  CHECK(assert_equal(expected, cfg1));
-}
-
-/* ************************************************************************* */
-TEST( DynamicValues, insert_bad )
-{
-  DynamicValues cfg1, cfg2;
-  LieVector v1(3, 5.0, 6.0, 7.0);
-  LieVector v2(3, 8.0, 9.0, 1.0);
-  LieVector v3(3, 2.0, 4.0, 3.0);
-  LieVector v4(3, 8.0, 3.0, 7.0);
-  cfg1.insert(key1, v1);
-  cfg1.insert(key2, v2);
-  cfg2.insert(key2, v3);
-  cfg2.insert(key3, v4);
-
-  CHECK_EXCEPTION(cfg1.insert(cfg2), DynamicValuesKeyAlreadyExists);
-}
-
-/* ************************************************************************* */
-TEST( DynamicValues, update_element )
-{
-  DynamicValues cfg;
-  LieVector v1(3, 5.0, 6.0, 7.0);
-  LieVector v2(3, 8.0, 9.0, 1.0);
-
-  cfg.insert(key1, v1);
-  CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v1, cfg.at(key1)));
-
-  cfg.update(key1, v2);
-  CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v2, cfg.at(key1)));
-}
-
-///* ************************************************************************* */
-//TEST(DynamicValues, dim_zero)
-//{
-//  DynamicValues config0;
-//  config0.insert(key1, LieVector(2, 2.0, 3.0));
-//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-//  LONGS_EQUAL(5, config0.dim());
-//
-//  VectorValues expected;
-//  expected.insert(key1, zero(2));
-//  expected.insert(key2, zero(3));
-//  CHECK(assert_equal(expected, config0.zero()));
-//}
-
-/* ************************************************************************* */
-TEST(DynamicValues, expmap_a)
-{
-  DynamicValues config0;
-  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-
-  Ordering ordering(*config0.orderingArbitrary());
-  VectorValues increment(config0.dims(ordering));
-  increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
-  increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
-
-  DynamicValues expected;
-  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
-  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
-
-  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
-}
-
-/* ************************************************************************* */
-TEST(DynamicValues, expmap_b)
-{
-  DynamicValues config0;
-  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-
-  Ordering ordering(*config0.orderingArbitrary());
-  VectorValues increment(VectorValues::Zero(config0.dims(ordering)));
-  increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
-
-  DynamicValues expected;
-  expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
-
-  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
-}
-
-/* ************************************************************************* */
-//TEST(DynamicValues, expmap_c)
-//{
-//  DynamicValues config0;
-//  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-//
-//  Vector increment = LieVector(6,
-//      1.0, 1.1, 1.2,
-//      1.3, 1.4, 1.5);
-//
-//  DynamicValues expected;
-//  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
-//  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
-//
-//  CHECK(assert_equal(expected, config0.retract(increment)));
-//}
-
-/* ************************************************************************* */
-TEST(DynamicValues, expmap_d)
-{
-  DynamicValues config0;
-  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-  //config0.print("config0");
-  CHECK(equal(config0, config0));
-  CHECK(config0.equals(config0));
-
-  typedef TypedSymbol<Pose2, 'p'> PoseKey;
-  DynamicValues poseconfig;
-  poseconfig.insert(PoseKey(1), Pose2(1,2,3));
-  poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
-
-  CHECK(equal(config0, config0));
-  CHECK(config0.equals(config0));
-}
-
-/* ************************************************************************* */
-TEST(DynamicValues, extract_keys)
-{
-	typedef TypedSymbol<Pose2, 'x'> PoseKey;
-	DynamicValues config;
-
-	config.insert(PoseKey(1), Pose2());
-	config.insert(PoseKey(2), Pose2());
-	config.insert(PoseKey(4), Pose2());
-	config.insert(PoseKey(5), Pose2());
-
-	FastList<Symbol> expected, actual;
-	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
-	actual = config.keys();
-
-	CHECK(actual.size() == expected.size());
-	FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
-	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
-		CHECK(assert_equal(*itExp, *itAct));
-	}
-}
-
-/* ************************************************************************* */
-TEST(DynamicValues, exists_)
-{
-	DynamicValues config0;
-	config0.insert(key1, LieVector(Vector_(1, 1.)));
-	config0.insert(key2, LieVector(Vector_(1, 2.)));
-
-	boost::optional<const LieVector&> v = config0.exists(key1);
-	CHECK(assert_equal(Vector_(1, 1.),*v));
-}
-
-/* ************************************************************************* */
-TEST(DynamicValues, update)
-{
-	DynamicValues config0;
-	config0.insert(key1, LieVector(1, 1.));
-	config0.insert(key2, LieVector(1, 2.));
-
-	DynamicValues superset;
-	superset.insert(key1, LieVector(1, -1.));
-	superset.insert(key2, LieVector(1, -2.));
-	config0.update(superset);
-
-	DynamicValues expected;
-	expected.insert(key1, LieVector(1, -1.));
-	expected.insert(key2, LieVector(1, -2.));
-	CHECK(assert_equal(expected,config0));
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */
diff --git a/gtsam/nonlinear/tests/testNonlinearFactor.cpp b/gtsam/nonlinear/tests/testNonlinearFactor.cpp
index 558c40036..6a210d6ed 100644
--- a/gtsam/nonlinear/tests/testNonlinearFactor.cpp
+++ b/gtsam/nonlinear/tests/testNonlinearFactor.cpp
@@ -76,7 +76,7 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor4) {
   TestFactor4 tf;
-  DynamicValues tv;
+  Values tv;
   tv.insert(TestKey(1), LieVector(1, 1.0));
   tv.insert(TestKey(2), LieVector(1, 2.0));
   tv.insert(TestKey(3), LieVector(1, 3.0));
@@ -124,7 +124,7 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor5) {
   TestFactor5 tf;
-  DynamicValues tv;
+  Values tv;
   tv.insert(TestKey(1), LieVector(1, 1.0));
   tv.insert(TestKey(2), LieVector(1, 2.0));
   tv.insert(TestKey(3), LieVector(1, 3.0));
@@ -177,7 +177,7 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor6) {
   TestFactor6 tf;
-  DynamicValues tv;
+  Values tv;
   tv.insert(TestKey(1), LieVector(1, 1.0));
   tv.insert(TestKey(2), LieVector(1, 2.0));
   tv.insert(TestKey(3), LieVector(1, 3.0));
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 3bf76d681..c66e59ead 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file testValues.cpp
+ * @file testDynamicValues.cpp
  * @author Richard Roberts
  */
 
@@ -22,6 +22,7 @@ using namespace boost::assign;
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/LieVector.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
@@ -29,27 +30,26 @@ using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
 typedef TypedSymbol<LieVector, 'v'> VecKey;
-typedef Values<VecKey> TestValues;
 
 VecKey key1(1), key2(2), key3(3), key4(4);
 
 /* ************************************************************************* */
-TEST( TestValues, equals1 )
+TEST( Values, equals1 )
 {
-  TestValues expected;
-  Vector v = Vector_(3, 5.0, 6.0, 7.0);
+  Values expected;
+  LieVector v(3, 5.0, 6.0, 7.0);
   expected.insert(key1,v);
-  TestValues actual;
+  Values actual;
   actual.insert(key1,v);
   CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( TestValues, equals2 )
+TEST( Values, equals2 )
 {
-  TestValues cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
+  Values cfg1, cfg2;
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 5.0, 6.0, 8.0);
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -57,11 +57,11 @@ TEST( TestValues, equals2 )
 }
 
 /* ************************************************************************* */
-TEST( TestValues, equals_nan )
+TEST( Values, equals_nan )
 {
-  TestValues cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, inf, inf, inf);
+  Values cfg1, cfg2;
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, inf, inf, inf);
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -69,13 +69,13 @@ TEST( TestValues, equals_nan )
 }
 
 /* ************************************************************************* */
-TEST( TestValues, insert_good )
+TEST( Values, insert_good )
 {
-  TestValues cfg1, cfg2, expected;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  Values cfg1, cfg2, expected;
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
+  LieVector v4(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key3, v4);
@@ -86,31 +86,31 @@ TEST( TestValues, insert_good )
   expected.insert(key2, v2);
   expected.insert(key3, v4);
 
-  CHECK(assert_equal(cfg1, expected));
+  CHECK(assert_equal(expected, cfg1));
 }
 
 /* ************************************************************************* */
-TEST( TestValues, insert_bad )
+TEST( Values, insert_bad )
 {
-  TestValues cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  Values cfg1, cfg2;
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
+  LieVector v4(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key2, v3);
   cfg2.insert(key3, v4);
 
-  CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
+  CHECK_EXCEPTION(cfg1.insert(cfg2), ValuesKeyAlreadyExists);
 }
 
 /* ************************************************************************* */
-TEST( TestValues, update_element )
+TEST( Values, update_element )
 {
-  TestValues cfg;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  Values cfg;
+  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
@@ -122,12 +122,12 @@ TEST( TestValues, update_element )
 }
 
 ///* ************************************************************************* */
-//TEST(TestValues, dim_zero)
+//TEST(Values, dim_zero)
 //{
-//  TestValues config0;
-//  config0.insert(key1, Vector_(2, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
-//  LONGS_EQUAL(5,config0.dim());
+//  Values config0;
+//  config0.insert(key1, LieVector(2, 2.0, 3.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+//  LONGS_EQUAL(5, config0.dim());
 //
 //  VectorValues expected;
 //  expected.insert(key1, zero(2));
@@ -136,151 +136,130 @@ TEST( TestValues, update_element )
 //}
 
 /* ************************************************************************* */
-TEST(TestValues, expmap_a)
+TEST(Values, expmap_a)
 {
-  TestValues config0;
-  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  Values config0;
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
   Ordering ordering(*config0.orderingArbitrary());
   VectorValues increment(config0.dims(ordering));
   increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
   increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
 
-  TestValues expected;
-  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
-  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+  Values expected;
+  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 
   CHECK(assert_equal(expected, config0.retract(increment, ordering)));
 }
 
-///* ************************************************************************* */
-//TEST(TestValues, expmap_b)
-//{
-//  TestValues config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
-//
-//  Ordering ordering(*config0.orderingArbitrary());
-//  VectorValues increment(config0.dims(ordering));
-//  increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
-//
-//  TestValues expected;
-//  expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
-//
-//  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
-//}
+/* ************************************************************************* */
+TEST(Values, expmap_b)
+{
+  Values config0;
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
-///* ************************************************************************* */
-//TEST(TestValues, expmap_c)
+  Ordering ordering(*config0.orderingArbitrary());
+  VectorValues increment(VectorValues::Zero(config0.dims(ordering)));
+  increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
+
+  Values expected;
+  expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
+}
+
+/* ************************************************************************* */
+//TEST(Values, expmap_c)
 //{
-//  TestValues config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//  Values config0;
+//  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 //
-//  Vector increment = Vector_(6,
+//  Vector increment = LieVector(6,
 //      1.0, 1.1, 1.2,
 //      1.3, 1.4, 1.5);
 //
-//  TestValues expected;
-//  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+//  Values expected;
+//  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
+//  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 //
 //  CHECK(assert_equal(expected, config0.retract(increment)));
 //}
 
 /* ************************************************************************* */
-/*TEST(TestValues, expmap_d)
+TEST(Values, expmap_d)
 {
-  TestValues config0;
-  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  Values config0;
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
 
-  TestValues<string,Pose2> poseconfig;
-  poseconfig.insert("p1", Pose2(1,2,3));
-  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
-  //poseconfig.print("poseconfig");
+  typedef TypedSymbol<Pose2, 'p'> PoseKey;
+  Values poseconfig;
+  poseconfig.insert(PoseKey(1), Pose2(1,2,3));
+  poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
+
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
-}*/
+}
 
 /* ************************************************************************* */
-/*TEST(TestValues, extract_keys)
+TEST(Values, extract_keys)
 {
 	typedef TypedSymbol<Pose2, 'x'> PoseKey;
-	TestValues<PoseKey, Pose2> config;
+	Values config;
 
 	config.insert(PoseKey(1), Pose2());
 	config.insert(PoseKey(2), Pose2());
 	config.insert(PoseKey(4), Pose2());
 	config.insert(PoseKey(5), Pose2());
 
-	list<PoseKey> expected, actual;
+	FastList<Symbol> expected, actual;
 	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
-	list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
 	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
 		CHECK(assert_equal(*itExp, *itAct));
 	}
-}*/
+}
 
 /* ************************************************************************* */
-TEST(TestValues, exists_)
+TEST(Values, exists_)
 {
-	TestValues config0;
-	config0.insert(key1, Vector_(1, 1.));
-	config0.insert(key2, Vector_(1, 2.));
+	Values config0;
+	config0.insert(key1, LieVector(Vector_(1, 1.)));
+	config0.insert(key2, LieVector(Vector_(1, 2.)));
 
-	boost::optional<LieVector> v = config0.exists_(key1);
+	boost::optional<const LieVector&> v = config0.exists(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
 }
 
 /* ************************************************************************* */
-TEST(TestValues, update)
+TEST(Values, update)
 {
-	TestValues config0;
-	config0.insert(key1, Vector_(1, 1.));
-	config0.insert(key2, Vector_(1, 2.));
+	Values config0;
+	config0.insert(key1, LieVector(1, 1.));
+	config0.insert(key2, LieVector(1, 2.));
 
-	TestValues superset;
-	superset.insert(key1, Vector_(1, -1.));
-	superset.insert(key2, Vector_(1, -2.));
-	superset.insert(key3, Vector_(1, -3.));
+	Values superset;
+	superset.insert(key1, LieVector(1, -1.));
+	superset.insert(key2, LieVector(1, -2.));
 	config0.update(superset);
 
-	TestValues expected;
-	expected.insert(key1, Vector_(1, -1.));
-	expected.insert(key2, Vector_(1, -2.));
+	Values expected;
+	expected.insert(key1, LieVector(1, -1.));
+	expected.insert(key2, LieVector(1, -2.));
 	CHECK(assert_equal(expected,config0));
 }
 
-/* ************************************************************************* */
-TEST(TestValues, dummy_initialization)
-{
-	typedef TypedSymbol<LieVector, 'z'> Key;
-	typedef Values<Key> Values1;
-
-	Values1 init1;
-	init1.insert(Key(1), Vector_(2, 1.0, 2.0));
-	init1.insert(Key(2), Vector_(2, 4.0, 3.0));
-
-	TestValues init2;
-	init2.insert(key1, Vector_(2, 1.0, 2.0));
-	init2.insert(key2, Vector_(2, 4.0, 3.0));
-
-	Values1 actual1(init2);
-	TestValues actual2(init1);
-
-	EXPECT(actual1.empty());
-	EXPECT(actual2.empty());
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index 274c0d83d..f3df76774 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<KEY> {
 			boost::none) const = 0;
 
 	/** active when constraint *NOT* met */
-	bool active(const DynamicValues& c) const {
+	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
 		double x = value(c[this->key_]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
 			boost::optional<Matrix&> H2 = boost::none) const = 0;
 
 	/** active when constraint *NOT* met */
-	bool active(const DynamicValues& c) const {
+	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
 		double x = value(c[this->key1_], c[this->key2_]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index d94749b7f..1428d9d8b 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -30,7 +30,7 @@ using namespace gtsam;
 #define LINESIZE 81920
 
 typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
-typedef boost::shared_ptr<DynamicValues> sharedPose2Values;
+typedef boost::shared_ptr<Values> sharedPose2Values;
 
 namespace gtsam {
 
@@ -81,7 +81,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 		exit(-1);
 	}
 
-	sharedPose2Values poses(new DynamicValues);
+	sharedPose2Values poses(new Values);
 	sharedPose2Graph graph(new Pose2Graph);
 
 	string tag;
@@ -149,14 +149,14 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 }
 
 /* ************************************************************************* */
-void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDiagonal model,
+void save2D(const Pose2Graph& graph, const Values& config, const SharedDiagonal model,
 		const string& filename) {
-//	typedef DynamicValues::Key Key;
+//	typedef Values::Key Key;
 
 	fstream stream(filename.c_str(), fstream::out);
 
 	// save poses
-	for (DynamicValues::const_iterator it = config.begin(); it != config.end(); ++it) {
+	for (Values::const_iterator it = config.begin(); it != config.end(); ++it) {
 		Pose2 pose = config.at<Pose2>(it->first);
 		stream << "VERTEX2 " << it->first.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
 	}
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 2b339db18..7dafc58b4 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -41,16 +41,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
  * @param maxID, if non-zero cut out vertices >= maxID
  * @param smart: try to reduce complexity of covariance to cheapest model
  */
-std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
+std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<Values> > load2D(
 		std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
 		int maxID = 0, bool addNoise=false, bool smart=true);
-std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
+std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<Values> > load2D(
 		const std::string& filename,
 		boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
 		int maxID = 0, bool addNoise=false, bool smart=true);
 
 /** save 2d graph */
-void save2D(const gtsam::Pose2Graph& graph, const DynamicValues& config, const gtsam::SharedDiagonal model,
+void save2D(const gtsam::Pose2Graph& graph, const Values& config, const gtsam::SharedDiagonal model,
 		const std::string& filename);
 
 /**
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 15f9ff61f..1ba91c4af 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -40,37 +40,6 @@ namespace gtsam {
 		/// Typedef for a PointKey with Point2 data and 'l' symbol
 		typedef TypedSymbol<Point2, 'l'> PointKey;
 
-		/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
-		struct Values: public DynamicValues {
-
-			/// Default constructor
-			Values() {}
-
-			/// Copy constructor
-			Values(const DynamicValues& values) :
-				DynamicValues(values) {
-			}
-
-			/// From sub-values
-//			Values(const DynamicValues& poses, const DynamicValues& points) :
-//				DynamicValues(poses, points) {
-//			}
-
-			// Convenience for MATLAB wrapper, which does not allow for identically named methods
-
-			/// get a pose
-	    Pose2 pose(int key) const {	return (*this)[PoseKey(key)]; }
-
-	    /// get a point
-	    Point2 point(int key) const {	return (*this)[PointKey(key)]; }
-
-			/// insert a pose
-		  void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
-
-			/// insert a point
-		  void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
-		};
-
 		/**
 		 * List of typedefs for factors
 		 */
@@ -120,7 +89,7 @@ namespace gtsam {
 					const Rot2& bearing, double range, const SharedNoiseModel& model);
 
 			/// Optimize
-			Values optimize(const DynamicValues& initialEstimate) {
+			Values optimize(const Values& initialEstimate) {
 				typedef NonlinearOptimizer<Graph> Optimizer;
 				return *Optimizer::optimizeLM(*this, initialEstimate,
 										NonlinearOptimizationParameters::LAMBDA);
diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp
index 090607d72..7fc1e8516 100644
--- a/gtsam/slam/pose2SLAM.cpp
+++ b/gtsam/slam/pose2SLAM.cpp
@@ -27,8 +27,8 @@ namespace gtsam {
 	namespace pose2SLAM {
 
 		/* ************************************************************************* */
-		DynamicValues circle(size_t n, double R) {
-			DynamicValues x;
+		Values circle(size_t n, double R) {
+			Values x;
 			double theta = 0, dtheta = 2 * M_PI / n;
 			for (size_t i = 0; i < n; i++, theta += dtheta)
 				x.insert(Key(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index cc688c597..dfa4e7fa3 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -43,7 +43,7 @@ namespace gtsam {
 		 * @param c character to use for keys
 		 * @return circle of n 2D poses
 		 */
-		DynamicValues circle(size_t n, double R);
+		Values circle(size_t n, double R);
 
 		/// A prior factor on Key with Pose2 data type.
 		typedef PriorFactor<Key> Prior;
diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp
index 29eb14ba9..4d063c12b 100644
--- a/gtsam/slam/pose3SLAM.cpp
+++ b/gtsam/slam/pose3SLAM.cpp
@@ -25,8 +25,8 @@ namespace gtsam {
 	namespace pose3SLAM {
 
 		/* ************************************************************************* */
-		DynamicValues circle(size_t n, double radius) {
-			DynamicValues x;
+		Values circle(size_t n, double radius) {
+			Values x;
 			double theta = 0, dtheta = 2 * M_PI / n;
 			// We use aerospace/navlab convention, X forward, Y right, Z down
 			// First pose will be at (R,0,0)
diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h
index 544dcb5f0..686f6bd0f 100644
--- a/gtsam/slam/pose3SLAM.h
+++ b/gtsam/slam/pose3SLAM.h
@@ -39,7 +39,7 @@ namespace gtsam {
 		 * @param R radius of circle
 		 * @return circle of n 3D poses
 		 */
-		DynamicValues circle(size_t n, double R);
+		Values circle(size_t n, double R);
 
 		/// A prior factor on Key with Pose3 data type.
 		typedef PriorFactor<Key> Prior;
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index 291bde25e..c0cab4078 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -32,58 +32,6 @@ namespace gtsam {
 		typedef TypedSymbol<Point2, 'x'> PoseKey;
 		typedef TypedSymbol<Point2, 'l'> PointKey;
 
-		/**
-		 *  Custom Values class that holds poses and points
-		 */
-		class Values: public DynamicValues {
-			size_t nrPoses_;
-			size_t nrPoints_;
-		public:
-			typedef DynamicValues Base;  ///< base class
-			typedef boost::shared_ptr<Point2> sharedPoint;  ///< shortcut to shared Point type
-
-			/// Constructor
-			Values() : nrPoses_(0), nrPoints_(0) {
-			}
-
-			/// Copy constructor
-			Values(const Base& base) :
-					Base(base), nrPoses_(0), nrPoints_(0) {
-			}
-
-			/// Insert a pose
-			void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
-				insert(i, p);
-				nrPoses_++;
-			}
-
-			/// Insert a point
-			void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
-				insert(j, p);
-				nrPoints_++;
-			}
-
-			/// Number of poses
-			int nrPoses() const {
-				return nrPoses_;
-			}
-
-			/// Number of points
-			int nrPoints() const {
-				return nrPoints_;
-			}
-
-			/// Return pose i
-			Point2 pose(const simulated2D::PoseKey& i) const {
-				return (*this)[i];
-			}
-
-			/// Return point j
-			Point2 point(const simulated2D::PointKey& j) const {
-				return (*this)[j];
-			}
-		};
-
 		/// Prior on a single pose
 		inline Point2 prior(const Point2& x) {
 			return x;
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index d54924c12..28770d6d9 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -32,30 +32,6 @@ namespace gtsam {
 		typedef TypedSymbol<Point2, 'l'> PointKey;
 		typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
-		/// Specialized Values structure with syntactic sugar for
-		/// compatibility with matlab
-		class Values: public DynamicValues {
-			int nrPoses_, nrPoints_;
-		public:
-			Values() : nrPoses_(0), nrPoints_(0) {}
-
-			void insertPose(const PoseKey& i, const Pose2& p) {
-				insert(i, p);
-				nrPoses_++;
-			}
-
-			void insertPoint(const PointKey& j, const Point2& p) {
-				insert(j, p);
-				nrPoints_++;
-			}
-
-			int nrPoses() const {	return nrPoses_;	}
-			int nrPoints() const { return nrPoints_;	}
-
-			Pose2 pose(const PoseKey& i) const { return (*this)[i];	}
-			Point2 point(const PointKey& j) const { return (*this)[j]; }
-		};
-
 		//TODO:: point prior is not implemented right now
 
 		/// Prior on a single pose
diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h
index 2380d86f2..96dcbaf09 100644
--- a/gtsam/slam/smallExample.h
+++ b/gtsam/slam/smallExample.h
@@ -28,7 +28,6 @@
 namespace gtsam {
 	namespace example {
 
-		typedef simulated2D::Values Values;
 		typedef NonlinearFactorGraph Graph;
 
 		/**
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index a62122705..8291b17ca 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, error ) {
 	boost::shared_ptr<Projection>
 	factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
 	// For the following configuration, the factor predicts 320,240
-	DynamicValues values;
+	Values values;
 	Rot3 R;
 	Point3 t1(0,0,-6);
 	Pose3 x1(R,t1);
@@ -170,7 +170,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 
 	// add initial
 	const double noise = baseline*0.1;
-	boost::shared_ptr<DynamicValues> values(new DynamicValues);
+	boost::shared_ptr<Values> values(new Values);
 	for ( size_t i = 0 ; i < X.size() ; ++i )
 	  values->insert(CameraKey((int)i), X[i]) ;
 
@@ -209,7 +209,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
@@ -255,7 +255,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
@@ -298,7 +298,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i ) {
     const double
       rot_noise = 1e-5,
@@ -359,7 +359,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 89b5e5f34..65126d3d0 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, error ) {
   boost::shared_ptr<Projection>
   factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
   // For the following configuration, the factor predicts 320,240
-  DynamicValues values;
+  Values values;
   Rot3 R;
   Point3 t1(0,0,-6);
   Pose3 x1(R,t1);
@@ -170,7 +170,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
@@ -209,7 +209,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
@@ -255,7 +255,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
@@ -298,7 +298,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i ) {
     const double
       rot_noise = 1e-5, trans_noise = 1e-3,
@@ -355,7 +355,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<DynamicValues> values(new DynamicValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
     values->insert(CameraKey((int)i), X[i]) ;
 
diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp
index 3241a1e99..a74f79d49 100644
--- a/gtsam/slam/tests/testPlanarSLAM.cpp
+++ b/gtsam/slam/tests/testPlanarSLAM.cpp
@@ -51,7 +51,7 @@ TEST( planarSLAM, BearingFactor )
 	planarSLAM::Bearing factor(2, 3, z, sigma);
 
 	// create config
-	DynamicValues c;
+	Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -79,7 +79,7 @@ TEST( planarSLAM, RangeFactor )
 	planarSLAM::Range factor(2, 3, z, sigma);
 
 	// create config
-	DynamicValues c;
+	Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -106,7 +106,7 @@ TEST( planarSLAM, BearingRangeFactor )
 	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
 
 	// create config
-	DynamicValues c;
+	Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -139,7 +139,7 @@ TEST( planarSLAM, PoseConstraint_equals )
 TEST( planarSLAM, constructor )
 {
 	// create config
-	DynamicValues c;
+	Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PoseKey(3), x3);
 	c.insert(PointKey(3), l3);
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index c17c20348..84878e869 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -116,7 +116,7 @@ TEST( Pose2SLAM, linearization )
 	// 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)
-	DynamicValues config;
+	Values config;
 	config.insert(Key(1),p1);
 	config.insert(Key(2),p2);
 	// Linearize
@@ -152,7 +152,7 @@ TEST(Pose2Graph, optimize) {
   fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
-  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  boost::shared_ptr<Values> initial(new Values());
   initial->insert(Key(0), Pose2(0,0,0));
   initial->insert(Key(1), Pose2(0,0,0));
 
@@ -166,7 +166,7 @@ TEST(Pose2Graph, optimize) {
   Optimizer optimizer = optimizer0.levenbergMarquardt();
 
   // Check with expected config
-  DynamicValues expected;
+  Values expected;
   expected.insert(Key(0), Pose2(0,0,0));
   expected.insert(Key(1), Pose2(1,2,M_PI_2));
   CHECK(assert_equal(expected, *optimizer.values()));
@@ -177,7 +177,7 @@ TEST(Pose2Graph, optimize) {
 TEST(Pose2Graph, optimizeThreePoses) {
 
 	// Create a hexagon of poses
-	DynamicValues hexagon = pose2SLAM::circle(3,1.0);
+	Values hexagon = pose2SLAM::circle(3,1.0);
   Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
@@ -189,7 +189,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
   fg->addConstraint(2, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  boost::shared_ptr<Values> initial(new Values());
   initial->insert(Key(0), p0);
   initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
   initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
@@ -203,7 +203,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  DynamicValues actual = *optimizer.values();
+  Values actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual));
@@ -214,7 +214,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
 TEST_UNSAFE(Pose2Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
-	DynamicValues hexagon = pose2SLAM::circle(6,1.0);
+	Values hexagon = pose2SLAM::circle(6,1.0);
   Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
@@ -229,7 +229,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
   fg->addConstraint(5, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  boost::shared_ptr<Values> initial(new Values());
   initial->insert(Key(0), p0);
   initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
   initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
@@ -246,7 +246,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  DynamicValues actual = *optimizer.values();
+  Values actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual));
@@ -280,7 +280,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
 //
 //  myOptimizer.update(x);
 //
-//  DynamicValues expected;
+//  Values expected;
 //  expected.insert(0, Pose2(0.,0.,0.));
 //  expected.insert(1, Pose2(1.,0.,0.));
 //  expected.insert(2, Pose2(2.,0.,0.));
@@ -341,38 +341,38 @@ TEST(Pose2Graph, optimize2) {
 using namespace pose2SLAM;
 
 /* ************************************************************************* */
-TEST( DynamicValues, pose2Circle )
+TEST( Values, pose2Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	DynamicValues expected;
+	Values expected;
 	expected.insert(Key(0), Pose2( 1,  0,   M_PI_2));
 	expected.insert(Key(1), Pose2( 0,  1, - M_PI  ));
 	expected.insert(Key(2), Pose2(-1,  0, - M_PI_2));
 	expected.insert(Key(3), Pose2( 0, -1,   0     ));
 
-	DynamicValues actual = pose2SLAM::circle(4,1.0);
+	Values actual = pose2SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( DynamicValues, expmap )
+TEST( Values, expmap )
 {
 	// expected is circle shifted to right
-	DynamicValues expected;
+	Values expected;
 	expected.insert(Key(0), Pose2( 1.1,  0,   M_PI_2));
 	expected.insert(Key(1), Pose2( 0.1,  1, - M_PI  ));
 	expected.insert(Key(2), Pose2(-0.9,  0, - M_PI_2));
 	expected.insert(Key(3), Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
-  DynamicValues circle(pose2SLAM::circle(4,1.0));
+  Values circle(pose2SLAM::circle(4,1.0));
   Ordering ordering(*circle.orderingArbitrary());
 	VectorValues delta(circle.dims(ordering));
 	delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0);
 	delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0);
 	delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0);
 	delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0);
-	DynamicValues actual = circle.retract(delta, ordering);
+	Values actual = circle.retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -385,7 +385,7 @@ TEST( Pose2Prior, error )
 {
 	// Choose a linearization point
 	Pose2 p1(1, 0, 0); // robot at (1,0)
-	DynamicValues x0;
+	Values x0;
 	x0.insert(Key(1), p1);
 
 	// Create factor
@@ -407,7 +407,7 @@ TEST( Pose2Prior, error )
 	VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
 	addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
 	VectorValues plus = delta + addition;
-	DynamicValues x1 = x0.retract(plus, ordering);
+	Values x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -429,7 +429,7 @@ LieVector hprior(const Pose2& p1) {
 TEST( Pose2Prior, linearize )
 {
 	// Choose a linearization point at ground truth
-	DynamicValues x0;
+	Values x0;
 	x0.insert(Key(1),priorVal);
 
 	// Actual linearization
@@ -449,7 +449,7 @@ TEST( Pose2Factor, error )
 	// Choose a linearization point
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
-	DynamicValues x0;
+	Values x0;
 	x0.insert(Key(1), p1);
 	x0.insert(Key(2), p2);
 
@@ -473,7 +473,7 @@ TEST( Pose2Factor, error )
 	// Check error after increasing p2
 	VectorValues plus = delta;
 	plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
-	DynamicValues x1 = x0.retract(plus, ordering);
+	Values x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -490,7 +490,7 @@ TEST( Pose2Factor, rhs )
 	// 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.1) looking at negative (ground truth is at -1,4)
-	DynamicValues x0;
+	Values x0;
 	x0.insert(Key(1),p1);
 	x0.insert(Key(2),p2);
 
@@ -520,7 +520,7 @@ TEST( Pose2Factor, linearize )
 	// Choose a linearization point at ground truth
 	Pose2 p1(1,2,M_PI_2);
 	Pose2 p2(-1,4,M_PI);
-	DynamicValues x0;
+	Values x0;
 	x0.insert(Key(1),p1);
 	x0.insert(Key(2),p2);
 
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 574d05b6d..94a3bd72f 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -49,7 +49,7 @@ TEST(Pose3Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
 	double radius = 10;
-	DynamicValues hexagon = pose3SLAM::circle(6,radius);
+	Values hexagon = pose3SLAM::circle(6,radius);
   Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
@@ -66,7 +66,7 @@ TEST(Pose3Graph, optimizeCircle) {
   fg->addConstraint(5,0, _0T1, covariance);
 
   // Create initial config
-  boost::shared_ptr<DynamicValues> initial(new DynamicValues());
+  boost::shared_ptr<Values> initial(new Values());
   initial->insert(Key(0), gT0);
   initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
   initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
@@ -81,7 +81,7 @@ TEST(Pose3Graph, optimizeCircle) {
   pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  DynamicValues actual = *optimizer.values();
+  Values actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual,1e-4));
@@ -110,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) {
 	pose3SLAM::Graph graph;
 	graph.add(height);
 
-	DynamicValues values;
+	Values values;
 	values.insert(key, init);
 
 	// linearization
 	EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
 
-	DynamicValues actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
+	Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
 	EXPECT(assert_equal(expected, actual[key], tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
@@ -134,7 +134,7 @@ TEST( Pose3Factor, error )
 	Pose3Factor factor(1,2, z, I6);
 
 	// Create config
-	DynamicValues x;
+	Values x;
 	x.insert(Key(1),t1);
 	x.insert(Key(2),t2);
 
@@ -165,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) {
 	pose3SLAM::Graph graph;
 	graph.add(priorXY);
 
-	DynamicValues values;
+	Values values;
 	values.insert(key, init);
 
-	DynamicValues actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
+	Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
 	EXPECT(assert_equal(expected, actual[key], tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
@@ -181,23 +181,23 @@ Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
 Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
 
 /* ************************************************************************* */
-TEST( DynamicValues, pose3Circle )
+TEST( Values, pose3Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	DynamicValues expected;
+	Values expected;
 	expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0)));
 	expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0)));
 	expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0)));
 	expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0)));
 
-	DynamicValues actual = pose3SLAM::circle(4,1.0);
+	Values actual = pose3SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( DynamicValues, expmap )
+TEST( Values, expmap )
 {
-	DynamicValues expected;
+	Values expected;
 	expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
 	expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
 	expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
@@ -210,7 +210,7 @@ TEST( DynamicValues, 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);
-	DynamicValues actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
+	Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index f5f41fd2e..1d8616c30 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -52,7 +52,7 @@ TEST( ProjectionFactor, error )
 	factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
 
 	// For the following values structure, the factor predicts 320,240
-	DynamicValues config;
+	Values config;
 	Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1);
 	Point3 l1;  config.insert(PointKey(1), l1);
 	// Point should project to Point2(320.,240.)
@@ -81,13 +81,13 @@ TEST( ProjectionFactor, error )
 	CHECK(assert_equal(expected_lfg,*actual_lfg));
 
 	// expmap on a config
-	DynamicValues expected_config;
+	Values expected_config;
   Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
   Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
 	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
 	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
-	DynamicValues actual_config = config.retract(delta, ordering);
+	Values actual_config = config.retract(delta, ordering);
 	CHECK(assert_equal(expected_config,actual_config,1e-9));
 }
 
diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp
index c9b2bfd61..50b302bd2 100644
--- a/gtsam/slam/tests/testSimulated2D.cpp
+++ b/gtsam/slam/tests/testSimulated2D.cpp
@@ -25,13 +25,14 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace gtsam::simulated2D;
 
 /* ************************************************************************* */
 TEST( simulated2D, Simulated2DValues )
 {
-	simulated2D::Values actual;
-	actual.insertPose(1,Point2(1,1));
-	actual.insertPoint(2,Point2(2,2));
+	Values actual;
+	actual.insert(PoseKey(1),Point2(1,1));
+	actual.insert(PointKey(2),Point2(2,2));
   EXPECT(assert_equal(actual,actual,1e-9));
 }
 
diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp
index 57951f6e1..c482281b7 100644
--- a/gtsam/slam/tests/testSimulated2DOriented.cpp
+++ b/gtsam/slam/tests/testSimulated2DOriented.cpp
@@ -56,7 +56,7 @@ TEST( simulated2DOriented, constructor )
 	SharedDiagonal model(Vector_(3, 1., 1., 1.));
 	simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
 
-	simulated2DOriented::Values config;
+	Values config;
 	config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
 	config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
 	boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp
index 5a90f2489..fb0dbaf88 100644
--- a/gtsam/slam/tests/testSimulated3D.cpp
+++ b/gtsam/slam/tests/testSimulated3D.cpp
@@ -28,7 +28,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( simulated3D, Values )
 {
-	DynamicValues actual;
+	Values actual;
 	actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
 	actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
 	EXPECT(assert_equal(actual,actual,1e-9));
diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp
index 1fd23b57f..29bca829c 100644
--- a/gtsam/slam/tests/testStereoFactor.cpp
+++ b/gtsam/slam/tests/testStereoFactor.cpp
@@ -60,7 +60,7 @@ TEST( StereoFactor, singlePoint)
 	graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
 
 	// Create a configuration corresponding to the ground truth
-	boost::shared_ptr<DynamicValues> values(new DynamicValues());
+	boost::shared_ptr<Values> values(new Values());
 	values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
 
 	Point3 l1(0, 0, 0);
diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp
index 9281e51dc..98c369e4f 100644
--- a/gtsam/slam/tests/testVSLAM.cpp
+++ b/gtsam/slam/tests/testVSLAM.cpp
@@ -92,7 +92,7 @@ TEST( Graph, optimizeLM)
   graph->addPointConstraint(3, landmark3);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
+  boost::shared_ptr<Values> initialEstimate(new Values);
   initialEstimate->insert(PoseKey(1), camera1);
   initialEstimate->insert(PoseKey(2), camera2);
   initialEstimate->insert(PointKey(1), landmark1);
@@ -129,7 +129,7 @@ TEST( Graph, optimizeLM2)
   graph->addPoseConstraint(2, camera2);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
+  boost::shared_ptr<Values> initialEstimate(new Values);
   initialEstimate->insert(PoseKey(1), camera1);
   initialEstimate->insert(PoseKey(2), camera2);
   initialEstimate->insert(PointKey(1), landmark1);
@@ -166,7 +166,7 @@ TEST( Graph, CHECK_ORDERING)
   graph->addPoseConstraint(2, camera2);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
+  boost::shared_ptr<Values> initialEstimate(new Values);
   initialEstimate->insert(PoseKey(1), camera1);
   initialEstimate->insert(PoseKey(2), camera2);
   initialEstimate->insert(PointKey(1), landmark1);
@@ -194,23 +194,23 @@ TEST( Graph, CHECK_ORDERING)
 TEST( Values, update_with_large_delta) {
 	// this test ensures that if the update for delta is larger than
 	// the size of the config, it only updates existing variables
-	DynamicValues init;
+	Values init;
 	init.insert(PoseKey(1), Pose3());
 	init.insert(PointKey(1), Point3(1.0, 2.0, 3.0));
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
 	expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1));
 
 	Ordering largeOrdering;
-	DynamicValues largeValues = init;
+	Values largeValues = init;
 	largeValues.insert(PoseKey(2), Pose3());
 	largeOrdering += "x1","l1","x2";
 	VectorValues delta(largeValues.dims(largeOrdering));
 	delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
 	delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
 	delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
-	DynamicValues actual = init.retract(delta, largeOrdering);
+	Values actual = init.retract(delta, largeOrdering);
 
 	CHECK(assert_equal(expected,actual));
 }
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index 958337677..4bf5d7b35 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -38,7 +38,7 @@ namespace gtsam {
    */
   typedef TypedSymbol<Pose3,'x'> PoseKey;									///< The key type used for poses
   typedef TypedSymbol<Point3,'l'> PointKey;								///< The key type used for points
-  typedef boost::shared_ptr<DynamicValues> shared_values;				///< shared pointer to values data structure
+  typedef boost::shared_ptr<Values> shared_values;				///< shared pointer to values data structure
 
   typedef NonlinearEquality<PoseKey> PoseConstraint;	 ///< put a hard constraint on a pose
   typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp
index d6bda8472..e1f8f574a 100644
--- a/tests/testBoundingConstraint.cpp
+++ b/tests/testBoundingConstraint.cpp
@@ -33,7 +33,7 @@ SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
 
 typedef NonlinearFactorGraph Graph;
 typedef boost::shared_ptr<Graph> shared_graph;
-typedef boost::shared_ptr<simulated2D::Values> shared_values;
+typedef boost::shared_ptr<Values> shared_values;
 typedef NonlinearOptimizer<Graph> Optimizer;
 
 // some simple inequality constraints
@@ -50,7 +50,7 @@ iq2D::PoseYInequality constraint4(key, 2.0, false, mu);
 /* ************************************************************************* */
 TEST( testBoundingConstraint, unary_basics_inactive1 ) {
 	Point2 pt1(2.0, 3.0);
-	simulated2D::Values config;
+	Values config;
 	config.insert(key, pt1);
 	EXPECT(!constraint1.active(config));
 	EXPECT(!constraint2.active(config));
@@ -69,7 +69,7 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, unary_basics_inactive2 ) {
 	Point2 pt2(-2.0, -3.0);
-	simulated2D::Values config;
+	Values config;
 	config.insert(key, pt2);
 	EXPECT(!constraint3.active(config));
 	EXPECT(!constraint4.active(config));
@@ -88,7 +88,7 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, unary_basics_active1 ) {
 	Point2 pt2(-2.0, -3.0);
-	simulated2D::Values config;
+	Values config;
 	config.insert(key, pt2);
 	EXPECT(constraint1.active(config));
 	EXPECT(constraint2.active(config));
@@ -103,7 +103,7 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, unary_basics_active2 ) {
 	Point2 pt1(2.0, 3.0);
-	simulated2D::Values config;
+	Values config;
 	config.insert(key, pt1);
 	EXPECT(constraint3.active(config));
 	EXPECT(constraint4.active(config));
@@ -118,7 +118,7 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, unary_linearization_inactive) {
 	Point2 pt1(2.0, 3.0);
-	simulated2D::Values config1;
+	Values config1;
 	config1.insert(key, pt1);
 	Ordering ordering;
 	ordering += key;
@@ -131,7 +131,7 @@ TEST( testBoundingConstraint, unary_linearization_inactive) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, unary_linearization_active) {
 	Point2 pt2(-2.0, -3.0);
-	simulated2D::Values config2;
+	Values config2;
 	config2.insert(key, pt2);
 	Ordering ordering;
 	ordering += key;
@@ -156,11 +156,11 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
 	graph->add(iq2D::PoseYInequality(x1, 2.0, true));
 	graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
 
-	shared_values initValues(new simulated2D::Values());
+	shared_values initValues(new Values());
 	initValues->insert(x1, start_pt);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
-	DynamicValues expected;
+	Values expected;
 	expected.insert(x1, goal_pt);
 	CHECK(assert_equal(expected, *actual, tol));
 }
@@ -178,11 +178,11 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 	graph->add(iq2D::PoseYInequality(x1, 2.0, false));
 	graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
 
-	shared_values initValues(new simulated2D::Values());
+	shared_values initValues(new Values());
 	initValues->insert(x1, start_pt);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
-	DynamicValues expected;
+	Values expected;
 	expected.insert(x1, goal_pt);
 	CHECK(assert_equal(expected, *actual, tol));
 }
@@ -201,7 +201,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
 	EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
 	EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));
 
-	simulated2D::Values config1;
+	Values config1;
 	config1.insert(key1, pt1);
 	config1.insert(key2, pt1);
 	Ordering ordering; ordering += key1, key2;
@@ -238,11 +238,11 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
 	graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2));
 	graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0));
 
-	simulated2D::Values initial_state;
+	Values initial_state;
 	initial_state.insert(x1, pt1);
 	initial_state.insert(x2, pt2_init);
 
-	simulated2D::Values expected;
+	Values expected;
 	expected.insert(x1, pt1);
 	expected.insert(x2, pt2_goal);
 
@@ -268,7 +268,7 @@ TEST( testBoundingConstraint, avoid_demo) {
 	graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3));
 	graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3));
 
-	simulated2D::Values init, expected;
+	Values init, expected;
 	init.insert(x1, x1_pt);
 	init.insert(x3, x3_pt);
 	init.insert(l1, l1_pt);
diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp
index 2792d08b8..27070b801 100644
--- a/tests/testDoglegOptimizer.cpp
+++ b/tests/testDoglegOptimizer.cpp
@@ -385,7 +385,7 @@ TEST(DoglegOptimizer, Iterate) {
 
   // config far from minimum
   Point2 x0(3,0);
-  boost::shared_ptr<simulated2D::Values> config(new example::Values);
+  boost::shared_ptr<Values> config(new Values);
   config->insert(simulated2D::PoseKey(1), x0);
 
   // ordering
@@ -404,7 +404,7 @@ TEST(DoglegOptimizer, Iterate) {
     DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config));
     Delta = result.Delta;
     EXPECT(result.f_error < fg->error(*config)); // Check that error decreases
-    simulated2D::Values newConfig(config->retract(result.dx_d, *ord));
+    Values newConfig(config->retract(result.dx_d, *ord));
     (*config) = newConfig;
     DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in
   }
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 1ff5fe047..a53f716b5 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -170,7 +170,7 @@ public:
    * calculate the error of the factor
    * Override for systems with unusual noise models
    */
-  virtual double error(const DynamicValues& c) const {
+  virtual double error(const Values& c) const {
     Vector w = whitenedError(c);
     return 0.5 * w.dot(w);
   }
@@ -181,7 +181,7 @@ public:
   }
 
   /** Vector of errors, whitened ! */
-  Vector whitenedError(const DynamicValues& c) const {
+  Vector whitenedError(const Values& c) const {
     return QInvSqrt(c[key1_])*unwhitenedError(c);
   }
 
@@ -190,7 +190,7 @@ public:
    * 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 DynamicValues& c, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
     const X1& x1 = c[key1_];
     const X2& x2 = c[key2_];
     Matrix A1, A2;
@@ -307,7 +307,7 @@ public:
    * calculate the error of the factor
    * Override for systems with unusual noise models
    */
-  virtual double error(const DynamicValues& c) const {
+  virtual double error(const Values& c) const {
     Vector w = whitenedError(c);
     return 0.5 * w.dot(w);
   }
@@ -318,7 +318,7 @@ public:
   }
 
   /** Vector of errors, whitened ! */
-  Vector whitenedError(const DynamicValues& c) const {
+  Vector whitenedError(const Values& c) const {
     return RInvSqrt(c[key_])*unwhitenedError(c);
   }
 
@@ -327,7 +327,7 @@ public:
    * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z
    * Hence b = z - h(x1) = - error_vector(x)
    */
-  boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
     const X& x1 = c[key_];
     Matrix A1;
     Vector b = -evaluateError(x1, A1);
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 7c39b8c60..c4f8c69fc 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -33,10 +33,10 @@ const double tol = 1e-4;
 TEST(ISAM2, AddVariables) {
 
   // Create initial state
-  DynamicValues theta;
+  Values theta;
   theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
   theta.insert(planarSLAM::PointKey(0), Point2(.4, .5));
-  DynamicValues newTheta;
+  Values newTheta;
   newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
 
   VectorValues deltaUnpermuted;
@@ -60,7 +60,7 @@ TEST(ISAM2, AddVariables) {
   EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]]));
 
   // Create expected state
-  DynamicValues thetaExpected;
+  Values thetaExpected;
   thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
   thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5));
   thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
@@ -95,7 +95,7 @@ TEST(ISAM2, AddVariables) {
 //TEST(ISAM2, IndicesFromFactors) {
 //
 //  using namespace gtsam::planarSLAM;
-//  typedef GaussianISAM2<planarSLAM::Values>::Impl Impl;
+//  typedef GaussianISAM2<Values>::Impl Impl;
 //
 //  Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1);
 //  planarSLAM::Graph graph;
@@ -114,7 +114,7 @@ TEST(ISAM2, AddVariables) {
 /* ************************************************************************* */
 //TEST(ISAM2, CheckRelinearization) {
 //
-//  typedef GaussianISAM2<planarSLAM::Values>::Impl Impl;
+//  typedef GaussianISAM2<Values>::Impl Impl;
 //
 //  // Create values where indices 1 and 3 are above the threshold of 0.1
 //  VectorValues values;
@@ -148,7 +148,7 @@ TEST(ISAM2, AddVariables) {
 TEST(ISAM2, optimize2) {
 
   // Create initialization
-  DynamicValues theta;
+  Values theta;
   theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
   // Create conditional
@@ -180,15 +180,15 @@ TEST(ISAM2, optimize2) {
 }
 
 /* ************************************************************************* */
-bool isam_check(const planarSLAM::Graph& fullgraph, const DynamicValues& fullinit, const GaussianISAM2<>& isam) {
-  DynamicValues actual = isam.calculateEstimate();
+bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) {
+  Values actual = isam.calculateEstimate();
   Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
   GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
 //  linearized.print("Expected linearized: ");
   GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
 //  gbn.print("Expected bayesnet: ");
   VectorValues delta = optimize(gbn);
-  DynamicValues expected = fullinit.retract(delta, ordering);
+  Values expected = fullinit.retract(delta, ordering);
 
   return assert_equal(expected, actual);
 }
@@ -211,7 +211,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
 
   // These variables will be reused and accumulate factors and values
   GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
-  DynamicValues fullinit;
+  Values fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -223,7 +223,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -238,7 +238,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -253,7 +253,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -271,7 +271,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -286,7 +286,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -344,7 +344,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
 
   // These variables will be reused and accumulate factors and values
   GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
-  DynamicValues fullinit;
+  Values fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -356,7 +356,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -371,7 +371,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -386,7 +386,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -404,7 +404,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -419,7 +419,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -472,7 +472,7 @@ TEST(ISAM2, clone) {
 
   // These variables will be reused and accumulate factors and values
   GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
-  DynamicValues fullinit;
+  Values fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -484,7 +484,7 @@ TEST(ISAM2, clone) {
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -499,7 +499,7 @@ TEST(ISAM2, clone) {
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -514,7 +514,7 @@ TEST(ISAM2, clone) {
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -532,7 +532,7 @@ TEST(ISAM2, clone) {
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -547,7 +547,7 @@ TEST(ISAM2, clone) {
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -645,7 +645,7 @@ TEST(ISAM2, removeFactors)
 
   // These variables will be reused and accumulate factors and values
   GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
-  DynamicValues fullinit;
+  Values fullinit;
   planarSLAM::Graph fullgraph;
 
   // i keeps track of the time step
@@ -657,7 +657,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -672,7 +672,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -687,7 +687,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -705,7 +705,7 @@ TEST(ISAM2, removeFactors)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -721,7 +721,7 @@ TEST(ISAM2, removeFactors)
     fullgraph.push_back(newfactors[0]);
     fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
@@ -731,7 +731,7 @@ TEST(ISAM2, removeFactors)
     // Remove the measurement on landmark 0
     FastVector<size_t> toRemove;
     toRemove.push_back(result.newFactorsIndices[1]);
-    isam.update(planarSLAM::Graph(), DynamicValues(), toRemove);
+    isam.update(planarSLAM::Graph(), Values(), toRemove);
   }
 
   // Compare solutions
@@ -784,7 +784,7 @@ TEST(ISAM2, constrained_ordering)
 
   // These variables will be reused and accumulate factors and values
   GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
-  DynamicValues fullinit;
+  Values fullinit;
   planarSLAM::Graph fullgraph;
 
   // We will constrain x3 and x4 to the end
@@ -799,7 +799,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
 
@@ -814,7 +814,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -832,7 +832,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@@ -850,7 +850,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
 
@@ -865,7 +865,7 @@ TEST(ISAM2, constrained_ordering)
     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
     fullgraph.push_back(newfactors);
 
-    DynamicValues init;
+    Values init;
     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
 
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index fc9662519..ee601e24f 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -110,7 +110,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
 {
 	// create a graph
 	example::Graph nlfg = createNonlinearFactorGraph();
-	example::Values noisy = createNoisyValues();
+	Values noisy = createNoisyValues();
   Ordering ordering; ordering += "x1","x2","l1";
 	GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering);
 
@@ -128,7 +128,7 @@ TEST(GaussianJunctionTree, slamlike) {
   typedef planarSLAM::PoseKey PoseKey;
   typedef planarSLAM::PointKey PointKey;
 
-  planarSLAM::Values init;
+  Values init;
   planarSLAM::Graph newfactors;
   planarSLAM::Graph fullgraph;
   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@@ -179,11 +179,11 @@ TEST(GaussianJunctionTree, slamlike) {
 
   GaussianJunctionTree gjt(linearized);
   VectorValues deltaactual = gjt.optimize(&EliminateQR);
-  planarSLAM::Values actual = init.retract(deltaactual, ordering);
+  Values actual = init.retract(deltaactual, ordering);
 
   GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
   VectorValues delta = optimize(gbn);
-  planarSLAM::Values expected = init.retract(delta, ordering);
+  Values expected = init.retract(delta, ordering);
 
   EXPECT(assert_equal(expected, actual));
 }
@@ -198,7 +198,7 @@ TEST(GaussianJunctionTree, simpleMarginal) {
   fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0));
   fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
 
-  DynamicValues init;
+  Values init;
   init.insert(pose2SLAM::Key(0), Pose2());
   init.insert(pose2SLAM::Key(1), Pose2(1.0, 0.0, 0.0));
 
diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp
index f6d8629bf..80c15c611 100644
--- a/tests/testGraph.cpp
+++ b/tests/testGraph.cpp
@@ -94,10 +94,10 @@ TEST( Graph, composePoses )
 
 	Pose2 rootPose = p2;
 
-	boost::shared_ptr<DynamicValues> actual = composePoses<Pose2Graph, Pose2Factor,
+	boost::shared_ptr<Values> actual = composePoses<Pose2Graph, Pose2Factor,
 			Pose2, pose2SLAM::Key> (graph, tree, rootPose);
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(pose2SLAM::Key(1), p1);
 	expected.insert(pose2SLAM::Key(2), p2);
 	expected.insert(pose2SLAM::Key(3), p3);
diff --git a/tests/testInference.cpp b/tests/testInference.cpp
index 555f8a061..e530393f8 100644
--- a/tests/testInference.cpp
+++ b/tests/testInference.cpp
@@ -61,7 +61,7 @@ TEST( Inference, marginals2)
   fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
   fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
 
-  planarSLAM::Values init;
+  Values init;
   init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0));
   init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0));
   init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0));
diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp
index b917185e9..328b26083 100644
--- a/tests/testNonlinearEquality.cpp
+++ b/tests/testNonlinearEquality.cpp
@@ -44,7 +44,7 @@ PoseKey key(1);
 /* ************************************************************************* */
 TEST ( NonlinearEquality, linearization ) {
 	Pose2 value = Pose2(2.1, 1.0, 2.0);
-	DynamicValues linearize;
+	Values linearize;
 	linearize.insert(key, value);
 
 	// create a nonlinear equality constraint
@@ -62,7 +62,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
 
 	PoseKey key(1);
 	Pose2 value;
-	DynamicValues config;
+	Values config;
 	config.insert(key, value);
 
 	// create a nonlinear equality constraint
@@ -76,7 +76,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
 TEST ( NonlinearEquality, linearization_fail ) {
 	Pose2 value = Pose2(2.1, 1.0, 2.0);
 	Pose2 wrong = Pose2(2.1, 3.0, 4.0);
-	DynamicValues bad_linearize;
+	Values bad_linearize;
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
@@ -92,7 +92,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
 	PoseKey key(1);
 	Pose2 value(2.0, 1.0, 2.0),
 		  wrong(2.0, 3.0, 4.0);
-	DynamicValues bad_linearize;
+	Values bad_linearize;
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
@@ -108,7 +108,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
 	PoseKey key(1);
 	Pose2 value,
 		  wrong(2.0, 3.0, 4.0);
-	DynamicValues bad_linearize;
+	Values bad_linearize;
 	bad_linearize.insert(key, wrong);
 
 	// create a nonlinear equality constraint
@@ -122,7 +122,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
 TEST ( NonlinearEquality, error ) {
 	Pose2 value = Pose2(2.1, 1.0, 2.0);
 	Pose2 wrong = Pose2(2.1, 3.0, 4.0);
-	DynamicValues feasible, bad_linearize;
+	Values feasible, bad_linearize;
 	feasible.insert(key, value);
 	bad_linearize.insert(key, wrong);
 
@@ -167,7 +167,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
 	EXPECT(assert_equal(expVec, actVec, 1e-5));
 
 	// the actual error should have a gain on it
-	DynamicValues config;
+	Values config;
 	config.insert(key1, badPoint1);
 	double actError = nle.error(config);
 	DOUBLES_EQUAL(500.0, actError, 1e-9);
@@ -194,7 +194,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
 
 	// initialize away from the ideal
 	Pose2 initPose(0.0, 2.0, 3.0);
-	boost::shared_ptr<DynamicValues> init(new DynamicValues());
+	boost::shared_ptr<Values> init(new Values());
 	init->insert(key1, initPose);
 
 	// optimize
@@ -205,7 +205,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
 	PoseOptimizer result = optimizer.levenbergMarquardt();
 
 	// verify
-	DynamicValues expected;
+	Values expected;
 	expected.insert(key1, feasible1);
 	EXPECT(assert_equal(expected, *result.values()));
 }
@@ -218,7 +218,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 	Pose2 feasible1(1.0, 2.0, 3.0);
 
 	// initialize away from the ideal
-	boost::shared_ptr<DynamicValues> init(new DynamicValues());
+	boost::shared_ptr<Values> init(new Values());
 	Pose2 initPose(0.0, 2.0, 3.0);
 	init->insert(key1, initPose);
 
@@ -241,7 +241,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 	PoseOptimizer result = optimizer.levenbergMarquardt();
 
 	// verify
-	DynamicValues expected;
+	Values expected;
 	expected.insert(key1, feasible1);
 	EXPECT(assert_equal(expected, *result.values()));
 }
@@ -252,7 +252,7 @@ SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
 
 typedef NonlinearFactorGraph Graph;
 typedef boost::shared_ptr<Graph> shared_graph;
-typedef boost::shared_ptr<DynamicValues> shared_values;
+typedef boost::shared_ptr<Values> shared_values;
 typedef NonlinearOptimizer<Graph> Optimizer;
 
 /* ************************************************************************* */
@@ -262,14 +262,14 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
 	double mu = 1000.0;
 	eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
 
-	simulated2D::Values config1;
+	Values config1;
 	config1.insert(key, pt);
 	EXPECT(constraint.active(config1));
 	EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol));
 	EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
 	EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
 
-	simulated2D::Values config2;
+	Values config2;
 	Point2 ptBad1(2.0, 2.0);
 	config2.insert(key, ptBad1);
 	EXPECT(constraint.active(config2));
@@ -287,13 +287,13 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
 	ordering += key;
 	eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
 
-	simulated2D::Values config1;
+	Values config1;
 	config1.insert(key, pt);
 	GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering);
 	GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model));
 	EXPECT(assert_equal(*expected1, *actual1, tol));
 
-	simulated2D::Values config2;
+	Values config2;
 	Point2 ptBad(2.0, 2.0);
 	config2.insert(key, ptBad);
 	GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering);
@@ -319,13 +319,13 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
 	graph->push_back(constraint);
 	graph->push_back(factor);
 
-	shared_values initValues(new simulated2D::Values());
+	shared_values initValues(new Values());
 	initValues->insert(key, badPt);
 
 	// verify error values
 	EXPECT(constraint->active(*initValues));
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(key, truth_pt);
 	EXPECT(constraint->active(expected));
 	EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
@@ -341,7 +341,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
 	double mu = 1000.0;
 	eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
 
-	simulated2D::Values config1;
+	Values config1;
 	config1.insert(key1, x1);
 	config1.insert(key2, x2);
 	EXPECT(constraint.active(config1));
@@ -349,7 +349,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
 	EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
 	EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
 
-	simulated2D::Values config2;
+	Values config2;
 	Point2 x1bad(2.0, 2.0);
 	Point2 x2bad(2.0, 2.0);
 	config2.insert(key1, x1bad);
@@ -369,7 +369,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
 	ordering += key1, key2;
 	eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
 
-	simulated2D::Values config1;
+	Values config1;
 	config1.insert(key1, x1);
 	config1.insert(key2, x2);
 	GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering);
@@ -378,7 +378,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
 					eye(2,2), zero(2), hard_model));
 	EXPECT(assert_equal(*expected1, *actual1, tol));
 
-	simulated2D::Values config2;
+	Values config2;
 	Point2 x1bad(2.0, 2.0);
 	Point2 x2bad(2.0, 2.0);
 	config2.insert(key1, x1bad);
@@ -417,12 +417,12 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
 	graph->push_back(constraint2);
 	graph->push_back(factor);
 
-	shared_values initValues(new simulated2D::Values());
+	shared_values initValues(new Values());
 	initValues->insert(key1, Point2());
 	initValues->insert(key2, badPt);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
-	DynamicValues expected;
+	Values expected;
 	expected.insert(key1, truth_pt1);
 	expected.insert(key2, truth_pt2);
 	CHECK(assert_equal(expected, *actual, tol));
@@ -454,7 +454,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
 
 	graph->add(eq2D::PointEqualityConstraint(l1, l2));
 
-	shared_values initialEstimate(new simulated2D::Values());
+	shared_values initialEstimate(new Values());
 	initialEstimate->insert(x1, pt_x1);
 	initialEstimate->insert(x2, Point2());
 	initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth
@@ -462,7 +462,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(x1, pt_x1);
 	expected.insert(l1, Point2(1.0, 6.0));
 	expected.insert(l2, Point2(1.0, 6.0));
@@ -497,7 +497,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
 	graph->add(eq2D::PointEqualityConstraint(l1, l2));
 
 	// create an initial estimate
-	shared_values initialEstimate(new simulated2D::Values());
+	shared_values initialEstimate(new Values());
 	initialEstimate->insert(x1, Point2( 1.0, 1.0));
 	initialEstimate->insert(l1, Point2( 1.0, 6.0));
 	initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
@@ -506,7 +506,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
 	// optimize
 	Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(x1, Point2(1.0, 1.0));
 	expected.insert(l1, Point2(1.0, 6.0));
 	expected.insert(l2, Point2(1.0, 6.0));
@@ -521,7 +521,7 @@ Cal3_S2 K(fov,w,h);
 boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
 
 // typedefs for visual SLAM example
-typedef boost::shared_ptr<DynamicValues> shared_vconfig;
+typedef boost::shared_ptr<Values> shared_vconfig;
 typedef visualSLAM::Graph VGraph;
 typedef NonlinearOptimizer<VGraph> VOptimizer;
 
@@ -565,7 +565,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 	Point3 landmark1(0.5, 5.0, 0.0);
 	Point3 landmark2(1.5, 5.0, 0.0);
 
-	shared_vconfig initValues(new DynamicValues());
+	shared_vconfig initValues(new Values());
 	initValues->insert(x1, pose1);
 	initValues->insert(x2, pose2);
 	initValues->insert(l1, landmark1);
@@ -575,7 +575,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 	VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues);
 
 	// create config
-	DynamicValues truthValues;
+	Values truthValues;
 	truthValues.insert(x1, camera1.pose());
 	truthValues.insert(x2, camera2.pose());
 	truthValues.insert(l1, landmark);
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index c9fa05c45..490692c90 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -82,7 +82,7 @@ TEST( NonlinearFactor, NonlinearFactor )
   Graph fg = createNonlinearFactorGraph();
 
   // create a values structure for the non linear factor graph
-  example::Values cfg = createNoisyValues();
+  Values cfg = createNoisyValues();
 
   // get the factor "f1" from the factor graph
   Graph::sharedFactor factor = fg[0];
@@ -103,7 +103,7 @@ TEST( NonlinearFactor, NonlinearFactor )
 /* ************************************************************************* */
 TEST( NonlinearFactor, linearize_f1 )
 {
-	example::Values c = createNoisyValues();
+	Values c = createNoisyValues();
 
   // Grab a non-linear factor
   Graph nfg = createNonlinearFactorGraph();
@@ -125,7 +125,7 @@ TEST( NonlinearFactor, linearize_f1 )
 /* ************************************************************************* */
 TEST( NonlinearFactor, linearize_f2 )
 {
-	example::Values c = createNoisyValues();
+	Values c = createNoisyValues();
 
   // Grab a non-linear factor
   Graph nfg = createNonlinearFactorGraph();
@@ -148,7 +148,7 @@ TEST( NonlinearFactor, linearize_f3 )
   Graph::sharedFactor nlf = nfg[2];
 
   // We linearize at noisy config from SmallExample
-  example::Values c = createNoisyValues();
+  Values c = createNoisyValues();
   GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
 
   GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
@@ -165,7 +165,7 @@ TEST( NonlinearFactor, linearize_f4 )
   Graph::sharedFactor nlf = nfg[3];
 
   // We linearize at noisy config from SmallExample
-  example::Values c = createNoisyValues();
+  Values c = createNoisyValues();
   GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
 
   GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
@@ -181,7 +181,7 @@ TEST( NonlinearFactor, size )
 	Graph fg = createNonlinearFactorGraph();
 
 	// create a values structure for the non linear factor graph
-	example::Values cfg = createNoisyValues();
+	Values cfg = createNoisyValues();
 
 	// get some factors from the graph
 	Graph::sharedFactor factor1 = fg[0], factor2 = fg[1],
@@ -201,7 +201,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
 	Point2 mu(1., -1.);
 	Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1));
 
-	example::Values config;
+	Values config;
 	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
 	GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
 
@@ -221,7 +221,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
 	Point2 z3(1.,-1.);
 	simulated2D::Measurement f0(z3, constraint, 1,1);
 
-	example::Values config;
+	Values config;
 	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
 	config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
 	GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
@@ -262,7 +262,7 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor4) {
   TestFactor4 tf;
-  DynamicValues tv;
+  Values tv;
   tv.insert(TestKey(1), LieVector(1, 1.0));
   tv.insert(TestKey(2), LieVector(1, 2.0));
   tv.insert(TestKey(3), LieVector(1, 3.0));
@@ -309,7 +309,7 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor5) {
   TestFactor5 tf;
-  DynamicValues tv;
+  Values tv;
   tv.insert(TestKey(1), LieVector(1, 1.0));
   tv.insert(TestKey(2), LieVector(1, 2.0));
   tv.insert(TestKey(3), LieVector(1, 3.0));
@@ -361,7 +361,7 @@ public:
 /* ************************************ */
 TEST(NonlinearFactor, NonlinearFactor6) {
   TestFactor6 tf;
-  DynamicValues tv;
+  Values tv;
   tv.insert(TestKey(1), LieVector(1, 1.0));
   tv.insert(TestKey(2), LieVector(1, 2.0));
   tv.insert(TestKey(3), LieVector(1, 3.0));
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 8606c027f..1cca4c988 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -51,11 +51,11 @@ TEST( Graph, equals )
 TEST( Graph, error )
 {
 	Graph fg = createNonlinearFactorGraph();
-	example::Values c1 = createValues();
+	Values c1 = createValues();
 	double actual1 = fg.error(c1);
 	DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
 
-	example::Values c2 = createNoisyValues();
+	Values c2 = createNoisyValues();
 	double actual2 = fg.error(c2);
 	DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
 }
@@ -89,7 +89,7 @@ TEST( Graph, GET_ORDERING)
 TEST( Graph, probPrime )
 {
 	Graph fg = createNonlinearFactorGraph();
-	example::Values cfg = createValues();
+	Values cfg = createValues();
 
 	// evaluate the probability of the factor graph
 	double actual = fg.probPrime(cfg);
@@ -101,7 +101,7 @@ TEST( Graph, probPrime )
 TEST( Graph, linearize )
 {
 	Graph fg = createNonlinearFactorGraph();
-	example::Values initial = createNoisyValues();
+	Values initial = createNoisyValues();
 	boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
 	FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
 	CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index 66fd410b6..0ad4e03ae 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
 	Graph start_factors;
 	start_factors.addPoseConstraint(key, cur_pose);
 
-	DynamicValues init;
-	DynamicValues expected;
+	Values init;
+	Values expected;
 	init.insert(key, cur_pose);
 	expected.insert(key, cur_pose);
 	isam.update(start_factors, init);
@@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 		Graph new_factors;
 		PoseKey key1(i-1), key2(i);
 		new_factors.addOdometry(key1, key2, z, model);
-		planarSLAM::Values new_init;
+		Values new_init;
 
 		// perform a check on changing orderings
 		if (i == 5) {
@@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 	}
 
 	// verify values - all but the last one should be very close
-	planarSLAM::Values actual = isam.estimate();
+	Values actual = isam.estimate();
 	for (size_t i=0; i<nrPoses; ++i) {
 		PoseKey cur_key(i);
 		EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 57366500e..b113f8149 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -113,7 +113,7 @@ TEST( NonlinearOptimizer, iterateLM )
 
 	// config far from minimum
 	Point2 x0(3,0);
-	boost::shared_ptr<example::Values> config(new example::Values);
+	boost::shared_ptr<Values> config(new Values);
 	config->insert(simulated2D::PoseKey(1), x0);
 
 	// ordering
@@ -149,13 +149,13 @@ TEST( NonlinearOptimizer, optimize )
 
 	// test error at minimum
 	Point2 xstar(0,0);
-	example::Values cstar;
+	Values cstar;
 	cstar.insert(simulated2D::PoseKey(1), xstar);
 	DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
 
 	// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
 	Point2 x0(3,3);
-	boost::shared_ptr<example::Values> c0(new example::Values);
+	boost::shared_ptr<Values> c0(new Values);
 	c0->insert(simulated2D::PoseKey(1), x0);
 	DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
 
@@ -185,7 +185,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
 			example::createReallyNonlinearFactorGraph()));
 
 	Point2 x0(3,3);
-	boost::shared_ptr<example::Values> c0(new example::Values);
+	boost::shared_ptr<Values> c0(new Values);
 	c0->insert(simulated2D::PoseKey(1), x0);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0);
@@ -198,7 +198,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared )
 	example::Graph fg = example::createReallyNonlinearFactorGraph();
 
 	Point2 x0(3,3);
-	example::Values c0;
+	Values c0;
 	c0.insert(simulated2D::PoseKey(1), x0);
 
 	Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0);
@@ -212,7 +212,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
 			example::createReallyNonlinearFactorGraph()));
 
 	Point2 x0(3,3);
-	boost::shared_ptr<example::Values> c0(new example::Values);
+	boost::shared_ptr<Values> c0(new Values);
 	c0->insert(simulated2D::PoseKey(1), x0);
 
 	Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0);
@@ -225,7 +225,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared )
 	example::Graph fg = example::createReallyNonlinearFactorGraph();
 
 	Point2 x0(3,3);
-	example::Values c0;
+	Values c0;
 	c0.insert(simulated2D::PoseKey(1), x0);
 
 	Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0);
@@ -238,14 +238,14 @@ TEST( NonlinearOptimizer, optimization_method )
 	example::Graph fg = example::createReallyNonlinearFactorGraph();
 
 	Point2 x0(3,3);
-	example::Values c0;
+	Values c0;
 	c0.insert(simulated2D::PoseKey(1), x0);
 
-	DynamicValues actualMFQR = optimize<example::Graph>(
+	Values actualMFQR = optimize<example::Graph>(
 			fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM);
 	DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
 
-	DynamicValues actualMFLDL = optimize<example::Graph>(
+	Values actualMFLDL = optimize<example::Graph>(
 			fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM);
 	DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
 }
@@ -255,7 +255,7 @@ TEST( NonlinearOptimizer, Factorization )
 {
 	typedef NonlinearOptimizer<Pose2Graph, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
 
-	boost::shared_ptr<DynamicValues> config(new DynamicValues);
+	boost::shared_ptr<Values> config(new Values);
 	config->insert(pose2SLAM::Key(1), Pose2(0.,0.,0.));
 	config->insert(pose2SLAM::Key(2), Pose2(1.5,0.,0.));
 
@@ -270,7 +270,7 @@ TEST( NonlinearOptimizer, Factorization )
 	Optimizer optimizer(graph, config, ordering);
 	Optimizer optimized = optimizer.iterateLM();
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(pose2SLAM::Key(1), Pose2(0.,0.,0.));
 	expected.insert(pose2SLAM::Key(2), Pose2(1.,0.,0.));
 	CHECK(assert_equal(expected, *optimized.values(), 1e-5));
@@ -287,13 +287,13 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) {
 
   // test error at minimum
   Point2 xstar(0,0);
-  example::Values cstar;
+  Values cstar;
   cstar.insert(simulated2D::PoseKey(1), xstar);
   DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
 
   // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
   Point2 x0(3,3);
-  boost::shared_ptr<example::Values> c0(new example::Values);
+  boost::shared_ptr<Values> c0(new Values);
   c0->insert(simulated2D::PoseKey(1), x0);
   DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
 
diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp
index 64585ff75..439228c79 100644
--- a/tests/testPose2SLAMwSPCG.cpp
+++ b/tests/testPose2SLAMwSPCG.cpp
@@ -40,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) {
 	graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
 	graph.addPrior(x1, Pose2(0,0,0), sigma) ;
 
-	DynamicValues initial;
+	Values initial;
 	initial.insert(x1, Pose2(  0,  0,   0));
 	initial.insert(x2, Pose2(  0, 2.1, 0.01));
 	initial.insert(x3, Pose2(  0, 3.9,-0.01));
@@ -51,7 +51,7 @@ TEST(testPose2SLAMwSPCG, example1) {
 	initial.insert(x8, Pose2(3.9, 2.1, 0.01));
 	initial.insert(x9, Pose2(4.1, 3.9,-0.01));
 
-	DynamicValues expected;
+	Values expected;
 	expected.insert(x1, Pose2(0.0, 0.0, 0.0));
 	expected.insert(x2, Pose2(0.0, 2.0, 0.0));
 	expected.insert(x3, Pose2(0.0, 4.0, 0.0));
@@ -62,7 +62,7 @@ TEST(testPose2SLAMwSPCG, example1) {
 	expected.insert(x8, Pose2(4.0, 2.0, 0.0));
 	expected.insert(x9, Pose2(4.0, 4.0, 0.0));
 
-	DynamicValues actual = optimizeSPCG(graph, initial);
+	Values actual = optimizeSPCG(graph, initial);
 
 	EXPECT(assert_equal(expected, actual, tol));
 }
diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp
index 1e57a0797..285428dab 100644
--- a/tests/testRot3Optimization.cpp
+++ b/tests/testRot3Optimization.cpp
@@ -38,8 +38,8 @@ typedef NonlinearFactorGraph Graph;
 TEST(Rot3, optimize) {
 
   // Optimize a circle
-  DynamicValues truth;
-  DynamicValues initial;
+  Values truth;
+  Values initial;
   Graph fg;
   fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01)));
   for(int j=0; j<6; ++j) {
@@ -49,7 +49,7 @@ TEST(Rot3, optimize) {
   }
 
   NonlinearOptimizationParameters params;
-  DynamicValues final = optimize(fg, initial, params);
+  Values final = optimize(fg, initial, params);
 
   EXPECT(assert_equal(truth, final, 1e-5));
 }
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index 31012aa44..f2e740180 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -445,7 +445,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Measurement, "gtsam::simulated2D::Me
 TEST (Serialization, smallExample) {
 	using namespace example;
 	Graph nfg = createNonlinearFactorGraph();
-	example::Values c1 = createValues();
+	Values c1 = createValues();
 	EXPECT(equalsObj(nfg));
 	EXPECT(equalsXML(nfg));
 
@@ -464,7 +464,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint,  "gtsam::planarSLAM::Cons
 /* ************************************************************************* */
 TEST (Serialization, planar_system) {
 	using namespace planarSLAM;
-	planarSLAM::Values values;
+	Values values;
 	values.insert(PointKey(3), Point2(1.0, 2.0));
 	values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
 
@@ -490,7 +490,7 @@ TEST (Serialization, planar_system) {
 	// text
 	EXPECT(equalsObj<PoseKey>(PoseKey(2)));
 	EXPECT(equalsObj<PointKey>(PointKey(3)));
-	EXPECT(equalsObj<planarSLAM::Values>(values));
+	EXPECT(equalsObj<Values>(values));
 	EXPECT(equalsObj<Prior>(prior));
 	EXPECT(equalsObj<Bearing>(bearing));
 	EXPECT(equalsObj<BearingRange>(bearingRange));
@@ -502,7 +502,7 @@ TEST (Serialization, planar_system) {
 	// xml
 	EXPECT(equalsXML<PoseKey>(PoseKey(2)));
 	EXPECT(equalsXML<PointKey>(PointKey(3)));
-	EXPECT(equalsXML<planarSLAM::Values>(values));
+	EXPECT(equalsXML<Values>(values));
 	EXPECT(equalsXML<Prior>(prior));
 	EXPECT(equalsXML<Bearing>(bearing));
 	EXPECT(equalsXML<BearingRange>(bearingRange));
@@ -524,7 +524,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::StereoFactor,    "gtsam::visualSLAM::
 /* ************************************************************************* */
 TEST (Serialization, visual_system) {
 	using namespace visualSLAM;
-	visualSLAM::Values values;
+	Values values;
 	PoseKey x1(1), x2(2);
 	PointKey l1(1), l2(2);
 	Pose3 pose1 = pose3, pose2 = pose3.inverse();
diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp
index a21096051..106b16ed1 100644
--- a/tests/timeMultifrontalOnDataset.cpp
+++ b/tests/timeMultifrontalOnDataset.cpp
@@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
   else if (argc == 4)
   	nrTrials = strtoul(argv[3], NULL, 10);
 
-  pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
+  pair<shared_ptr<Pose2Graph>, shared_ptr<Values> > data = load2D(dataset(datasetname));
 
   // Add a prior on the first pose
   if (soft_prior)
diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp
index 8845b901e..4d6d3660c 100644
--- a/tests/timeSequentialOnDataset.cpp
+++ b/tests/timeSequentialOnDataset.cpp
@@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
   else if (argc == 4)
   	nrTrials = strtoul(argv[3], NULL, 10);
 
-  pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
+  pair<shared_ptr<Pose2Graph>, shared_ptr<Values> > data = load2D(dataset(datasetname));
 
   // Add a prior on the first pose
   if (soft_prior)

From 4b2b9d815865e2bcf8fcdec5bf4507d82fc1d78e Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 2 Feb 2012 22:06:33 +0000
Subject: [PATCH 26/88] Fixed Values iterators and removed apply function

---
 gtsam/nonlinear/ISAM2-impl-inl.h | 85 ++++++++++-------------------
 gtsam/nonlinear/Values-inl.h     |  4 +-
 gtsam/nonlinear/Values.cpp       | 46 +++++++---------
 gtsam/nonlinear/Values.h         | 94 ++++++++++++++------------------
 4 files changed, 94 insertions(+), 135 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index c37892654..eae39bf52 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -117,22 +117,6 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
 
 };
 
-/* ************************************************************************* */
-struct _VariableAdder {
-  Ordering& ordering;
-  Permuted<VectorValues>& vconfig;
-  Index nextVar;
-  _VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
-  template<typename I>
-  void operator()(I xIt) {
-    const bool debug = ISDEBUG("ISAM2 AddVariables");
-    vconfig.permutation()[nextVar] = nextVar;
-    ordering.insert(xIt->first, nextVar);
-    if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
-    ++ nextVar;
-  }
-};
-
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
@@ -151,8 +135,13 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
   delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   delta.permutation().resize(originalnVars + newTheta.size());
   {
-    _VariableAdder vadder(ordering, delta, originalnVars);
-    newTheta.apply(vadder);
+    Index nextVar = originalnVars;
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
+      delta.permutation()[nextVar] = nextVar;
+      ordering.insert(key_value.first, nextVar);
+      if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl;
+      ++ nextVar;
+    }
     assert(delta.permutation().size() == delta.container().size());
     assert(ordering.nVars() == delta.size());
     assert(ordering.size() == delta.size());
@@ -222,41 +211,6 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::
   }
 }
 
-/* ************************************************************************* */
-// Internal class used in ExpmapMasked()
-// This debug version sets delta entries that are applied to "Inf".  The
-// idea is that if a delta is applied, the variable is being relinearized,
-// so the same delta should not be re-applied because it will be recalc-
-// ulated.  This is a debug check to prevent against a mix-up of indices
-// or not keeping track of recalculated variables.
-struct _SelectiveExpmapAndClear {
-  const Permuted<VectorValues>& delta;
-  const Ordering& ordering;
-  const vector<bool>& mask;
-  const boost::optional<Permuted<VectorValues>&> invalidate;
-  _SelectiveExpmapAndClear(const Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask, boost::optional<Permuted<VectorValues>&> _invalidate) :
-    delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {}
-  template<typename I>
-  void operator()(I it_x) {
-    Index var = ordering[it_x->first];
-    if(ISDEBUG("ISAM2 update verbose")) {
-      if(mask[var])
-        cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
-      else
-        cout << "       " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
-    }
-    assert(delta[var].size() == (int)it_x->second->dim());
-    assert(delta[var].unaryExpr(&isfinite<double>).all());
-    if(mask[var]) {
-      Value* retracted = it_x->second->retract_(delta[var]);
-      *it_x->second = *retracted;
-      retracted->deallocate_();
-      if(invalidate)
-        (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
-    }
-  }
-};
-
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
@@ -268,8 +222,29 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
   invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
 #endif
 
-  _SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug);
-  values.apply(selectiveExpmapper);
+  assert(values.size() == ordering.size());
+  Values::iterator key_value;
+  Ordering::const_iterator key_index;
+  for(key_value = values.begin(), key_index = ordering.begin();
+      key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
+    assert(key_value->first == key_index->first);
+    const Index var = key_index->second;
+    if(ISDEBUG("ISAM2 update verbose")) {
+      if(mask[var])
+        cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+      else
+        cout << "       " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+    }
+    assert(delta[var].size() == (int)key_value->second.dim());
+    assert(delta[var].unaryExpr(&isfinite<double>).all());
+    if(mask[var]) {
+      Value* retracted = key_value->second.retract_(delta[var]);
+      key_value->second = *retracted;
+      retracted->deallocate_();
+      if(invalidateIfDebug)
+        (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
+    }
+  }
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index b49049a02..76a1ce035 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -41,7 +41,7 @@ namespace gtsam {
   template<typename ValueType>
   const ValueType& Values::at(const Symbol& j) const {
     // Find the item
-    const_iterator item = values_.find(j);
+    KeyValueMap::const_iterator item = values_.find(j);
 
     // Throw exception if it does not exist
     if(item == values_.end())
@@ -69,7 +69,7 @@ namespace gtsam {
   template<typename ValueType>
   boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
     // Find the item
-    const_iterator item = values_.find(j);
+    KeyValueMap::const_iterator item = values_.find(j);
 
     if(item != values_.end()) {
       // Check the type and throw exception if incorrect
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index c5190cbc8..2d8b72120 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -42,9 +42,9 @@ namespace gtsam {
   /* ************************************************************************* */
   void Values::print(const string& str) const {
     cout << str << "Values with " << size() << " values:\n" << endl;
-    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
       cout << "  " << (string)key_value->first << ": ";
-      key_value->second->print("");
+      key_value->second.print("");
     }
   }
 
@@ -53,11 +53,11 @@ namespace gtsam {
     if(this->size() != other.size())
       return false;
     for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
-      if(typeid(*it1->second) != typeid(*it2->second))
+      if(typeid(it1->second) != typeid(it2->second))
         return false;
       if(it1->first != it2->first)
         return false;
-      if(!it1->second->equals_(*it2->second, tol))
+      if(!it1->second.equals_(it2->second, tol))
         return false;
     }
     return true; // We return false earlier if we find anything that does not match
@@ -77,10 +77,10 @@ namespace gtsam {
   Values Values::retract(const VectorValues& delta, const Ordering& ordering) const {
     Values result;
 
-    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
       const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
       Symbol key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
-      Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract
+      Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
       result.values_.insert(key, retractedValue); // Add retracted result directly to result values
     }
 
@@ -102,7 +102,7 @@ namespace gtsam {
       if(it1->first != it2->first)
         throw DynamicValuesMismatched(); // If keys do not match
       // Will throw a dynamic_cast exception if types do not match
-      result.insert(ordering[it1->first], it1->second->localCoordinates_(*it2->second));
+      result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second));
     }
   }
 
@@ -116,16 +116,16 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void Values::insert(const Values& values) {
-    for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
       Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
-      insert(key, *key_value->second);
+      insert(key, key_value->second);
     }
   }
 
   /* ************************************************************************* */
   void Values::update(const Symbol& j, const Value& val) {
   	// Find the value to update
-  	iterator item = values_.find(j);
+  	KeyValueMap::iterator item = values_.find(j);
   	if(item == values_.end())
   		throw ValuesKeyDoesNotExist("update", j);
 
@@ -138,14 +138,14 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void Values::update(const Values& values) {
-    for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
-      this->update(key_value->first, *key_value->second);
+    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+      this->update(key_value->first, key_value->second);
     }
   }
 
   /* ************************************************************************* */
   void Values::erase(const Symbol& j) {
-    iterator item = values_.find(j);
+    KeyValueMap::iterator item = values_.find(j);
     if(item == values_.end())
       throw ValuesKeyDoesNotExist("erase", j);
     values_.erase(item);
@@ -154,7 +154,7 @@ namespace gtsam {
   /* ************************************************************************* */
   FastList<Symbol> Values::keys() const {
     FastList<Symbol> result;
-    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value)
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value)
       result.push_back(key_value->first);
     return result;
   }
@@ -168,23 +168,17 @@ namespace gtsam {
 
   /* ************************************************************************* */
   vector<size_t> Values::dims(const Ordering& ordering) const {
-//    vector<size_t> result(values_.size());
-//    // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
-//    result.assign(
-//        boost::make_transform_iterator(values_.begin(),
-//            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
-//        boost::make_transform_iterator(values_.end(),
-//            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
-//    return result;
-    _ValuesDimensionCollector dimCollector(ordering);
-    this->apply(dimCollector);
-    return dimCollector.dimensions;
+    vector<size_t> result(values_.size());
+    BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
+      result[ordering[key_value.first]] = key_value.second.dim();
+    }
+    return result;
   }
 
   /* ************************************************************************* */
   Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
     Ordering::shared_ptr ordering(new Ordering);
-    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
       ordering->insert(key_value->first, firstVar++);
     }
     return ordering;
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 27da4d7e4..ae06daa74 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -29,6 +29,9 @@
 
 #include <boost/pool/pool_alloc.hpp>
 #include <boost/ptr_container/ptr_map.hpp>
+#include <boost/iterator/transform_iterator.hpp>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
 
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
@@ -41,19 +44,6 @@ namespace gtsam {
   // Forward declarations
   class ValueCloneAllocator;
 
-  struct _ValuesDimensionCollector {
-    const Ordering& ordering;
-    std::vector<size_t> dimensions;
-    _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
-    template<typename I> void operator()(const I& key_value) {
-      Index var;
-      if(ordering.tryAt(key_value->first, var)) {
-        assert(var < dimensions.size());
-        dimensions[var] = key_value->second->dim();
-      }
-    }
-  };
-
 /**
   * A non-templated config holding any types of Manifold-group elements.  A
   * values structure is a map from keys to values. It is used to specify the
@@ -81,15 +71,33 @@ namespace gtsam {
     // The member to store the values, see just above
     KeyValueMap values_;
 
-    // Type obtained by iterating
-    typedef KeyValueMap::const_iterator::value_type KeyValuePair;
+    // Types obtained by iterating
+    typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
+    typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
 
   public:
 
-    typedef KeyValueMap::iterator iterator;
-    typedef KeyValueMap::const_iterator const_iterator;
-    typedef KeyValueMap::reverse_iterator reverse_iterator;
-    typedef KeyValueMap::const_reverse_iterator const_reverse_iterator;
+    /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
+    typedef std::pair<const Symbol&, const Value&> ConstKeyValuePair;
+
+    /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
+    typedef std::pair<const Symbol&, Value&> KeyValuePair;
+
+    /// Mutable forward iterator, with value type KeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
+
+    /// Const forward iterator, with value type ConstKeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
+
+    /// Mutable reverse iterator, with value type KeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
+
+    /// Const reverse iterator, with value type ConstKeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
 
     /** Default constructor creates an empty Values class */
     Values() {}
@@ -165,14 +173,21 @@ namespace gtsam {
     /** Get a zero VectorValues of the correct structure */
     VectorValues zeroVectors(const Ordering& ordering) 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(); }
-    const_reverse_iterator rbegin() const { return values_.rbegin(); }
-    const_reverse_iterator rend() const { return values_.rend(); }
-    reverse_iterator rbegin() { return values_.rbegin(); }
-    reverse_iterator rend() { return values_.rend(); }
+  private:
+    static std::pair<const Symbol&, const Value&> make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
+      return std::make_pair<const Symbol&, const Value&>(key_value.first, *key_value.second); }
+    static std::pair<const Symbol&, Value&> make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
+      return std::make_pair<const Symbol&, Value&>(key_value.first, *key_value.second); }
+
+  public:
+    const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
+    const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
+    iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
+    iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
+    const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
+    const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
+    reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
+    reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
 
     /// @name Manifold Operations
     /// @{
@@ -188,8 +203,6 @@ namespace gtsam {
 
     ///@}
 
-    // imperative methods:
-
     /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
     void insert(const Symbol& j, const Value& val);
 
@@ -205,12 +218,6 @@ namespace gtsam {
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(const Symbol& j);
 
-    /** Remove a variable from the config while returning the dimensionality of
-     * the removed element (normally not needed by user code), throws
-     * KeyDoesNotExist<J> if j is not present.
-     */
-    //void erase(const J& j, size_t& dim);
-
     /**
      * Returns a set of keys in the config
      * Note: by construction, the list is ordered
@@ -226,23 +233,6 @@ namespace gtsam {
     /** Create an array of variable dimensions using the given ordering */
     std::vector<size_t> dims(const Ordering& ordering) const;
 
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      for(iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-
-    template<typename A>
-    void apply(A& operation) const {
-      for(const_iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-
     /**
      * Generate a default ordering, simply in key sort order.  To instead
      * create a fill-reducing ordering, use

From 3793ad3e0ee8c472c09e6a627138a5c2505b3d8b Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 3 Feb 2012 06:07:31 +0000
Subject: [PATCH 28/88] remove redundant typedef Values in ISAM2.h

---
 gtsam/nonlinear/ISAM2.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 62a86cf64..0f8776ec2 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -314,7 +314,6 @@ public:
 
   typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
   typedef ISAM2<CONDITIONAL> This; ///< This class
-  typedef Values Values;
   typedef GRAPH Graph;
 
   /** Create an empty ISAM2 instance */

From 191c3e610a885f178b9197269a7d4f50e94a4f7f Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 3 Feb 2012 06:15:59 +0000
Subject: [PATCH 29/88] enable serialization for Values.

---
 gtsam/base/DerivedValue.h   | 11 ++++++++++-
 gtsam/base/Value.h          | 12 +++++++++++-
 gtsam/nonlinear/Values.h    |  9 +++++++++
 tests/testSerialization.cpp |  5 +++++
 4 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index f59303675..92f294848 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -18,7 +18,16 @@ private:
 	struct PoolTag { };
 
 protected:
-	DerivedValue() {}
+	DerivedValue() {
+  	// Register the base/derived class relationship for boost::serialization
+		// See: http://www.boost.org/doc/libs/1_45_0/libs/serialization/doc/serialization.html#runtimecasting
+		static bool first = true;
+		if (first) {
+			boost::serialization::void_cast_register<DERIVED, Value>(
+					static_cast<DERIVED *>(NULL), static_cast<Value *>(NULL) );
+			first = false;
+		}
+	}
 
 public:
 
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 8d0b2dd05..99b5248d6 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -19,7 +19,7 @@
 #pragma once
 
 #include <memory>
-
+#include <boost/serialization/assume_abstract.hpp>
 #include <gtsam/base/Vector.h>
 
 namespace gtsam {
@@ -142,6 +142,16 @@ namespace gtsam {
     /** Virutal destructor */
     virtual ~Value() {}
 
+  private:
+  	/** Serialization function */
+  	friend class boost::serialization::access;
+  	template<class ARCHIVE>
+  	void serialize(ARCHIVE & ar, const unsigned int version) {
+  		std::cout << "value serialization" << std::endl;
+  	}
+
   };
 
 } /* namespace gtsam */
+
+BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index ae06daa74..2a721d591 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -32,6 +32,7 @@
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/function.hpp>
 #include <boost/bind.hpp>
+#include <boost/ptr_container/serialize_ptr_map.hpp>
 
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
@@ -241,6 +242,14 @@ namespace gtsam {
      */
     Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
 
+  private:
+  	/** Serialization function */
+  	friend class boost::serialization::access;
+  	template<class ARCHIVE>
+  	void serialize(ARCHIVE & ar, const unsigned int version) {
+  		ar & BOOST_SERIALIZATION_NVP(values_);
+  	}
+
   };
 
   /* ************************************************************************* */
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index f2e740180..68f817f8b 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -37,6 +37,7 @@
 #include <boost/archive/xml_iarchive.hpp>
 #include <boost/archive/xml_oarchive.hpp>
 
+#include <gtsam/geometry/Point2.h>
 // whether to print the serialized text to stdout
 const bool verbose = false;
 
@@ -441,6 +442,10 @@ TEST (Serialization, gaussianISAM) {
 BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Prior,       "gtsam::simulated2D::Prior"      );
 BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Odometry,    "gtsam::simulated2D::Odometry"   );
 BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Measurement, "gtsam::simulated2D::Measurement");
+BOOST_CLASS_EXPORT(gtsam::Point2)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+BOOST_CLASS_EXPORT(gtsam::Pose2)
+BOOST_CLASS_EXPORT(gtsam::Pose3)
 /* ************************************************************************* */
 TEST (Serialization, smallExample) {
 	using namespace example;

From 3d9e818d1e3984e0661b79924a18ae5b8afe44d0 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 3 Feb 2012 17:20:23 +0000
Subject: [PATCH 30/88] (2.0_prep branch) Merged in additional changes from
 virtual_values branch r8901-r8902

---
 gtsam/base/DerivedValue.h   | 11 ++++++++++-
 gtsam/base/Value.h          | 12 +++++++++++-
 gtsam/nonlinear/Values.h    |  9 +++++++++
 tests/testSerialization.cpp |  5 +++++
 4 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index f59303675..92f294848 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -18,7 +18,16 @@ private:
 	struct PoolTag { };
 
 protected:
-	DerivedValue() {}
+	DerivedValue() {
+  	// Register the base/derived class relationship for boost::serialization
+		// See: http://www.boost.org/doc/libs/1_45_0/libs/serialization/doc/serialization.html#runtimecasting
+		static bool first = true;
+		if (first) {
+			boost::serialization::void_cast_register<DERIVED, Value>(
+					static_cast<DERIVED *>(NULL), static_cast<Value *>(NULL) );
+			first = false;
+		}
+	}
 
 public:
 
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 8d0b2dd05..99b5248d6 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -19,7 +19,7 @@
 #pragma once
 
 #include <memory>
-
+#include <boost/serialization/assume_abstract.hpp>
 #include <gtsam/base/Vector.h>
 
 namespace gtsam {
@@ -142,6 +142,16 @@ namespace gtsam {
     /** Virutal destructor */
     virtual ~Value() {}
 
+  private:
+  	/** Serialization function */
+  	friend class boost::serialization::access;
+  	template<class ARCHIVE>
+  	void serialize(ARCHIVE & ar, const unsigned int version) {
+  		std::cout << "value serialization" << std::endl;
+  	}
+
   };
 
 } /* namespace gtsam */
+
+BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 9f154609d..a0dc33a74 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -32,6 +32,7 @@
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/function.hpp>
 #include <boost/bind.hpp>
+#include <boost/ptr_container/serialize_ptr_map.hpp>
 
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
@@ -244,6 +245,14 @@ namespace gtsam {
      */
     Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
 
+  private:
+  	/** Serialization function */
+  	friend class boost::serialization::access;
+  	template<class ARCHIVE>
+  	void serialize(ARCHIVE & ar, const unsigned int version) {
+  		ar & BOOST_SERIALIZATION_NVP(values_);
+  	}
+
   };
 
   /* ************************************************************************* */
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index 2bd37ebbb..ab58aeb99 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -37,6 +37,7 @@
 #include <boost/archive/xml_iarchive.hpp>
 #include <boost/archive/xml_oarchive.hpp>
 
+#include <gtsam/geometry/Point2.h>
 // whether to print the serialized text to stdout
 const bool verbose = false;
 
@@ -439,6 +440,10 @@ TEST (Serialization, gaussianISAM) {
 BOOST_CLASS_EXPORT_GUID(simulated2D::Prior,       "gtsam::simulated2D::Prior"      );
 BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry,    "gtsam::simulated2D::Odometry"   );
 BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement");
+BOOST_CLASS_EXPORT(gtsam::Point2)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+BOOST_CLASS_EXPORT(gtsam::Pose2)
+BOOST_CLASS_EXPORT(gtsam::Pose3)
 /* ************************************************************************* */
 TEST (Serialization, smallExample) {
 	using namespace example;

From 7e7205181430276cfba9299ac296f337a00a4c4c Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 3 Feb 2012 17:27:31 +0000
Subject: [PATCH 31/88] Trying to debug serialization error

---
 .cproject                 | 54 ++++++++++++++++++++++++++++++++++++++-
 .project                  |  4 +--
 gtsam/base/DerivedValue.h |  3 +++
 3 files changed, 58 insertions(+), 3 deletions(-)

diff --git a/.cproject b/.cproject
index 1d070ceed..7c478cd64 100644
--- a/.cproject
+++ b/.cproject
@@ -98,11 +98,63 @@
 			<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
 			<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
 		</cconfiguration>
+		<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" name="fast" parent="org.eclipse.cdt.build.core.emptycfg">
+					<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216." name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.1052998998" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.1735323246" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
+							<builder arguments="" buildPath="${workspace_loc:/gtsam/build-fast}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.1127775962" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
+							<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.1922513433" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
+							<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1097733515" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
+								<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.2101986359" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.assembler.macosx.base.1665834395" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.macosx.base">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1096646805" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.1662212593" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.165336396" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
+								<option id="gnu.cpp.compiler.option.include.paths.1336290606" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/gtsam}&quot;"/>
+									<listOptionValue builtIn="false" value="/usr/include/c++/4.6.1"/>
+									<listOptionValue builtIn="false" value="/usr/include/c++/4.6"/>
+									<listOptionValue builtIn="false" value="/usr/include/c++/4.6/backward"/>
+								</option>
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.515209182" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.compiler.macosx.base.302039063" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.macosx.base">
+								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.623172045" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+			<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
+			<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+		</cconfiguration>
 	</storageModule>
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
 		<project id="gtsam.null.1344331286" name="gtsam"/>
 	</storageModule>
-	<storageModule moduleId="refreshScope"/>
+	<storageModule moduleId="refreshScope" versionNumber="1">
+		<resource resourceType="PROJECT" workspacePath="/gtsam"/>
+	</storageModule>
 	<storageModule moduleId="scannerConfiguration">
 		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
 		<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
diff --git a/.project b/.project
index d20fd358e..a3c3ae46b 100644
--- a/.project
+++ b/.project
@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j2</value>
+					<value>-j5</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>
@@ -31,7 +31,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildLocation</key>
-					<value>${workspace_loc:/gtsam/build}</value>
+					<value>${workspace_loc:/gtsam/build-fast}</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index 92f294848..66e7d79a4 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -8,7 +8,10 @@
 #pragma once
 
 #include <boost/pool/singleton_pool.hpp>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/export.hpp>
 #include <gtsam/base/Value.h>
+
 namespace gtsam {
 
 template<class DERIVED>

From 651f593c0f8158d2e1cdf73f34211962dd217829 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 3 Feb 2012 18:44:33 +0000
Subject: [PATCH 32/88] fix serialization linking error

---
 gtsam/base/DerivedValue.h         | 13 +------------
 gtsam/base/Value.h                | 15 +++++++++++----
 gtsam/geometry/Cal3Bundler.h      |  2 ++
 gtsam/geometry/Cal3DS2.h          |  2 ++
 gtsam/geometry/Cal3_S2.h          |  2 ++
 gtsam/geometry/Cal3_S2Stereo.h    |  2 ++
 gtsam/geometry/CalibratedCamera.h |  2 ++
 gtsam/geometry/PinholeCamera.h    |  3 +++
 gtsam/geometry/Point2.h           |  2 ++
 gtsam/geometry/Point3.h           |  2 ++
 gtsam/geometry/Pose2.h            |  2 ++
 gtsam/geometry/Pose3.h            |  2 ++
 gtsam/geometry/Rot2.h             |  2 ++
 gtsam/geometry/Rot3.h             |  2 ++
 gtsam/geometry/StereoPoint2.h     |  2 ++
 15 files changed, 39 insertions(+), 16 deletions(-)

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index 66e7d79a4..0a66eb01f 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -8,8 +8,6 @@
 #pragma once
 
 #include <boost/pool/singleton_pool.hpp>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/export.hpp>
 #include <gtsam/base/Value.h>
 
 namespace gtsam {
@@ -21,16 +19,7 @@ private:
 	struct PoolTag { };
 
 protected:
-	DerivedValue() {
-  	// Register the base/derived class relationship for boost::serialization
-		// See: http://www.boost.org/doc/libs/1_45_0/libs/serialization/doc/serialization.html#runtimecasting
-		static bool first = true;
-		if (first) {
-			boost::serialization::void_cast_register<DERIVED, Value>(
-					static_cast<DERIVED *>(NULL), static_cast<Value *>(NULL) );
-			first = false;
-		}
-	}
+	DerivedValue() {}
 
 public:
 
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 99b5248d6..3f1afdbbc 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -143,12 +143,19 @@ namespace gtsam {
     virtual ~Value() {}
 
   private:
-  	/** Serialization function */
+  	/** Serialization function
+  	 * All DERIVED classes derived from Value must put the following line in their serialization function:
+  	 * 		ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
+  	 * Alternatively, if the derived class is template-based, for example PinholeCamera<Calibration>,
+  	 * it can put the following code in its serialization function
+  	 *    ar & boost::serialization::void_cast_register<PinholeCamera<Calibration>, Value>(
+     * 	            static_cast<PinholeCamera<Calibration> *>(NULL),
+     * 	            static_cast<Value *>(NULL));
+  	 * See more: http://www.boost.org/doc/libs/1_45_0/libs/serialization/doc/serialization.html#runtimecasting
+  	 * */
   	friend class boost::serialization::access;
   	template<class ARCHIVE>
-  	void serialize(ARCHIVE & ar, const unsigned int version) {
-  		std::cout << "value serialization" << std::endl;
-  	}
+  	void serialize(ARCHIVE & ar, const unsigned int version) {}
 
   };
 
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 65dd1a19a..259a07c9b 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -111,6 +111,8 @@ private:
 	template<class Archive>
 	void serialize(Archive & ar, const unsigned int version)
 	{
+		ar & boost::serialization::make_nvp("Cal3Bundler",
+				boost::serialization::base_object<Value>(*this));
 		ar & BOOST_SERIALIZATION_NVP(f_);
 		ar & BOOST_SERIALIZATION_NVP(k1_);
 		ar & BOOST_SERIALIZATION_NVP(k2_);
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 0044f1c35..9b94eccb5 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -114,6 +114,8 @@ private:
 	template<class Archive>
 	void serialize(Archive & ar, const unsigned int version)
 	{
+		ar & boost::serialization::make_nvp("Cal3DS2",
+				boost::serialization::base_object<Value>(*this));
 		ar & BOOST_SERIALIZATION_NVP(fx_);
 		ar & BOOST_SERIALIZATION_NVP(fy_);
 		ar & BOOST_SERIALIZATION_NVP(s_);
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 910994df7..93e43483a 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -174,6 +174,8 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("Cal3_S2",
+					boost::serialization::base_object<Value>(*this));
 			ar & BOOST_SERIALIZATION_NVP(fx_);
 			ar & BOOST_SERIALIZATION_NVP(fy_);
 			ar & BOOST_SERIALIZATION_NVP(s_);
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index a8366e35f..7bbafd7d5 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -81,6 +81,8 @@ namespace gtsam {
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version)
 		{
+			ar & boost::serialization::make_nvp("Cal3_S2Stereo",
+					boost::serialization::base_object<Value>(*this));
 			ar & BOOST_SERIALIZATION_NVP(b_);
 			ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2);
 		}
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 5458b88f0..12cfc706d 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -157,6 +157,8 @@ private:
 	    friend class boost::serialization::access;
 	    template<class Archive>
 	    void serialize(Archive & ar, const unsigned int version) {
+	  		ar & boost::serialization::make_nvp("CalibratedCamera",
+	  				boost::serialization::base_object<Value>(*this));
 	      ar & BOOST_SERIALIZATION_NVP(pose_);
 	    }
 
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 7d6cb218d..fa907dd40 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -279,6 +279,9 @@ private:
       friend class boost::serialization::access;
       template<class Archive>
       void serialize(Archive & ar, const unsigned int version) {
+      	ar & boost::serialization::void_cast_register<PinholeCamera<Calibration>, Value>(
+      	            static_cast<PinholeCamera<Calibration> *>(NULL),
+      	            static_cast<Value *>(NULL));
         ar & BOOST_SERIALIZATION_NVP(pose_);
         ar & BOOST_SERIALIZATION_NVP(k_);
       }
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 898307334..fe7792733 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -189,6 +189,8 @@ private:
 	template<class ARCHIVE>
 	void serialize(ARCHIVE & ar, const unsigned int version)
 	{
+		ar & boost::serialization::make_nvp("Point2",
+ 			 boost::serialization::base_object<Value>(*this));
 		ar & BOOST_SERIALIZATION_NVP(x_);
 		ar & BOOST_SERIALIZATION_NVP(y_);
 	}
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 8233b341a..ad66a845c 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -204,6 +204,8 @@ namespace gtsam {
     template<class ARCHIVE>
       void serialize(ARCHIVE & ar, const unsigned int version)
     {
+    	ar & boost::serialization::make_nvp("Point3",
+   			 boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(x_);
       ar & BOOST_SERIALIZATION_NVP(y_);
       ar & BOOST_SERIALIZATION_NVP(z_);
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 2bcf65181..877eb93a1 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -269,6 +269,8 @@ private:
 	friend class boost::serialization::access;
 	template<class Archive>
 	void serialize(Archive & ar, const unsigned int version) {
+		ar & boost::serialization::make_nvp("Pose2",
+ 			 boost::serialization::base_object<Value>(*this));
 		ar & BOOST_SERIALIZATION_NVP(t_);
 		ar & BOOST_SERIALIZATION_NVP(r_);
 	}
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index f052d86f1..fd4bc08df 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -233,6 +233,8 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class Archive>
     void serialize(Archive & ar, const unsigned int version) {
+    	ar & boost::serialization::make_nvp("Pose3",
+    	 			 boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(R_);
       ar & BOOST_SERIALIZATION_NVP(t_);
     }
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 2adbae4bd..8e33794e4 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -239,6 +239,8 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("Rot2",
+					boost::serialization::base_object<Value>(*this));
 			ar & BOOST_SERIALIZATION_NVP(c_);
 			ar & BOOST_SERIALIZATION_NVP(s_);
 		}
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index b640ac615..8eead3ff9 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -362,6 +362,8 @@ namespace gtsam {
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version)
     {
+    	 ar & boost::serialization::make_nvp("Rot3",
+    			 boost::serialization::base_object<Value>(*this));
 #ifndef GTSAM_DEFAULT_QUATERNIONS
       ar & BOOST_SERIALIZATION_NVP(r1_);
       ar & BOOST_SERIALIZATION_NVP(r2_);
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 63538b6d5..43701e4e8 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -148,6 +148,8 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("StereoPoint2",
+					boost::serialization::base_object<Value>(*this));
 			ar & BOOST_SERIALIZATION_NVP(uL_);
 			ar & BOOST_SERIALIZATION_NVP(uR_);
 			ar & BOOST_SERIALIZATION_NVP(v_);

From 563e8fe77c33cb2c92a80176621b992abc02940d Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 3 Feb 2012 21:34:07 +0000
Subject: [PATCH 34/88] correct PinholeCamera serialization problem. Add
 detailed comments for serialization in Value.h

---
 gtsam/base/Value.h             | 34 +++++++++++++++++------
 gtsam/geometry/PinholeCamera.h |  5 ++--
 tests/testSerialization.cpp    | 50 ++++++++++++++++++++++++++++------
 3 files changed, 69 insertions(+), 20 deletions(-)

diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 3f1afdbbc..8753ff0fa 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -143,15 +143,31 @@ namespace gtsam {
     virtual ~Value() {}
 
   private:
-  	/** Serialization function
-  	 * All DERIVED classes derived from Value must put the following line in their serialization function:
-  	 * 		ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
-  	 * Alternatively, if the derived class is template-based, for example PinholeCamera<Calibration>,
-  	 * it can put the following code in its serialization function
-  	 *    ar & boost::serialization::void_cast_register<PinholeCamera<Calibration>, Value>(
-     * 	            static_cast<PinholeCamera<Calibration> *>(NULL),
-     * 	            static_cast<Value *>(NULL));
-  	 * See more: http://www.boost.org/doc/libs/1_45_0/libs/serialization/doc/serialization.html#runtimecasting
+  	/** Empty serialization function.
+  	 *
+  	 * There are two important notes for successfully serializing derived objects in Values:
+  	 * (Those derived objects are stored as pointer to this abstract base class Value)
+  	 *
+  	 * 		1. All DERIVED classes derived from Value must put the following line in their serialization function:
+  	 * 					ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
+  	 * 			or, alternatively
+  	 * 		      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
+  	 * 			See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting
+  	 *
+  	 * 		2. The source module that includes archive class headers to serialize objects of derived classes
+  	 * 		 (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either
+  	 * 		 BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros:
+  	 *
+  	 * 					BOOST_CLASS_EXPORT(DERIVED_CLASS_1)
+  	 * 					BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING")
+  	 *
+  	 * 		  See: 	http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers
+  	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export
+  	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\
+  	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export
+  	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export
+  	 * 		  The last two links explain why this export line has to be in the same source module thaat includes
+  	 * 		  any of the archive class headers.
   	 * */
   	friend class boost::serialization::access;
   	template<class ARCHIVE>
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index fa907dd40..d00f0fd90 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -20,6 +20,7 @@
 
 #include <cmath>
 #include <boost/optional.hpp>
+#include <boost/serialization/nvp.hpp>
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
@@ -279,9 +280,7 @@ private:
       friend class boost::serialization::access;
       template<class Archive>
       void serialize(Archive & ar, const unsigned int version) {
-      	ar & boost::serialization::void_cast_register<PinholeCamera<Calibration>, Value>(
-      	            static_cast<PinholeCamera<Calibration> *>(NULL),
-      	            static_cast<Value *>(NULL));
+      	ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
         ar & BOOST_SERIALIZATION_NVP(pose_);
         ar & BOOST_SERIALIZATION_NVP(k_);
       }
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index ab58aeb99..7c67953d0 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -165,6 +165,7 @@ bool equalsDereferencedXML(const T& input = T()) {
 using namespace std;
 using namespace gtsam;
 
+
 /* ************************************************************************* */
 TEST (Serialization, matrix_vector) {
 	EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
@@ -174,6 +175,24 @@ TEST (Serialization, matrix_vector) {
 	EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
 }
 
+/* ************************************************************************* */
+// Export all classes derived from Value
+BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
+BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
+BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
+BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
+BOOST_CLASS_EXPORT(gtsam::Point2)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+BOOST_CLASS_EXPORT(gtsam::Pose2)
+BOOST_CLASS_EXPORT(gtsam::Pose3)
+BOOST_CLASS_EXPORT(gtsam::Rot2)
+BOOST_CLASS_EXPORT(gtsam::Rot3)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
+BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
+
+/* ************************************************************************* */
 Point3 pt3(1.0, 2.0, 3.0);
 Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
 Pose3 pose3(rt3, pt3);
@@ -189,7 +208,6 @@ PinholeCamera<Cal3_S2> cam1(pose3, cal1);
 StereoCamera cam2(pose3, cal4ptr);
 StereoPoint2 spt(1.0, 2.0, 3.0);
 
-
 /* ************************************************************************* */
 TEST (Serialization, text_geometry) {
 	EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
@@ -232,6 +250,25 @@ TEST (Serialization, xml_geometry) {
 	EXPECT(equalsXML(spt));
 }
 
+/* ************************************************************************* */
+typedef PinholeCamera<Cal3_S2> 				PinholeCal3S2;
+typedef PinholeCamera<Cal3DS2> 				PinholeCal3DS2;
+typedef PinholeCamera<Cal3Bundler> 		PinholeCal3Bundler;
+
+typedef TypedSymbol<Cal3_S2,'a'> 				PinholeCal3S2Key;
+typedef TypedSymbol<Cal3DS2,'s'> 				PinholeCal3DS2Key;
+typedef TypedSymbol<Cal3Bundler,'d'> 		PinholeCal3BundlerKey;
+
+TEST (Serialization, TemplatedValues) {
+	Values values;
+	values.insert(PinholeCal3S2Key(0), 	PinholeCal3S2(pose3, cal1));
+	values.insert(PinholeCal3DS2Key(5), PinholeCal3DS2(pose3, cal2));
+	values.insert(PinholeCal3BundlerKey(47), PinholeCal3Bundler(pose3, cal3));
+	values.insert(PinholeCal3S2Key(5), 	PinholeCal3S2(pose3, cal1));
+	EXPECT(equalsObj(values));
+	EXPECT(equalsXML(values));
+}
+
 /* ************************************************************************* */
 // Export Noisemodels
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
@@ -437,13 +474,10 @@ TEST (Serialization, gaussianISAM) {
 
 /* ************************************************************************* */
 /* Create GUIDs for factors in simulated2D */
-BOOST_CLASS_EXPORT_GUID(simulated2D::Prior,       "gtsam::simulated2D::Prior"      );
-BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry,    "gtsam::simulated2D::Odometry"   );
-BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement");
-BOOST_CLASS_EXPORT(gtsam::Point2)
-BOOST_CLASS_EXPORT(gtsam::Point3)
-BOOST_CLASS_EXPORT(gtsam::Pose2)
-BOOST_CLASS_EXPORT(gtsam::Pose3)
+BOOST_CLASS_EXPORT_GUID(simulated2D::Prior,       "gtsam::simulated2D::Prior"      )
+BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry,    "gtsam::simulated2D::Odometry"   )
+BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
+
 /* ************************************************************************* */
 TEST (Serialization, smallExample) {
 	using namespace example;

From 8ee1370965dc318086f65681c980a247b6b60b76 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 3 Feb 2012 21:37:49 +0000
Subject: [PATCH 35/88] fix typo in comment

---
 gtsam/base/Value.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 8753ff0fa..8a6482b37 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -145,8 +145,8 @@ namespace gtsam {
   private:
   	/** Empty serialization function.
   	 *
-  	 * There are two important notes for successfully serializing derived objects in Values:
-  	 * (Those derived objects are stored as pointer to this abstract base class Value)
+  	 * There are two important things that users need to do to serialize derived objects in Values successfully:
+  	 * (Those derived objects are stored in Values as pointer to this abstract base class Value)
   	 *
   	 * 		1. All DERIVED classes derived from Value must put the following line in their serialization function:
   	 * 					ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
@@ -166,7 +166,7 @@ namespace gtsam {
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export
-  	 * 		  The last two links explain why this export line has to be in the same source module thaat includes
+  	 * 		  The last two links explain why this export line has to be in the same source module that includes
   	 * 		  any of the archive class headers.
   	 * */
   	friend class boost::serialization::access;

From 1ef82b6e84bed7b38a1d19e58eb1fdfe679ebe0e Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 3 Feb 2012 21:40:36 +0000
Subject: [PATCH 36/88] update comment for serialization

---
 gtsam/base/Value.h | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 8a6482b37..394a11e72 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -149,24 +149,28 @@ namespace gtsam {
   	 * (Those derived objects are stored in Values as pointer to this abstract base class Value)
   	 *
   	 * 		1. All DERIVED classes derived from Value must put the following line in their serialization function:
-  	 * 					ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
+  	 * 			\code
+  	  					ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
+						\endcode
   	 * 			or, alternatively
-  	 * 		      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
+  	 * 			\code
+  	  		      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
+  	  		  \endcode
   	 * 			See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting
   	 *
   	 * 		2. The source module that includes archive class headers to serialize objects of derived classes
   	 * 		 (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either
   	 * 		 BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros:
-  	 *
-  	 * 					BOOST_CLASS_EXPORT(DERIVED_CLASS_1)
-  	 * 					BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING")
-  	 *
+  	 	 	 	 	 \code
+  	  					BOOST_CLASS_EXPORT(DERIVED_CLASS_1)
+  	  					BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING")
+  	 	 	 	 	 \endcode
   	 * 		  See: 	http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export
   	 * 		  			http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export
-  	 * 		  The last two links explain why this export line has to be in the same source module that includes
+  	 * 		  The last two links explain why these export lines have to be in the same source module that includes
   	 * 		  any of the archive class headers.
   	 * */
   	friend class boost::serialization::access;

From b849fbec1627e09a20565a52150e9d8657e99eb8 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 3 Feb 2012 22:23:41 +0000
Subject: [PATCH 37/88] Fixed compilation errors

---
 gtsam/nonlinear/ISAM2.h  |  1 -
 gtsam/nonlinear/Values.h | 20 ++++++++++++++------
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 1bde623a8..00d07a040 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -315,7 +315,6 @@ public:
 
   typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
   typedef ISAM2<CONDITIONAL> This; ///< This class
-  typedef Values Values;
   typedef GRAPH Graph;
 
   /** Create an empty ISAM2 instance */
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index a0dc33a74..0aa67c63d 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -82,10 +82,18 @@ namespace gtsam {
     typedef boost::shared_ptr<Values> shared_ptr;
 
     /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
-    typedef std::pair<const Symbol&, const Value&> ConstKeyValuePair;
+    struct ConstKeyValuePair {
+      const Symbol& first;
+      const Value& second;
+      ConstKeyValuePair(const Symbol& key, const Value& value) : first(key), second(value) {}
+    };
 
     /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
-    typedef std::pair<const Symbol&, Value&> KeyValuePair;
+    struct KeyValuePair {
+      const Symbol& first;
+      Value& second;
+      KeyValuePair(const Symbol& key, Value& value) : first(key), second(value) {}
+    };
 
     /// Mutable forward iterator, with value type KeyValuePair
     typedef boost::transform_iterator<
@@ -178,10 +186,10 @@ namespace gtsam {
     VectorValues zeroVectors(const Ordering& ordering) const;
 
   private:
-    static std::pair<const Symbol&, const Value&> make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
-      return std::make_pair<const Symbol&, const Value&>(key_value.first, *key_value.second); }
-    static std::pair<const Symbol&, Value&> make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
-      return std::make_pair<const Symbol&, Value&>(key_value.first, *key_value.second); }
+    static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
+      return ConstKeyValuePair(key_value.first, *key_value.second); }
+    static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
+      return KeyValuePair(key_value.first, *key_value.second); }
 
   public:
     const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }

From f4515d7b30c2c631216525de79270c8628160db6 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Sun, 5 Feb 2012 22:34:35 +0000
Subject: [PATCH 38/88] put back Value in slam sub-namespaces for the wrapper
 to interface with MATLAB. Cannot solve the return shared_ptr problem in
 NonlinearOptimizationParameters::newDecreaseThresholds

---
 examples/PlanarSLAMExample_easy.cpp          |   4 +-
 examples/Pose2SLAMExample_advanced.cpp       |   4 +-
 examples/Pose2SLAMExample_easy.cpp           |   4 +-
 gtsam.h                                      |   6 +-
 gtsam/slam/planarSLAM.h                      | 127 +++++++++++--------
 gtsam/slam/pose2SLAM.h                       |  19 +++
 gtsam/slam/simulated2D.h                     |  54 ++++++++
 gtsam/slam/simulated2DOriented.h             |  24 ++++
 gtsam/slam/tests/testPlanarSLAM.cpp          |   8 +-
 gtsam/slam/tests/testPose2SLAM.cpp           |  24 ++--
 gtsam/slam/tests/testSimulated2D.cpp         |   2 +-
 gtsam/slam/tests/testSimulated2DOriented.cpp |   2 +-
 tests/testNonlinearISAM.cpp                  |   8 +-
 tests/testSerialization.cpp                  |   6 +-
 14 files changed, 209 insertions(+), 83 deletions(-)

diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp
index b71b80876..d3aecb4b6 100644
--- a/examples/PlanarSLAMExample_easy.cpp
+++ b/examples/PlanarSLAMExample_easy.cpp
@@ -76,7 +76,7 @@ int main(int argc, char** argv) {
 	graph.print("full graph");
 
 	// initialize to noisy points
-	Values initialEstimate;
+	planarSLAM::Values initialEstimate;
 	initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -86,7 +86,7 @@ int main(int argc, char** argv) {
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	Values result = optimize<Graph>(graph, initialEstimate);
+	planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp
index 398fec627..bb7f60516 100644
--- a/examples/Pose2SLAMExample_advanced.cpp
+++ b/examples/Pose2SLAMExample_advanced.cpp
@@ -57,7 +57,7 @@ int main(int argc, char** argv) {
 
   /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
-	shared_ptr<Values> initial(new Values);
+	shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
 	Optimizer optimizer(graph, initial, ordering, params);
 	Optimizer optimizer_result = optimizer.levenbergMarquardt();
 
-	Values result = *optimizer_result.values();
+	pose2SLAM::Values result = *optimizer_result.values();
 	result.print("final result");
 
 	/* Get covariances */
diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp
index 402a3d30f..43a648398 100644
--- a/examples/Pose2SLAMExample_easy.cpp
+++ b/examples/Pose2SLAMExample_easy.cpp
@@ -55,7 +55,7 @@ int main(int argc, char** argv) {
 
     /* 3. Create the data structure to hold the initial estinmate to the solution
      * initialize to noisy points */
-	Values initial;
+	pose2SLAM::Values initial;
 	initial.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -63,7 +63,7 @@ int main(int argc, char** argv) {
 
 	/* 4 Single Step Optimization
 	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
-	Values result = optimize<Graph>(graph, initial);
+	pose2SLAM::Values result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 
diff --git a/gtsam.h b/gtsam.h
index 97dea0b31..5f11d88ad 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -389,6 +389,8 @@ class NonlinearOptimizationParameters {
 	NonlinearOptimizationParameters(double absDecrease, double relDecrease,
 			double sumError, int iIters, double lambda, double lambdaFactor);
 	void print(string s) const;
+	static gtsam::NonlinearOptimizationParameters* newDrecreaseThresholds(double absDecrease,
+			double relDecrease);
 };
 
 
@@ -427,14 +429,14 @@ class Graph {
 	void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
 	void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range,
 			const gtsam::SharedNoiseModel& noiseModel);
-	Values optimize(const Values& initialEstimate);
+	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
 };
 
 class Odometry {
 	Odometry(int key1, int key2, const gtsam::Pose2& measured,
 			const gtsam::SharedNoiseModel& model);
 	void print(string s) const;
-	gtsam::GaussianFactor* linearize(const Values& center, const Ordering& ordering) const;
+	gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const;
 };
 
 class Optimizer {
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 5740d893f..50fae36a5 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -31,70 +31,97 @@
 // Use planarSLAM namespace for specific SLAM instance
 namespace planarSLAM {
 
-  using namespace gtsam;
+	using namespace gtsam;
 
-  /// Typedef for a PoseKey with Pose2 data and 'x' symbol
-  typedef TypedSymbol<Pose2, 'x'> PoseKey;
+	/// Typedef for a PoseKey with Pose2 data and 'x' symbol
+	typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
-  /// Typedef for a PointKey with Point2 data and 'l' symbol
-  typedef TypedSymbol<Point2, 'l'> PointKey;
-  /**
-   * List of typedefs for factors
+	/// Typedef for a PointKey with Point2 data and 'l' symbol
+	typedef TypedSymbol<Point2, 'l'> PointKey;
+	/**
+	 * List of typedefs for factors
+	 */
+	/// A hard constraint for PoseKeys to enforce particular values
+	typedef NonlinearEquality<PoseKey> Constraint;
+	/// A prior factor to bias the value of a PoseKey
+	typedef PriorFactor<PoseKey> Prior;
+	/// A factor between two PoseKeys set with a Pose2
+	typedef BetweenFactor<PoseKey> Odometry;
+	/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
+	typedef BearingFactor<PoseKey, PointKey> Bearing;
+	/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
+	typedef RangeFactor<PoseKey, PointKey> Range;
+	/// A factor between a PoseKey and a PointKey to express difference in rotation and location
+	typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
+
+  /**  Values class, using specific PoseKeys and PointKeys
+   * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
    */
-		/// A hard constraint for PoseKeys to enforce particular values
-		typedef NonlinearEquality<PoseKey> Constraint;
-		/// A prior factor to bias the value of a PoseKey
-		typedef PriorFactor<PoseKey> Prior;
-		/// A factor between two PoseKeys set with a Pose2
-		typedef BetweenFactor<PoseKey> Odometry;
-		/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
-		typedef BearingFactor<PoseKey, PointKey> Bearing;
-		/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
-		typedef RangeFactor<PoseKey, PointKey> Range;
-		/// A factor between a PoseKey and a PointKey to express difference in rotation and location
-		typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
+  struct Values: public gtsam::Values {
 
-		/// Creates a NonlinearFactorGraph with the Values type
-		struct Graph: public NonlinearFactorGraph {
+    /// Default constructor
+    Values() {}
 
-		  /// Default constructor for a NonlinearFactorGraph
-		  Graph(){}
+    /// Copy constructor
+    Values(const gtsam::Values& values) :
+      gtsam::Values(values) {
+    }
 
-		  /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
-		  Graph(const NonlinearFactorGraph& graph);
+    /// get a pose
+    Pose2 pose(int key) const {	return (*this)[PoseKey(key)]; }
 
-		  /// Biases the value of PoseKey key with Pose2 p given a noise model
-		  void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
+    /// get a point
+    Point2 point(int key) const {	return (*this)[PointKey(key)]; }
 
-		  /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
-		  void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
+    /// insert a pose
+    void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
 
-		  /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
-		  void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
-		      const SharedNoiseModel& model);
+    /// insert a point
+    void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
+  };
 
-		  /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
-		  void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
-		      const SharedNoiseModel& model);
 
-		  /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
-		  void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
-		      const SharedNoiseModel& model);
+	/// Creates a NonlinearFactorGraph with the Values type
+	struct Graph: public NonlinearFactorGraph {
 
-		  /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
-		  void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
-		      const Rot2& bearing, double range, const SharedNoiseModel& model);
+		/// Default constructor for a NonlinearFactorGraph
+		Graph(){}
 
-		  /// Optimize
-		  Values optimize(const Values& initialEstimate) {
-		    typedef NonlinearOptimizer<Graph> Optimizer;
-		    return *Optimizer::optimizeLM(*this, initialEstimate,
-		        NonlinearOptimizationParameters::LAMBDA);
-		  }
-		};
+		/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
+		Graph(const NonlinearFactorGraph& graph);
 
-		/// Optimizer
-		typedef NonlinearOptimizer<Graph> Optimizer;
+		/// Biases the value of PoseKey key with Pose2 p given a noise model
+		void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
+
+		/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
+		void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
+
+		/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
+		void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
+				const SharedNoiseModel& model);
+
+		/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
+		void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
+				const SharedNoiseModel& model);
+
+		/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
+		void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
+				const SharedNoiseModel& model);
+
+		/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
+		void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
+				const Rot2& bearing, double range, const SharedNoiseModel& model);
+
+		/// Optimize
+		Values optimize(const Values& initialEstimate) {
+			typedef NonlinearOptimizer<Graph> Optimizer;
+			return *Optimizer::optimizeLM(*this, initialEstimate,
+					NonlinearOptimizationParameters::LAMBDA);
+		}
+	};
+
+/// Optimizer
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 } // planarSLAM
 
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 89a3b7ec6..23e087be6 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -36,6 +36,25 @@ namespace pose2SLAM {
   /// Keys with Pose2 and symbol 'x'
   typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
+  /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
+  struct Values: public gtsam::Values {
+
+    /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) :
+        gtsam::Values(values) {
+    }
+
+    // Convenience for MATLAB wrapper, which does not allow for identically named methods
+
+    /// get a pose
+    Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
+
+    /// insert a pose
+    void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
+  };
 
   /**
    * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index 4798bae84..a9ab58d3c 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -32,6 +32,60 @@ namespace simulated2D {
   typedef TypedSymbol<Point2, 'x'> PoseKey;
   typedef TypedSymbol<Point2, 'l'> PointKey;
 
+  /**
+   *  Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
+   */
+  class Values: public gtsam::Values {
+  private:
+  	int nrPoses_, nrPoints_;
+
+  public:
+    typedef gtsam::Values Base;  ///< base class
+    typedef boost::shared_ptr<Point2> sharedPoint;  ///< shortcut to shared Point type
+
+    /// Constructor
+    Values() : nrPoses_(0), nrPoints_(0) {
+    }
+
+    /// Copy constructor
+    Values(const Base& base) :
+        Base(base), nrPoses_(0), nrPoints_(0) {
+    }
+
+    /// Insert a pose
+    void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
+      insert(i, p);
+      nrPoses_++;
+    }
+
+    /// Insert a point
+    void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
+      insert(j, p);
+      nrPoints_++;
+    }
+
+    /// Number of poses
+    int nrPoses() const {
+      return nrPoses_;
+    }
+
+    /// Number of points
+    int nrPoints() const {
+      return nrPoints_;
+    }
+
+    /// Return pose i
+    Point2 pose(const simulated2D::PoseKey& i) const {
+      return (*this)[i];
+    }
+
+    /// Return point j
+    Point2 point(const simulated2D::PointKey& j) const {
+      return (*this)[j];
+    }
+  };
+
+
   /// Prior on a single pose
   inline Point2 prior(const Point2& x) {
     return x;
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index 51a2903fe..0c5533291 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -31,6 +31,30 @@ namespace simulated2DOriented {
   typedef TypedSymbol<Point2, 'l'> PointKey;
   typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
+  /// Specialized Values structure with syntactic sugar for
+  /// compatibility with matlab
+  class Values: public gtsam::Values {
+  	int nrPoses_, nrPoints_;
+  public:
+    Values() : nrPoses_(0), nrPoints_(0)  {}
+
+    void insertPose(const PoseKey& i, const Pose2& p) {
+      insert(i, p);
+      nrPoses_++;
+    }
+
+    void insertPoint(const PointKey& j, const Point2& p) {
+      insert(j, p);
+      nrPoints_++;
+    }
+
+    int nrPoses() const {	return nrPoses_;	}
+    int nrPoints() const { return nrPoints_;	}
+
+    Pose2 pose(const PoseKey& i) const { return (*this)[i];	}
+    Point2 point(const PointKey& j) const { return (*this)[j]; }
+  };
+
   //TODO:: point prior is not implemented right now
 
   /// Prior on a single pose
diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp
index a74f79d49..56ac71923 100644
--- a/gtsam/slam/tests/testPlanarSLAM.cpp
+++ b/gtsam/slam/tests/testPlanarSLAM.cpp
@@ -51,7 +51,7 @@ TEST( planarSLAM, BearingFactor )
 	planarSLAM::Bearing factor(2, 3, z, sigma);
 
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -79,7 +79,7 @@ TEST( planarSLAM, RangeFactor )
 	planarSLAM::Range factor(2, 3, z, sigma);
 
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -106,7 +106,7 @@ TEST( planarSLAM, BearingRangeFactor )
 	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
 
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -139,7 +139,7 @@ TEST( planarSLAM, PoseConstraint_equals )
 TEST( planarSLAM, constructor )
 {
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PoseKey(3), x3);
 	c.insert(PointKey(3), l3);
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index 9151dd6af..a8828a83a 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -345,13 +345,13 @@ using namespace pose2SLAM;
 TEST(Pose2Values, pose2Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	Values expected;
+	pose2SLAM::Values expected;
 	expected.insert(pose2SLAM::PoseKey(0), Pose2( 1,  0,   M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(1), Pose2( 0,  1, - M_PI  ));
 	expected.insert(pose2SLAM::PoseKey(2), Pose2(-1,  0, - M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1,   0     ));
 
-	Values actual = pose2SLAM::circle(4,1.0);
+	pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -359,21 +359,21 @@ TEST(Pose2Values, pose2Circle )
 TEST(Pose2SLAM, expmap )
 {
 	// expected is circle shifted to right
-	Values expected;
+	pose2SLAM::Values expected;
 	expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1,  0,   M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1,  1, - M_PI  ));
 	expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9,  0, - M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
-  Values circle(pose2SLAM::circle(4,1.0));
+	pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
   Ordering ordering(*circle.orderingArbitrary());
 	VectorValues delta(circle.dims(ordering));
 	delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
 	delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
 	delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
 	delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
-	Values actual = circle.retract(delta, ordering);
+	pose2SLAM::Values actual = circle.retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -386,7 +386,7 @@ TEST( Pose2Prior, error )
 {
 	// Choose a linearization point
 	Pose2 p1(1, 0, 0); // robot at (1,0)
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1), p1);
 
 	// Create factor
@@ -408,7 +408,7 @@ TEST( Pose2Prior, error )
 	VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
 	addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
 	VectorValues plus = delta + addition;
-	Values x1 = x0.retract(plus, ordering);
+	pose2SLAM::Values x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -430,7 +430,7 @@ LieVector hprior(const Pose2& p1) {
 TEST( Pose2Prior, linearize )
 {
 	// Choose a linearization point at ground truth
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1),priorVal);
 
 	// Actual linearization
@@ -450,7 +450,7 @@ TEST( Pose2Factor, error )
 	// Choose a linearization point
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1), p1);
 	x0.insert(pose2SLAM::PoseKey(2), p2);
 
@@ -474,7 +474,7 @@ TEST( Pose2Factor, error )
 	// Check error after increasing p2
 	VectorValues plus = delta;
 	plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
-	Values x1 = x0.retract(plus, ordering);
+	pose2SLAM::Values x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -491,7 +491,7 @@ TEST( Pose2Factor, rhs )
 	// 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.1) looking at negative (ground truth is at -1,4)
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1),p1);
 	x0.insert(pose2SLAM::PoseKey(2),p2);
 
@@ -521,7 +521,7 @@ TEST( Pose2Factor, linearize )
 	// Choose a linearization point at ground truth
 	Pose2 p1(1,2,M_PI_2);
 	Pose2 p2(-1,4,M_PI);
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1),p1);
 	x0.insert(pose2SLAM::PoseKey(2),p2);
 
diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp
index 30c5561ce..3513927a7 100644
--- a/gtsam/slam/tests/testSimulated2D.cpp
+++ b/gtsam/slam/tests/testSimulated2D.cpp
@@ -30,7 +30,7 @@ using namespace simulated2D;
 /* ************************************************************************* */
 TEST( simulated2D, Simulated2DValues )
 {
-	Values actual;
+	simulated2D::Values actual;
 	actual.insert(simulated2D::PoseKey(1),Point2(1,1));
 	actual.insert(simulated2D::PointKey(2),Point2(2,2));
   EXPECT(assert_equal(actual,actual,1e-9));
diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp
index f5ebfa177..64844cb2a 100644
--- a/gtsam/slam/tests/testSimulated2DOriented.cpp
+++ b/gtsam/slam/tests/testSimulated2DOriented.cpp
@@ -57,7 +57,7 @@ TEST( simulated2DOriented, constructor )
 	SharedDiagonal model(Vector_(3, 1., 1., 1.));
 	simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
 
-	Values config;
+	simulated2DOriented::Values config;
 	config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
 	config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
 	boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index 0ad4e03ae..2316c5652 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
 	Graph start_factors;
 	start_factors.addPoseConstraint(key, cur_pose);
 
-	Values init;
-	Values expected;
+	planarSLAM::Values init;
+	planarSLAM::Values expected;
 	init.insert(key, cur_pose);
 	expected.insert(key, cur_pose);
 	isam.update(start_factors, init);
@@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 		Graph new_factors;
 		PoseKey key1(i-1), key2(i);
 		new_factors.addOdometry(key1, key2, z, model);
-		Values new_init;
+		planarSLAM::Values new_init;
 
 		// perform a check on changing orderings
 		if (i == 5) {
@@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 	}
 
 	// verify values - all but the last one should be very close
-	Values actual = isam.estimate();
+	planarSLAM::Values actual = isam.estimate();
 	for (size_t i=0; i<nrPoses; ++i) {
 		PoseKey cur_key(i);
 		EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index 7c67953d0..23551bbae 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -501,7 +501,7 @@ BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint,  "gtsam::planarSLAM::Constraint"
 /* ************************************************************************* */
 TEST (Serialization, planar_system) {
 	using namespace planarSLAM;
-	Values values;
+	planarSLAM::Values values;
 	values.insert(PointKey(3), Point2(1.0, 2.0));
 	values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
 
@@ -527,7 +527,7 @@ TEST (Serialization, planar_system) {
 	// text
 	EXPECT(equalsObj<PoseKey>(PoseKey(2)));
 	EXPECT(equalsObj<PointKey>(PointKey(3)));
-	EXPECT(equalsObj<Values>(values));
+	EXPECT(equalsObj<planarSLAM::Values>(values));
 	EXPECT(equalsObj<Prior>(prior));
 	EXPECT(equalsObj<Bearing>(bearing));
 	EXPECT(equalsObj<BearingRange>(bearingRange));
@@ -539,7 +539,7 @@ TEST (Serialization, planar_system) {
 	// xml
 	EXPECT(equalsXML<PoseKey>(PoseKey(2)));
 	EXPECT(equalsXML<PointKey>(PointKey(3)));
-	EXPECT(equalsXML<Values>(values));
+	EXPECT(equalsXML<planarSLAM::Values>(values));
 	EXPECT(equalsXML<Prior>(prior));
 	EXPECT(equalsXML<Bearing>(bearing));
 	EXPECT(equalsXML<BearingRange>(bearingRange));

From 71e757d2cf7a04ff9e9a7b24becc16977e5adb98 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Sun, 5 Feb 2012 23:00:57 +0000
Subject: [PATCH 39/88] fix typo in "newDrecreaseThresholds" in
 NonlinearOptimizationParams

---
 examples/Pose2SLAMExample_advanced.cpp              | 2 +-
 gtsam.h                                             | 2 +-
 gtsam/nonlinear/NonlinearOptimizationParameters.cpp | 2 +-
 gtsam/nonlinear/NonlinearOptimizationParameters.h   | 2 +-
 gtsam/slam/tests/testPose2SLAM.cpp                  | 6 +++---
 gtsam/slam/tests/testPose3SLAM.cpp                  | 2 +-
 6 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp
index bb7f60516..cab263047 100644
--- a/examples/Pose2SLAMExample_advanced.cpp
+++ b/examples/Pose2SLAMExample_advanced.cpp
@@ -68,7 +68,7 @@ int main(int argc, char** argv) {
 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
 
 	/* 4.2.2 set up solver and optimize */
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
 	Optimizer optimizer(graph, initial, ordering, params);
 	Optimizer optimizer_result = optimizer.levenbergMarquardt();
 
diff --git a/gtsam.h b/gtsam.h
index 5f11d88ad..970980108 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -389,7 +389,7 @@ class NonlinearOptimizationParameters {
 	NonlinearOptimizationParameters(double absDecrease, double relDecrease,
 			double sumError, int iIters, double lambda, double lambdaFactor);
 	void print(string s) const;
-	static gtsam::NonlinearOptimizationParameters* newDrecreaseThresholds(double absDecrease,
+	static gtsam::NonlinearOptimizationParameters* newDecreaseThresholds(double absDecrease,
 			double relDecrease);
 };
 
diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.cpp b/gtsam/nonlinear/NonlinearOptimizationParameters.cpp
index 4da8fc7a5..e80c3b59c 100644
--- a/gtsam/nonlinear/NonlinearOptimizationParameters.cpp
+++ b/gtsam/nonlinear/NonlinearOptimizationParameters.cpp
@@ -102,7 +102,7 @@ NonlinearOptimizationParameters::newLambda(double lambda) {
 
 /* ************************************************************************* */
 NonlinearOptimizationParameters::shared_ptr
-NonlinearOptimizationParameters::newDrecreaseThresholds(double absDecrease,
+NonlinearOptimizationParameters::newDecreaseThresholds(double absDecrease,
 		double relDecrease) {
 	shared_ptr ptr(boost::make_shared<NonlinearOptimizationParameters>());
 	ptr->absDecrease_ = absDecrease;
diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.h b/gtsam/nonlinear/NonlinearOptimizationParameters.h
index 68595b711..07e01ea58 100644
--- a/gtsam/nonlinear/NonlinearOptimizationParameters.h
+++ b/gtsam/nonlinear/NonlinearOptimizationParameters.h
@@ -110,7 +110,7 @@ namespace gtsam {
 		static shared_ptr newLambda(double lambda);
 
 		/// a copy of old instance except new thresholds
-		static shared_ptr newDrecreaseThresholds(double absDecrease,
+		static shared_ptr newDecreaseThresholds(double absDecrease,
 				double relDecrease);
 
 		/// a copy of old instance except new QR flag
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index a8828a83a..73fec2a29 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -162,7 +162,7 @@ TEST(Pose2Graph, optimize) {
   *ordering += "x0","x1";
   typedef NonlinearOptimizer<pose2SLAM::Graph> Optimizer;
 
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
   Optimizer optimizer0(fg, initial, ordering, params);
   Optimizer optimizer = optimizer0.levenbergMarquardt();
 
@@ -200,7 +200,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
   *ordering += "x0","x1","x2";
 
   // optimize
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
@@ -243,7 +243,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
   *ordering += "x0","x1","x2","x3","x4","x5";
 
   // optimize
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 03ca40dfa..79cc9fe1e 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -77,7 +77,7 @@ TEST(Pose3Graph, optimizeCircle) {
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
   *ordering += "x0","x1","x2","x3","x4","x5";
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
   pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 

From 811be62ed3f42cdfa32fde131db10bf027bf270e Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 6 Feb 2012 00:44:25 +0000
Subject: [PATCH 41/88] Working on templating factors on Value type instead of
 key type

---
 gtsam/nonlinear/NonlinearFactor.h   | 306 ++++++++++++----------------
 gtsam/slam/BearingFactor.h          |  30 +--
 gtsam/slam/BearingRangeFactor.h     |  40 ++--
 gtsam/slam/BetweenFactor.h          |  30 ++-
 gtsam/slam/BoundingConstraint.h     |  14 +-
 gtsam/slam/GeneralSFMFactor.h       |  30 +--
 gtsam/slam/PartialPriorFactor.h     |  16 +-
 gtsam/slam/PriorFactor.h            |  29 ++-
 gtsam/slam/ProjectionFactor.h       |  40 ++--
 gtsam/slam/RangeFactor.h            |  33 ++-
 gtsam/slam/planarSLAM.cpp           |  28 ++-
 gtsam/slam/planarSLAM.h             | 103 +++++-----
 gtsam/slam/pose2SLAM.cpp            |  12 +-
 gtsam/slam/pose2SLAM.h              |  17 +-
 gtsam/slam/pose3SLAM.cpp            |  14 +-
 gtsam/slam/pose3SLAM.h              |  18 +-
 gtsam/slam/simulated2D.h            |  83 ++++----
 gtsam/slam/simulated2DConstraints.h |  60 +++---
 gtsam/slam/simulated2DOriented.h    |  39 ++--
 tests/testNonlinearFactor.cpp       |  53 +++--
 20 files changed, 467 insertions(+), 528 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 07334eac9..ba2198b1b 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -68,7 +68,7 @@ protected:
 
 public:
 
-  typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
+  typedef boost::shared_ptr<NonlinearFactor> shared_ptr;
 
 	/// @name Standard Constructors
 	/// @{
@@ -77,18 +77,6 @@ public:
   NonlinearFactor() {
   }
 
-  /**
-   * Constructor
-   * @param keys A boost::tuple containing the variables involved in this factor,
-   * example: <tt>NonlinearFactor(make_tuple(symbol1, symbol2, symbol3))</tt>
-   */
-  template<class U1, class U2>
-  NonlinearFactor(const boost::tuples::cons<U1,U2>& keys) {
-    this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
-    // Use helper function to fill key vector, using 'cons' representation of tuple
-    __fill_from_tuple(this->keys(), 0, keys);
-  }
-
   /**
    * Constructor
    * @param keys The variables involved in this factor
@@ -186,16 +174,6 @@ public:
   /** Destructor */
   virtual ~NoiseModelFactor() {}
 
-  /**
-   * Constructor
-   * @param keys A boost::tuple containing the variables involved in this factor,
-   * example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
-   */
-  template<class U1, class U2>
-  NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys)
-  : Base(keys), noiseModel_(noiseModel) {
-  }
-
   /**
    * Constructor
    * @param keys The variables involved in this factor
@@ -320,21 +298,18 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 1
  * variable.  To derive from this class, implement evaluateError(). */
-template<class KEY>
+template<class VALUE>
 class NonlinearFactor1: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY::Value X;
+  typedef VALUE X;
 
 protected:
 
-  // The value of the key. Not const to allow serialization
-  KEY key_;
-
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor1<KEY> This;
+  typedef NonlinearFactor1<VALUE> This;
 
 public:
 
@@ -343,22 +318,24 @@ public:
 
   virtual ~NonlinearFactor1() {}
 
-  inline const KEY& key() const {	return key_; }
+  inline const Symbol& key() const { return keys_[0]; }
 
   /**
    *  Constructor
    *  @param z measurement
    *  @param key by which to look up X value in Values
    */
-  NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) :
-    Base(noiseModel, make_tuple(key1)), key_(key1) {
+  NonlinearFactor1(const SharedNoiseModel& noiseModel, const Symbol& key1) :
+    Base(noiseModel) {
+    keys_.resize(1);
+    keys_[0] = key1;
   }
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
   virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
-      const X& x1 = x[key_];
+      const X& x1 = x.at<X>(keys_[0]);
       if(H) {
         return evaluateError(x1, (*H)[0]);
       } else {
@@ -371,7 +348,7 @@ public:
 
   /** Print */
   virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor1(" << (std::string) this->key_ << ")\n";
+    std::cout << s << ": NonlinearFactor1(" << (std::string) keys_[0] << ")\n";
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -391,7 +368,6 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(key_);
   }
 };// \class NonlinearFactor1
 
@@ -399,23 +375,19 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 2
  * variables.  To derive from this class, implement evaluateError(). */
-template<class KEY1, class KEY2>
+template<class VALUE1, class VALUE2>
 class NonlinearFactor2: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
-  typedef typename KEY2::Value X2;
+  typedef VALUE1 X1;
+  typedef VALUE2 X2;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
-  KEY1 key1_;
-  KEY2 key2_;
-
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor2<KEY1, KEY2> This;
+  typedef NonlinearFactor2<VALUE1, VALUE2> This;
 
 public:
 
@@ -429,21 +401,25 @@ public:
    * @param j1 key of the first variable
    * @param j2 key of the second variable
    */
-  NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) :
-    Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {}
+  NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
+    Base(noiseModel) {
+    keys_.resize(2);
+    keys_[0] = j1;
+    keys_[1] = j2;
+  }
 
   virtual ~NonlinearFactor2() {}
 
   /** methods to retrieve both keys */
-  inline const KEY1& key1() const { return key1_;	}
-  inline const KEY2& key2() const {	return key2_;	}
+  inline const Symbol& key1() const { return keys_[0];	}
+  inline const Symbol& key2() const {	return keys_[1];	}
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
   virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
-      const X1& x1 = x[key1_];
-      const X2& x2 = x[key2_];
+      const X1& x1 = x.at<X1>(keys_[0]);
+      const X2& x2 = x.at<X2>(keys_[1]);
       if(H) {
         return evaluateError(x1, x2, (*H)[0], (*H)[1]);
       } else {
@@ -457,8 +433,8 @@ public:
   /** Print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearFactor2("
-    		<< (std::string) this->key1_ << ","
-    		<< (std::string) this->key2_ << ")\n";
+    		<< (std::string) keys_[0] << ","
+    		<< (std::string) keys_[1] << ")\n";
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -479,33 +455,26 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(key1_);
-    ar & BOOST_SERIALIZATION_NVP(key2_);
   }
 }; // \class NonlinearFactor2
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 3
  * variables.  To derive from this class, implement evaluateError(). */
-template<class KEY1, class KEY2, class KEY3>
+template<class VALUE1, class VALUE2, class VALUE3>
 class NonlinearFactor3: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
-  typedef typename KEY2::Value X2;
-  typedef typename KEY3::Value X3;
+  typedef VALUE1 X1;
+  typedef VALUE2 X2;
+  typedef VALUE3 X3;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
-  KEY1 key1_;
-  KEY2 key2_;
-  KEY3 key3_;
-
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
+  typedef NonlinearFactor3<VALUE1, VALUE2, VALUE3> This;
 
 public:
 
@@ -520,24 +489,29 @@ public:
    * @param j2 key of the second variable
    * @param j3 key of the third variable
    */
-  NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) :
-    Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {}
+  NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) :
+    Base(noiseModel) {
+    keys_.resize(3);
+    keys_[0] = j1;
+    keys_[1] = j2;
+    keys_[2] = j3;
+  }
 
   virtual ~NonlinearFactor3() {}
 
   /** methods to retrieve keys */
-  inline const KEY1& key1() const { return key1_; }
-  inline const KEY2& key2() const { return key2_; }
-  inline const KEY3& key3() const { return key3_; }
+  inline const Symbol& key1() const { return keys_[0]; }
+  inline const Symbol& key2() const { return keys_[1]; }
+  inline const Symbol& key3() const { return keys_[2]; }
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
   virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
-        return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
+        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
       else
-        return evaluateError(x[key1_], x[key2_], x[key3_]);
+        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
     } else {
       return zero(this->dim());
     }
@@ -546,9 +520,9 @@ public:
   /** Print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearFactor3("
-    		<< (std::string) this->key1_ << ","
-    		<< (std::string) this->key2_ << ","
-    		<< (std::string) this->key3_ << ")\n";
+    		<< (std::string) this->keys_[0] << ","
+    		<< (std::string) this->keys_[1] << ","
+    		<< (std::string) this->keys_[2] << ")\n";
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -572,36 +546,27 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(key1_);
-    ar & BOOST_SERIALIZATION_NVP(key2_);
-    ar & BOOST_SERIALIZATION_NVP(key3_);
   }
 }; // \class NonlinearFactor3
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 4
  * variables.  To derive from this class, implement evaluateError(). */
-template<class KEY1, class KEY2, class KEY3, class KEY4>
+template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
 class NonlinearFactor4: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
-  typedef typename KEY2::Value X2;
-  typedef typename KEY3::Value X3;
-  typedef typename KEY4::Value X4;
+  typedef VALUE1 X1;
+  typedef VALUE2 X2;
+  typedef VALUE3 X3;
+  typedef VALUE4 X4;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
-  KEY1 key1_;
-  KEY2 key2_;
-  KEY3 key3_;
-  KEY4 key4_;
-
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
+  typedef NonlinearFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
 
 public:
 
@@ -617,25 +582,31 @@ public:
    * @param j3 key of the third variable
    * @param j4 key of the fourth variable
    */
-  NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) :
-    Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {}
+  NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) :
+    Base(noiseModel) {
+    keys_.resize(4);
+    keys_[0] = j1;
+    keys_[1] = j2;
+    keys_[2] = j3;
+    keys_[3] = j4;
+  }
 
   virtual ~NonlinearFactor4() {}
 
   /** methods to retrieve keys */
-  inline const KEY1& key1() const { return key1_; }
-  inline const KEY2& key2() const { return key2_; }
-  inline const KEY3& key3() const { return key3_; }
-  inline const KEY4& key4() const { return key4_; }
+  inline const Symbol& key1() const { return keys_[0]; }
+  inline const Symbol& key2() const { return keys_[1]; }
+  inline const Symbol& key3() const { return keys_[2]; }
+  inline const Symbol& key4() const { return keys_[3]; }
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
   virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
-  			return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
+  			return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
   		else
-  			return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
+  			return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
   	} else {
   		return zero(this->dim());
   	}
@@ -644,10 +615,10 @@ public:
   /** Print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearFactor4("
-    		<< (std::string) this->key1_ << ","
-    		<< (std::string) this->key2_ << ","
-     		<< (std::string) this->key3_ << ","
-    		<< (std::string) this->key4_ << ")\n";
+    		<< (std::string) this->keys_[0] << ","
+    		<< (std::string) this->keys_[1] << ","
+     		<< (std::string) this->keys_[2] << ","
+    		<< (std::string) this->keys_[3] << ")\n";
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -671,39 +642,28 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(key1_);
-    ar & BOOST_SERIALIZATION_NVP(key2_);
-    ar & BOOST_SERIALIZATION_NVP(key3_);
-    ar & BOOST_SERIALIZATION_NVP(key4_);
   }
 }; // \class NonlinearFactor4
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 5
  * variables.  To derive from this class, implement evaluateError(). */
-template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
+template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
 class NonlinearFactor5: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
-  typedef typename KEY2::Value X2;
-  typedef typename KEY3::Value X3;
-  typedef typename KEY4::Value X4;
-  typedef typename KEY5::Value X5;
+  typedef VALUE1 X1;
+  typedef VALUE2 X2;
+  typedef VALUE3 X3;
+  typedef VALUE4 X4;
+  typedef VALUE5 X5;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
-  KEY1 key1_;
-  KEY2 key2_;
-  KEY3 key3_;
-  KEY4 key4_;
-  KEY5 key5_;
-
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
+  typedef NonlinearFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
 
 public:
 
@@ -720,26 +680,33 @@ public:
    * @param j4 key of the fourth variable
    * @param j5 key of the fifth variable
    */
-  NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) :
-    Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {}
+  NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) :
+    Base(noiseModel) {
+    keys_.resize(5);
+    keys_[0] = j1;
+    keys_[1] = j2;
+    keys_[2] = j3;
+    keys_[3] = j4;
+    keys_[4] = j5;
+  }
 
   virtual ~NonlinearFactor5() {}
 
   /** methods to retrieve keys */
-  inline const KEY1& key1() const { return key1_; }
-  inline const KEY2& key2() const { return key2_; }
-  inline const KEY3& key3() const { return key3_; }
-  inline const KEY4& key4() const { return key4_; }
-  inline const KEY5& key5() const { return key5_; }
+  inline const Symbol& key1() const { return keys_[0]; }
+  inline const Symbol& key2() const { return keys_[1]; }
+  inline const Symbol& key3() const { return keys_[2]; }
+  inline const Symbol& key4() const { return keys_[3]; }
+  inline const Symbol& key5() const { return keys_[4]; }
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
   virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
-        return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
+        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
       else
-        return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
+        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
   	} else {
   		return zero(this->dim());
   	}
@@ -748,11 +715,11 @@ public:
   /** Print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearFactor5("
-    		<< (std::string) this->key1_ << ","
-    		<< (std::string) this->key2_ << ","
-     		<< (std::string) this->key3_ << ","
-    		<< (std::string) this->key4_ << ","
-    		<< (std::string) this->key5_ << ")\n";
+    		<< (std::string) this->keys_[0] << ","
+    		<< (std::string) this->keys_[1] << ","
+     		<< (std::string) this->keys_[2] << ","
+    		<< (std::string) this->keys_[3] << ","
+    		<< (std::string) this->keys_[4] << ")\n";
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -777,42 +744,29 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(key1_);
-    ar & BOOST_SERIALIZATION_NVP(key2_);
-    ar & BOOST_SERIALIZATION_NVP(key3_);
-    ar & BOOST_SERIALIZATION_NVP(key4_);
-    ar & BOOST_SERIALIZATION_NVP(key5_);
   }
 }; // \class NonlinearFactor5
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 6
  * variables.  To derive from this class, implement evaluateError(). */
-template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
+template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
 class NonlinearFactor6: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
-  typedef typename KEY2::Value X2;
-  typedef typename KEY3::Value X3;
-  typedef typename KEY4::Value X4;
-  typedef typename KEY5::Value X5;
-  typedef typename KEY6::Value X6;
+  typedef VALUE1 X1;
+  typedef VALUE2 X2;
+  typedef VALUE3 X3;
+  typedef VALUE4 X4;
+  typedef VALUE5 X5;
+  typedef VALUE6 X6;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
-  KEY1 key1_;
-  KEY2 key2_;
-  KEY3 key3_;
-  KEY4 key4_;
-  KEY5 key5_;
-  KEY6 key6_;
-
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
+  typedef NonlinearFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
 
 public:
 
@@ -830,27 +784,35 @@ public:
    * @param j5 key of the fifth variable
    * @param j6 key of the fifth variable
    */
-  NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) :
-    Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {}
+  NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) :
+    Base(noiseModel) {
+    keys_.resize(6);
+    keys_[0] = j1;
+    keys_[1] = j2;
+    keys_[2] = j3;
+    keys_[3] = j4;
+    keys_[4] = j5;
+    keys_[5] = j6;
+  }
 
   virtual ~NonlinearFactor6() {}
 
   /** methods to retrieve keys */
-  inline const KEY1& key1() const { return key1_; }
-  inline const KEY2& key2() const { return key2_; }
-  inline const KEY3& key3() const { return key3_; }
-  inline const KEY4& key4() const { return key4_; }
-  inline const KEY5& key5() const { return key5_; }
-  inline const KEY6& key6() const { return key6_; }
+  inline const Symbol& key1() const { return keys_[0]; }
+  inline const Symbol& key2() const { return keys_[1]; }
+  inline const Symbol& key3() const { return keys_[2]; }
+  inline const Symbol& key4() const { return keys_[3]; }
+  inline const Symbol& key5() const { return keys_[4]; }
+  inline const Symbol& key6() const { return keys_[5]; }
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
   virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
-        return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
+        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
       else
-        return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
+        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
   	} else {
   		return zero(this->dim());
   	}
@@ -859,12 +821,12 @@ public:
   /** Print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearFactor6("
-    		<< (std::string) this->key1_ << ","
-    		<< (std::string) this->key2_ << ","
-     		<< (std::string) this->key3_ << ","
-    		<< (std::string) this->key4_ << ","
-    		<< (std::string) this->key5_ << ","
-    		<< (std::string) this->key6_ << ")\n";
+    		<< (std::string) this->keys_[0] << ","
+    		<< (std::string) this->keys_[1] << ","
+     		<< (std::string) this->keys_[2] << ","
+    		<< (std::string) this->keys_[3] << ","
+    		<< (std::string) this->keys_[4] << ","
+    		<< (std::string) this->keys_[5] << ")\n";
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -890,12 +852,6 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(key1_);
-    ar & BOOST_SERIALIZATION_NVP(key2_);
-    ar & BOOST_SERIALIZATION_NVP(key3_);
-    ar & BOOST_SERIALIZATION_NVP(key4_);
-    ar & BOOST_SERIALIZATION_NVP(key5_);
-    ar & BOOST_SERIALIZATION_NVP(key6_);
   }
 }; // \class NonlinearFactor6
 
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index eaaf3d4e9..460f32b0c 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -25,18 +25,18 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class POSEKEY, class POINTKEY>
-	class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
+	template<class POSE, class POINT>
+	class BearingFactor: public NonlinearFactor2<POSE, POINT> {
 	private:
 
-		typedef typename POSEKEY::Value Pose;
-		typedef typename POSEKEY::Value::Rotation Rot;
-		typedef typename POINTKEY::Value Point;
+		typedef typename POSE Pose;
+		typedef typename Pose::Rotation Rot;
+		typedef typename POINT Point;
 
-		typedef BearingFactor<POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
+		typedef BearingFactor<POSE, POINT> This;
+		typedef NonlinearFactor2<POSE, POINT> Base;
 
-		Rot z_; /** measurement */
+		Rot measured_; /** measurement */
 
 		/** concept check by type */
 		GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
@@ -48,9 +48,9 @@ namespace gtsam {
 		BearingFactor() {}
 
 		/** primary constructor */
-		BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z,
+		BearingFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measured,
 				const SharedNoiseModel& model) :
-					Base(model, i, j), z_(z) {
+					Base(model, poseKey, pointKey), measured_(measured) {
 		}
 
 		virtual ~BearingFactor() {}
@@ -59,18 +59,18 @@ namespace gtsam {
 		Vector evaluateError(const Pose& pose, const Point& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 			Rot hx = pose.bearing(point, H1, H2);
-			return Rot::Logmap(z_.between(hx));
+			return Rot::Logmap(measured_.between(hx));
 		}
 
 		/** return the measured */
-		inline const Rot measured() const {
-			return z_;
+		const Rot& measured() const {
+			return measured_;
 		}
 
 		/** equals */
 		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
-			return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol);
+			return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
 		}
 
 	private:
@@ -81,7 +81,7 @@ namespace gtsam {
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & boost::serialization::make_nvp("NonlinearFactor2",
 					boost::serialization::base_object<Base>(*this));
-			ar & BOOST_SERIALIZATION_NVP(z_);
+			ar & BOOST_SERIALIZATION_NVP(measured_);
 		}
 
 	}; // BearingFactor
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index ef71f8f13..a81896b94 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -27,20 +27,20 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class POSEKEY, class POINTKEY>
-	class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
+	template<class POSE, class POINT>
+	class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> {
 	private:
 
-		typedef typename POSEKEY::Value Pose;
-		typedef typename POSEKEY::Value::Rotation Rot;
-		typedef typename POINTKEY::Value Point;
+		typedef typename POSE Pose;
+		typedef typename POSE::Rotation Rot;
+		typedef typename POINT Point;
 
-		typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
+		typedef BearingRangeFactor<POSE, POINT> This;
+		typedef NonlinearFactor2<POSE, POINT> Base;
 
 		// the measurement
-		Rot bearing_;
-		double range_;
+		Rot measuredBearing_;
+		double measuredRange_;
 
 		/** concept check by type */
 		GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
@@ -50,9 +50,9 @@ namespace gtsam {
 	public:
 
 		BearingRangeFactor() {} /* Default constructor */
-		BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range,
+		BearingRangeFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measuredBearing, const double measuredRange,
 				const SharedNoiseModel& model) :
-					Base(model, i, j), bearing_(bearing), range_(range) {
+					Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) {
 		}
 
 		virtual ~BearingRangeFactor() {}
@@ -63,7 +63,7 @@ namespace gtsam {
 	    		<< (std::string) this->key1_ << ","
 	    		<< (std::string) this->key2_ << ")\n";
 	    bearing_.print("  measured bearing");
-	    std::cout << "  measured range: " << range_ << std::endl;
+	    std::cout << "  measured range: " << measuredRange_ << std::endl;
 	    this->noiseModel_->print("  noise model");
 	  }
 
@@ -71,8 +71,8 @@ namespace gtsam {
 		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) &&
-					fabs(this->range_ - e->range_) < tol &&
-					this->bearing_.equals(e->bearing_, tol);
+					fabs(this->measuredRange_ - e->measuredRange_) < tol &&
+					this->measuredBearing_.equals(e->measuredBearing_, tol);
 		}
 
 		/** h(x)-z -> between(z,h(x)) for Rot manifold */
@@ -85,10 +85,10 @@ namespace gtsam {
 			boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
 
 			Rot y1 = pose.bearing(point, H11_, H12_);
-			Vector e1 = Rot::Logmap(bearing_.between(y1));
+			Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
 
 			double y2 = pose.range(point, H21_, H22_);
-			Vector e2 = Vector_(1, y2 - range_);
+			Vector e2 = Vector_(1, y2 - measuredRange_);
 
 			if (H1) *H1 = gtsam::stack(2, &H11, &H21);
 			if (H2) *H2 = gtsam::stack(2, &H12, &H22);
@@ -96,8 +96,8 @@ namespace gtsam {
 		}
 
 		/** return the measured */
-		inline const std::pair<Rot, double> measured() const {
-			return std::make_pair(bearing_, range_);
+		const std::pair<Rot, double> measured() const {
+			return std::make_pair(measuredBearing_, measuredRange_);
 		}
 
 	private:
@@ -108,8 +108,8 @@ namespace gtsam {
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & boost::serialization::make_nvp("NonlinearFactor2",
 					boost::serialization::base_object<Base>(*this));
-			ar & BOOST_SERIALIZATION_NVP(bearing_);
-			ar & BOOST_SERIALIZATION_NVP(range_);
+			ar & BOOST_SERIALIZATION_NVP(measuredBearing_);
+			ar & BOOST_SERIALIZATION_NVP(measuredRange_);
 		}
 	}; // BearingRangeFactor
 
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 707dc3350..0fd8f6846 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -29,15 +29,15 @@ namespace gtsam {
 	 * KEY1::Value is the Lie Group type
 	 * T is the measurement type, by default the same
 	 */
-	template<class KEY1, class T = typename KEY1::Value>
-	class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
+	template<class VALUE>
+	class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> {
 
 	private:
 
-		typedef BetweenFactor<KEY1, T> This;
-		typedef NonlinearFactor2<KEY1, KEY1> Base;
+		typedef BetweenFactor<VALUE> This;
+		typedef NonlinearFactor2<VALUE, VALUE> Base;
 
-		T measured_; /** The measurement */
+		VALUE measured_; /** The measurement */
 
 		/** concept check by type */
 		GTSAM_CONCEPT_LIE_TYPE(T)
@@ -52,7 +52,7 @@ namespace gtsam {
 		BetweenFactor() {}
 
 		/** Constructor */
-		BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured,
+		BetweenFactor(const Symbol& key1, const Symbol& key2, const VALUE& measured,
 				const SharedNoiseModel& model) :
 			Base(model, key1, key2), measured_(measured) {
 		}
@@ -88,12 +88,12 @@ namespace gtsam {
 		}
 
 		/** return the measured */
-		inline const T measured() const {
+		const VALUE& measured() const {
 			return measured_;
 		}
 
 		/** number of variables attached to this factor */
-		inline std::size_t size() const {
+		std::size_t size() const {
 			return 2;
 		}
 
@@ -114,16 +114,14 @@ namespace gtsam {
 	 * This constraint requires the underlying type to a Lie type
 	 *
 	 */
-	template<class KEY>
-	class BetweenConstraint : public BetweenFactor<KEY> {
+	template<class VALUE>
+	class BetweenConstraint : public BetweenFactor<VALUE> {
 	public:
-		typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr;
+		typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
 
 		/** Syntactic sugar for constrained version */
-		BetweenConstraint(const typename KEY::Value& measured,
-				const KEY& key1, const KEY& key2, double mu = 1000.0)
-			: BetweenFactor<KEY>(key1, key2, measured,
-					noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
+		BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
+		  BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
 
 	private:
 
@@ -132,7 +130,7 @@ namespace gtsam {
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & boost::serialization::make_nvp("BetweenFactor",
-					boost::serialization::base_object<BetweenFactor<KEY> >(*this));
+					boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
 		}
 	}; // \class BetweenConstraint
 
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index f3df76774..3c28afa07 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -28,16 +28,16 @@ namespace gtsam {
  * will need to have its value function implemented to return
  * a scalar for comparison.
  */
-template<class KEY>
-struct BoundingConstraint1: public NonlinearFactor1<KEY> {
-	typedef typename KEY::Value X;
-	typedef NonlinearFactor1<KEY> Base;
-	typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
+template<class VALUE>
+struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
+	typedef VALUE X;
+	typedef NonlinearFactor1<VALUE> Base;
+	typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
 
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
 
-	BoundingConstraint1(const KEY& key, double threshold,
+	BoundingConstraint1(const Symbol& key, double threshold,
 			bool isGreaterThan, double mu = 1000.0) :
 				Base(noiseModel::Constrained::All(1, mu), key),
 				threshold_(threshold), isGreaterThan_(isGreaterThan) {
@@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearFactor1<KEY> {
 	/** active when constraint *NOT* met */
 	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
-		double x = value(c[this->key_]);
+		double x = value(c[this->key()]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
 	}
 
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index c0cd9e6c3..c6d2e2ede 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -29,21 +29,21 @@ namespace gtsam {
 	/**
 	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
 	 */
-	template <class CamK, class LmK>
+	template <class CAMERA, class LANDMARK>
 	class GeneralSFMFactor:
-	public NonlinearFactor2<CamK, LmK> {
+	public NonlinearFactor2<CAMERA, LANDMARK> {
 	protected:
-		Point2 z_;			///< the 2D measurement
+		Point2 measured_;			///< the 2D measurement
 
 	public:
 
-		typedef typename CamK::Value Cam;										///< typedef for camera type
-		typedef GeneralSFMFactor<CamK, LmK> Self ;	///< typedef for this object
-		typedef NonlinearFactor2<CamK, LmK> Base;		///< typedef for the base class
+		typedef typename CAMERA Cam;										///< typedef for camera type
+		typedef GeneralSFMFactor<CAMERA, LANDMARK> This ;	///< typedef for this object
+		typedef NonlinearFactor2<CAMERA, LANDMARK> Base;		///< typedef for the base class
 		typedef Point2 Measurement;													///< typedef for the measurement
 
 		// shorthand for a smart pointer to a factor
-		typedef boost::shared_ptr<GeneralSFMFactor<LmK, CamK> > shared_ptr;
+		typedef boost::shared_ptr<This> shared_ptr;
 
 		/**
 		 * Constructor
@@ -52,11 +52,11 @@ namespace gtsam {
 		 * @param i is basically the frame number
 		 * @param j is the index of the landmark
 		 */
-		GeneralSFMFactor(const Point2& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
+		GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {}
 
-		GeneralSFMFactor():z_(0.0,0.0) {} 							///< default constructor
-		GeneralSFMFactor(const Point2 & p):z_(p) {}			///< constructor that takes a Point2
-		GeneralSFMFactor(double x, double y):z_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
+		GeneralSFMFactor():measured_(0.0,0.0) {} 							///< default constructor
+		GeneralSFMFactor(const Point2 & p):measured_(p) {}			///< constructor that takes a Point2
+		GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
 
 		virtual ~GeneralSFMFactor() {} ///< destructor
 
@@ -73,7 +73,7 @@ namespace gtsam {
 		 * equals
 		 */
 		bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const	{
-			return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
+			return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ;
 		}
 
 		/** h(x)-z */
@@ -83,14 +83,14 @@ namespace gtsam {
 				boost::optional<Matrix&> H1=boost::none,
 				boost::optional<Matrix&> H2=boost::none) const {
 
-			Vector error = z_.localCoordinates(camera.project2(point,H1,H2));
+			Vector error = measured_.localCoordinates(camera.project2(point,H1,H2));
 //			gtsam::print(error, "error");
 			return error;
 		}
 
 		/** return the measured */
 		inline const Point2 measured() const {
-			return z_;
+			return measured_;
 		}
 
 	private:
@@ -98,7 +98,7 @@ 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(measured_);
 		}
 	};
 
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index 651e55500..e29893e77 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -38,16 +38,16 @@ namespace gtsam {
 	 * For practical use, it would be good to subclass this factor and have the class type
 	 * construct the mask.
 	 */
-	template<class KEY>
-	class PartialPriorFactor: public NonlinearFactor1<KEY> {
+	template<class VALUE>
+	class PartialPriorFactor: public NonlinearFactor1<VALUE> {
 
 	public:
-		typedef typename KEY::Value T;
+		typedef VALUE T;
 
 	protected:
 
-		typedef NonlinearFactor1<KEY> Base;
-		typedef PartialPriorFactor<KEY> This;
+		typedef NonlinearFactor1<VALUE> Base;
+		typedef PartialPriorFactor<VALUE> This;
 
 		Vector prior_;             ///< measurement on logmap parameters, in compressed form
 		std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
@@ -60,7 +60,7 @@ namespace gtsam {
 		 * constructor with just minimum requirements for a factor - allows more
 		 * computation in the constructor.  This should only be used by subclasses
 		 */
-		PartialPriorFactor(const KEY& key, const SharedNoiseModel& model)
+		PartialPriorFactor(const Symbol& key, const SharedNoiseModel& model)
 		  : Base(model, key) {}
 
 	public:
@@ -71,14 +71,14 @@ namespace gtsam {
 		virtual ~PartialPriorFactor() {}
 
 		/** Single Element Constructor: acts on a single parameter specified by idx */
-		PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) :
+		PartialPriorFactor(const Symbol& key, size_t idx, double prior, const SharedNoiseModel& model) :
 			Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
 			assert(model->dim() == 1);
 			this->fillH();
 		}
 
 		/** Indices Constructor: specify the mask with a set of indices */
-		PartialPriorFactor(const KEY& key, const std::vector<size_t>& mask, const Vector& prior,
+		PartialPriorFactor(const Symbol& key, const std::vector<size_t>& mask, const Vector& prior,
 				const SharedNoiseModel& model) :
 			Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
 			assert((size_t)prior_.size() == mask.size());
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index fa33a9bc7..a5ff16c56 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -21,32 +21,30 @@
 namespace gtsam {
 
 	/**
-	 * A class for a soft prior on any Lie type
-	 * It takes two template parameters:
-	 *   Key (typically TypedSymbol) is used to look up T's in a Values
-	 *   Values where the T's are stored, typically Values<Key> or a TupleValues<...>
-	 * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
-	 * a simple type like int will not work
+	 * A class for a soft prior on any Value type
 	 */
-	template<class KEY>
-	class PriorFactor: public NonlinearFactor1<KEY> {
+	template<class VALUE>
+	class PriorFactor: public NonlinearFactor1<VALUE> {
 
 	public:
-		typedef typename KEY::Value T;
+		typedef VALUE T;
 
 	private:
 
-		typedef NonlinearFactor1<KEY> Base;
+		typedef NonlinearFactor1<VALUE> Base;
 
-		T prior_; /** The measurement */
+		VALUE prior_; /** The measurement */
 
 		/** concept check by type */
 		GTSAM_CONCEPT_TESTABLE_TYPE(T)
 
 	public:
 
-		// shorthand for a smart pointer to a factor
-		typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
+		/// shorthand for a smart pointer to a factor
+		typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
+
+		/// Typedef to this class
+		typedef PriorFactor<VALUE> This;
 
 		/** default constructor - only use for serialization */
 		PriorFactor() {}
@@ -54,8 +52,7 @@ namespace gtsam {
 		virtual ~PriorFactor() {}
 
 		/** Constructor */
-		PriorFactor(const KEY& key, const T& prior,
-				const SharedNoiseModel& model) :
+		PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) :
 			Base(model, key), prior_(prior) {
 		}
 
@@ -70,7 +67,7 @@ namespace gtsam {
 
 		/** equals */
 		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
-			const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
+			const This* e = dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
 		}
 
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index dcf425aaa..646f2a003 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -29,21 +29,24 @@ namespace gtsam {
 	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
 	 * i.e. the main building block for visual SLAM.
 	 */
-	template<class LMK, class POSK>
-	class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
+	template<class POSE, class LANDMARK>
+	class GenericProjectionFactor: public NonlinearFactor2<POSE, LANDMARK> {
 	protected:
 
 		// Keep a copy of measurement and calibration for I/O
-		Point2 z_;											///< 2D measurement
+		Point2 measured_;					  		///< 2D measurement
 		boost::shared_ptr<Cal3_S2> K_;  ///< shared pointer to calibration object
 
 	public:
 
 		/// shorthand for base class type
-		typedef NonlinearFactor2<POSK, LMK> Base;
+		typedef NonlinearFactor2<POSE, LANDMARK> Base;
+
+		/// shorthand for this class
+		typedef GenericProjectionFactor<POSE, LANDMARK> This;
 
 		/// shorthand for a smart pointer to a factor
-		typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
+		typedef boost::shared_ptr<This> shared_ptr;
 
 		/// Default constructor
 		GenericProjectionFactor() :
@@ -52,15 +55,16 @@ namespace gtsam {
 
 		/**
 		 * Constructor
+		 * TODO: Mark argument order standard (keys, measurement, parameters)
 		 * @param z is the 2 dimensional location of point in image (the measurement)
 		 * @param model is the standard deviation
 		 * @param j_pose is basically the frame number
 		 * @param j_landmark is the index of the landmark
 		 * @param K shared pointer to the constant calibration
 		 */
-		GenericProjectionFactor(const Point2& z, const SharedNoiseModel& model,
-				POSK j_pose, LMK j_landmark, const shared_ptrK& K) :
-				Base(model, j_pose, j_landmark), z_(z), K_(K) {
+		GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
+				const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) :
+				  Base(model, poseKey, pointKey), measured_(measured), K_(K) {
 		}
 
 		/**
@@ -69,14 +73,13 @@ namespace gtsam {
 		 */
 		void print(const std::string& s = "ProjectionFactor") const {
 			Base::print(s);
-			z_.print(s + ".z");
+			measured_.print(s + ".z");
 		}
 
 		/// equals
-		bool equals(const GenericProjectionFactor<LMK, POSK>& p
-				, double tol = 1e-9) const {
-			return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
-					&& this->K_->equals(*p.K_, tol);
+		virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+      const This *e = dynamic_cast<const This*> (&expected);
+			return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol);
 		}
 
 		/// Evaluate error h(x)-z and optionally derivatives
@@ -84,10 +87,9 @@ namespace gtsam {
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 			try {
 	      SimpleCamera camera(*K_, pose);
-			  Point2 reprojectionError(camera.project(point, H1, H2) - z_);
+			  Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
 	      return reprojectionError.vector();
-			}
-			catch( CheiralityException& e) {
+			} catch( CheiralityException& e) {
 			  if (H1) *H1 = zeros(2,6);
 			  if (H2) *H2 = zeros(2,3);
 			  cout << e.what() << ": Landmark "<< this->key2_.index() <<
@@ -97,8 +99,8 @@ namespace gtsam {
 		}
 
     /** return the measurement */
-    inline const Point2 measured() const {
-      return z_;
+    const Point2& measured() const {
+      return measured_;
     }
 
 	private:
@@ -108,7 +110,7 @@ namespace gtsam {
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-			ar & BOOST_SERIALIZATION_NVP(z_);
+			ar & BOOST_SERIALIZATION_NVP(measured_);
 			ar & BOOST_SERIALIZATION_NVP(K_);
 		}
 	};
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index b762c5690..77c11d00d 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -25,17 +25,17 @@ namespace gtsam {
 	/**
 	 * Binary factor for a range measurement
 	 */
-	template<class POSEKEY, class POINTKEY>
-	class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
+	template<class POSE, class POINT>
+	class RangeFactor: public NonlinearFactor2<POSE, POINT> {
 	private:
 
-		double z_; /** measurement */
+		double measured_; /** measurement */
 
-		typedef RangeFactor<POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
+		typedef RangeFactor<POSE, POINT> This;
+		typedef NonlinearFactor2<POSE, POINT> Base;
 
-		typedef typename POSEKEY::Value Pose;
-		typedef typename POINTKEY::Value Point;
+		typedef typename POSE Pose;
+		typedef typename POINT Point;
 
 		// Concept requirements for this factor
 		GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
@@ -44,34 +44,33 @@ namespace gtsam {
 
 		RangeFactor() {} /* Default constructor */
 
-		RangeFactor(const POSEKEY& i, const POINTKEY& j, double z,
+		RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
 				const SharedNoiseModel& model) :
-					Base(model, i, j), z_(z) {
+					Base(model, poseKey, PointKey), measured_(measured) {
 		}
 
 		virtual ~RangeFactor() {}
 
 		/** h(x)-z */
-		Vector evaluateError(const typename POSEKEY::Value& pose, const typename POINTKEY::Value& point,
-				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+		Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 			double hx = pose.range(point, H1, H2);
-			return Vector_(1, hx - z_);
+			return Vector_(1, hx - measured_);
 		}
 
 		/** return the measured */
-		inline double measured() const {
-			return z_;
+		double measured() const {
+			return measured_;
 		}
 
 		/** equals specialized to this factor */
 		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e = dynamic_cast<const This*> (&expected);
-			return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol;
+			return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->z_) < tol;
 		}
 
 		/** print contents */
 		void print(const std::string& s="") const {
-			Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(z_));
+			Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(measured_));
 		}
 
 	private:
@@ -82,7 +81,7 @@ namespace gtsam {
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & boost::serialization::make_nvp("NonlinearFactor2",
 					boost::serialization::base_object<Base>(*this));
-			ar & BOOST_SERIALIZATION_NVP(z_);
+			ar & BOOST_SERIALIZATION_NVP(measured_);
 		}
 	}; // RangeFactor
 
diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp
index e8d674f6c..f7e562640 100644
--- a/gtsam/slam/planarSLAM.cpp
+++ b/gtsam/slam/planarSLAM.cpp
@@ -26,38 +26,34 @@ namespace planarSLAM {
   Graph::Graph(const NonlinearFactorGraph& graph) :
       NonlinearFactorGraph(graph) {}
 
-  void Graph::addPrior(const PoseKey& i, const Pose2& p,
-      const SharedNoiseModel& model) {
-    sharedFactor factor(new Prior(i, p, model));
+  void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) {
+    sharedFactor factor(new Prior(PoseKey(i), p, model));
     push_back(factor);
   }
 
-  void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
-    sharedFactor factor(new Constraint(i, p));
+  void Graph::addPoseConstraint(Index i, const Pose2& p) {
+    sharedFactor factor(new Constraint(PoseKey(i), p));
     push_back(factor);
   }
 
-  void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
-      const SharedNoiseModel& model) {
-    sharedFactor factor(new Odometry(i, j, z, model));
+  void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
+    sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model));
     push_back(factor);
   }
 
-  void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
-      const SharedNoiseModel& model) {
-    sharedFactor factor(new Bearing(i, j, z, model));
+  void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) {
+    sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model));
     push_back(factor);
   }
 
-  void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
-      const SharedNoiseModel& model) {
-    sharedFactor factor(new Range(i, j, z, model));
+  void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) {
+    sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model));
     push_back(factor);
   }
 
-  void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1,
+  void Graph::addBearingRange(Index i, Index j, const Rot2& z1,
       double z2, const SharedNoiseModel& model) {
-    sharedFactor factor(new BearingRange(i, j, z1, z2, model));
+    sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model));
     push_back(factor);
   }
 
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 50fae36a5..8fd568a3b 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -31,28 +31,32 @@
 // Use planarSLAM namespace for specific SLAM instance
 namespace planarSLAM {
 
-	using namespace gtsam;
+  using namespace gtsam;
 
-	/// Typedef for a PoseKey with Pose2 data and 'x' symbol
-	typedef TypedSymbol<Pose2, 'x'> PoseKey;
+  /// Typedef for a PoseKey with Pose2 data and 'x' symbol
+  typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
-	/// Typedef for a PointKey with Point2 data and 'l' symbol
-	typedef TypedSymbol<Point2, 'l'> PointKey;
-	/**
-	 * List of typedefs for factors
-	 */
-	/// A hard constraint for PoseKeys to enforce particular values
-	typedef NonlinearEquality<PoseKey> Constraint;
-	/// A prior factor to bias the value of a PoseKey
-	typedef PriorFactor<PoseKey> Prior;
-	/// A factor between two PoseKeys set with a Pose2
-	typedef BetweenFactor<PoseKey> Odometry;
-	/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
-	typedef BearingFactor<PoseKey, PointKey> Bearing;
-	/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
-	typedef RangeFactor<PoseKey, PointKey> Range;
-	/// A factor between a PoseKey and a PointKey to express difference in rotation and location
-	typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
+  /// Typedef for a PointKey with Point2 data and 'l' symbol
+  typedef TypedSymbol<Point2, 'l'> PointKey;
+
+  /// Convenience function for constructing a pose key
+  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
+
+  /*
+   * List of typedefs for factors
+   */
+  /// A hard constraint for PoseKeys to enforce particular values
+  typedef NonlinearEquality<Pose2> Constraint;
+  /// A prior factor to bias the value of a PoseKey
+  typedef PriorFactor<Pose2> Prior;
+  /// A factor between two PoseKeys set with a Pose2
+  typedef BetweenFactor<Pose2> Odometry;
+  /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
+  typedef BearingFactor<Pose2, Point2> Bearing;
+  /// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
+  typedef RangeFactor<Pose2, Point2> Range;
+  /// A factor between a PoseKey and a PointKey to express difference in rotation and location
+  typedef BearingRangeFactor<Pose2, Point2> BearingRange;
 
   /**  Values class, using specific PoseKeys and PointKeys
    * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
@@ -80,48 +84,43 @@ namespace planarSLAM {
     void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
   };
 
+  /// Creates a NonlinearFactorGraph with the Values type
+  struct Graph: public NonlinearFactorGraph {
 
-	/// Creates a NonlinearFactorGraph with the Values type
-	struct Graph: public NonlinearFactorGraph {
+    /// Default constructor for a NonlinearFactorGraph
+    Graph(){}
 
-		/// Default constructor for a NonlinearFactorGraph
-		Graph(){}
+    /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
+    Graph(const NonlinearFactorGraph& graph);
 
-		/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
-		Graph(const NonlinearFactorGraph& graph);
+    /// Biases the value of PoseKey key with Pose2 p given a noise model
+    void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel);
 
-		/// Biases the value of PoseKey key with Pose2 p given a noise model
-		void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
+    /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
+    void addPoseConstraint(Index poseKey, const Pose2& pose);
 
-		/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
-		void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
+    /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
+    void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model);
 
-		/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
-		void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
-				const SharedNoiseModel& model);
+    /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
+    void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model);
 
-		/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
-		void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
-				const SharedNoiseModel& model);
+    /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
+    void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model);
 
-		/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
-		void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
-				const SharedNoiseModel& model);
+    /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
+    void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model);
 
-		/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
-		void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
-				const Rot2& bearing, double range, const SharedNoiseModel& model);
+    /// Optimize
+    Values optimize(const Values& initialEstimate) {
+      typedef NonlinearOptimizer<Graph> Optimizer;
+      return *Optimizer::optimizeLM(*this, initialEstimate,
+          NonlinearOptimizationParameters::LAMBDA);
+    }
+  };
 
-		/// Optimize
-		Values optimize(const Values& initialEstimate) {
-			typedef NonlinearOptimizer<Graph> Optimizer;
-			return *Optimizer::optimizeLM(*this, initialEstimate,
-					NonlinearOptimizationParameters::LAMBDA);
-		}
-	};
-
-/// Optimizer
-typedef NonlinearOptimizer<Graph> Optimizer;
+  /// Optimizer
+  typedef NonlinearOptimizer<Graph> Optimizer;
 
 } // planarSLAM
 
diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp
index a425f8fb9..b006c61f2 100644
--- a/gtsam/slam/pose2SLAM.cpp
+++ b/gtsam/slam/pose2SLAM.cpp
@@ -35,20 +35,20 @@ namespace pose2SLAM {
   }
 
   /* ************************************************************************* */
-  void Graph::addPrior(const PoseKey& i, const Pose2& p,
+  void Graph::addPrior(Index i, const Pose2& p,
       const SharedNoiseModel& model) {
-    sharedFactor factor(new Prior(i, p, model));
+    sharedFactor factor(new Prior(PoseKey(i), p, model));
     push_back(factor);
   }
 
-  void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
-    sharedFactor factor(new HardConstraint(i, p));
+  void Graph::addPoseConstraint(Index i, const Pose2& p) {
+    sharedFactor factor(new HardConstraint(PoseKey(i), p));
     push_back(factor);
   }
 
-  void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
+  void Graph::addOdometry(Index i, Index j, const Pose2& z,
       const SharedNoiseModel& model) {
-    sharedFactor factor(new Odometry(i, j, z, model));
+    sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model));
     push_back(factor);
   }
 
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 23e087be6..ab0c60050 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -33,9 +33,8 @@ namespace pose2SLAM {
 
   using namespace gtsam;
 
-  /// Keys with Pose2 and symbol 'x'
-  typedef TypedSymbol<Pose2, 'x'> PoseKey;
-
+  /// Convenience function for constructing a pose key
+  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
   /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
   struct Values: public gtsam::Values {
 
@@ -70,11 +69,11 @@ namespace pose2SLAM {
    */
 
   /// A hard constraint to enforce a specific value for a pose
-  typedef NonlinearEquality<PoseKey> HardConstraint;
+  typedef NonlinearEquality<Pose2> HardConstraint;
   /// A prior factor on a pose with Pose2 data type.
-  typedef PriorFactor<PoseKey> Prior;
+  typedef PriorFactor<Pose2> Prior;
   /// A factor to add an odometry measurement between two poses.
-  typedef BetweenFactor<PoseKey> Odometry;
+  typedef BetweenFactor<Pose2> Odometry;
 
   /// Graph
   struct Graph: public NonlinearFactorGraph {
@@ -86,13 +85,13 @@ namespace pose2SLAM {
     Graph(const NonlinearFactorGraph& graph);
 
     /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
-    void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model);
+    void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model);
 
     /// Creates a hard constraint for key i with the given Pose2 p.
-    void addPoseConstraint(const PoseKey& i, const Pose2& p);
+    void addPoseConstraint(Index i, const Pose2& p);
 
     /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
-    void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
+    void addOdometry(Index i, Index j, const Pose2& z,
         const SharedNoiseModel& model);
 
     /// Optimize
diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp
index 4d063c12b..3ff8fa99c 100644
--- a/gtsam/slam/pose3SLAM.cpp
+++ b/gtsam/slam/pose3SLAM.cpp
@@ -45,20 +45,18 @@ namespace gtsam {
 		}
 
 		/* ************************************************************************* */
-		void Graph::addPrior(const Key& i, const Pose3& p,
-				const SharedNoiseModel& model) {
-			sharedFactor factor(new Prior(i, p, model));
+		void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
+			sharedFactor factor(new Prior(PoseKey(i), p, model));
 			push_back(factor);
 		}
 
-		void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z,
-				const SharedNoiseModel& model) {
-			sharedFactor factor(new Constraint(i, j, z, model));
+		void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
+			sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
 			push_back(factor);
 		}
 
-		void Graph::addHardConstraint(const Key& i, const Pose3& p) {
-			sharedFactor factor(new HardConstraint(i, p));
+		void Graph::addHardConstraint(Index i, const Pose3& p) {
+			sharedFactor factor(new HardConstraint(PoseKey(i), p));
 			push_back(factor);
 		}
 
diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h
index 686f6bd0f..555e5fc91 100644
--- a/gtsam/slam/pose3SLAM.h
+++ b/gtsam/slam/pose3SLAM.h
@@ -30,8 +30,8 @@ namespace gtsam {
 	/// Use pose3SLAM namespace for specific SLAM instance
 	namespace pose3SLAM {
 
-		/// Creates a Key with data Pose3 and symbol 'x'
-		typedef TypedSymbol<Pose3, 'x'> Key;
+	  /// Convenience function for constructing a pose key
+	  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
 
 		/**
 		 * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@@ -42,25 +42,23 @@ namespace gtsam {
 		Values circle(size_t n, double R);
 
 		/// A prior factor on Key with Pose3 data type.
-		typedef PriorFactor<Key> Prior;
+		typedef PriorFactor<Pose3> Prior;
 		/// A factor to put constraints between two factors.
-		typedef BetweenFactor<Key> Constraint;
+		typedef BetweenFactor<Pose3> Constraint;
 		/// A hard constraint would enforce that the given key would have the input value in the results.
-		typedef NonlinearEquality<Key> HardConstraint;
+		typedef NonlinearEquality<Pose3> HardConstraint;
 
 		/// Graph
 		struct Graph: public NonlinearFactorGraph {
 
 			/// Adds a factor between keys of the same type
-			void addPrior(const Key& i, const Pose3& p,
-					const SharedNoiseModel& model);
+			void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
 
 			/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
-			void addConstraint(const Key& i, const Key& j, const Pose3& z,
-					const SharedNoiseModel& model);
+			void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
 
 			/// Creates a hard constraint for key i with the given Pose3 p.
-			void addHardConstraint(const Key& i, const Pose3& p);
+			void addHardConstraint(Index i, const Pose3& p);
 		};
 
 		/// Optimizer
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index a9ab58d3c..ca78bb334 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -29,8 +29,12 @@ namespace simulated2D {
   using namespace gtsam;
 
   // Simulated2D robots have no orientation, just a position
-  typedef TypedSymbol<Point2, 'x'> PoseKey;
-  typedef TypedSymbol<Point2, 'l'> PointKey;
+
+  /// Convenience function for constructing a pose key
+  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
+
+  /// Convenience function for constructing a landmark key
+  inline Symbol PointKey(Index j) { return Symbol('l', j); }
 
   /**
    *  Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
@@ -115,24 +119,23 @@ namespace simulated2D {
   /**
    *  Unary factor encoding a soft prior on a vector
    */
-  template<class KEY = PoseKey>
-  class GenericPrior: public NonlinearFactor1<KEY> {
+  template<class VALUE = Point2>
+  class GenericPrior: public NonlinearFactor1<VALUE> {
   public:
-    typedef NonlinearFactor1<KEY> Base;  ///< base class
-    typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
-    typedef typename KEY::Value Pose; ///< shortcut to Pose type
+    typedef NonlinearFactor1<VALUE> Base;  ///< base class
+    typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
+    typedef VALUE Pose; ///< shortcut to Pose type
 
-    Pose z_; ///< prior mean
+    Pose measured_; ///< prior mean
 
     /// Create generic prior
-    GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
-      NonlinearFactor1<KEY>(model, key), z_(z) {
+    GenericPrior(const Pose& z, const SharedNoiseModel& model, const Symbol& key) :
+      Base(model, key), measured_(z) {
     }
 
     /// Return error and optional derivative
-    Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
-        boost::none) const {
-      return (prior(x, H) - z_).vector();
+    Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const {
+      return (prior(x, H) - measured_).vector();
     }
 
   private:
@@ -146,33 +149,32 @@ namespace simulated2D {
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-      ar & BOOST_SERIALIZATION_NVP(z_);
+      ar & BOOST_SERIALIZATION_NVP(measured_);
     }
   };
 
   /**
    * Binary factor simulating "odometry" between two Vectors
    */
-  template<class KEY = PoseKey>
-  class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
+  template<class VALUE = Pose2>
+  class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
   public:
-    typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
-    typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
-    typedef typename KEY::Value Pose; ///< shortcut to Pose type
+    typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class
+    typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
+    typedef VALUE Pose; ///< shortcut to Pose type
 
-    Pose z_; ///< odometry measurement
+    Pose measured_; ///< odometry measurement
 
     /// Create odometry
-    GenericOdometry(const Pose& z, const SharedNoiseModel& model,
-        const KEY& i1, const KEY& i2) :
-          NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
+    GenericOdometry(const Pose& measured, const SharedNoiseModel& model, const Symbol& key1, const Symbol& key2) :
+          Base(model, key1, key2), measured_(measured) {
     }
 
     /// Evaluate error and optionally return derivatives
     Vector evaluateError(const Pose& x1, const Pose& x2,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none) const {
-      return (odo(x1, x2, H1, H2) - z_).vector();
+      return (odo(x1, x2, H1, H2) - measured_).vector();
     }
 
   private:
@@ -186,34 +188,33 @@ namespace simulated2D {
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-      ar & BOOST_SERIALIZATION_NVP(z_);
+      ar & BOOST_SERIALIZATION_NVP(measured_);
     }
   };
 
   /**
    * Binary factor simulating "measurement" between two Vectors
    */
-  template<class XKEY = PoseKey, class LKEY = PointKey>
-  class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
+  template<class POSE, class LANDMARK>
+  class GenericMeasurement: public NonlinearFactor2<POSE, LANDMARK> {
   public:
-    typedef NonlinearFactor2<XKEY, LKEY> Base;  ///< base class
-    typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
-    typedef typename XKEY::Value Pose; ///< shortcut to Pose type
-    typedef typename LKEY::Value Point; ///< shortcut to Point type
+    typedef NonlinearFactor2<POSE, LANDMARK> Base;  ///< base class
+    typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
+    typedef POSE Pose; ///< shortcut to Pose type
+    typedef LANDMARK Landmark; ///< shortcut to Landmark type
 
-    Point z_; ///< Measurement
+    Landmark z_; ///< Measurement
 
     /// Create measurement factor
-    GenericMeasurement(const Point& z, const SharedNoiseModel& model,
-        const XKEY& i, const LKEY& j) :
-          NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
+    GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :
+          Base(model, poseKey, landmarkKey), measured_(measured) {
     }
 
     /// Evaluate error and optionally return derivatives
-    Vector evaluateError(const Pose& x1, const Point& x2,
+    Vector evaluateError(const Pose& x1, const Landmark& x2,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none) const {
-      return (mea(x1, x2, H1, H2) - z_).vector();
+      return (mea(x1, x2, H1, H2) - measured_).vector();
     }
 
   private:
@@ -227,14 +228,14 @@ namespace simulated2D {
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-      ar & BOOST_SERIALIZATION_NVP(z_);
+      ar & BOOST_SERIALIZATION_NVP(measured_);
     }
   };
 
   /** Typedefs for regular use */
-  typedef GenericPrior<PoseKey> Prior;
-  typedef GenericOdometry<PoseKey> Odometry;
-  typedef GenericMeasurement<PoseKey, PointKey> Measurement;
+  typedef GenericPrior<Point2> Prior;
+  typedef GenericOdometry<Point2> Odometry;
+  typedef GenericMeasurement<Point2, Point2> Measurement;
 
   // Specialization of a graph for this example domain
   // TODO: add functions to add factor types
diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h
index ffd4e54fb..99475ce91 100644
--- a/gtsam/slam/simulated2DConstraints.h
+++ b/gtsam/slam/simulated2DConstraints.h
@@ -33,13 +33,13 @@ namespace simulated2D {
   namespace equality_constraints {
 
     /** Typedefs for regular use */
-    typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint;
-    typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint;
-    typedef BetweenConstraint<PoseKey> OdoEqualityConstraint;
+    typedef NonlinearEquality1<Point2> UnaryEqualityConstraint;
+    typedef NonlinearEquality1<Point2> UnaryEqualityPointConstraint;
+    typedef BetweenConstraint<Point2> OdoEqualityConstraint;
 
     /** Equality between variables */
-    typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint;
-    typedef NonlinearEquality2<PointKey> PointEqualityConstraint;
+    typedef NonlinearEquality2<Point2> PoseEqualityConstraint;
+    typedef NonlinearEquality2<Point2> PointEqualityConstraint;
 
   } // \namespace equality_constraints
 
@@ -47,15 +47,14 @@ namespace simulated2D {
 
     /**
      * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
-     * @tparam VALUES is the values structure for the graph
-     * @tparam KEY is the key type for the variable constrained
+     * @tparam VALUE is the value type for the variable constrained, e.g. Pose2, Point3, etc.
      * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
      */
-    template<class KEY, unsigned int IDX>
-    struct ScalarCoordConstraint1: public BoundingConstraint1<KEY> {
-      typedef BoundingConstraint1<KEY> Base;  ///< Base class convenience typedef
-      typedef boost::shared_ptr<ScalarCoordConstraint1<KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
-      typedef typename KEY::Value Point; ///< Constrained variable type
+    template<class VALUE, unsigned int IDX>
+    struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> {
+      typedef BoundingConstraint1<VALUE> Base;  ///< Base class convenience typedef
+      typedef boost::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
+      typedef VALUE Point; ///< Constrained variable type
 
       virtual ~ScalarCoordConstraint1() {}
 
@@ -66,7 +65,7 @@ namespace simulated2D {
        * @param isGreaterThan is a flag to set inequality as greater than or less than
        * @param mu is the penalty function gain
        */
-      ScalarCoordConstraint1(const KEY& key, double c,
+      ScalarCoordConstraint1(const Symbol& key, double c,
           bool isGreaterThan, double mu = 1000.0) :
             Base(key, c, isGreaterThan, mu) {
       }
@@ -94,8 +93,8 @@ namespace simulated2D {
     };
 
     /** typedefs for use with simulated2D systems */
-    typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
-    typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
+    typedef ScalarCoordConstraint1<Point2, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
+    typedef ScalarCoordConstraint1<Point2, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
 
     /**
      * Trait for distance constraints to provide distance
@@ -114,10 +113,10 @@ namespace simulated2D {
      * @tparam VALUES is the variable set for the graph
      * @tparam KEY is the type of the keys for the variables constrained
      */
-    template<class KEY>
-    struct MaxDistanceConstraint : public BoundingConstraint2<KEY, KEY> {
-      typedef BoundingConstraint2<KEY, KEY> Base;  ///< Base class for factor
-      typedef typename KEY::Value Point; ///< Type of variable constrained
+    template<class VALUE>
+    struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> {
+      typedef BoundingConstraint2<VALUE, VALUE> Base;  ///< Base class for factor
+      typedef VALUE Point; ///< Type of variable constrained
 
       virtual ~MaxDistanceConstraint() {}
 
@@ -128,8 +127,8 @@ namespace simulated2D {
        * @param range_bound is the maximum range allowed between the variables
        * @param mu is the gain for the penalty function
        */
-      MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0)
-      : Base(key1, key2, range_bound, false, mu) {}
+      MaxDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) :
+        Base(key1, key2, range_bound, false, mu) {}
 
       /**
        * computes the range with derivatives
@@ -153,15 +152,14 @@ namespace simulated2D {
     /**
      * Binary inequality constraint forcing a minimum range
      * NOTE: this is not a convex function!  Be careful with initialization.
-     * @tparam VALUES is the variable set for the graph
-     * @tparam XKEY is the type of the pose key constrained
-     * @tparam PKEY is the type of the point key constrained
+     * @tparam POSE is the type of the pose value constrained
+     * @tparam POINT is the type of the point value constrained
      */
-    template<class XKEY, class PKEY>
-    struct MinDistanceConstraint : public BoundingConstraint2<XKEY, PKEY> {
-      typedef BoundingConstraint2<XKEY, PKEY> Base; ///< Base class for factor
-      typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
-      typedef typename PKEY::Value Point; ///< Type of point variable constrained
+    template<class POSE, class POINT>
+    struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
+      typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
+      typedef POSE Pose; ///< Type of pose variable constrained
+      typedef POINT Point; ///< Type of point variable constrained
 
       virtual ~MinDistanceConstraint() {}
 
@@ -172,7 +170,7 @@ namespace simulated2D {
        * @param range_bound is the minimum range allowed between the variables
        * @param mu is the gain for the penalty function
        */
-      MinDistanceConstraint(const XKEY& key1, const PKEY& key2,
+      MinDistanceConstraint(const Symbol& key1, const Symbol& key2,
           double range_bound, double mu = 1000.0)
       : Base(key1, key2, range_bound, true, mu) {}
 
@@ -193,7 +191,7 @@ namespace simulated2D {
       }
     };
 
-    typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
+    typedef MinDistanceConstraint<Point2, Point2> LandmarkAvoid; ///< Simulated2D domain example factor
 
 
   } // \namespace inequality_constraints
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index 0c5533291..d0880b8f3 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -27,9 +27,11 @@ namespace simulated2DOriented {
 
   using namespace gtsam;
 
-  // The types that take an oriented pose2 rather than point2
-  typedef TypedSymbol<Point2, 'l'> PointKey;
-  typedef TypedSymbol<Pose2, 'x'> PoseKey;
+  /// Convenience function for constructing a pose key
+  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
+
+  /// Convenience function for constructing a landmark key
+  inline Symbol PointKey(Index j) { return Symbol('l', j); }
 
   /// Specialized Values structure with syntactic sugar for
   /// compatibility with matlab
@@ -75,21 +77,20 @@ namespace simulated2DOriented {
       boost::none, boost::optional<Matrix&> H2 = boost::none);
 
   /// Unary factor encoding a soft prior on a vector
-		template<class Key = PoseKey>
-		struct GenericPosePrior: public NonlinearFactor1<Key> {
+  template<class VALUE = Pose2>
+  struct GenericPosePrior: public NonlinearFactor1<VALUE> {
 
-    Pose2 z_; ///< measurement
+    Pose2 measured_; ///< measurement
 
     /// Create generic pose prior
-    GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
-        const Key& key) :
-					NonlinearFactor1<Key>(model, key), z_(z) {
+    GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, const Symbol& key) :
+      NonlinearFactor1<VALUE>(model, key), measured_(measured) {
     }
 
     /// Evaluate error and optionally derivative
     Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
         boost::none) const {
-      return z_.localCoordinates(prior(x, H));
+      return measured_.localCoordinates(prior(x, H));
     }
 
   };
@@ -97,31 +98,31 @@ namespace simulated2DOriented {
   /**
    * Binary factor simulating "odometry" between two Vectors
    */
-		template<class KEY = PoseKey>
-		struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
-    Pose2 z_;   ///< Between measurement for odometry factor
+  template<class VALUE = Pose2>
+  struct GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
+    Pose2 measured_;   ///< Between measurement for odometry factor
 
     /**
      * Creates an odometry factor between two poses
      */
-    GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
-        const KEY& i1, const KEY& i2) :
-					NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
+    GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
+        const Symbol& i1, const Symbol& i2) :
+          NonlinearFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
     }
 
     /// Evaluate error and optionally derivative
     Vector evaluateError(const Pose2& x1, const Pose2& x2,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none) const {
-      return z_.localCoordinates(odo(x1, x2, H1, H2));
+      return measured_.localCoordinates(odo(x1, x2, H1, H2));
     }
 
   };
 
-		typedef GenericOdometry<PoseKey> Odometry;
+  typedef GenericOdometry<Pose2> Odometry;
 
   /// Graph specialization for syntactic sugar use with matlab
-		class Graph : public NonlinearFactorGraph {
+  class Graph : public NonlinearFactorGraph {
   public:
     Graph() {}
     // TODO: add functions to add factors
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 490692c90..53a2af385 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -235,13 +235,10 @@ TEST( NonlinearFactor, linearize_constraint2 )
 }
 
 /* ************************************************************************* */
-typedef TypedSymbol<LieVector, 'x'> TestKey;
-
-/* ************************************************************************* */
-class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
+class TestFactor4 : public NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
-  TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
+  typedef NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> Base;
+  TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {}
 
   virtual Vector
     evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@@ -263,13 +260,13 @@ public:
 TEST(NonlinearFactor, NonlinearFactor4) {
   TestFactor4 tf;
   Values tv;
-  tv.insert(TestKey(1), LieVector(1, 1.0));
-  tv.insert(TestKey(2), LieVector(1, 2.0));
-  tv.insert(TestKey(3), LieVector(1, 3.0));
-  tv.insert(TestKey(4), LieVector(1, 4.0));
+  tv.insert("x1", LieVector(1, 1.0));
+  tv.insert("x2", LieVector(1, 2.0));
+  tv.insert("x3", LieVector(1, 3.0));
+  tv.insert("x4", LieVector(1, 4.0));
   EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
-  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
+  Ordering ordering; ordering += "x1", "x2", "x3", "x4";
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
   LONGS_EQUAL(jf.keys()[0], 0);
   LONGS_EQUAL(jf.keys()[1], 1);
@@ -283,9 +280,9 @@ TEST(NonlinearFactor, NonlinearFactor4) {
 }
 
 /* ************************************************************************* */
-class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> {
+class TestFactor5 : public NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  typedef NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
   TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
 
   virtual Vector
@@ -310,14 +307,14 @@ public:
 TEST(NonlinearFactor, NonlinearFactor5) {
   TestFactor5 tf;
   Values tv;
-  tv.insert(TestKey(1), LieVector(1, 1.0));
-  tv.insert(TestKey(2), LieVector(1, 2.0));
-  tv.insert(TestKey(3), LieVector(1, 3.0));
-  tv.insert(TestKey(4), LieVector(1, 4.0));
-  tv.insert(TestKey(5), LieVector(1, 5.0));
+  tv.insert("x1", LieVector(1, 1.0));
+  tv.insert("x2", LieVector(1, 2.0));
+  tv.insert("x3", LieVector(1, 3.0));
+  tv.insert("x4", LieVector(1, 4.0));
+  tv.insert("x5", LieVector(1, 5.0));
   EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
-  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
+  Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5";
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
   LONGS_EQUAL(jf.keys()[0], 0);
   LONGS_EQUAL(jf.keys()[1], 1);
@@ -333,9 +330,9 @@ TEST(NonlinearFactor, NonlinearFactor5) {
 }
 
 /* ************************************************************************* */
-class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
+class TestFactor6 : public NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  typedef NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
   TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
 
   virtual Vector
@@ -362,15 +359,15 @@ public:
 TEST(NonlinearFactor, NonlinearFactor6) {
   TestFactor6 tf;
   Values tv;
-  tv.insert(TestKey(1), LieVector(1, 1.0));
-  tv.insert(TestKey(2), LieVector(1, 2.0));
-  tv.insert(TestKey(3), LieVector(1, 3.0));
-  tv.insert(TestKey(4), LieVector(1, 4.0));
-  tv.insert(TestKey(5), LieVector(1, 5.0));
-  tv.insert(TestKey(6), LieVector(1, 6.0));
+  tv.insert("x1", LieVector(1, 1.0));
+  tv.insert("x2", LieVector(1, 2.0));
+  tv.insert("x3", LieVector(1, 3.0));
+  tv.insert("x4", LieVector(1, 4.0));
+  tv.insert("x5", LieVector(1, 5.0));
+  tv.insert("x6", LieVector(1, 6.0));
   EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
-  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
+  Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5", "x6";
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
   LONGS_EQUAL(jf.keys()[0], 0);
   LONGS_EQUAL(jf.keys()[1], 1);

From 7c7c3e3836d8cfd72075c6f445d3f07418859fd1 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 6 Feb 2012 00:57:05 +0000
Subject: [PATCH 42/88] Fixed some merging errors

---
 gtsam/slam/planarSLAM.h | 17 +++++++----------
 gtsam/slam/pose2SLAM.h  |  5 +++--
 2 files changed, 10 insertions(+), 12 deletions(-)

diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 8fd568a3b..5c9c62d46 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -33,15 +33,12 @@ namespace planarSLAM {
 
   using namespace gtsam;
 
-  /// Typedef for a PoseKey with Pose2 data and 'x' symbol
-  typedef TypedSymbol<Pose2, 'x'> PoseKey;
-
-  /// Typedef for a PointKey with Point2 data and 'l' symbol
-  typedef TypedSymbol<Point2, 'l'> PointKey;
-
   /// Convenience function for constructing a pose key
   inline Symbol PoseKey(Index j) { return Symbol('x', j); }
 
+  /// Convenience function for constructing a pose key
+  inline Symbol PointKey(Index j) { return Symbol('l', j); }
+
   /*
    * List of typedefs for factors
    */
@@ -72,16 +69,16 @@ namespace planarSLAM {
     }
 
     /// get a pose
-    Pose2 pose(int key) const {	return (*this)[PoseKey(key)]; }
+    Pose2 pose(Index key) const { return at<Pose2>(PoseKey(key)); }
 
     /// get a point
-    Point2 point(int key) const {	return (*this)[PointKey(key)]; }
+    Point2 point(Index key) const { return at<Point2>(PointKey(key)); }
 
     /// insert a pose
-    void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
+    void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); }
 
     /// insert a point
-    void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
+    void insertPoint(Index key, const Point2& point) { insert(PointKey(key), point); }
   };
 
   /// Creates a NonlinearFactorGraph with the Values type
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index ab0c60050..7917a3c79 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -35,6 +35,7 @@ namespace pose2SLAM {
 
   /// Convenience function for constructing a pose key
   inline Symbol PoseKey(Index j) { return Symbol('x', j); }
+
   /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
   struct Values: public gtsam::Values {
 
@@ -49,10 +50,10 @@ namespace pose2SLAM {
     // Convenience for MATLAB wrapper, which does not allow for identically named methods
 
     /// get a pose
-    Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
+    Pose2 pose(Index key) const { return at<Pose2>(PoseKey(key)); }
 
     /// insert a pose
-    void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
+    void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); }
   };
 
   /**

From 6f4c95a65b30abe51fdb9083fe0fb796d6ee8df5 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Mon, 6 Feb 2012 03:29:14 +0000
Subject: [PATCH 43/88] apply bug fixes in MATLAB wrapper shared_ptr return
 from trunk. All tests work now.

---
 tests/matlab/testJacobianFactor.m | 11 ++++++-----
 tests/matlab/testKalmanFilter.m   |  6 +++---
 wrap/ReturnValue.cpp              |  6 +++---
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/tests/matlab/testJacobianFactor.m b/tests/matlab/testJacobianFactor.m
index ef7ed71f3..2209b36a6 100644
--- a/tests/matlab/testJacobianFactor.m
+++ b/tests/matlab/testJacobianFactor.m
@@ -29,8 +29,9 @@ x1 = 3;
 
 % the RHS
 b2=[-1;1.5;2;-1];
-model4 = SharedDiagonal([1;1;1;1]);
-combined = JacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
+sigmas = [1;1;1;1];
+model4 = gtsamSharedDiagonal(sigmas);
+combined = gtsamJacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
 
 % eliminate the first variable (x2) in the combined factor, destructive !
 actualCG = combined.eliminateFirst();
@@ -49,7 +50,7 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]);
+expectedCG = gtsamGaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]);
 % check if the result matches
 CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4));
 
@@ -68,8 +69,8 @@ Bx1 = [
 % the RHS
 b1= [0.0;0.894427];
 
-model2 = SharedDiagonal([1;1]);
-expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
+model2 = gtsamSharedDiagonal([1;1]);
+expectedLF = gtsamJacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
 
 % check if the result matches the combined (reduced) factor
 % FIXME: JacobianFactor/GaussianFactor mismatch
diff --git a/tests/matlab/testKalmanFilter.m b/tests/matlab/testKalmanFilter.m
index a074c75d3..e9c1b7149 100644
--- a/tests/matlab/testKalmanFilter.m
+++ b/tests/matlab/testKalmanFilter.m
@@ -22,13 +22,13 @@
 F = eye(2,2);
 B = eye(2,2);
 u = [1.0; 0.0];
-modelQ = SharedDiagonal([0.1;0.1]);
+modelQ = gtsamSharedDiagonal([0.1;0.1]);
 Q = 0.01*eye(2,2);
 H = eye(2,2);
 z1 = [1.0, 0.0]';
 z2 = [2.0, 0.0]';
 z3 = [3.0, 0.0]';
-modelR = SharedDiagonal([0.1;0.1]);
+modelR = gtsamSharedDiagonal([0.1;0.1]);
 R = 0.01*eye(2,2);
 
 %% Create the set of expected output TestValues
@@ -48,7 +48,7 @@ P23 = inv(I22) + Q;
 I33 = inv(P23) + inv(R);
 
 %% Create an KalmanFilter object
-KF = KalmanFilter(2);
+KF = gtsamKalmanFilter(2);
 
 %% Create the Kalman Filter initialization point
 x_initial = [0.0;0.0];
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 1aad0a08a..2095abc5e 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -59,17 +59,17 @@ void ReturnValue::wrap_result(FileWriter& file) const {
 
     // second return value in pair
     if (isPtr2) // if we already have a pointer
-      file.oss << "  out[1] = wrap_shared_ptr(result.second,\"" << type2 << "\");\n";
+      file.oss << "  out[1] = wrap_shared_ptr(result.second,\"" << matlabType2 << "\");\n";
     else if (category2 == ReturnValue::CLASS) // if we are going to make one
     	file.oss << "  out[1] = wrap_shared_ptr(make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n";
     else
       file.oss << "  out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
   }
   else if (isPtr1)
-    file.oss << "  out[0] = wrap_shared_ptr(result,\"" << type1 << "\");\n";
+    file.oss << "  out[0] = wrap_shared_ptr(result,\"" << matlabType1 << "\");\n";
   else if (category1 == ReturnValue::CLASS)
   	file.oss << "  out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n";
-  else if (type1!="void")
+  else if (matlabType1!="void")
     file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
 }
 

From 9f055fc81238bf11fe1a5a9976991b2502177984 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Mon, 6 Feb 2012 03:33:40 +0000
Subject: [PATCH 44/88] bug fix in advanced example. Question: Currently,
 pose2SLAMOptimizer is in fact useless! Is it needed? Why don't we add more
 functionalities for it?

---
 examples/matlab/Pose2SLAMExample_advanced.m | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/examples/matlab/Pose2SLAMExample_advanced.m b/examples/matlab/Pose2SLAMExample_advanced.m
index 7a7e10802..ce29119c6 100644
--- a/examples/matlab/Pose2SLAMExample_advanced.m
+++ b/examples/matlab/Pose2SLAMExample_advanced.m
@@ -55,19 +55,19 @@ initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
 initialEstimate.print('initial estimate');
 
 %% set up solver, choose ordering and optimize
-params = NonlinearOptimizationParameters_newDrecreaseThresholds(1e-15, 1e-15);
+params = gtsamNonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15);
 
 ord = graph.orderingCOLAMD(initialEstimate);
 
-result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);        
-              
-result.print('final result');        
+%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);                      
+%result.print('final result');
+
 % %     
 % % disp('\\\');
 % % 
 % % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-% % result = graph.optimize(initialEstimate);
-% % result.print('final result');
+result = graph.optimize(initialEstimate);
+result.print('final result');
 
 %% Get the corresponding dense matrix
 ord = graph.orderingCOLAMD(result);

From eaa9e4d7399561d7be9bc6d1390c206177a62cb1 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 6 Feb 2012 23:32:59 +0000
Subject: [PATCH 45/88] Continuing changes from templating NonlinearFactors on
 value instead of key

---
 gtsam/base/DerivedValue.h                     | 10 +++
 gtsam/nonlinear/NonlinearEquality.h           | 63 +++++++++--------
 gtsam/nonlinear/NonlinearFactor.h             | 10 ++-
 gtsam/nonlinear/Values-inl.h                  | 44 ++++++++++++
 gtsam/nonlinear/Values.h                      | 13 ++++
 gtsam/slam/BearingFactor.h                    |  4 +-
 gtsam/slam/BearingRangeFactor.h               | 10 +--
 gtsam/slam/BetweenFactor.h                    | 15 ++--
 gtsam/slam/GeneralSFMFactor.h                 | 18 ++---
 gtsam/slam/PriorFactor.h                      |  2 +-
 gtsam/slam/ProjectionFactor.h                 |  8 +--
 gtsam/slam/RangeFactor.h                      |  8 +--
 gtsam/slam/StereoFactor.h                     | 53 ++++++---------
 gtsam/slam/dataset.cpp                        |  4 +-
 gtsam/slam/pose3SLAM.cpp                      |  2 +-
 gtsam/slam/simulated2D.h                      | 20 +++---
 gtsam/slam/simulated2DOriented.h              | 14 ++--
 gtsam/slam/simulated3D.h                      | 46 ++++++-------
 gtsam/slam/smallExample.cpp                   | 49 ++++++-------
 gtsam/slam/tests/testAntiFactor.cpp           | 18 ++---
 gtsam/slam/tests/testGeneralSFMFactor.cpp     | 25 +++----
 .../testGeneralSFMFactor_Cal3Bundler.cpp      | 29 ++++----
 gtsam/slam/tests/testPlanarSLAM.cpp           | 20 +++---
 gtsam/slam/tests/testPose2SLAM.cpp            | 63 ++++++++---------
 gtsam/slam/tests/testPose3SLAM.cpp            | 22 +++---
 gtsam/slam/tests/testProjectionFactor.cpp     |  6 +-
 gtsam/slam/tests/testStereoFactor.cpp         |  4 +-
 gtsam/slam/visualSLAM.h                       | 68 ++++++++++---------
 28 files changed, 358 insertions(+), 290 deletions(-)

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index 0a66eb01f..3cac7c7dc 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -83,6 +83,16 @@ public:
     return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
 	}
 
+	/// Conversion to the derived class
+	operator const DERIVED& () const {
+	  return static_cast<const DERIVED&>(*this);
+	}
+
+  /// Conversion to the derived class
+  operator DERIVED& () {
+    return static_cast<DERIVED&>(*this);
+  }
+
 protected:
 	/// Assignment operator, protected because only the Value or DERIVED
 	/// assignment operators should be used.
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index e01623601..a95044dee 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -47,11 +47,11 @@ namespace gtsam {
 	 *
 	 * \nosubgrouping
 	 */
-	template<class KEY>
-	class NonlinearEquality: public NonlinearFactor1<KEY> {
+	template<class VALUE>
+	class NonlinearEquality: public NonlinearFactor1<VALUE> {
 
 	public:
-		typedef typename KEY::Value T;
+		typedef VALUE T;
 
 	private:
 
@@ -64,6 +64,12 @@ namespace gtsam {
 		// error gain in allow error case
 		double error_gain_;
 
+		// typedef to this class
+		typedef NonlinearEquality<VALUE> This;
+
+		// typedef to base class
+		typedef NonlinearFactor1<VALUE> Base;
+
 	public:
 
 		/**
@@ -71,7 +77,6 @@ namespace gtsam {
 		 */
 		bool (*compare_)(const T& a, const T& b);
 
-		typedef NonlinearFactor1<KEY> Base;
 
 		/** default constructor - only for serialization */
 		NonlinearEquality() {}
@@ -84,7 +89,7 @@ namespace gtsam {
 		/**
 		 * Constructor - forces exact evaluation
 		 */
-		NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
+		NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
 			Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
 			allow_error_(false), error_gain_(0.0),
 			compare_(_compare) {
@@ -93,7 +98,7 @@ namespace gtsam {
 		/**
 		 * Constructor - allows inexact evaluation
 		 */
-		NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
+		NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
 			Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
 			allow_error_(true), error_gain_(error_gain),
 			compare_(_compare) {
@@ -103,17 +108,17 @@ namespace gtsam {
 		/// @name Testable
 		/// @{
 
-		void print(const std::string& s = "") const {
-			std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
+		virtual void print(const std::string& s = "") const {
+			std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n";
 			gtsam::print(feasible_,"Feasible Point");
 			std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
 		}
 
 		/** Check if two factors are equal */
-		bool equals(const NonlinearEquality<KEY>& f, double tol = 1e-9) const {
-			if (!Base::equals(f)) return false;
-			return feasible_.equals(f.feasible_, tol) &&
-					fabs(error_gain_ - f.error_gain_) < tol;
+		virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+		  const This* e = dynamic_cast<const This*>(&f);
+		  return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
+          fabs(error_gain_ - e->error_gain_) < tol;
 		}
 
 		/// @}
@@ -122,7 +127,7 @@ namespace gtsam {
 
 		/** actual error function calculation */
 		virtual double error(const Values& c) const {
-			const T& xj = c[this->key_];
+			const T& xj = c.at<T>(this->key());
 			Vector e = this->unwhitenedError(c);
 			if (allow_error_ || !compare_(xj, feasible_)) {
 				return error_gain_ * dot(e,e);
@@ -132,7 +137,7 @@ namespace gtsam {
 		}
 
 		/** error function */
-		inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
+		Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
 			size_t nj = feasible_.dim();
 			if (allow_error_) {
 				if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
@@ -142,18 +147,18 @@ namespace gtsam {
 				return zero(nj); // set error to zero if equal
 			} else {
 				if (H) throw std::invalid_argument(
-						"Linearization point not feasible for " + (std::string)(this->key_) + "!");
+						"Linearization point not feasible for " + (std::string)(this->key()) + "!");
 				return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
 			}
 		}
 
 		// Linearize is over-written, because base linearization tries to whiten
 		virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
-			const T& xj = x[this->key_];
+			const T& xj = x.at<T>(this->key());
 			Matrix A;
 			Vector b = evaluateError(xj, A);
 			SharedDiagonal model = noiseModel::Constrained::All(b.size());
-			return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
+			return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
 		}
 
 		/// @}
@@ -177,14 +182,14 @@ namespace gtsam {
 	/**
 	 * Simple unary equality constraint - fixes a value for a variable
 	 */
-	template<class KEY>
-	class NonlinearEquality1 : public NonlinearFactor1<KEY> {
+	template<class VALUE>
+	class NonlinearEquality1 : public NonlinearFactor1<VALUE> {
 
 	public:
-		typedef typename KEY::Value X;
+		typedef VALUE X;
 
 	protected:
-		typedef NonlinearFactor1<KEY> Base;
+		typedef NonlinearFactor1<VALUE> Base;
 
 		/** default constructor to allow for serialization */
 		NonlinearEquality1() {}
@@ -196,10 +201,10 @@ namespace gtsam {
 
 	public:
 
-		typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
 
 		///TODO: comment
-		NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
+		NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0)
 			: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
 
 		virtual ~NonlinearEquality1() {}
@@ -236,13 +241,13 @@ namespace gtsam {
 	 * Simple binary equality constraint - this constraint forces two factors to
 	 * be the same.
 	 */
-	template<class KEY>
-	class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
+	template<class VALUE>
+	class NonlinearEquality2 : public NonlinearFactor2<VALUE, VALUE> {
 	public:
-		typedef typename KEY::Value X;
+		typedef VALUE X;
 
 	protected:
-		typedef NonlinearFactor2<KEY, KEY> Base;
+		typedef NonlinearFactor2<VALUE, VALUE> Base;
 
 		GTSAM_CONCEPT_MANIFOLD_TYPE(X);
 
@@ -251,10 +256,10 @@ namespace gtsam {
 
 	public:
 
-		typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
 
 		///TODO: comment
-		NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
+		NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0)
 			: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
 		virtual ~NonlinearEquality2() {}
 
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index ba2198b1b..166ad6980 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -95,6 +95,11 @@ public:
     std::cout << s << ": NonlinearFactor\n";
   }
 
+  /** Check if two factors are equal */
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+    return Base::equals(f);
+  }
+
 	/// @}
 	/// @name Standard Interface
 	/// @{
@@ -202,8 +207,9 @@ public:
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const {
-    return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+    const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
+    return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol);
   }
 
   /** get the dimension of the factor (number of rows on linearization) */
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 76a1ce035..31d6c318a 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -23,7 +23,10 @@
  */
 
 #include <utility>
+#include <boost/type_traits/conditional.hpp>
+#include <boost/type_traits/is_base_of.hpp>
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
 
 namespace gtsam {
@@ -37,6 +40,30 @@ namespace gtsam {
     ValueCloneAllocator() {}
   };
 
+  /* ************************************************************************* */
+  class ValueAutomaticCasting {
+    const Symbol& key_;
+    const Value& value_;
+  public:
+    ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {}
+
+    template<class ValueType>
+    operator const ValueType& () const {
+      // Check the type and throw exception if incorrect
+      if(typeid(ValueType) != typeid(value_))
+        throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_));
+
+      // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
+      return static_cast<const ValueType&>(value_);
+    }
+  };
+
+//  /* ************************************************************************* */
+//  template<class ValueType>
+//  ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) {
+//    lhs = rhs;
+//  }
+
   /* ************************************************************************* */
   template<typename ValueType>
   const ValueType& Values::at(const Symbol& j) const {
@@ -55,6 +82,18 @@ namespace gtsam {
     return static_cast<const ValueType&>(*item->second);
   }
 
+  /* ************************************************************************* */
+  inline ValueAutomaticCasting Values::at(const Symbol& j) const {
+    // Find the item
+    KeyValueMap::const_iterator item = values_.find(j);
+
+    // Throw exception if it does not exist
+    if(item == values_.end())
+      throw ValuesKeyDoesNotExist("retrieve", j);
+
+    return ValueAutomaticCasting(item->first, *item->second);
+  }
+
   /* ************************************************************************* */
   template<typename TypedKey>
   const typename TypedKey::Value& Values::at(const TypedKey& j) const {
@@ -65,6 +104,11 @@ namespace gtsam {
     return at<typename TypedKey::Value>(symbol);
   }
 
+  /* ************************************************************************* */
+  inline ValueAutomaticCasting Values::operator[](const Symbol& j) const {
+    return at(j);
+  }
+
   /* ************************************************************************* */
   template<typename ValueType>
   boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 0aa67c63d..f226007f1 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -44,6 +44,7 @@ namespace gtsam {
 
   // Forward declarations
   class ValueCloneAllocator;
+  class ValueAutomaticCasting;
 
 /**
   * A non-templated config holding any types of Manifold-group elements.  A
@@ -138,6 +139,15 @@ namespace gtsam {
     template<typename ValueType>
     const ValueType& at(const Symbol& j) const;
 
+    /** Retrieve a variable by key \c j.  This non-templated version returns a
+     * special ValueAutomaticCasting object that may be assigned to the proper
+     * value.
+     * @param j Retrieve the value associated with this key
+     * @return A ValueAutomaticCasting object that may be assignmed to a Value
+     * of the proper type.
+     */
+    ValueAutomaticCasting at(const Symbol& j) const;
+
     /** Retrieve a variable using a special key (typically TypedSymbol), which
      * contains the type of the value associated with the key, and which must
      * be conversion constructible to a Symbol, e.g.
@@ -153,6 +163,9 @@ namespace gtsam {
     const typename TypedKey::Value& operator[](const TypedKey& j) const {
       return at(j); }
 
+    /** operator[] syntax for at(const Symbol& j) */
+    ValueAutomaticCasting operator[](const Symbol& j) const;
+
     /** Check if a value exists with key \c j.  See exists<>(const Symbol& j)
      * and exists(const TypedKey& j) for versions that return the value if it
      * exists. */
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index 460f32b0c..70d19061e 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -29,9 +29,9 @@ namespace gtsam {
 	class BearingFactor: public NonlinearFactor2<POSE, POINT> {
 	private:
 
-		typedef typename POSE Pose;
+		typedef POSE Pose;
 		typedef typename Pose::Rotation Rot;
-		typedef typename POINT Point;
+		typedef POINT Point;
 
 		typedef BearingFactor<POSE, POINT> This;
 		typedef NonlinearFactor2<POSE, POINT> Base;
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index a81896b94..4e442ad86 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -31,9 +31,9 @@ namespace gtsam {
 	class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> {
 	private:
 
-		typedef typename POSE Pose;
+		typedef POSE Pose;
 		typedef typename POSE::Rotation Rot;
-		typedef typename POINT Point;
+		typedef POINT Point;
 
 		typedef BearingRangeFactor<POSE, POINT> This;
 		typedef NonlinearFactor2<POSE, POINT> Base;
@@ -60,9 +60,9 @@ namespace gtsam {
 	  /** Print */
 	  virtual void print(const std::string& s = "") const {
 	    std::cout << s << ": BearingRangeFactor("
-	    		<< (std::string) this->key1_ << ","
-	    		<< (std::string) this->key2_ << ")\n";
-	    bearing_.print("  measured bearing");
+	    		<< (std::string) this->key1() << ","
+	    		<< (std::string) this->key2() << ")\n";
+	    measuredBearing_.print("  measured bearing");
 	    std::cout << "  measured range: " << measuredRange_ << std::endl;
 	    this->noiseModel_->print("  noise model");
 	  }
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 0fd8f6846..313241eee 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -26,12 +26,15 @@ namespace gtsam {
 
 	/**
 	 * A class for a measurement predicted by "between(config[key1],config[key2])"
-	 * KEY1::Value is the Lie Group type
-	 * T is the measurement type, by default the same
+	 * @tparam VALUE the Value type
 	 */
 	template<class VALUE>
 	class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> {
 
+	public:
+
+	  typedef VALUE T;
+
 	private:
 
 		typedef BetweenFactor<VALUE> This;
@@ -64,8 +67,8 @@ namespace gtsam {
 		/** print */
 		virtual void print(const std::string& s) const {
 	    std::cout << s << "BetweenFactor("
-	    		<< (std::string) this->key1_ << ","
-	    		<< (std::string) this->key2_ << ")\n";
+	    		<< (std::string) this->key1() << ","
+	    		<< (std::string) this->key2() << ")\n";
 			measured_.print("  measured");
 	    this->noiseModel_->print("  noise model");
 		}
@@ -79,7 +82,7 @@ namespace gtsam {
 		/** implement functions needed to derive from Factor */
 
 		/** vector of errors */
-		Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2,
+		Vector evaluateError(const T& p1, const T& p2,
 				boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
 						boost::none) const {
 			T hx = p1.between(p2, H1, H2); // h(x)
@@ -121,7 +124,7 @@ namespace gtsam {
 
 		/** Syntactic sugar for constrained version */
 		BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
-		  BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
+		  BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
 
 	private:
 
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index c6d2e2ede..60092c15d 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -37,10 +37,10 @@ namespace gtsam {
 
 	public:
 
-		typedef typename CAMERA Cam;										///< typedef for camera type
-		typedef GeneralSFMFactor<CAMERA, LANDMARK> This ;	///< typedef for this object
-		typedef NonlinearFactor2<CAMERA, LANDMARK> Base;		///< typedef for the base class
-		typedef Point2 Measurement;													///< typedef for the measurement
+		typedef CAMERA Cam;					            					///< typedef for camera type
+		typedef GeneralSFMFactor<CAMERA, LANDMARK> This;	///< typedef for this object
+		typedef NonlinearFactor2<CAMERA, LANDMARK> Base;	///< typedef for the base class
+		typedef Point2 Measurement;												///< typedef for the measurement
 
 		// shorthand for a smart pointer to a factor
 		typedef boost::shared_ptr<This> shared_ptr;
@@ -52,7 +52,8 @@ namespace gtsam {
 		 * @param i is basically the frame number
 		 * @param j is the index of the landmark
 		 */
-		GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {}
+		GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
+		  Base(model, cameraKey, cameraKey), measured_(measured) {}
 
 		GeneralSFMFactor():measured_(0.0,0.0) {} 							///< default constructor
 		GeneralSFMFactor(const Point2 & p):measured_(p) {}			///< constructor that takes a Point2
@@ -66,14 +67,15 @@ namespace gtsam {
 		 */
 		void print(const std::string& s = "SFMFactor") const {
 			Base::print(s);
-			z_.print(s + ".z");
+			measured_.print(s + ".z");
 		}
 
 		/**
 		 * equals
 		 */
-		bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const	{
-			return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ;
+		bool equals(const NonlinearFactor &p, double tol = 1e-9) const	{
+		  const This* e = dynamic_cast<const This*>(&p);
+			return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
 		}
 
 		/** h(x)-z */
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index a5ff16c56..43a33a9fe 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -60,7 +60,7 @@ namespace gtsam {
 
 		/** print */
 		virtual void print(const std::string& s) const {
-			std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n";
+			std::cout << s << "PriorFactor(" << (std::string) this->key() << ")\n";
 			prior_.print("  prior");
 			this->noiseModel_->print("  noise model");
 		}
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 646f2a003..f654e5d33 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -78,8 +78,8 @@ namespace gtsam {
 
 		/// equals
 		virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
-      const This *e = dynamic_cast<const This*> (&expected);
-			return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol);
+      const This *e = dynamic_cast<const This*>(&p);
+			return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
 		}
 
 		/// Evaluate error h(x)-z and optionally derivatives
@@ -92,8 +92,8 @@ namespace gtsam {
 			} catch( CheiralityException& e) {
 			  if (H1) *H1 = zeros(2,6);
 			  if (H2) *H2 = zeros(2,3);
-			  cout << e.what() << ": Landmark "<< this->key2_.index() <<
-			      " moved behind camera " << this->key1_.index() << endl;
+			  cout << e.what() << ": Landmark "<< this->key2().index() <<
+			      " moved behind camera " << this->key1().index() << endl;
 			  return ones(2) * 2.0 * K_->fx();
 			}
 		}
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index 77c11d00d..43cc3da57 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -34,8 +34,8 @@ namespace gtsam {
 		typedef RangeFactor<POSE, POINT> This;
 		typedef NonlinearFactor2<POSE, POINT> Base;
 
-		typedef typename POSE Pose;
-		typedef typename POINT Point;
+		typedef POSE Pose;
+		typedef POINT Point;
 
 		// Concept requirements for this factor
 		GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
@@ -46,7 +46,7 @@ namespace gtsam {
 
 		RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
 				const SharedNoiseModel& model) :
-					Base(model, poseKey, PointKey), measured_(measured) {
+					Base(model, poseKey, pointKey), measured_(measured) {
 		}
 
 		virtual ~RangeFactor() {}
@@ -65,7 +65,7 @@ namespace gtsam {
 		/** equals specialized to this factor */
 		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e = dynamic_cast<const This*> (&expected);
-			return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->z_) < tol;
+			return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol;
 		}
 
 		/** print contents */
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 7f1e8077d..76359d8c9 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -22,20 +22,20 @@
 
 namespace gtsam {
 
-template<class KEY1, class KEY2>
-class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
+template<class POSE, class LANDMARK>
+class GenericStereoFactor: public NonlinearFactor2<POSE, LANDMARK> {
 private:
 
 	// Keep a copy of measurement and calibration for I/O
-	StereoPoint2 z_;											///< the measurement
+	StereoPoint2 measured_;											///< the measurement
 	boost::shared_ptr<Cal3_S2Stereo> K_;	///< shared pointer to calibration
 
 public:
 
 	// shorthand for base class type
-	typedef NonlinearFactor2<KEY1, KEY2> Base;					///< typedef for base class
+	typedef NonlinearFactor2<POSE, LANDMARK> Base;		     			///< typedef for base class
 	typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
-	typedef typename KEY1::Value CamPose;												///< typedef for Pose Lie Value type
+	typedef POSE CamPose;												///< typedef for Pose Lie Value type
 
 	/**
 	 * Default constructor
@@ -44,18 +44,17 @@ public:
 
 	/**
 	 * Constructor
-	 * @param z is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair
+	 * @param measured is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair
 	 * @param model is the noise model in on the measurement
-	 * @param j_pose the pose index
-	 * @param j_landmark the landmark index
+	 * @param poseKey the pose variable key
+	 * @param landmarkKey the landmark variable key
 	 * @param K the constant calibration
 	 */
-	GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose,
-			KEY2 j_landmark, const shared_ptrKStereo& K) :
-		Base(model, j_pose, j_landmark), z_(z), K_(K) {
+	GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) :
+		Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {
 	}
 
-	~GenericStereoFactor() {}  ///< desctructor
+	~GenericStereoFactor() {}  ///< destructor
 
 	/**
 	 * print
@@ -63,41 +62,27 @@ public:
 	 */
 	void print(const std::string& s) const {
 		Base::print(s);
-		z_.print(s + ".z");
+		measured_.print(s + ".z");
 	}
 
 	/**
 	 * equals
 	 */
-	bool equals(const GenericStereoFactor& f, double tol) const {
-		const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
-		if (p == NULL)
-			goto fail;
-		//if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
-		if (!z_.equals(p->z_, tol))
-			goto fail;
-		return true;
-
-		fail: print("actual");
-		p->print("expected");
-		return false;
+	virtual bool equals(const NonlinearFactor& f, double tol) const {
+	  const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
+		return p && Base::equals(f) && measured_.equals(p->measured_, tol);
 	}
 
 	/** h(x)-z */
 	Vector evaluateError(const Pose3& pose, const Point3& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 		StereoCamera stereoCam(pose, K_);
-		return (stereoCam.project(point, H1, H2) - z_).vector();
-	}
-
-	/// get the measurement z
-	StereoPoint2 z() {
-		return z_;
+		return (stereoCam.project(point, H1, H2) - measured_).vector();
 	}
 
 	/** return the measured */
-	inline const StereoPoint2 measured() const {
-	  return z_;
+	const StereoPoint2& measured() const {
+	  return measured_;
 	}
 
 private:
@@ -105,7 +90,7 @@ private:
 	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(measured_);
 		ar & BOOST_SERIALIZATION_NVP(K_);
 	}
 };
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 73fee971e..3f0551863 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -133,9 +133,9 @@ pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
 
 			// Insert vertices if pure odometry file
 			if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2());
-			if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2);
+			if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at<Pose2>(pose2SLAM::PoseKey(id1)) * l1Xl2);
 
-			pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
+			pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model));
 			graph->push_back(factor);
 		}
 		is.ignore(LINESIZE, '\n');
diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp
index 3ff8fa99c..2b9a3db1d 100644
--- a/gtsam/slam/pose3SLAM.cpp
+++ b/gtsam/slam/pose3SLAM.cpp
@@ -39,7 +39,7 @@ namespace gtsam {
 				Point3 gti(radius*cos(theta), radius*sin(theta), 0);
 				Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
 				Pose3 gTi(gR0 * _0Ri, gti);
-				x.insert(Key(i), gTi);
+				x.insert(PoseKey(i), gTi);
 			}
 			return x;
 		}
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index ca78bb334..bb9f143c5 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -57,14 +57,14 @@ namespace simulated2D {
     }
 
     /// Insert a pose
-    void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
-      insert(i, p);
+    void insertPose(Index j, const Point2& p) {
+      insert(PoseKey(j), p);
       nrPoses_++;
     }
 
     /// Insert a point
-    void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
-      insert(j, p);
+    void insertPoint(Index j, const Point2& p) {
+      insert(PointKey(j), p);
       nrPoints_++;
     }
 
@@ -79,13 +79,13 @@ namespace simulated2D {
     }
 
     /// Return pose i
-    Point2 pose(const simulated2D::PoseKey& i) const {
-      return (*this)[i];
+    Point2 pose(Index j) const {
+      return at<Point2>(PoseKey(j));
     }
 
     /// Return point j
-    Point2 point(const simulated2D::PointKey& j) const {
-      return (*this)[j];
+    Point2 point(Index j) const {
+      return at<Point2>(PointKey(j));
     }
   };
 
@@ -156,7 +156,7 @@ namespace simulated2D {
   /**
    * Binary factor simulating "odometry" between two Vectors
    */
-  template<class VALUE = Pose2>
+  template<class VALUE = Point2>
   class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
   public:
     typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class
@@ -203,7 +203,7 @@ namespace simulated2D {
     typedef POSE Pose; ///< shortcut to Pose type
     typedef LANDMARK Landmark; ///< shortcut to Landmark type
 
-    Landmark z_; ///< Measurement
+    Landmark measured_; ///< Measurement
 
     /// Create measurement factor
     GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index d0880b8f3..f38e9c34d 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -40,21 +40,21 @@ namespace simulated2DOriented {
   public:
     Values() : nrPoses_(0), nrPoints_(0)  {}
 
-    void insertPose(const PoseKey& i, const Pose2& p) {
-      insert(i, p);
+    void insertPose(Index j, const Pose2& p) {
+      insert(PoseKey(j), p);
       nrPoses_++;
     }
 
-    void insertPoint(const PointKey& j, const Point2& p) {
-      insert(j, p);
+    void insertPoint(Index j, const Point2& p) {
+      insert(PointKey(j), p);
       nrPoints_++;
     }
 
     int nrPoses() const {	return nrPoses_;	}
     int nrPoints() const { return nrPoints_;	}
 
-    Pose2 pose(const PoseKey& i) const { return (*this)[i];	}
-    Point2 point(const PointKey& j) const { return (*this)[j]; }
+    Pose2 pose(Index j) const { return at<Pose2>(PoseKey(j));	}
+    Point2 point(Index j) const { return at<Point2>(PointKey(j)); }
   };
 
   //TODO:: point prior is not implemented right now
@@ -111,7 +111,7 @@ namespace simulated2DOriented {
     }
 
     /// Evaluate error and optionally derivative
-    Vector evaluateError(const Pose2& x1, const Pose2& x2,
+    Vector evaluateError(const VALUE& x1, const VALUE& x2,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none) const {
       return measured_.localCoordinates(odo(x1, x2, H1, H2));
diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index 4a24bcaa6..40adcdbe2 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -36,8 +36,11 @@ namespace simulated3D {
  * the simulated2D domain.
  */
 
-typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
-typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
+  /// Convenience function for constructing a pose key
+  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
+
+  /// Convenience function for constructing a pose key
+  inline Symbol PointKey(Index j) { return Symbol('l', j); }
 
 /**
  * Prior on a single pose
@@ -61,19 +64,18 @@ Point3 mea(const Point3& x, const Point3& l,
 /**
  * A prior factor on a single linear robot pose
  */
-struct PointPrior3D: public NonlinearFactor1<PoseKey> {
+struct PointPrior3D: public NonlinearFactor1<Point3> {
 
-	Point3 z_; ///< The prior pose value for the variable attached to this factor
+	Point3 measured_; ///< The prior pose value for the variable attached to this factor
 
 	/**
 	 * Constructor for a prior factor
-	 * @param z is the measured/prior position for the pose
+	 * @param measured is the measured/prior position for the pose
 	 * @param model is the measurement model for the factor (Dimension: 3)
-	 * @param j is the key for the pose
+	 * @param key is the key for the pose
 	 */
-	PointPrior3D(const Point3& z,
-			const SharedNoiseModel& model, const PoseKey& j) :
-				NonlinearFactor1<PoseKey> (model, j), z_(z) {
+	PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) :
+	  NonlinearFactor1<Point3> (model, key), measured_(measured) {
 	}
 
 	/**
@@ -85,29 +87,27 @@ struct PointPrior3D: public NonlinearFactor1<PoseKey> {
 	 */
 	Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
 			boost::none) {
-		return (prior(x, H) - z_).vector();
+		return (prior(x, H) - measured_).vector();
 	}
 };
 
 /**
  * Models a linear 3D measurement between 3D points
  */
-struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
+struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
 
-	Point3 z_; ///< Linear displacement between a pose and landmark
+	Point3 measured_; ///< Linear displacement between a pose and landmark
 
 	/**
 	 * Creates a measurement factor with a given measurement
-	 * @param z is the measurement, a linear displacement between poses and landmarks
+	 * @param measured is the measurement, a linear displacement between poses and landmarks
 	 * @param model is a measurement model for the factor (Dimension: 3)
-	 * @param j1 is the pose key of the robot
-	 * @param j2 is the point key for the landmark
+	 * @param poseKey is the pose key of the robot
+	 * @param pointKey is the point key for the landmark
 	 */
-	Simulated3DMeasurement(const Point3& z,
-			const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
-				NonlinearFactor2<PoseKey, PointKey> (
-						model, j1, j2), z_(z) {
-	}
+	Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
+	    const Symbol& poseKey, const Symbol& pointKey) :
+	      NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
 
 	/**
 	 * Error function with optional derivatives
@@ -117,9 +117,9 @@ struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
 	 * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
 	 * @return vector error between measurement and prediction (Dimension: 3)
 	 */
-	Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
-			Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
-		return (mea(x1, x2, H1, H2) - z_).vector();
+	Vector evaluateError(const Point3& x1, const Point3& x2,
+	    boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
+		return (mea(x1, x2, H1, H2) - measured_).vector();
 	}
 };
 
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 948616ea6..8ee8cb09a 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -39,7 +39,10 @@ using namespace std;
 namespace gtsam {
 namespace example {
 
-	typedef boost::shared_ptr<NonlinearFactor > shared;
+  using simulated2D::PoseKey;
+  using simulated2D::PointKey;
+
+	typedef boost::shared_ptr<NonlinearFactor> shared;
 
 	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
 	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
@@ -57,22 +60,22 @@ namespace example {
 
 		// prior on x1
 		Point2 mu;
-		shared f1(new simulated2D::Prior(mu, sigma0_1, 1));
+		shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1)));
 		nlfg->push_back(f1);
 
 		// odometry between x1 and x2
 		Point2 z2(1.5, 0);
-		shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2));
+		shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2)));
 		nlfg->push_back(f2);
 
 		// measurement between x1 and l1
 		Point2 z3(0, -1);
-		shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1));
+		shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1)));
 		nlfg->push_back(f3);
 
 		// measurement between x2 and l1
 		Point2 z4(-1.5, -1.);
-		shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1));
+		shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1)));
 		nlfg->push_back(f4);
 
 		return nlfg;
@@ -86,9 +89,9 @@ namespace example {
 	/* ************************************************************************* */
 	Values createValues() {
 		Values c;
-		c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0));
-		c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0));
-		c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0));
+		c.insert(PoseKey(1), Point2(0.0, 0.0));
+		c.insert(PoseKey(2), Point2(1.5, 0.0));
+		c.insert(PointKey(1), Point2(0.0, -1.0));
 		return c;
 	}
 
@@ -104,9 +107,9 @@ namespace example {
 	/* ************************************************************************* */
 	boost::shared_ptr<const Values> sharedNoisyValues() {
 		boost::shared_ptr<Values> c(new Values);
-		c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1));
-		c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2));
-		c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1));
+		c->insert(PoseKey(1), Point2(0.1, 0.1));
+		c->insert(PoseKey(2), Point2(1.4, 0.2));
+		c->insert(PointKey(1), Point2(0.1, -1.1));
 		return c;
 	}
 
@@ -195,17 +198,15 @@ namespace example {
 					 0.0, cos(v.y()));
 		}
 
-		struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
+		struct UnaryFactor: public gtsam::NonlinearFactor1<Point2> {
 
 			Point2 z_;
 
-			UnaryFactor(const Point2& z, const SharedNoiseModel& model,
-					const simulated2D::PoseKey& key) :
-				gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
+			UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) :
+				gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
 			}
 
-			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
-					boost::none) const {
+			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
 				if (A) *A = H(x);
 				return (h(x) - z_).vector();
 			}
@@ -220,7 +221,7 @@ namespace example {
 		Vector z = Vector_(2, 1.0, 0.0);
 		double sigma = 0.1;
 		boost::shared_ptr<smallOptimize::UnaryFactor> factor(
-				new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1));
+				new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1)));
 		fg->push_back(factor);
 		return fg;
 	}
@@ -238,23 +239,23 @@ namespace example {
 
 		// prior on x1
 		Point2 x1(1.0, 0.0);
-		shared prior(new simulated2D::Prior(x1, sigma1_0, 1));
+		shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1)));
 		nlfg.push_back(prior);
 		poses.insert(simulated2D::PoseKey(1), x1);
 
 		for (int t = 2; t <= T; t++) {
 			// odometry between x_t and x_{t-1}
 			Point2 odo(1.0, 0.0);
-			shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t));
+			shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t)));
 			nlfg.push_back(odometry);
 
 			// measurement on x_t is like perfect GPS
 			Point2 xt(t, 0);
-			shared measurement(new simulated2D::Prior(xt, sigma1_0, t));
+			shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t)));
 			nlfg.push_back(measurement);
 
 			// initial estimate
-			poses.insert(simulated2D::PoseKey(t), xt);
+			poses.insert(PoseKey(t), xt);
 		}
 
 		return make_pair(nlfg, poses);
@@ -414,8 +415,8 @@ namespace example {
 
 	/* ************************************************************************* */
 	// Create key for simulated planar graph
-	simulated2D::PoseKey key(int x, int y) {
-		return simulated2D::PoseKey(1000*x+y);
+	Symbol key(int x, int y) {
+		return PoseKey(1000*x+y);
 	}
 
 	/* ************************************************************************* */
diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp
index 99be5d09a..183b0c600 100644
--- a/gtsam/slam/tests/testAntiFactor.cpp
+++ b/gtsam/slam/tests/testAntiFactor.cpp
@@ -27,6 +27,8 @@
 using namespace std;
 using namespace gtsam;
 
+using pose3SLAM::PoseKey;
+
 /* ************************************************************************* */
 TEST( AntiFactor, NegativeHessian)
 {
@@ -40,17 +42,17 @@ TEST( AntiFactor, NegativeHessian)
 
   // Create a configuration corresponding to the ground truth
   boost::shared_ptr<Values> values(new Values());
-  values->insert(pose3SLAM::Key(1), pose1);
-  values->insert(pose3SLAM::Key(2), pose2);
+  values->insert(PoseKey(1), pose1);
+  values->insert(PoseKey(2), pose2);
 
   // Define an elimination ordering
   Ordering::shared_ptr ordering(new Ordering());
-  ordering->insert(pose3SLAM::Key(1), 0);
-  ordering->insert(pose3SLAM::Key(2), 1);
+  ordering->insert(PoseKey(1), 0);
+  ordering->insert(PoseKey(2), 1);
 
 
   // Create a "standard" factor
-  BetweenFactor<pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Key>(1, 2, z, sigma));
+  BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(PoseKey(1), PoseKey(2), z, sigma));
 
   // Linearize it into a Jacobian Factor
   GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@@ -101,8 +103,8 @@ TEST( AntiFactor, EquivalentBayesNet)
 
 	// Create a configuration corresponding to the ground truth
 	boost::shared_ptr<Values> values(new Values());
-	values->insert(pose3SLAM::Key(1), pose1);
-	values->insert(pose3SLAM::Key(2), pose2);
+	values->insert(PoseKey(1), pose1);
+	values->insert(PoseKey(2), pose2);
 
 	// Define an elimination ordering
 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@@ -115,7 +117,7 @@ TEST( AntiFactor, EquivalentBayesNet)
   VectorValues expectedDeltas = optimize(*expectedBayesNet);
 
 	// Add an additional factor between Pose1 and Pose2
-  pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma));
+  pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma));
   graph->push_back(f1);
 
   // Add the corresponding AntiFactor between Pose1 and Pose2
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index e887cac06..022a76960 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -28,25 +28,23 @@ using namespace std;
 using namespace gtsam;
 
 typedef PinholeCamera<Cal3_S2> GeneralCamera;
-typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
-typedef TypedSymbol<Point3, 'l'> PointKey;
-typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
-typedef NonlinearEquality<CameraKey> CameraConstraint;
-typedef NonlinearEquality<PointKey> Point3Constraint;
+typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
+typedef NonlinearEquality<GeneralCamera> CameraConstraint;
+typedef NonlinearEquality<Point3> Point3Constraint;
 
 class Graph: public NonlinearFactorGraph {
 public:
-	void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
-		push_back(boost::make_shared<Projection>(z, model, i, j));
+	void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
+		push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
 	}
 
 	void addCameraConstraint(int j, const GeneralCamera& p) {
-		boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(j, p));
+		boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x',j), p));
 		push_back(factor);
 	}
 
   void addPoint3Constraint(int j, const Point3& p) {
-    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(j, p));
+    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('x',j), p));
     push_back(factor);
   }
 
@@ -76,7 +74,7 @@ TEST( GeneralSFMFactor, equals )
 {
 	// Create two identical factors and make sure they're equal
 	Vector z = Vector_(2,323.,240.);
-	const int cameraFrameNumber=1, landmarkNumber=1;
+  const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
 	const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 	boost::shared_ptr<Projection>
 	  factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@@ -90,17 +88,16 @@ TEST( GeneralSFMFactor, equals )
 /* ************************************************************************* */
 TEST( GeneralSFMFactor, error ) {
 	Point2 z(3.,0.);
-	const int cameraFrameNumber=1, landmarkNumber=1;
 	const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 	boost::shared_ptr<Projection>
-	factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
+	factor(new Projection(z, sigma, "x1", "l1"));
 	// For the following configuration, the factor predicts 320,240
 	Values values;
 	Rot3 R;
 	Point3 t1(0,0,-6);
 	Pose3 x1(R,t1);
-	values.insert(CameraKey(1), GeneralCamera(x1));
-	Point3 l1;  values.insert(PointKey(1), l1);
+	values.insert("x1", GeneralCamera(x1));
+	Point3 l1;  values.insert("l1", l1);
 	EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 5a626d095..7c1d395ba 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -28,26 +28,24 @@ using namespace std;
 using namespace gtsam;
 
 typedef PinholeCamera<Cal3Bundler> GeneralCamera;
-typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
-typedef TypedSymbol<Point3, 'l'> PointKey;
-typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
-typedef NonlinearEquality<CameraKey> CameraConstraint;
-typedef NonlinearEquality<PointKey> Point3Constraint;
+typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
+typedef NonlinearEquality<GeneralCamera> CameraConstraint;
+typedef NonlinearEquality<Point3> Point3Constraint;
 
 /* ************************************************************************* */
 class Graph: public NonlinearFactorGraph {
 public:
-  void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
-    push_back(boost::make_shared<Projection>(z, model, i, j));
+  void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
+    push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
   }
 
   void addCameraConstraint(int j, const GeneralCamera& p) {
-    boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(j, p));
+    boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x', j), p));
     push_back(factor);
   }
 
   void addPoint3Constraint(int j, const Point3& p) {
-    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(j, p));
+    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l', j), p));
     push_back(factor);
   }
 
@@ -77,7 +75,7 @@ TEST( GeneralSFMFactor, equals )
 {
   // Create two identical factors and make sure they're equal
   Vector z = Vector_(2,323.,240.);
-  const int cameraFrameNumber=1, landmarkNumber=1;
+  const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
   const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
   boost::shared_ptr<Projection>
     factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@@ -91,17 +89,16 @@ TEST( GeneralSFMFactor, equals )
 /* ************************************************************************* */
 TEST( GeneralSFMFactor, error ) {
   Point2 z(3.,0.);
-  const int cameraFrameNumber=1, landmarkNumber=1;
   const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
   boost::shared_ptr<Projection>
-  factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
+  factor(new Projection(z, sigma, "x1", "l1"));
   // For the following configuration, the factor predicts 320,240
   Values values;
   Rot3 R;
   Point3 t1(0,0,-6);
   Pose3 x1(R,t1);
-  values.insert(CameraKey(1), GeneralCamera(x1));
-  Point3 l1;  values.insert(PointKey(1), l1);
+  values.insert("x1", GeneralCamera(x1));
+  Point3 l1;  values.insert("l1", l1);
   EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -171,13 +168,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
   const double noise = baseline*0.1;
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(PointKey(i), pt) ;
+    values->insert(Symbol('l',i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp
index 56ac71923..3daf5f4ab 100644
--- a/gtsam/slam/tests/testPlanarSLAM.cpp
+++ b/gtsam/slam/tests/testPlanarSLAM.cpp
@@ -37,7 +37,7 @@ SharedNoiseModel
 /* ************************************************************************* */
 TEST( planarSLAM, PriorFactor_equals )
 {
-	planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3);
+	planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3);
 	EXPECT(assert_equal(factor1, factor1, 1e-5));
 	EXPECT(assert_equal(factor2, factor2, 1e-5));
 	EXPECT(assert_inequal(factor1, factor2, 1e-5));
@@ -48,7 +48,7 @@ TEST( planarSLAM, BearingFactor )
 {
 	// Create factor
 	Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
-	planarSLAM::Bearing factor(2, 3, z, sigma);
+	planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma);
 
 	// create config
 	planarSLAM::Values c;
@@ -64,8 +64,8 @@ TEST( planarSLAM, BearingFactor )
 TEST( planarSLAM, BearingFactor_equals )
 {
 	planarSLAM::Bearing
-		factor1(2, 3, Rot2::fromAngle(0.1), sigma),
-		factor2(2, 3, Rot2::fromAngle(2.3), sigma);
+		factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma),
+		factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma);
 	EXPECT(assert_equal(factor1, factor1, 1e-5));
 	EXPECT(assert_equal(factor2, factor2, 1e-5));
 	EXPECT(assert_inequal(factor1, factor2, 1e-5));
@@ -76,7 +76,7 @@ TEST( planarSLAM, RangeFactor )
 {
 	// Create factor
 	double z(sqrt(2) - 0.22); // h(x) - z = 0.22
-	planarSLAM::Range factor(2, 3, z, sigma);
+	planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma);
 
 	// create config
 	planarSLAM::Values c;
@@ -91,7 +91,7 @@ TEST( planarSLAM, RangeFactor )
 /* ************************************************************************* */
 TEST( planarSLAM, RangeFactor_equals )
 {
-	planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma);
+	planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma);
 	EXPECT(assert_equal(factor1, factor1, 1e-5));
 	EXPECT(assert_equal(factor2, factor2, 1e-5));
 	EXPECT(assert_inequal(factor1, factor2, 1e-5));
@@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor )
 	// Create factor
 	Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
 	double b(sqrt(2) - 0.22); // h(x) - z = 0.22
-	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
+	planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2);
 
 	// create config
 	planarSLAM::Values c;
@@ -119,8 +119,8 @@ TEST( planarSLAM, BearingRangeFactor )
 TEST( planarSLAM, BearingRangeFactor_equals )
 {
 	planarSLAM::BearingRange
-		factor1(2, 3, Rot2::fromAngle(0.1), 7.3,  sigma2),
-		factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2);
+		factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3,  sigma2),
+		factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2);
 	EXPECT(assert_equal(factor1, factor1, 1e-5));
 	EXPECT(assert_equal(factor2, factor2, 1e-5));
 	EXPECT(assert_inequal(factor1, factor2, 1e-5));
@@ -129,7 +129,7 @@ TEST( planarSLAM, BearingRangeFactor_equals )
 /* ************************************************************************* */
 TEST( planarSLAM, PoseConstraint_equals )
 {
-	planarSLAM::Constraint factor1(2, x2), factor2(2, x3);
+	planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3);
 	EXPECT(assert_equal(factor1, factor1, 1e-5));
 	EXPECT(assert_equal(factor2, factor2, 1e-5));
 	EXPECT(assert_inequal(factor1, factor2, 1e-5));
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index 73fec2a29..29c28dbc1 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -34,6 +34,7 @@ using namespace boost::assign;
 using namespace std;
 
 typedef pose2SLAM::Odometry Pose2Factor;
+using pose2SLAM::PoseKey;
 
 // common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
@@ -49,7 +50,7 @@ static noiseModel::Gaussian::shared_ptr covariance(
 // Test constraint, small displacement
 Vector f1(const Pose2& pose1, const Pose2& pose2) {
 	Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
-	Pose2Factor constraint(1, 2, z, covariance);
+	Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
 	return constraint.evaluateError(pose1, pose2);
 }
 
@@ -57,7 +58,7 @@ TEST( Pose2SLAM, constraint1 )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
-	Pose2Factor constraint(1, 2, pose2, covariance);
+	Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
 	Matrix H1, H2;
 	Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
 
@@ -72,7 +73,7 @@ TEST( Pose2SLAM, constraint1 )
 // Test constraint, large displacement
 Vector f2(const Pose2& pose1, const Pose2& pose2) {
 	Pose2 z(2,2,M_PI_2);
-	Pose2Factor constraint(1, 2, z, covariance);
+	Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
 	return constraint.evaluateError(pose1, pose2);
 }
 
@@ -80,7 +81,7 @@ TEST( Pose2SLAM, constraint2 )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 pose1, pose2(2,2,M_PI_2);
-	Pose2Factor constraint(1, 2, pose2, covariance);
+	Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
 	Matrix H1, H2;
 	Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
 
@@ -110,7 +111,7 @@ TEST( Pose2SLAM, linearization )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 measured(2,2,M_PI_2);
-	Pose2Factor constraint(1,2,measured, covariance);
+	Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance);
 	pose2SLAM::Graph graph;
 	graph.addOdometry(1,2,measured, covariance);
 
@@ -178,8 +179,8 @@ TEST(Pose2Graph, optimize) {
 TEST(Pose2Graph, optimizeThreePoses) {
 
 	// Create a hexagon of poses
-	Values hexagon = pose2SLAM::circle(3,1.0);
-  Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
+	pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
+  Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@@ -190,10 +191,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
   fg->addOdometry(2, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<Values> initial(new Values());
-  initial->insert(pose2SLAM::PoseKey(0), p0);
-  initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
+  initial->insertPose(0, p0);
+  initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
@@ -207,7 +208,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
   Values actual = *optimizer.values();
 
   // Check with ground truth
-  CHECK(assert_equal(hexagon, actual));
+  CHECK(assert_equal((const Values&)hexagon, actual));
 }
 
 /* ************************************************************************* */
@@ -215,8 +216,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
 TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
 
 	// Create a hexagon of poses
-	Values hexagon = pose2SLAM::circle(6,1.0);
-  Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
+	pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
+  Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@@ -230,13 +231,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
   fg->addOdometry(5, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<Values> initial(new Values());
-  initial->insert(pose2SLAM::PoseKey(0), p0);
-  initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
+  initial->insertPose(0, p0);
+  initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
@@ -250,10 +251,10 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
   Values actual = *optimizer.values();
 
   // Check with ground truth
-  CHECK(assert_equal(hexagon, actual));
+  CHECK(assert_equal((const Values&)hexagon, actual));
 
   // Check loop closure
-  CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)])));
+  CHECK(assert_equal(delta, actual.at<Pose2>(PoseKey(5)).between(actual.at<Pose2>(PoseKey(0)))));
 
 //  Pose2SLAMOptimizer myOptimizer("3");
 
@@ -387,10 +388,10 @@ TEST( Pose2Prior, error )
 	// Choose a linearization point
 	Pose2 p1(1, 0, 0); // robot at (1,0)
 	pose2SLAM::Values x0;
-	x0.insert(pose2SLAM::PoseKey(1), p1);
+	x0.insert(PoseKey(1), p1);
 
 	// Create factor
-	pose2SLAM::Prior factor(1, p1, sigmas);
+	pose2SLAM::Prior factor(PoseKey(1), p1, sigmas);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -417,7 +418,7 @@ TEST( Pose2Prior, error )
 /* ************************************************************************* */
 // common Pose2Prior for tests below
 static gtsam::Pose2 priorVal(2,2,M_PI_2);
-static pose2SLAM::Prior priorFactor(1,priorVal, sigmas);
+static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas);
 
 /* ************************************************************************* */
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
@@ -431,7 +432,7 @@ TEST( Pose2Prior, linearize )
 {
 	// Choose a linearization point at ground truth
 	pose2SLAM::Values x0;
-	x0.insert(pose2SLAM::PoseKey(1),priorVal);
+	x0.insertPose(1, priorVal);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -451,12 +452,12 @@ TEST( Pose2Factor, error )
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
 	pose2SLAM::Values x0;
-	x0.insert(pose2SLAM::PoseKey(1), p1);
-	x0.insert(pose2SLAM::PoseKey(2), p2);
+	x0.insertPose(1, p1);
+	x0.insertPose(2, p2);
 
 	// Create factor
 	Pose2 z = p1.between(p2);
-	Pose2Factor factor(1, 2, z, covariance);
+	Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -483,7 +484,7 @@ TEST( Pose2Factor, error )
 /* ************************************************************************* */
 // common Pose2Factor for tests below
 static Pose2 measured(2,2,M_PI_2);
-static Pose2Factor factor(1,2,measured, covariance);
+static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance);
 
 /* ************************************************************************* */
 TEST( Pose2Factor, rhs )
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 79cc9fe1e..a9b743145 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -50,7 +50,7 @@ TEST(Pose3Graph, optimizeCircle) {
 	// Create a hexagon of poses
 	double radius = 10;
 	Values hexagon = pose3SLAM::circle(6,radius);
-  Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
+  Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose3Graph> fg(new Pose3Graph);
@@ -67,12 +67,12 @@ TEST(Pose3Graph, optimizeCircle) {
 
   // Create initial config
   boost::shared_ptr<Values> initial(new Values());
-  initial->insert(Key(0), gT0);
-  initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(PoseKey(0), gT0);
+  initial->insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(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);
@@ -87,17 +87,17 @@ TEST(Pose3Graph, optimizeCircle) {
   CHECK(assert_equal(hexagon, actual,1e-4));
 
   // Check loop closure
-  CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5));
+  CHECK(assert_equal(_0T1, actual.at<Pose3>(PoseKey(5)).between(actual.at<Pose3>(PoseKey(0))),1e-5));
 }
 
 /* ************************************************************************* */
 TEST(Pose3Graph, partial_prior_height) {
-	typedef PartialPriorFactor<pose3SLAM::Key> Partial;
+	typedef PartialPriorFactor<Pose3> Partial;
 
 	// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
 
 	// height prior - single element interface
-	pose3SLAM::Key key(1);
+	Symbol key = PoseKey(1);
 	double exp_height = 5.0;
 	SharedDiagonal model = noiseModel::Unit::Create(1);
 	Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
@@ -117,7 +117,7 @@ TEST(Pose3Graph, partial_prior_height) {
 	EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
 
 	Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
-	EXPECT(assert_equal(expected, actual[key], tol));
+	EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
 
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index 1d8616c30..f7f804fb9 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -49,7 +49,7 @@ TEST( ProjectionFactor, error )
 	Point2 z(323.,240.);
 	int cameraFrameNumber=1, landmarkNumber=1;
 	boost::shared_ptr<visualSLAM::ProjectionFactor>
-	factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
+	factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
 
 	// For the following values structure, the factor predicts 320,240
 	Values config;
@@ -98,10 +98,10 @@ TEST( ProjectionFactor, equals )
 	Vector z = Vector_(2,323.,240.);
 	int cameraFrameNumber=1, landmarkNumber=1;
 	boost::shared_ptr<visualSLAM::ProjectionFactor>
-	  factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
+	  factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
 
 	boost::shared_ptr<visualSLAM::ProjectionFactor>
-		factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
+		factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
 
 	CHECK(assert_equal(*factor1, *factor2));
 }
diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp
index 94a4d4060..fc026fb35 100644
--- a/gtsam/slam/tests/testStereoFactor.cpp
+++ b/gtsam/slam/tests/testStereoFactor.cpp
@@ -52,11 +52,11 @@ TEST( StereoFactor, singlePoint)
 	boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
 	boost::shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph());
 
-	graph->add(visualSLAM::PoseConstraint(1,camera1));
+	graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1));
 
 	StereoPoint2 z14(320,320.0-50, 240);
   // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
-	graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
+	graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
 
 	// Create a configuration corresponding to the ground truth
 	boost::shared_ptr<Values> values(new Values());
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index c1a9123d9..4ecbf2e43 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -34,22 +34,24 @@ namespace visualSLAM {
 
 	using namespace gtsam;
 
+  /// Convenience function for constructing a pose key
+  inline Symbol PoseKey(Index j) { return Symbol('x', j); }
+
+  /// Convenience function for constructing a pose key
+  inline Symbol PointKey(Index j) { return Symbol('l', j); }
+
   /**
    * Typedefs that make up the visualSLAM namespace.
    */
-  typedef TypedSymbol<Pose3,'x'> PoseKey;									///< The key type used for poses
-  typedef TypedSymbol<Point3,'l'> PointKey;								///< The key type used for points
-  typedef boost::shared_ptr<Values> shared_values;				///< shared pointer to values data structure
-
-  typedef NonlinearEquality<PoseKey> PoseConstraint;	 ///< put a hard constraint on a pose
-  typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
-  typedef PriorFactor<PoseKey> PosePrior;							 ///< put a soft prior on a Pose
-  typedef PriorFactor<PointKey> PointPrior;						 ///< put a soft prior on a point
-  typedef RangeFactor<PoseKey, PointKey> RangeFactor;  ///< put a factor on the range from a pose to a point
+  typedef NonlinearEquality<Pose3> PoseConstraint;	 ///< put a hard constraint on a pose
+  typedef NonlinearEquality<Point3> PointConstraint; ///< put a hard constraint on a point
+  typedef PriorFactor<Pose3> PosePrior;							 ///< put a soft prior on a Pose
+  typedef PriorFactor<Point3> PointPrior;						 ///< put a soft prior on a point
+  typedef RangeFactor<Pose3, Point3> RangeFactor;  ///< put a factor on the range from a pose to a point
   
   /// monocular and stereo camera typedefs for general use
-  typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
-  typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
+  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
+  typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
 
   /**
    * Non-linear factor graph for vanilla visual SLAM
@@ -76,69 +78,69 @@ namespace visualSLAM {
 
     /**
      *  Add a projection factor measurement (monocular)
-     *  @param z the measurement
+     *  @param measured the measurement
      *  @param model the noise model for the measurement
-     *  @param i index of camera
-     *  @param j index of point
+     *  @param poseKey variable key for the camera pose
+     *  @param pointKey variable key for the landmark
      *  @param K shared pointer to calibration object
      */
-    void addMeasurement(const Point2& z, const SharedNoiseModel& model,
-        PoseKey i, PointKey j, const shared_ptrK& K) {
-      boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K));
+    void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
+        const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) {
+      boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
       push_back(factor);
     }
 
     /**
      *  Add a constraint on a pose (for now, *must* be satisfied in any Values)
-     *  @param j index of camera
+     *  @param key variable key of the camera pose
      *  @param p to which pose to constrain it to
      */
-    void addPoseConstraint(int j, const Pose3& p = Pose3()) {
-      boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p));
+    void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) {
+      boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(key, p));
       push_back(factor);
     }
 
     /**
      *  Add a constraint on a point (for now, *must* be satisfied in any Values)
-     *  @param j index of landmark
+     *  @param key variable key of the landmark
      *  @param p point around which soft prior is defined
      */
-    void addPointConstraint(int j, const Point3& p = Point3()) {
-      boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p));
+    void addPointConstraint(const Symbol& key, const Point3& p = Point3()) {
+      boost::shared_ptr<PointConstraint> factor(new PointConstraint(key, p));
       push_back(factor);
     }
 
     /**
      *  Add a prior on a pose
-     *  @param j index of camera
+     *  @param key variable key of the camera pose
      *  @param p around which soft prior is defined
      *  @param model uncertainty model of this prior
      */
-    void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
-      boost::shared_ptr<PosePrior> factor(new PosePrior(j, p, model));
+    void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
+      boost::shared_ptr<PosePrior> factor(new PosePrior(key, p, model));
       push_back(factor);
     }
 
     /**
      *  Add a prior on a landmark
-     *  @param j index of landmark
+     *  @param key variable key of the landmark
      *  @param p to which point to constrain it to
      *  @param model uncertainty model of this prior
      */
-    void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
-      boost::shared_ptr<PointPrior> factor(new PointPrior(j, p, model));
+    void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
+      boost::shared_ptr<PointPrior> factor(new PointPrior(key, p, model));
       push_back(factor);
     }
 
     /**
      *  Add a range prior to a landmark
-     *  @param i index of pose
-     *  @param j index of landmark
+     *  @param poseKey variable key of the camera pose
+     *  @param pointKey variable key of the landmark
      *  @param range approximate range to landmark
      *  @param model uncertainty model of this prior
      */
-    void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
-      push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(i, j, range, model)));
+    void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
+      push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(poseKey, pointKey, range, model)));
     }
 
   }; // Graph

From 2f7f535f347bd6cf41f27df55731968ea7c6f3b4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 7 Feb 2012 04:02:20 +0000
Subject: [PATCH 46/88] All unit tests pass with nonlinear factors templated on
 value instead of key

---
 examples/CameraResectioning.cpp               | 13 ++--
 examples/PlanarSLAMExample_easy.cpp           | 29 ++++----
 examples/PlanarSLAMSelfContained_advanced.cpp | 25 +++----
 examples/Pose2SLAMExample_advanced.cpp        | 19 ++---
 examples/Pose2SLAMExample_easy.cpp            | 14 ++--
 examples/SimpleRotation.cpp                   | 35 ++-------
 examples/easyPoint2KalmanFilter.cpp           | 21 +++---
 examples/elaboratePoint2KalmanFilter.cpp      | 36 +++++----
 gtsam/inference/graph-inl.h                   |  2 +-
 gtsam/nonlinear/ExtendedKalmanFilter-inl.h    | 26 +++----
 gtsam/nonlinear/ExtendedKalmanFilter.h        | 12 +--
 gtsam/nonlinear/NonlinearEquality.h           |  2 +-
 gtsam/slam/BoundingConstraint.h               | 16 ++--
 gtsam/slam/GeneralSFMFactor.h                 |  2 +-
 gtsam/slam/planarSLAM.cpp                     |  2 +-
 gtsam/slam/planarSLAM.h                       |  2 +-
 gtsam/slam/simulated2DConstraints.h           |  2 +-
 gtsam/slam/tests/testGeneralSFMFactor.cpp     | 29 ++++----
 .../testGeneralSFMFactor_Cal3Bundler.cpp      | 20 ++---
 gtsam/slam/tests/testPose3SLAM.cpp            | 28 +++----
 gtsam/slam/visualSLAM.h                       | 24 +++---
 tests/testBoundingConstraint.cpp              | 15 ++--
 tests/testExtendedKalmanFilter.cpp            | 73 +++++++++----------
 tests/testGaussianISAM2.cpp                   | 20 ++---
 tests/testGaussianJunctionTree.cpp            |  6 +-
 tests/testGraph.cpp                           | 12 +--
 tests/testInference.cpp                       | 12 +--
 tests/testNonlinearEquality.cpp               | 49 ++++++-------
 tests/testNonlinearFactor.cpp                 | 15 ++--
 tests/testNonlinearISAM.cpp                   | 17 ++---
 tests/testRot3Optimization.cpp                | 13 ++--
 tests/testSerialization.cpp                   | 14 ++--
 32 files changed, 283 insertions(+), 322 deletions(-)

diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 511e7eb3f..957931af4 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -26,28 +26,25 @@
 
 using namespace gtsam;
 
-typedef TypedSymbol<Pose3, 'x'> PoseKey;
-
 /**
  * Unary factor for the pose.
  */
-class ResectioningFactor: public NonlinearFactor1<PoseKey> {
-  typedef NonlinearFactor1<PoseKey> Base;
+class ResectioningFactor: public NonlinearFactor1<Pose3> {
+  typedef NonlinearFactor1<Pose3> Base;
 
   shared_ptrK K_; // camera's intrinsic parameters
   Point3 P_; // 3D point on the calibration rig
   Point2 p_; // 2D measurement of the 3D point
 
 public:
-  ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key,
+  ResectioningFactor(const SharedNoiseModel& model, const Symbol& key,
       const shared_ptrK& calib, const Point2& p, const Point3& P) :
     Base(model, key), K_(calib), P_(P), p_(p) {
   }
 
   virtual ~ResectioningFactor() {}
 
-  virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
-      boost::none) const {
+  virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = boost::none) const {
     SimpleCamera camera(*K_, X);
     Point2 reprojectionError(camera.project(P_, H) - p_);
     return reprojectionError.vector();
@@ -69,7 +66,7 @@ int main(int argc, char* argv[]) {
 
   /* create keys for variables */
   // we have only 1 variable to solve: the camera pose
-  PoseKey X(1);
+  Symbol X('x',1);
 
   /* 1. create graph */
   NonlinearFactorGraph graph;
diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp
index d3aecb4b6..1ff3a8677 100644
--- a/examples/PlanarSLAMExample_easy.cpp
+++ b/examples/PlanarSLAMExample_easy.cpp
@@ -36,25 +36,22 @@ using namespace planarSLAM;
  *  - Landmarks are 2 meters away from the robot trajectory
  */
 int main(int argc, char** argv) {
-	// create keys for variables
-	PoseKey x1(1), x2(2), x3(3);
-	PointKey l1(1), l2(2);
 
-	// create graph container and add factors to it
+  // create graph container and add factors to it
 	Graph graph;
 
 	/* add prior  */
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
+	graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
 
 	/* add odometry */
 	// general noisemodel for odometry
 	SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-	graph.addOdometry(x1, x2, odom_measurement, odom_model);
-	graph.addOdometry(x2, x3, odom_measurement, odom_model);
+	graph.addOdometry(1, 2, odom_measurement, odom_model);
+	graph.addOdometry(2, 3, odom_measurement, odom_model);
 
 	/* add measurements */
 	// general noisemodel for measurements
@@ -69,24 +66,24 @@ int main(int argc, char** argv) {
 		   range32 = 2.0;
 
 	// create bearing/range factors and add them
-	graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
-	graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
-	graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
+	graph.addBearingRange(1, 1, bearing11, range11, meas_model);
+	graph.addBearingRange(2, 1, bearing21, range21, meas_model);
+	graph.addBearingRange(3, 2, bearing32, range32, meas_model);
 
 	graph.print("full graph");
 
 	// initialize to noisy points
 	planarSLAM::Values initialEstimate;
-	initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
-	initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
-	initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
-	initialEstimate.insert(l1, Point2(1.8, 2.1));
-	initialEstimate.insert(l2, Point2(4.1, 1.8));
+	initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
+	initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+	initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
+	initialEstimate.insertPoint(1, Point2(1.8, 2.1));
+	initialEstimate.insertPoint(2, Point2(4.1, 1.8));
 
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
+	planarSLAM::Values result = optimize(graph, initialEstimate);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp
index fd6d2cbfe..9dc908bc0 100644
--- a/examples/PlanarSLAMSelfContained_advanced.cpp
+++ b/examples/PlanarSLAMSelfContained_advanced.cpp
@@ -40,11 +40,8 @@
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
 
 // Main typedefs
-typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey;       // Key for poses, with type included
-typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey;      // Key for points, with type included
-typedef gtsam::NonlinearFactorGraph Graph;			 // graph structure
-typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
-typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
+typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
+typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
 
 using namespace std;
 using namespace gtsam;
@@ -60,17 +57,17 @@ using namespace gtsam;
  */
 int main(int argc, char** argv) {
 	// create keys for variables
-	PoseKey x1(1), x2(2), x3(3);
-	PointKey l1(1), l2(2);
+	Symbol x1('x',1), x2('x',2), x3('x',3);
+	Symbol l1('l',1), l2('l',2);
 
 	// create graph container and add factors to it
-	Graph::shared_ptr graph(new Graph);
+	NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
 
 	/* add prior  */
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	PriorFactor<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
+	PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
 	graph->add(posePrior);  // add the factor to the graph
 
 	/* add odometry */
@@ -78,8 +75,8 @@ int main(int argc, char** argv) {
 	SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
 	// create between factors to represent odometry
-	BetweenFactor<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
-	BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
+	BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
+	BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
 	graph->add(odom12); // add both to graph
 	graph->add(odom23);
 
@@ -96,9 +93,9 @@ int main(int argc, char** argv) {
 		   range32 = 2.0;
 
 	// create bearing/range factors
-	BearingRangeFactor<PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
-	BearingRangeFactor<PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
-	BearingRangeFactor<PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
+	BearingRangeFactor<Pose2, Point2> meas11(x1, l1, bearing11, range11, meas_model);
+	BearingRangeFactor<Pose2, Point2> meas21(x2, l1, bearing21, range21, meas_model);
+	BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
 
 	// add the factors
 	graph->add(meas11);
diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp
index cab263047..1c196488f 100644
--- a/examples/Pose2SLAMExample_advanced.cpp
+++ b/examples/Pose2SLAMExample_advanced.cpp
@@ -33,9 +33,6 @@ using namespace boost;
 using namespace pose2SLAM;
 
 int main(int argc, char** argv) {
-	// create keys for robot positions
-	PoseKey x1(1), x2(2), x3(3);
-
 	/* 1. create graph container and add factors to it */
 	shared_ptr<Graph> graph(new Graph);
 
@@ -43,7 +40,7 @@ int main(int argc, char** argv) {
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph
+	graph->addPrior(1, prior_measurement, prior_model); // add directly to graph
 
 	/* 2.b add odometry */
 	// general noisemodel for odometry
@@ -51,16 +48,16 @@ int main(int argc, char** argv) {
 
 	/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-	graph->addOdometry(x1, x2, odom_measurement, odom_model);
-	graph->addOdometry(x2, x3, odom_measurement, odom_model);
+	graph->addOdometry(1, 2, odom_measurement, odom_model);
+	graph->addOdometry(2, 3, odom_measurement, odom_model);
 	graph->print("full graph");
 
   /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
 	shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
-	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
-	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
-	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
+	initial->insertPose(1, Pose2(0.5, 0.0, 0.2));
+	initial->insertPose(2, Pose2(2.3, 0.1,-0.2));
+	initial->insertPose(3, Pose2(4.1, 0.1, 0.1));
 	initial->print("initial estimate");
 
 	/* 4.2.1 Alternatively, you can go through the process step by step
@@ -76,8 +73,8 @@ int main(int argc, char** argv) {
 	result.print("final result");
 
 	/* Get covariances */
-	Matrix covariance1  = optimizer_result.marginalCovariance(x1);
-	Matrix covariance2  = optimizer_result.marginalCovariance(x2);
+	Matrix covariance1  = optimizer_result.marginalCovariance(PoseKey(1));
+	Matrix covariance2  = optimizer_result.marginalCovariance(PoseKey(1));
 
 	print(covariance1, "Covariance1");
 	print(covariance2, "Covariance2");
diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp
index 43a648398..2593c043c 100644
--- a/examples/Pose2SLAMExample_easy.cpp
+++ b/examples/Pose2SLAMExample_easy.cpp
@@ -31,8 +31,6 @@ using namespace gtsam;
 using namespace pose2SLAM;
 
 int main(int argc, char** argv) {
-	// create keys for robot positions
-	PoseKey x1(1), x2(2), x3(3);
 
 	/* 1. create graph container and add factors to it */
 	Graph graph ;
@@ -41,7 +39,7 @@ int main(int argc, char** argv) {
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
+	graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
 
 	/* 2.b add odometry */
 	// general noisemodel for odometry
@@ -49,16 +47,16 @@ int main(int argc, char** argv) {
 
 	/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-	graph.addOdometry(x1, x2, odom_measurement, odom_model);
-	graph.addOdometry(x2, x3, odom_measurement, odom_model);
+	graph.addOdometry(1, 2, odom_measurement, odom_model);
+	graph.addOdometry(2, 3, odom_measurement, odom_model);
 	graph.print("full graph");
 
     /* 3. Create the data structure to hold the initial estinmate to the solution
      * initialize to noisy points */
 	pose2SLAM::Values initial;
-	initial.insert(x1, Pose2(0.5, 0.0, 0.2));
-	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
-	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
+	initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
+	initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
+	initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
 	initial.print("initial estimate");
 
 	/* 4 Single Step Optimization
diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp
index 821b177dc..29bab981a 100644
--- a/examples/SimpleRotation.cpp
+++ b/examples/SimpleRotation.cpp
@@ -35,25 +35,6 @@
 using namespace std;
 using namespace gtsam;
 
-/**
- *   Step 1: Setup basic types for optimization of a single variable type
- * This can be considered to be specifying the domain of the problem we wish
- * to solve.  In this case, we will create a very simple domain that operates
- * on variables of a specific type, in this case, Rot2.
- *
- * To create a domain:
- *  - variable types need to have a key defined to act as a label in graphs
- *  - a "RotValues" structure needs to be defined to store the system state
- *  - a graph container acting on a given RotValues
- *
- * In a typical scenario, these typedefs could be placed in a header
- * file and reused between projects.  Also, RotValues can be combined to
- * form a "TupleValues" to enable multiple variable types, such as 2D points
- * and 2D poses.
- */
-typedef TypedSymbol<Rot2, 'x'> Key; 								/// Variable labels for a specific type
-typedef NonlinearFactorGraph Graph;					/// Graph container for constraints - needs to know type of variables
-
 const double degree = M_PI / 180;
 
 int main() {
@@ -64,7 +45,7 @@ int main() {
 	 */
 
 	/**
-	 *    Step 2: create a factor on to express a unary constraint
+	 *    Step 1: create a factor on to express a unary constraint
 	 * The "prior" in this case is the measurement from a sensor,
 	 * with a model of the noise on the measurement.
 	 *
@@ -80,11 +61,11 @@ int main() {
 	Rot2 prior = Rot2::fromAngle(30 * degree);
 	prior.print("goal angle");
 	SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
-	Key key(1);
-	PriorFactor<Key> factor(key, prior, model);
+	Symbol key('x',1);
+	PriorFactor<Rot2> factor(key, prior, model);
 
 	/**
-	 *    Step 3: create a graph container and add the factor to it
+	 *    Step 2: create a graph container and add the factor to it
 	 * Before optimizing, all factors need to be added to a Graph container,
 	 * which provides the necessary top-level functionality for defining a
 	 * system of constraints.
@@ -92,12 +73,12 @@ int main() {
 	 * In this case, there is only one factor, but in a practical scenario,
 	 * many more factors would be added.
 	 */
-	Graph graph;
+	NonlinearFactorGraph graph;
 	graph.add(factor);
 	graph.print("full graph");
 
 	/**
-	 *    Step 4: create an initial estimate
+	 *    Step 3: create an initial estimate
 	 * An initial estimate of the solution for the system is necessary to
 	 * start optimization.  This system state is the "RotValues" structure,
 	 * which is similar in structure to a STL map, in that it maps
@@ -117,14 +98,14 @@ int main() {
 	initial.print("initial estimate");
 
 	/**
-	 *    Step 5: optimize
+	 *    Step 4: optimize
 	 * After formulating the problem with a graph of constraints
 	 * and an initial estimate, executing optimization is as simple
 	 * as calling a general optimization function with the graph and
 	 * initial estimate.  This will yield a new RotValues structure
 	 * with the final state of the optimization.
 	 */
-	Values result = optimize<Graph>(graph, initial);
+	Values result = optimize(graph, initial);
 	result.print("final result");
 
 	return 0;
diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp
index d190c38dc..b13058a07 100644
--- a/examples/easyPoint2KalmanFilter.cpp
+++ b/examples/easyPoint2KalmanFilter.cpp
@@ -30,7 +30,6 @@ using namespace std;
 using namespace gtsam;
 
 // Define Types for Linear System Test
-typedef TypedSymbol<Point2, 'x'> LinearKey;
 typedef Point2 LinearMeasurement;
 
 int main() {
@@ -40,7 +39,7 @@ int main() {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
 
 
 
@@ -61,11 +60,11 @@ int main() {
 
   // This simple motion can be modeled with a BetweenFactor
   // Create Keys
-  LinearKey x0(0), x1(1);
+  Symbol x0('x',0), x1('x',1);
   // Predict delta based on controls
   Point2 difference(1,0);
   // Create Factor
-  BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
+  BetweenFactor<Point2> factor1(x0, x1, difference, Q);
 
   // Predict the new value with the EKF class
   Point2 x1_predict = ekf.predict(factor1);
@@ -86,7 +85,7 @@ int main() {
 
   // This simple measurement can be modeled with a PriorFactor
   Point2 z1(1.0, 0.0);
-  PriorFactor<LinearKey> factor2(x1, z1, R);
+  PriorFactor<Point2> factor2(x1, z1, R);
 
   // Update the Kalman Filter with the measurement
   Point2 x1_update = ekf.update(factor2);
@@ -96,15 +95,15 @@ int main() {
 
   // Do the same thing two more times...
   // Predict
-  LinearKey x2(2);
+  Symbol x2('x',2);
   difference = Point2(1,0);
-  BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
+  BetweenFactor<Point2> factor3(x1, x2, difference, Q);
   Point2 x2_predict = ekf.predict(factor1);
   x2_predict.print("X2 Predict");
 
   // Update
   Point2 z2(2.0, 0.0);
-  PriorFactor<LinearKey> factor4(x2, z2, R);
+  PriorFactor<Point2> factor4(x2, z2, R);
   Point2 x2_update = ekf.update(factor4);
   x2_update.print("X2 Update");
 
@@ -112,15 +111,15 @@ int main() {
 
   // Do the same thing one more time...
   // Predict
-  LinearKey x3(3);
+  Symbol x3('x',3);
   difference = Point2(1,0);
-  BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
+  BetweenFactor<Point2> factor5(x2, x3, difference, Q);
   Point2 x3_predict = ekf.predict(factor5);
   x3_predict.print("X3 Predict");
 
   // Update
   Point2 z3(3.0, 0.0);
-  PriorFactor<LinearKey> factor6(x3, z3, R);
+  PriorFactor<Point2> factor6(x3, z3, R);
   Point2 x3_update = ekf.update(factor6);
   x3_update.print("X3 Update");
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index 30d77aedc..1c3415e61 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -33,8 +33,6 @@
 using namespace std;
 using namespace gtsam;
 
-typedef TypedSymbol<Point2, 'x'> Key;               /// Variable labels for a specific type
-
 int main() {
 
 	// [code below basically does SRIF with LDL]
@@ -55,14 +53,14 @@ int main() {
 	// i.e., we should get 0,0  0,1  0,2 if there is no noise
 
   // Create new state variable
-  Key x0(0);
+  Symbol x0('x',0);
   ordering->insert(x0, 0);
 
   // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
   // This is equivalent to x_0 and P_0
   Point2 x_initial(0,0);
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  PriorFactor<Key> factor1(x0, x_initial, P_initial);
+  PriorFactor<Point2> factor1(x0, x_initial, P_initial);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x0, x_initial);
   linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
@@ -88,12 +86,12 @@ int main() {
   // so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t}
   //                                  = (F - I)*x_{t} + B*u_{t}
   //                                  = B*u_{t} (for our example)
-  Key x1(1);
+  Symbol x1('x',1);
   ordering->insert(x1, 1);
 
   Point2 difference(1,0);
   SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<Key> factor2(x0, x1, difference, Q);
+  BetweenFactor<Point2> factor2(x0, x1, difference, Q);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x1, x_initial);
   linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering));
@@ -116,7 +114,7 @@ int main() {
 
   // Extract the current estimate of x1,P1 from the Bayes Network
   VectorValues result = optimize(*linearBayesNet);
-  Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]);
+  Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
   x1_predict.print("X1 Predict");
 
   // Update the new linearization point to the new estimate
@@ -171,7 +169,7 @@ int main() {
   // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
   Point2 z1(1.0, 0.0);
   SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<Key> factor4(x1, z1, R1);
+  PriorFactor<Point2> factor4(x1, z1, R1);
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
 
@@ -188,7 +186,7 @@ int main() {
 
   // Extract the current estimate of x1 from the Bayes Network
   result = optimize(*linearBayesNet);
-  Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]);
+  Point2 x1_update = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
   x1_update.print("X1 Update");
 
   // Update the linearization point to the new estimate
@@ -213,7 +211,7 @@ int main() {
   linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
 
   // Create a key for the new state
-  Key x2(2);
+  Symbol x2('x',2);
 
   // Create the desired ordering
   ordering = Ordering::shared_ptr(new Ordering);
@@ -223,7 +221,7 @@ int main() {
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
   Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<Key> factor6(x1, x2, difference, Q);
+  BetweenFactor<Point2> factor6(x1, x2, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x2, x1_update);
@@ -235,7 +233,7 @@ int main() {
 
   // Extract the current estimate of x2 from the Bayes Network
   result = optimize(*linearBayesNet);
-  Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]);
+  Point2 x2_predict = linearizationPoints.at<Point2>(x2).retract(result[ordering->at(x2)]);
   x2_predict.print("X2 Predict");
 
   // Update the linearization point to the new estimate
@@ -261,7 +259,7 @@ int main() {
   // And update using z2 ...
   Point2 z2(2.0, 0.0);
   SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<Key> factor8(x2, z2, R2);
+  PriorFactor<Point2> factor8(x2, z2, R2);
 
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@@ -279,7 +277,7 @@ int main() {
 
   // Extract the current estimate of x2 from the Bayes Network
   result = optimize(*linearBayesNet);
-  Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]);
+  Point2 x2_update = linearizationPoints.at<Point2>(x2).retract(result[ordering->at(x2)]);
   x2_update.print("X2 Update");
 
   // Update the linearization point to the new estimate
@@ -302,7 +300,7 @@ int main() {
   linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model());
 
   // Create a key for the new state
-  Key x3(3);
+  Symbol x3('x',3);
 
   // Create the desired ordering
   ordering = Ordering::shared_ptr(new Ordering);
@@ -312,7 +310,7 @@ int main() {
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
   Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<Key> factor10(x2, x3, difference, Q);
+  BetweenFactor<Point2> factor10(x2, x3, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x3, x2_update);
@@ -324,7 +322,7 @@ int main() {
 
   // Extract the current estimate of x3 from the Bayes Network
   result = optimize(*linearBayesNet);
-  Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]);
+  Point2 x3_predict = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
   x3_predict.print("X3 Predict");
 
   // Update the linearization point to the new estimate
@@ -350,7 +348,7 @@ int main() {
   // And update using z3 ...
   Point2 z3(3.0, 0.0);
   SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<Key> factor12(x3, z3, R3);
+  PriorFactor<Point2> factor12(x3, z3, R3);
 
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));
@@ -368,7 +366,7 @@ int main() {
 
   // Extract the current estimate of x2 from the Bayes Network
   result = optimize(*linearBayesNet);
-  Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]);
+  Point2 x3_update = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
   x3_update.print("X3 Update");
 
   // Update the linearization point to the new estimate
diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h
index be4ddf840..d8395942a 100644
--- a/gtsam/inference/graph-inl.h
+++ b/gtsam/inference/graph-inl.h
@@ -153,7 +153,7 @@ public:
 		KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
 		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, (*config_)[key_from].compose(relativePose));
+		config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
 	}
 
 };
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 2c4e382a1..7cd67f6d2 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -27,10 +27,10 @@
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class KEY>
-	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
+	template<class VALUE>
+	typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
 			const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
-			const Values& linearizationPoints, const KEY& lastKey,
+			const Values& linearizationPoints, const Symbol& lastKey,
 			JacobianFactor::shared_ptr& newPrior) const {
 
 		// Extract the Index of the provided last key
@@ -43,7 +43,7 @@ namespace gtsam {
 
 		// Extract the current estimate of x1,P1 from the Bayes Network
 		VectorValues result = optimize(*linearBayesNet);
-		T x = linearizationPoints[lastKey].retract(result[lastIndex]);
+		T x = linearizationPoints.at<T>(lastKey).retract(result[lastIndex]);
 
 		// Create a Jacobian Factor from the root node of the produced Bayes Net.
 		// This will act as a prior for the next iteration.
@@ -66,8 +66,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class KEY>
-	ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
+	template<class VALUE>
+	ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(T x_initial,
 			noiseModel::Gaussian::shared_ptr P_initial) {
 
 		// Set the initial linearization point to the provided mean
@@ -82,8 +82,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class KEY>
-	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
+	template<class VALUE>
+	typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
 			const MotionFactor& motionFactor) {
 
 		// TODO: This implementation largely ignores the actual factor symbols.
@@ -91,8 +91,8 @@ namespace gtsam {
 		// different keys will still compute as if a common key-set was used
 
 		// Create Keys
-		KEY x0 = motionFactor.key1();
-		KEY x1 = motionFactor.key2();
+		Symbol x0 = motionFactor.key1();
+		Symbol x1 = motionFactor.key2();
 
 		// Create an elimination ordering
 		Ordering ordering;
@@ -122,8 +122,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class KEY>
-	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
+	template<class VALUE>
+	typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
 			const MeasurementFactor& measurementFactor) {
 
 		// TODO: This implementation largely ignores the actual factor symbols.
@@ -131,7 +131,7 @@ namespace gtsam {
 		// different keys will still compute as if a common key-set was used
 
 		// Create Keys
-		KEY x0 = measurementFactor.key();
+		Symbol x0 = measurementFactor.key();
 
 		// Create an elimination ordering
 		Ordering ordering;
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h
index 6e85d6e27..bdfaa7651 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter.h
@@ -40,14 +40,14 @@ namespace gtsam {
 	 * \nosubgrouping
 	 */
 
-	template<class KEY>
+	template<class VALUE>
 	class ExtendedKalmanFilter {
 	public:
 
-		typedef boost::shared_ptr<ExtendedKalmanFilter<KEY> > shared_ptr;
-		typedef typename KEY::Value T;
-		typedef NonlinearFactor2<KEY, KEY> MotionFactor;
-		typedef NonlinearFactor1<KEY> MeasurementFactor;
+		typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
+		typedef VALUE T;
+		typedef NonlinearFactor2<VALUE, VALUE> MotionFactor;
+		typedef NonlinearFactor1<VALUE> MeasurementFactor;
 
 	protected:
 		T x_; // linearization point
@@ -55,7 +55,7 @@ namespace gtsam {
 
 		T solve_(const GaussianFactorGraph& linearFactorGraph,
 				const Ordering& ordering, const Values& linearizationPoints,
-				const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
+				const Symbol& x, JacobianFactor::shared_ptr& newPrior) const;
 
 	public:
 
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index a95044dee..1a1fce839 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -219,7 +219,7 @@ namespace gtsam {
 		/** Print */
 	  virtual void print(const std::string& s = "") const {
 	    std::cout << s << ": NonlinearEquality1("
-	    		<< (std::string) this->key_ << "),"<< "\n";
+	    		<< (std::string) this->key() << "),"<< "\n";
 	    this->noiseModel_->print();
 	    value_.print("Value");
 	  }
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index 3c28afa07..42f42e429 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -95,18 +95,18 @@ private:
  * Binary scalar inequality constraint, with a similar value() function
  * to implement for specific systems
  */
-template<class KEY1, class KEY2>
-struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
-	typedef typename KEY1::Value X1;
-	typedef typename KEY2::Value X2;
+template<class VALUE1, class VALUE2>
+struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
+	typedef VALUE1 X1;
+	typedef VALUE2 X2;
 
-	typedef NonlinearFactor2<KEY1, KEY2> Base;
-	typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
+	typedef NonlinearFactor2<VALUE1, VALUE2> Base;
+	typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
 
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
 
-	BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold,
+	BoundingConstraint2(const Symbol& key1, const Symbol& key2, double threshold,
 			bool isGreaterThan, double mu = 1000.0)
 	: Base(noiseModel::Constrained::All(1, mu), key1, key2),
 	  threshold_(threshold), isGreaterThan_(isGreaterThan) {}
@@ -127,7 +127,7 @@ struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
 	/** active when constraint *NOT* met */
 	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
-		double x = value(c[this->key1_], c[this->key2_]);
+		double x = value(c[this->key1()], c[this->key2()]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
 	}
 
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 60092c15d..868b064d7 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -53,7 +53,7 @@ namespace gtsam {
 		 * @param j is the index of the landmark
 		 */
 		GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
-		  Base(model, cameraKey, cameraKey), measured_(measured) {}
+		  Base(model, cameraKey, landmarkKey), measured_(measured) {}
 
 		GeneralSFMFactor():measured_(0.0,0.0) {} 							///< default constructor
 		GeneralSFMFactor(const Point2 & p):measured_(p) {}			///< constructor that takes a Point2
diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp
index f7e562640..db3d6b146 100644
--- a/gtsam/slam/planarSLAM.cpp
+++ b/gtsam/slam/planarSLAM.cpp
@@ -37,7 +37,7 @@ namespace planarSLAM {
   }
 
   void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
-    sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model));
+    sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model));
     push_back(factor);
   }
 
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 5c9c62d46..33dc97a8c 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -97,7 +97,7 @@ namespace planarSLAM {
     void addPoseConstraint(Index poseKey, const Pose2& pose);
 
     /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
-    void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model);
+    void addOdometry(Index poseKey1, Index poseKey2, const Pose2& odometry, const SharedNoiseModel& model);
 
     /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
     void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model);
diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h
index 99475ce91..5a6b39861 100644
--- a/gtsam/slam/simulated2DConstraints.h
+++ b/gtsam/slam/simulated2DConstraints.h
@@ -147,7 +147,7 @@ namespace simulated2D {
       }
     };
 
-    typedef MaxDistanceConstraint<PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
+    typedef MaxDistanceConstraint<Point2> PoseMaxDistConstraint; ///< Simulated2D domain example factor
 
     /**
      * Binary inequality constraint forcing a minimum range
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 022a76960..8c59d63ac 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -44,7 +44,7 @@ public:
 	}
 
   void addPoint3Constraint(int j, const Point3& p) {
-    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('x',j), p));
+    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l',j), p));
     push_back(factor);
   }
 
@@ -89,8 +89,7 @@ TEST( GeneralSFMFactor, equals )
 TEST( GeneralSFMFactor, error ) {
 	Point2 z(3.,0.);
 	const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
-	boost::shared_ptr<Projection>
-	factor(new Projection(z, sigma, "x1", "l1"));
+	boost::shared_ptr<Projection>	factor(new Projection(z, sigma, "x1", "l1"));
 	// For the following configuration, the factor predicts 320,240
 	Values values;
 	Rot3 R;
@@ -167,13 +166,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 	const double noise = baseline*0.1;
 	boost::shared_ptr<Values> values(new Values);
 	for ( size_t i = 0 ; i < X.size() ; ++i )
-	  values->insert(CameraKey((int)i), X[i]) ;
+	  values->insert(Symbol('x',i), X[i]) ;
 
 	for ( size_t i = 0 ; i < L.size() ; ++i ) {
 		Point3 pt(L[i].x()+noise*getGaussian(),
 		          L[i].y()+noise*getGaussian(),
 		          L[i].z()+noise*getGaussian());
-		values->insert(PointKey(i), pt) ;
+		values->insert(Symbol('l',i), pt) ;
 	}
 
 	graph->addCameraConstraint(0, X[0]);
@@ -206,7 +205,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
   const double noise = baseline*0.1;
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
@@ -214,10 +213,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
       Point3 pt(L[i].x()+noise*getGaussian(),
                 L[i].y()+noise*getGaussian(),
                 L[i].z()+noise*getGaussian());
-      values->insert(PointKey(i), pt) ;
+      values->insert(Symbol('l',i), pt) ;
     }
     else {
-      values->insert(PointKey(i), L[i]) ;
+      values->insert(Symbol('l',i), L[i]) ;
     }
   }
 
@@ -252,14 +251,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
     //Point3 pt(L[i].x(), L[i].y(), L[i].z());
-    values->insert(PointKey(i), pt) ;
+    values->insert(Symbol('l',i), pt) ;
   }
 
   for ( size_t i = 0 ; i < X.size() ; ++i )
@@ -301,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
       focal_noise = 1,
       skew_noise = 1e-5;
     if ( i == 0 ) {
-      values->insert(CameraKey((int)i), X[i]) ;
+      values->insert(Symbol('x',i), X[i]) ;
     }
     else {
 
@@ -312,12 +311,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
           skew_noise, // s
           trans_noise, trans_noise // ux, uy
           ) ;
-      values->insert(CameraKey((int)i), X[i].retract(delta)) ;
+      values->insert(Symbol('x',i), X[i].retract(delta)) ;
     }
   }
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
-    values->insert(PointKey(i), L[i]) ;
+    values->insert(Symbol('l',i), L[i]) ;
   }
 
   // fix X0 and all landmarks, allow only the X[1] to move
@@ -357,14 +356,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
   const double noise = baseline*0.1;
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(PointKey(i), pt) ;
+    values->insert(Symbol('l',i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 7c1d395ba..6b1ace287 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -207,7 +207,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
   const double noise = baseline*0.1;
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
@@ -215,10 +215,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
       Point3 pt(L[i].x()+noise*getGaussian(),
                 L[i].y()+noise*getGaussian(),
                 L[i].z()+noise*getGaussian());
-      values->insert(PointKey(i), pt) ;
+      values->insert(Symbol('l',i), pt) ;
     }
     else {
-      values->insert(PointKey(i), L[i]) ;
+      values->insert(Symbol('l',i), L[i]) ;
     }
   }
 
@@ -253,14 +253,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
     //Point3 pt(L[i].x(), L[i].y(), L[i].z());
-    values->insert(PointKey(i), pt) ;
+    values->insert(Symbol('l',i), pt) ;
   }
 
   for ( size_t i = 0 ; i < X.size() ; ++i )
@@ -300,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
       rot_noise = 1e-5, trans_noise = 1e-3,
       focal_noise = 1, distort_noise = 1e-3;
     if ( i == 0 ) {
-      values->insert(CameraKey((int)i), X[i]) ;
+      values->insert(Symbol('x',i), X[i]) ;
     }
     else {
 
@@ -309,12 +309,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
           trans_noise, trans_noise, trans_noise, // translation
           focal_noise, distort_noise, distort_noise // f, k1, k2
           ) ;
-      values->insert(CameraKey((int)i), X[i].retract(delta)) ;
+      values->insert(Symbol('x',i), X[i].retract(delta)) ;
     }
   }
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
-    values->insert(PointKey(i), L[i]) ;
+    values->insert(Symbol('l',i), L[i]) ;
   }
 
   // fix X0 and all landmarks, allow only the X[1] to move
@@ -353,14 +353,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
   const double noise = baseline*0.1;
   boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert(CameraKey((int)i), X[i]) ;
+    values->insert(Symbol('x',i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(PointKey(i), pt) ;
+    values->insert(Symbol('l',i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index a9b743145..21c0fd458 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -131,12 +131,12 @@ TEST( Pose3Factor, error )
 
 	// Create factor
 	SharedNoiseModel I6(noiseModel::Unit::Create(6));
-	Pose3Factor factor(1,2, z, I6);
+	Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6);
 
 	// Create config
 	Values x;
-	x.insert(Key(1),t1);
-	x.insert(Key(2),t2);
+	x.insert(PoseKey(1),t1);
+	x.insert(PoseKey(2),t2);
 
 	// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
 	Vector actual = factor.unwhitenedError(x);
@@ -146,10 +146,10 @@ TEST( Pose3Factor, error )
 
 /* ************************************************************************* */
 TEST(Pose3Graph, partial_prior_xy) {
-	typedef PartialPriorFactor<pose3SLAM::Key> Partial;
+	typedef PartialPriorFactor<Pose3> Partial;
 
 	// XY prior - full mask interface
-	pose3SLAM::Key key(1);
+	Symbol key = PoseKey(1);
 	Vector exp_xy = Vector_(2, 3.0, 4.0);
 	SharedDiagonal model = noiseModel::Unit::Create(2);
 	Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
@@ -169,7 +169,7 @@ TEST(Pose3Graph, partial_prior_xy) {
 	values.insert(key, init);
 
 	Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
-	EXPECT(assert_equal(expected, actual[key], tol));
+	EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
 
@@ -185,10 +185,10 @@ TEST( Values, pose3Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
 	Values expected;
-	expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0)));
-	expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0)));
-	expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0)));
-	expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0)));
+	expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0)));
+	expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0)));
+	expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0)));
+	expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0)));
 
 	Values actual = pose3SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
@@ -198,10 +198,10 @@ TEST( Values, pose3Circle )
 TEST( Values, expmap )
 {
 	Values expected;
-	expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
-	expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
-	expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
-	expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
+	expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
+	expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
+	expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
+	expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
 
 	Ordering ordering(*expected.orderingArbitrary());
 	VectorValues delta(expected.dims(ordering));
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index 4ecbf2e43..df137ad47 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -85,8 +85,8 @@ namespace visualSLAM {
      *  @param K shared pointer to calibration object
      */
     void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
-        const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) {
-      boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
+        Index poseKey, Index pointKey, const shared_ptrK& K) {
+      boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
       push_back(factor);
     }
 
@@ -95,8 +95,8 @@ namespace visualSLAM {
      *  @param key variable key of the camera pose
      *  @param p to which pose to constrain it to
      */
-    void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) {
-      boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(key, p));
+    void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) {
+      boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
       push_back(factor);
     }
 
@@ -105,8 +105,8 @@ namespace visualSLAM {
      *  @param key variable key of the landmark
      *  @param p point around which soft prior is defined
      */
-    void addPointConstraint(const Symbol& key, const Point3& p = Point3()) {
-      boost::shared_ptr<PointConstraint> factor(new PointConstraint(key, p));
+    void addPointConstraint(Index pointKey, const Point3& p = Point3()) {
+      boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
       push_back(factor);
     }
 
@@ -116,8 +116,8 @@ namespace visualSLAM {
      *  @param p around which soft prior is defined
      *  @param model uncertainty model of this prior
      */
-    void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
-      boost::shared_ptr<PosePrior> factor(new PosePrior(key, p, model));
+    void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
+      boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
       push_back(factor);
     }
 
@@ -127,8 +127,8 @@ namespace visualSLAM {
      *  @param p to which point to constrain it to
      *  @param model uncertainty model of this prior
      */
-    void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
-      boost::shared_ptr<PointPrior> factor(new PointPrior(key, p, model));
+    void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
+      boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model));
       push_back(factor);
     }
 
@@ -139,8 +139,8 @@ namespace visualSLAM {
      *  @param range approximate range to landmark
      *  @param model uncertainty model of this prior
      */
-    void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
-      push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(poseKey, pointKey, range, model)));
+    void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
+      push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model)));
     }
 
   }; // Graph
diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp
index c15070b4c..4fddf1ba0 100644
--- a/tests/testBoundingConstraint.cpp
+++ b/tests/testBoundingConstraint.cpp
@@ -17,6 +17,8 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#define GTSAM_MAGIC_KEY
+
 #include <gtsam/slam/simulated2DConstraints.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
@@ -37,7 +39,7 @@ typedef boost::shared_ptr<Values> shared_values;
 typedef NonlinearOptimizer<Graph> Optimizer;
 
 // some simple inequality constraints
-simulated2D::PoseKey key(1);
+Symbol key(simulated2D::PoseKey(1));
 double mu = 10.0;
 // greater than
 iq2D::PoseXInequality constraint1(key, 1.0, true, mu);
@@ -151,7 +153,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
 	Point2 start_pt(0.0, 1.0);
 
 	shared_graph graph(new Graph());
-	simulated2D::PoseKey x1(1);
+	Symbol x1("x1");
 	graph->add(iq2D::PoseXInequality(x1, 1.0, true));
 	graph->add(iq2D::PoseYInequality(x1, 2.0, true));
 	graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
@@ -173,7 +175,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 	Point2 start_pt(2.0, 3.0);
 
 	shared_graph graph(new Graph());
-	simulated2D::PoseKey x1(1);
+	Symbol x1("x1");
 	graph->add(iq2D::PoseXInequality(x1, 1.0, false));
 	graph->add(iq2D::PoseYInequality(x1, 2.0, false));
 	graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
@@ -189,7 +191,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 
 /* ************************************************************************* */
 TEST( testBoundingConstraint, MaxDistance_basics) {
-	simulated2D::PoseKey key1(1), key2(2);
+	Symbol key1("x1"), key2("x2");
 	Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
 	iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
 	EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
@@ -231,7 +233,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
 TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
 
 	Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
-	simulated2D::PoseKey x1(1), x2(2);
+  Symbol x1("x1"), x2("x2");
 
 	Graph graph;
 	graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
@@ -254,8 +256,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, avoid_demo) {
 
-	simulated2D::PoseKey x1(1), x2(2), x3(3);
-	simulated2D::PointKey l1(1);
+  Symbol x1("x1"), x2("x2"), x3("x3"), l1("l1");
 	double radius = 1.0;
 	Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
 	Point2 odo(2.0, 0.0);
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 5a87950d0..532305f24 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -16,6 +16,8 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#define GTSAM_MAGIC_KEY
+
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
@@ -24,9 +26,6 @@
 
 using namespace gtsam;
 
-// Define Types for System Test
-typedef TypedSymbol<Point2, 'x'> TestKey;
-
 /* ************************************************************************* */
 TEST( ExtendedKalmanFilter, linear ) {
 
@@ -35,10 +34,10 @@ TEST( ExtendedKalmanFilter, linear ) {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
 
   // Create the TestKeys for our example
-  TestKey x0(0), x1(1), x2(2), x3(3);
+  Symbol x0("x0"), x1("x1"), x2("x2"), x3("x3");
 
   // Create the controls and measurement properties for our example
   double dt = 1.0;
@@ -57,30 +56,30 @@ TEST( ExtendedKalmanFilter, linear ) {
 
   // Run iteration 1
   // Create motion factor
-  BetweenFactor<TestKey> factor1(x0, x1, difference, Q);
+  BetweenFactor<Point2> factor1(x0, x1, difference, Q);
   Point2 predict1 = ekf.predict(factor1);
   EXPECT(assert_equal(expected1,predict1));
 
   // Create the measurement factor
-  PriorFactor<TestKey> factor2(x1, z1, R);
+  PriorFactor<Point2> factor2(x1, z1, R);
   Point2 update1 = ekf.update(factor2);
   EXPECT(assert_equal(expected1,update1));
 
   // Run iteration 2
-  BetweenFactor<TestKey> factor3(x1, x2, difference, Q);
+  BetweenFactor<Point2> factor3(x1, x2, difference, Q);
   Point2 predict2 = ekf.predict(factor3);
   EXPECT(assert_equal(expected2,predict2));
 
-  PriorFactor<TestKey> factor4(x2, z2, R);
+  PriorFactor<Point2> factor4(x2, z2, R);
   Point2 update2 = ekf.update(factor4);
   EXPECT(assert_equal(expected2,update2));
 
   // Run iteration 3
-  BetweenFactor<TestKey> factor5(x2, x3, difference, Q);
+  BetweenFactor<Point2> factor5(x2, x3, difference, Q);
   Point2 predict3 = ekf.predict(factor5);
   EXPECT(assert_equal(expected3,predict3));
 
-  PriorFactor<TestKey> factor6(x3, z3, R);
+  PriorFactor<Point2> factor6(x3, z3, R);
   Point2 update3 = ekf.update(factor6);
   EXPECT(assert_equal(expected3,update3));
 
@@ -89,12 +88,12 @@ TEST( ExtendedKalmanFilter, linear ) {
 
 
 // Create Motion Model Factor
-class NonlinearMotionModel : public NonlinearFactor2<TestKey,TestKey> {
+class NonlinearMotionModel : public NonlinearFactor2<Point2,Point2> {
 public:
-  typedef TestKey::Value T;
+  typedef Point2 T;
 
 private:
-  typedef NonlinearFactor2<TestKey,TestKey> Base;
+  typedef NonlinearFactor2<Point2, Point2> Base;
   typedef NonlinearMotionModel This;
 
 protected:
@@ -104,7 +103,7 @@ protected:
 public:
   NonlinearMotionModel(){}
 
-  NonlinearMotionModel(const TestKey& TestKey1, const TestKey& TestKey2) :
+  NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) :
     Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) {
 
     // Initialize motion model parameters:
@@ -156,14 +155,14 @@ public:
   /* print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearMotionModel\n";
-    std::cout << "  TestKey1: " << (std::string) key1_ << std::endl;
-    std::cout << "  TestKey2: " << (std::string) key2_ << std::endl;
+    std::cout << "  TestKey1: " << (std::string) key1() << std::endl;
+    std::cout << "  TestKey2: " << (std::string) key2() << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
-  virtual bool equals(const NonlinearFactor2<TestKey,TestKey>& f, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
-    return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_);
+    return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2());
   }
 
   /**
@@ -182,7 +181,7 @@ public:
 
   /** Vector of errors, whitened ! */
   Vector whitenedError(const Values& c) const {
-    return QInvSqrt(c[key1_])*unwhitenedError(c);
+    return QInvSqrt(c[key1()])*unwhitenedError(c);
   }
 
   /**
@@ -191,11 +190,11 @@ public:
    * Hence b = z - h(x1,x2) = - error_vector(x)
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
-    const X1& x1 = c[key1_];
-    const X2& x2 = c[key2_];
+    const X1& x1 = c[key1()];
+    const X2& x2 = c[key2()];
     Matrix A1, A2;
     Vector b = -evaluateError(x1, x2, A1, A2);
-    const Index var1 = ordering[key1_], var2 = ordering[key2_];
+    const Index var1 = ordering[key1()], var2 = ordering[key2()];
     SharedDiagonal constrained =
         boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
     if (constrained.get() != NULL) {
@@ -236,13 +235,13 @@ public:
 };
 
 // Create Measurement Model Factor
-class NonlinearMeasurementModel : public NonlinearFactor1<TestKey> {
+class NonlinearMeasurementModel : public NonlinearFactor1<Point2> {
 public:
-  typedef TestKey::Value T;
+  typedef Point2 T;
 
 private:
 
-  typedef NonlinearFactor1<TestKey> Base;
+  typedef NonlinearFactor1<Point2> Base;
   typedef NonlinearMeasurementModel This;
 
 protected:
@@ -253,7 +252,7 @@ protected:
 public:
   NonlinearMeasurementModel(){}
 
-  NonlinearMeasurementModel(const TestKey& TestKey, Vector z) :
+  NonlinearMeasurementModel(const Symbol& TestKey, Vector z) :
     Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey), z_(z), R_(1,1) {
 
     // Initialize nonlinear measurement model parameters:
@@ -294,13 +293,13 @@ public:
   /* print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearMeasurementModel\n";
-    std::cout << "  TestKey: " << (std::string) key_ << std::endl;
+    std::cout << "  TestKey: " << (std::string) key() << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
-  virtual bool equals(const NonlinearFactor1<TestKey>& f, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
-    return (e != NULL) && (key_ == e->key_);
+    return (e != NULL) && Base::equals(f);
   }
 
   /**
@@ -319,7 +318,7 @@ public:
 
   /** Vector of errors, whitened ! */
   Vector whitenedError(const Values& c) const {
-    return RInvSqrt(c[key_])*unwhitenedError(c);
+    return RInvSqrt(c[key()])*unwhitenedError(c);
   }
 
   /**
@@ -328,10 +327,10 @@ public:
    * Hence b = z - h(x1) = - error_vector(x)
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
-    const X& x1 = c[key_];
+    const X& x1 = c[key()];
     Matrix A1;
     Vector b = -evaluateError(x1, A1);
-    const Index var1 = ordering[key_];
+    const Index var1 = ordering[key()];
     SharedDiagonal constrained =
         boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
     if (constrained.get() != NULL) {
@@ -345,7 +344,7 @@ public:
   }
 
   /** vector of errors */
-  Vector evaluateError(const TestKey::Value& p, boost::optional<Matrix&> H1 = boost::none) const {
+  Vector evaluateError(const T& p, boost::optional<Matrix&> H1 = boost::none) const {
     // error = z - h(p)
     // H = d error / d p = -d h/ d p = -H
     Vector z_hat = h(p);
@@ -406,17 +405,17 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
 
   // Enter Predict-Update Loop
   Point2 x_predict, x_update;
   for(unsigned int i = 0; i < 10; ++i){
     // Create motion factor
-    NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i));
+    NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i));
     x_predict = ekf.predict(motionFactor);
 
     // Create a measurement factor
-    NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, z[i]));
+    NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i]));
     x_update = ekf.update(measurementFactor);
 
     EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6));
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index c4f8c69fc..b0a955484 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -202,8 +202,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
 //  SETDEBUG("ISAM2 recalculate", true);
 
   // Pose and landmark key types from planarSLAM
-  typedef planarSLAM::PoseKey PoseKey;
-  typedef planarSLAM::PointKey PointKey;
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
 
   // Set up parameters
   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@@ -335,8 +335,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
 //  SETDEBUG("ISAM2 recalculate", true);
 
   // Pose and landmark key types from planarSLAM
-  typedef planarSLAM::PoseKey PoseKey;
-  typedef planarSLAM::PointKey PointKey;
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
 
   // Set up parameters
   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@@ -463,8 +463,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
 TEST(ISAM2, clone) {
 
   // Pose and landmark key types from planarSLAM
-  typedef planarSLAM::PoseKey PoseKey;
-  typedef planarSLAM::PointKey PointKey;
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
 
   // Set up parameters
   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@@ -636,8 +636,8 @@ TEST(ISAM2, removeFactors)
   // then removes the 2nd-to-last landmark measurement
 
   // Pose and landmark key types from planarSLAM
-  typedef planarSLAM::PoseKey PoseKey;
-  typedef planarSLAM::PointKey PointKey;
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
 
   // Set up parameters
   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@@ -775,8 +775,8 @@ TEST(ISAM2, constrained_ordering)
 //  SETDEBUG("ISAM2 recalculate", true);
 
   // Pose and landmark key types from planarSLAM
-  typedef planarSLAM::PoseKey PoseKey;
-  typedef planarSLAM::PointKey PointKey;
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
 
   // Set up parameters
   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index 271f625b6..ff1732ec5 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -125,8 +125,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
 
 /* ************************************************************************* */
 TEST(GaussianJunctionTree, slamlike) {
-  typedef planarSLAM::PoseKey PoseKey;
-  typedef planarSLAM::PointKey PointKey;
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
 
   Values init;
   planarSLAM::Graph newfactors;
@@ -195,7 +195,7 @@ TEST(GaussianJunctionTree, simpleMarginal) {
 
   // Create a simple graph
   pose2SLAM::Graph fg;
-  fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0));
+  fg.addPrior(0, Pose2(), sharedSigma(3, 10.0));
   fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
 
   Values init;
diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp
index b41da9286..34439b73a 100644
--- a/tests/testGraph.cpp
+++ b/tests/testGraph.cpp
@@ -86,16 +86,16 @@ TEST( Graph, composePoses )
 	graph.addOdometry(2,3, p23, cov);
 	graph.addOdometry(4,3, p43, cov);
 
-	PredecessorMap<pose2SLAM::PoseKey> tree;
-	tree.insert(pose2SLAM::PoseKey(1),2);
-	tree.insert(pose2SLAM::PoseKey(2),2);
-	tree.insert(pose2SLAM::PoseKey(3),2);
-	tree.insert(pose2SLAM::PoseKey(4),3);
+	PredecessorMap<Symbol> tree;
+	tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2));
+	tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2));
+	tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2));
+	tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3));
 
 	Pose2 rootPose = p2;
 
 	boost::shared_ptr<Values> actual = composePoses<pose2SLAM::Graph, pose2SLAM::Odometry,
-			Pose2, pose2SLAM::PoseKey> (graph, tree, rootPose);
+			Pose2, Symbol> (graph, tree, rootPose);
 
 	Values expected;
 	expected.insert(pose2SLAM::PoseKey(1), p1);
diff --git a/tests/testInference.cpp b/tests/testInference.cpp
index e530393f8..5c60f08e7 100644
--- a/tests/testInference.cpp
+++ b/tests/testInference.cpp
@@ -54,12 +54,12 @@ TEST( Inference, marginals2)
   SharedDiagonal poseModel(sharedSigma(3, 0.1));
   SharedDiagonal pointModel(sharedSigma(3, 0.1));
 
-  fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel);
-  fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0), poseModel);
-  fg.addOdometry(planarSLAM::PoseKey(1), planarSLAM::PoseKey(2), Pose2(1.0,0.0,0.0), poseModel);
-  fg.addBearingRange(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
-  fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
-  fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
+  fg.addPrior(0, Pose2(), poseModel);
+  fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel);
+  fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel);
+  fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel);
+  fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel);
+  fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel);
 
   Values init;
   init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0));
diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp
index bdbb3fdd2..2ceb066e2 100644
--- a/tests/testNonlinearEquality.cpp
+++ b/tests/testNonlinearEquality.cpp
@@ -31,15 +31,14 @@ namespace eq2D = simulated2D::equality_constraints;
 
 static const double tol = 1e-5;
 
-typedef TypedSymbol<Pose2, 'x'> PoseKey;
-typedef PriorFactor<PoseKey> PosePrior;
-typedef NonlinearEquality<PoseKey> PoseNLE;
+typedef PriorFactor<Pose2> PosePrior;
+typedef NonlinearEquality<Pose2> PoseNLE;
 typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
 
 typedef NonlinearFactorGraph PoseGraph;
 typedef NonlinearOptimizer<PoseGraph> PoseOptimizer;
 
-PoseKey key(1);
+Symbol key('x',1);
 
 /* ************************************************************************* */
 TEST ( NonlinearEquality, linearization ) {
@@ -60,7 +59,7 @@ TEST ( NonlinearEquality, linearization ) {
 /* ********************************************************************** */
 TEST ( NonlinearEquality, linearization_pose ) {
 
-	PoseKey key(1);
+  Symbol key('x',1);
 	Pose2 value;
 	Values config;
 	config.insert(key, value);
@@ -89,7 +88,7 @@ TEST ( NonlinearEquality, linearization_fail ) {
 /* ********************************************************************** */
 TEST ( NonlinearEquality, linearization_fail_pose ) {
 
-	PoseKey key(1);
+  Symbol key('x',1);
 	Pose2 value(2.0, 1.0, 2.0),
 		  wrong(2.0, 3.0, 4.0);
 	Values bad_linearize;
@@ -105,7 +104,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
 /* ********************************************************************** */
 TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
 
-	PoseKey key(1);
+  Symbol key('x',1);
 	Pose2 value,
 		  wrong(2.0, 3.0, 4.0);
 	Values bad_linearize;
@@ -155,7 +154,7 @@ TEST ( NonlinearEquality, equals ) {
 
 /* ************************************************************************* */
 TEST ( NonlinearEquality, allow_error_pose ) {
-	PoseKey key1(1);
+	Symbol key1('x',1);
 	Pose2 feasible1(1.0, 2.0, 3.0);
 	double error_gain = 500.0;
 	PoseNLE nle(key1, feasible1, error_gain);
@@ -183,7 +182,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
 
 /* ************************************************************************* */
 TEST ( NonlinearEquality, allow_error_optimize ) {
-	PoseKey key1(1);
+  Symbol key1('x',1);
 	Pose2 feasible1(1.0, 2.0, 3.0);
 	double error_gain = 500.0;
 	PoseNLE nle(key1, feasible1, error_gain);
@@ -200,7 +199,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
 	// optimize
 	boost::shared_ptr<Ordering> ord(new Ordering());
 	ord->push_back(key1);
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5);
 	PoseOptimizer optimizer(graph, init, ord, params);
 	PoseOptimizer result = optimizer.levenbergMarquardt();
 
@@ -214,7 +213,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
 TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 
 	// create a hard constraint
-	PoseKey key1(1);
+  Symbol key1('x',1);
 	Pose2 feasible1(1.0, 2.0, 3.0);
 
 	// initialize away from the ideal
@@ -236,7 +235,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 	// optimize
 	boost::shared_ptr<Ordering> ord(new Ordering());
 	ord->push_back(key1);
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5);
+  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5);
 	PoseOptimizer optimizer(graph, init, ord, params);
 	PoseOptimizer result = optimizer.levenbergMarquardt();
 
@@ -258,7 +257,7 @@ typedef NonlinearOptimizer<Graph> Optimizer;
 /* ************************************************************************* */
 TEST( testNonlinearEqualityConstraint, unary_basics ) {
 	Point2 pt(1.0, 2.0);
-	simulated2D::PoseKey key(1);
+  Symbol key1('x',1);
 	double mu = 1000.0;
 	eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
 
@@ -281,7 +280,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
 /* ************************************************************************* */
 TEST( testNonlinearEqualityConstraint, unary_linearization ) {
 	Point2 pt(1.0, 2.0);
-	simulated2D::PoseKey key(1);
+  Symbol key1('x',1);
 	double mu = 1000.0;
 	Ordering ordering;
 	ordering += key;
@@ -306,7 +305,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
 	// create a single-node graph with a soft and hard constraint to
 	// ensure that the hard constraint overrides the soft constraint
 	Point2 truth_pt(1.0, 2.0);
-	simulated2D::PoseKey key(1);
+  Symbol key('x',1);
 	double mu = 10.0;
 	eq2D::UnaryEqualityConstraint::shared_ptr constraint(
 			new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
@@ -337,7 +336,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
 /* ************************************************************************* */
 TEST( testNonlinearEqualityConstraint, odo_basics ) {
 	Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
-	simulated2D::PoseKey key1(1), key2(2);
+  Symbol key1('x',1), key2('x',2);
 	double mu = 1000.0;
 	eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
 
@@ -363,7 +362,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
 /* ************************************************************************* */
 TEST( testNonlinearEqualityConstraint, odo_linearization ) {
 	Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
-	simulated2D::PoseKey key1(1), key2(2);
+  Symbol key1('x',1), key2('x',2);
 	double mu = 1000.0;
 	Ordering ordering;
 	ordering += key1, key2;
@@ -396,7 +395,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
 	// a hard prior on one variable, and a conflicting soft prior
 	// on the other variable - the constraints should override the soft constraint
 	Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
-	simulated2D::PoseKey key1(1), key2(2);
+	Symbol key1('x',1), key2('x',2);
 
 	// hard prior on x1
 	eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
@@ -438,8 +437,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
 
 	shared_graph graph(new Graph());
 
-	simulated2D::PoseKey x1(1), x2(2);
-	simulated2D::PointKey l1(1), l2(2);
+  Symbol x1('x',1), x2('x',2);
+  Symbol l1('l',1), l2('l',2);
 	Point2 pt_x1(1.0, 1.0),
 		   pt_x2(5.0, 6.0);
 	graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
@@ -476,8 +475,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
 	shared_graph graph(new Graph());
 
 	// keys
-	simulated2D::PoseKey x1(1), x2(2);
-	simulated2D::PointKey l1(1), l2(2);
+  Symbol x1('x',1), x2('x',2);
+  Symbol l1('l',1), l2('l',2);
 
 	// constant constraint on x1
 	Point2 pose1(1.0, 1.0);
@@ -526,7 +525,7 @@ typedef visualSLAM::Graph VGraph;
 typedef NonlinearOptimizer<VGraph> VOptimizer;
 
 // factors for visual slam
-typedef NonlinearEquality2<visualSLAM::PointKey> Point3Equality;
+typedef NonlinearEquality2<Point3> Point3Equality;
 
 /* ********************************************************************* */
 TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
@@ -543,8 +542,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 	Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
 
 	// keys
-	visualSLAM::PoseKey x1(1), x2(2);
-	visualSLAM::PointKey l1(1), l2(2);
+  Symbol x1('x',1), x2('x',2);
+  Symbol l1('l',1), l2('l',2);
 
 	// create graph
 	VGraph::shared_graph graph(new VGraph());
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 53a2af385..488d406dc 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -40,6 +40,9 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
+using simulated2D::PoseKey;
+using simulated2D::PointKey;
+
 typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
 
 /* ************************************************************************* */
@@ -49,11 +52,11 @@ TEST( NonlinearFactor, equals )
 
 	// create two nonlinear2 factors
 	Point2 z3(0.,-1.);
-	simulated2D::Measurement f0(z3, sigma, 1,1);
+	simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1));
 
 	// measurement between x2 and l1
 	Point2 z4(-1.5, -1.);
-	simulated2D::Measurement f1(z4, sigma, 2,1);
+	simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1));
 
 	CHECK(assert_equal(f0,f0));
 	CHECK(f0.equals(f0));
@@ -199,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
 	SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
 
 	Point2 mu(1., -1.);
-	Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1));
+	Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1)));
 
 	Values config;
 	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
@@ -219,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
 	SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
 
 	Point2 z3(1.,-1.);
-	simulated2D::Measurement f0(z3, constraint, 1,1);
+	simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1));
 
 	Values config;
 	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
@@ -283,7 +286,7 @@ TEST(NonlinearFactor, NonlinearFactor4) {
 class TestFactor5 : public NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
   typedef NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
-  TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
+  TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5") {}
 
   virtual Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
@@ -333,7 +336,7 @@ TEST(NonlinearFactor, NonlinearFactor5) {
 class TestFactor6 : public NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
   typedef NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
-  TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
+  TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5", "x6") {}
 
   virtual Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index 2316c5652..09a3d4db0 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -25,15 +25,14 @@ TEST(testNonlinearISAM, markov_chain ) {
 	Sampler sampler(model, 42u);
 
 	// create initial graph
-	PoseKey key(0);
 	Pose2 cur_pose; // start at origin
 	Graph start_factors;
-	start_factors.addPoseConstraint(key, cur_pose);
+	start_factors.addPoseConstraint(0, cur_pose);
 
 	planarSLAM::Values init;
 	planarSLAM::Values expected;
-	init.insert(key, cur_pose);
-	expected.insert(key, cur_pose);
+	init.insertPose(0, cur_pose);
+	expected.insertPose(0, cur_pose);
 	isam.update(start_factors, init);
 
 	// loop for a period of time to verify memory usage
@@ -41,8 +40,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 	Pose2 z(1.0, 2.0, 0.1);
 	for (size_t i=1; i<=nrPoses; ++i) {
 		Graph new_factors;
-		PoseKey key1(i-1), key2(i);
-		new_factors.addOdometry(key1, key2, z, model);
+		new_factors.addOdometry(i-1, i, z, model);
 		planarSLAM::Values new_init;
 
 		// perform a check on changing orderings
@@ -61,16 +59,15 @@ TEST(testNonlinearISAM, markov_chain ) {
 		}
 
 		cur_pose = cur_pose.compose(z);
-		new_init.insert(key2, cur_pose.retract(sampler.sample()));
-		expected.insert(key2, cur_pose);
+		new_init.insertPose(i, cur_pose.retract(sampler.sample()));
+		expected.insertPose(i, cur_pose);
 		isam.update(new_factors, new_init);
 	}
 
 	// verify values - all but the last one should be very close
 	planarSLAM::Values actual = isam.estimate();
 	for (size_t i=0; i<nrPoses; ++i) {
-		PoseKey cur_key(i);
-		EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));
+		EXPECT(assert_equal(expected.pose(i), actual.pose(i), tol));
 	}
 }
 
diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp
index 285428dab..105bc8331 100644
--- a/tests/testRot3Optimization.cpp
+++ b/tests/testRot3Optimization.cpp
@@ -29,9 +29,8 @@
 
 using namespace gtsam;
 
-typedef TypedSymbol<Rot3, 'r'> Key;
-typedef PriorFactor<Key> Prior;
-typedef BetweenFactor<Key> Between;
+typedef PriorFactor<Rot3> Prior;
+typedef BetweenFactor<Rot3> Between;
 typedef NonlinearFactorGraph Graph;
 
 /* ************************************************************************* */
@@ -41,11 +40,11 @@ TEST(Rot3, optimize) {
   Values truth;
   Values initial;
   Graph fg;
-  fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01)));
+  fg.add(Prior(Symbol('r',0), Rot3(), sharedSigma(3, 0.01)));
   for(int j=0; j<6; ++j) {
-    truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j)));
-    initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
-    fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
+    truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
+    initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
+    fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
   }
 
   NonlinearOptimizationParameters params;
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index 23551bbae..6ab1ad929 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -525,8 +525,8 @@ TEST (Serialization, planar_system) {
 	graph.add(constraint);
 
 	// text
-	EXPECT(equalsObj<PoseKey>(PoseKey(2)));
-	EXPECT(equalsObj<PointKey>(PointKey(3)));
+	EXPECT(equalsObj<Symbol>(PoseKey(2)));
+	EXPECT(equalsObj<Symbol>(PointKey(3)));
 	EXPECT(equalsObj<planarSLAM::Values>(values));
 	EXPECT(equalsObj<Prior>(prior));
 	EXPECT(equalsObj<Bearing>(bearing));
@@ -537,8 +537,8 @@ TEST (Serialization, planar_system) {
 	EXPECT(equalsObj<Graph>(graph));
 
 	// xml
-	EXPECT(equalsXML<PoseKey>(PoseKey(2)));
-	EXPECT(equalsXML<PointKey>(PointKey(3)));
+	EXPECT(equalsXML<Symbol>(PoseKey(2)));
+	EXPECT(equalsXML<Symbol>(PointKey(3)));
 	EXPECT(equalsXML<planarSLAM::Values>(values));
 	EXPECT(equalsXML<Prior>(prior));
 	EXPECT(equalsXML<Bearing>(bearing));
@@ -562,8 +562,8 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor,    "gtsam::visualSLAM::StereoF
 TEST (Serialization, visual_system) {
 	using namespace visualSLAM;
 	Values values;
-	PoseKey x1(1), x2(2);
-	PointKey l1(1), l2(2);
+	Symbol x1('x',1), x2('x',2);
+	Symbol l1('l',1), l2('l',2);
 	Pose3 pose1 = pose3, pose2 = pose3.inverse();
 	Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0);
 	values.insert(x1, pose1);
@@ -574,7 +574,7 @@ TEST (Serialization, visual_system) {
 	boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
 
 	Graph graph;
-	graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K);
+	graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K);
 	graph.addPointConstraint(1, pt1);
 	graph.addPointPrior(1, pt2, model3);
 	graph.addPoseConstraint(1, pose1);

From 3d40f5e6fc602ab8525a0d0c441c335323da7e0a Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 7 Feb 2012 04:58:11 +0000
Subject: [PATCH 47/88] All unit tests pass with TypedSymbol removed

---
 examples/CameraResectioning.cpp               |   2 +-
 examples/PlanarSLAMSelfContained_advanced.cpp |   2 +-
 examples/SimpleRotation.cpp                   |   2 +-
 examples/elaboratePoint2KalmanFilter.cpp      |   2 +-
 examples/vSLAMexample/vISAMexample.cpp        |   2 +-
 examples/vSLAMexample/vSFMexample.cpp         |   2 +-
 gtsam/inference/tests/testFactorGraph.cpp     |   2 +-
 gtsam/inference/tests/testISAM.cpp            |   2 +-
 gtsam/inference/tests/testJunctionTree.cpp    |   2 +-
 gtsam/inference/tests/timeSymbolMaps.cpp      |   2 +-
 gtsam/linear/tests/timeGaussianFactor.cpp     |   3 -
 gtsam/nonlinear/Key.h                         | 371 ------------------
 gtsam/nonlinear/Makefile.am                   |   2 +-
 gtsam/nonlinear/Ordering.h                    |   2 +-
 gtsam/nonlinear/Symbol.h                      | 136 +++++++
 gtsam/nonlinear/Values.h                      |   2 +-
 gtsam/nonlinear/tests/testKey.cpp             |  97 +----
 gtsam/nonlinear/tests/testValues.cpp          |  28 +-
 gtsam/slam/pose2SLAM.h                        |   2 +-
 gtsam/slam/pose3SLAM.h                        |   2 +-
 gtsam/slam/simulated3D.h                      |   2 +-
 gtsam/slam/smallExample.cpp                   |   2 +-
 gtsam/slam/tests/testGeneralSFMFactor.cpp     |   2 +-
 .../testGeneralSFMFactor_Cal3Bundler.cpp      |   2 +-
 gtsam/slam/tests/testPose2SLAM.cpp            |   2 +-
 gtsam/slam/tests/testPose3SLAM.cpp            |   2 +-
 gtsam/slam/tests/testProjectionFactor.cpp     |   2 +-
 gtsam/slam/tests/testVSLAM.cpp                |   2 +-
 gtsam/slam/visualSLAM.h                       |   2 +-
 tests/testGaussianBayesNet.cpp                |   2 +-
 tests/testGaussianFactor.cpp                  |   2 +-
 tests/testGaussianFactorGraph.cpp             |   2 +-
 tests/testGaussianISAM.cpp                    |   2 +-
 tests/testGaussianJunctionTree.cpp            |   2 +-
 tests/testGraph.cpp                           |  25 +-
 tests/testInference.cpp                       |   2 +-
 tests/testNonlinearFactor.cpp                 |   2 +-
 tests/testNonlinearFactorGraph.cpp            |   2 +-
 tests/testNonlinearOptimizer.cpp              |   2 +-
 tests/testSerialization.cpp                   |  14 +-
 tests/testSymbolicBayesNet.cpp                |   2 +-
 tests/testSymbolicFactorGraph.cpp             |   2 +-
 tests/testTupleValues.cpp                     |   4 +-
 tests/timeGaussianFactorGraph.cpp             |   2 +-
 44 files changed, 206 insertions(+), 544 deletions(-)
 delete mode 100644 gtsam/nonlinear/Key.h
 create mode 100644 gtsam/nonlinear/Symbol.h

diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 957931af4..1bfd6c785 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -16,7 +16,7 @@
  * @date	  Aug 23, 2011
  */
 
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/SimpleCamera.h>
diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp
index 9dc908bc0..65b201ab7 100644
--- a/examples/PlanarSLAMSelfContained_advanced.cpp
+++ b/examples/PlanarSLAMSelfContained_advanced.cpp
@@ -19,7 +19,7 @@
 #include <iostream>
 
 // for all nonlinear keys
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 // for points and poses
 #include <gtsam/geometry/Point2.h>
diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp
index 29bab981a..71727f750 100644
--- a/examples/SimpleRotation.cpp
+++ b/examples/SimpleRotation.cpp
@@ -22,7 +22,7 @@
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/linear/NoiseModel.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index 1c3415e61..ee0d82501 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -23,7 +23,7 @@
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/Ordering.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index 89ee250f9..ae26a9c47 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -20,7 +20,7 @@
 #include <boost/foreach.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp
index 0ecd2e3e9..468620f29 100644
--- a/examples/vSLAMexample/vSFMexample.cpp
+++ b/examples/vSLAMexample/vSFMexample.cpp
@@ -19,7 +19,7 @@
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp
index 832d03cea..641b1f4f9 100644
--- a/gtsam/inference/tests/testFactorGraph.cpp
+++ b/gtsam/inference/tests/testFactorGraph.cpp
@@ -25,7 +25,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/inference/SymbolicFactorGraph.h>
diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/inference/tests/testISAM.cpp
index 55922589c..1c6a2897c 100644
--- a/gtsam/inference/tests/testISAM.cpp
+++ b/gtsam/inference/tests/testISAM.cpp
@@ -21,7 +21,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/inference/BayesNet.h>
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 13f047e88..7374bbad8 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -24,7 +24,7 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/nonlinear/Ordering.h>
diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp
index 1bf83ca5d..61ce1b30c 100644
--- a/gtsam/inference/tests/timeSymbolMaps.cpp
+++ b/gtsam/inference/tests/timeSymbolMaps.cpp
@@ -22,7 +22,7 @@
 #include <vector>
 #include <map>
 
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 using namespace std;
 using namespace boost;
diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/gtsam/linear/tests/timeGaussianFactor.cpp
index d39be1061..3895ab520 100644
--- a/gtsam/linear/tests/timeGaussianFactor.cpp
+++ b/gtsam/linear/tests/timeGaussianFactor.cpp
@@ -15,9 +15,6 @@
  * @author  Alireza Fathi
  */
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
-#define GTSAM_MAGIC_KEY
-
 #include <time.h>
 
 /*STL/C++*/
diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h
deleted file mode 100644
index 9bff0511d..000000000
--- a/gtsam/nonlinear/Key.h
+++ /dev/null
@@ -1,371 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file Key.h
- * @date Jan 12, 2010
- * @author: Frank Dellaert
- * @author: Richard Roberts
- */
-
-#pragma once
-
-#include <list>
-#include <iostream>
-#include <boost/mpl/char.hpp>
-#include <boost/format.hpp>
-#include <boost/serialization/nvp.hpp>
-#ifdef GTSAM_MAGIC_KEY
-#include <boost/lexical_cast.hpp>
-#endif
-
-#define ALPHA '\224'
-
-namespace gtsam {
-
-  // Forward declarations
-  class Symbol;
-
-/**
- * TypedSymbol 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
- */
-template<class T, char C>
-class TypedSymbol {
-
-protected:
-  size_t j_;
-
-public:
-
-  // typedefs
-  typedef T Value;
-  typedef boost::mpl::char_<C> Chr;  // to reconstruct the type: use Chr::value
-
-  // Constructors:
-
-  TypedSymbol() :
-    j_(0) {
-  }
-  TypedSymbol(size_t j) :
-    j_(j) {
-  }
-
-  virtual ~TypedSymbol() {}
-
-  // Get stuff:
-  ///TODO: comment
-  size_t index() const {
-    return j_;
-  }
-  static char chr() {
-    return C;
-  }
-  const char* c_str() const {
-    return ((std::string) (*this)).c_str();
-  }
-  operator std::string() const {
-    return (boost::format("%c%d") % C % j_).str();
-  }
-  std::string latex() const {
-    return (boost::format("%c_{%d}") % C % j_).str();
-  }
-  Symbol symbol() const;
-
-  // logic:
-
-  bool operator<(const TypedSymbol& compare) const {
-    return j_ < compare.j_;
-  }
-  bool operator==(const TypedSymbol& compare) const {
-    return j_ == compare.j_;
-  }
-  bool operator!=(const TypedSymbol& compare) const {
-    return j_ != compare.j_;
-  }
-  int compare(const TypedSymbol& compare) const {
-    return j_ - compare.j_;
-  }
-
-  // Testable Requirements
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": " << (std::string) (*this) << std::endl;
-  }
-  bool equals(const TypedSymbol& expected, double tol = 0.0) const {
-    return (*this) == expected;
-  }
-
-private:
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(j_);
-  }
-};
-
-/** forward declaration to avoid circular dependencies */
-template<class T, char C, typename L>
-class TypedLabeledSymbol;
-
-/**
- * Character and index key used in VectorValues, GaussianFactorGraph,
- * GaussianFactor, etc.  These keys are generated at runtime from TypedSymbol
- * keys when linearizing a nonlinear factor graph.  This key is not type
- * safe, so cannot be used with any Nonlinear* classes.
- */
-class Symbol {
-protected:
-  unsigned char c_;
-  size_t j_;
-
-public:
-  /** Default constructor */
-  Symbol() :
-    c_(0), j_(0) {
-  }
-
-  /** Copy constructor */
-  Symbol(const Symbol& key) :
-    c_(key.c_), j_(key.j_) {
-  }
-
-  /** Constructor */
-  Symbol(unsigned char c, size_t j) :
-    c_(c), j_(j) {
-  }
-
-  /** Casting constructor from TypedSymbol */
-  template<class T, char C>
-  Symbol(const TypedSymbol<T, C>& symbol) :
-    c_(C), j_(symbol.index()) {
-  }
-
-  /** Casting constructor from TypedLabeledSymbol */
-  template<class T, char C, typename L>
-  Symbol(const TypedLabeledSymbol<T, C, L>& symbol) :
-    c_(C), j_(symbol.encode()) {
-  }
-
-  /** "Magic" key casting constructor from string */
-#ifdef GTSAM_MAGIC_KEY
-  Symbol(const std::string& str) {
-    if(str.length() < 1)
-    throw std::invalid_argument("Cannot parse string key '" + str + "'");
-    else {
-      const char *c_str = str.c_str();
-      c_ = c_str[0];
-      if(str.length() > 1)
-      j_ = boost::lexical_cast<size_t>(c_str+1);
-      else
-      j_ = 0;
-    }
-  }
-
-  Symbol(const char *c_str) {
-    std::string str(c_str);
-    if(str.length() < 1)
-    throw std::invalid_argument("Cannot parse string key '" + str + "'");
-    else {
-      c_ = c_str[0];
-      if(str.length() > 1)
-      j_ = boost::lexical_cast<size_t>(c_str+1);
-      else
-      j_ = 0;
-    }
-  }
-#endif
-
-  // Testable Requirements
-  void print(const std::string& s = "") const {
-    std::cout << s << ": " << (std::string) (*this) << std::endl;
-  }
-  bool equals(const Symbol& expected, double tol = 0.0) const {
-    return (*this) == expected;
-  }
-
-  /** Retrieve key character */
-  unsigned char chr() const {
-    return c_;
-  }
-
-  /** Retrieve key index */
-  size_t index() const {
-    return j_;
-  }
-
-  /** Create a string from the key */
-  operator std::string() const {
-    return str(boost::format("%c%d") % c_ % j_);
-  }
-
-  /** Comparison for use in maps */
-  bool operator<(const Symbol& comp) const {
-    return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
-  }
-  bool operator==(const Symbol& comp) const {
-    return comp.c_ == c_ && comp.j_ == j_;
-  }
-  bool operator!=(const Symbol& comp) const {
-    return comp.c_ != c_ || comp.j_ != j_;
-  }
-
-private:
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(c_);
-    ar & BOOST_SERIALIZATION_NVP(j_);
-  }
-};
-
-// Conversion utilities
-
-template<class KEY> Symbol key2symbol(KEY key) {
-  return Symbol(key);
-}
-
-template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
-  std::list<Symbol> symbols;
-  std::transform(keys.begin(), keys.end(), std::back_inserter(symbols),
-      key2symbol<KEY> );
-  return symbols;
-}
-
-/**
- * TypedLabeledSymbol is a variation of the TypedSymbol that allows
- * for a runtime label to be placed on the label, so as to express
- * "Pose 5 for robot 3"
- * Labels should be kept to base datatypes (int, char, etc) to
- * minimize cost of comparisons
- *
- * The labels will be compared first when comparing Keys, followed by the
- * index
- */
-template<class T, char C, typename L>
-class TypedLabeledSymbol: public TypedSymbol<T, C> {
-
-protected:
-  // Label
-  L label_;
-
-public:
-
-  typedef TypedSymbol<T, C> Base;
-
-  // Constructors:
-
-  TypedLabeledSymbol() {
-  }
-  TypedLabeledSymbol(size_t j, L label) :
-    Base(j), label_(label) {
-  }
-
-  /** Constructor that decodes encoded labels */
-  TypedLabeledSymbol(const Symbol& sym) :
-    TypedSymbol<T, C> (0) {
-    size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
-    this->j_ = (sym.index() << shift) >> shift; // truncate upper bits
-    label_ = (L) (sym.index() >> shift); // remove lower bits
-  }
-
-  /** Constructor to upgrade an existing typed label with a label */
-  TypedLabeledSymbol(const Base& key, L label) :
-    Base(key.index()), label_(label) {
-  }
-
-  // Get stuff:
-
-  L label() const {
-    return label_;
-  }
-  const char* c_str() const {
-    return ((std::string)(*this)).c_str();
-  }
-  operator std::string() const {
-    std::string label_s = (boost::format("%1%") % label_).str();
-    return (boost::format("%c%s_%d") % C % label_s % this->j_).str();
-  }
-  std::string latex() const {
-    std::string label_s = (boost::format("%1%") % label_).str();
-    return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
-  }
-  Symbol symbol() const {
-    return Symbol(*this);
-  }
-
-  // Needed for conversion to LabeledSymbol
-  size_t convertLabel() const {
-    return label_;
-  }
-
-  /**
-   * Encoding two numbers into a single size_t for conversion to Symbol
-   * Stores the label in the upper bytes of the index
-   */
-  size_t encode() const {
-    short label = (short) label_; //bound size of label to 2 bytes
-    size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
-    size_t modifier = ((size_t) label) << shift;
-    return this->j_ + modifier;
-  }
-
-  // logic:
-
-  bool operator<(const TypedLabeledSymbol& compare) const {
-    if (label_ == compare.label_) // sort by label first
-      return this->j_ < compare.j_;
-    else
-      return label_ < compare.label_;
-  }
-  bool operator==(const TypedLabeledSymbol& compare) const {
-    return this->j_ == compare.j_ && label_ == compare.label_;
-  }
-  int compare(const TypedLabeledSymbol& compare) const {
-    if (label_ == compare.label_) // sort by label first
-      return this->j_ - compare.j_;
-    else
-      return label_ - compare.label_;
-  }
-
-  // Testable Requirements
-  void print(const std::string& s = "") const {
-    std::cout << s << ": " << (std::string) (*this) << std::endl;
-  }
-  bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const {
-    return (*this) == expected;
-  }
-
-private:
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    typedef TypedSymbol<T, C> Base;
-    ar & boost::serialization::make_nvp("TypedLabeledSymbol",
-        boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(label_);
-  }
-};
-
-/* ************************************************************************* */
-template<class T, char C>
-Symbol TypedSymbol<T,C>::symbol() const {
-  return Symbol(*this);
-}
-
-} // namespace gtsam
-
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 76a41247c..0512c36e1 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -21,7 +21,7 @@ sources += Values.cpp
 check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
 
 # Nonlinear nonlinear
-headers += Key.h 
+headers += Symbol.h 
 headers += NonlinearFactorGraph.h
 headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h 
 headers += NonlinearFactor.h
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 4d10b15b5..47d048f2b 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -19,7 +19,7 @@
 
 #include <map>
 #include <set>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/inference/inference.h>
 
 #include <boost/foreach.hpp>
diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h
new file mode 100644
index 000000000..f2963dc32
--- /dev/null
+++ b/gtsam/nonlinear/Symbol.h
@@ -0,0 +1,136 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Symbol.h
+ * @date Jan 12, 2010
+ * @author: Frank Dellaert
+ * @author: Richard Roberts
+ */
+
+#pragma once
+
+#include <list>
+#include <iostream>
+#include <boost/mpl/char.hpp>
+#include <boost/format.hpp>
+#include <boost/serialization/nvp.hpp>
+#ifdef GTSAM_MAGIC_KEY
+#include <boost/lexical_cast.hpp>
+#endif
+
+#define ALPHA '\224'
+
+namespace gtsam {
+
+/**
+ * Character and index key used in VectorValues, GaussianFactorGraph,
+ * GaussianFactor, etc.  These keys are generated at runtime from TypedSymbol
+ * keys when linearizing a nonlinear factor graph.  This key is not type
+ * safe, so cannot be used with any Nonlinear* classes.
+ */
+class Symbol {
+protected:
+  unsigned char c_;
+  size_t j_;
+
+public:
+  /** Default constructor */
+  Symbol() :
+    c_(0), j_(0) {
+  }
+
+  /** Copy constructor */
+  Symbol(const Symbol& key) :
+    c_(key.c_), j_(key.j_) {
+  }
+
+  /** Constructor */
+  Symbol(unsigned char c, size_t j) :
+    c_(c), j_(j) {
+  }
+
+  /** "Magic" key casting constructor from string */
+#ifdef GTSAM_MAGIC_KEY
+  Symbol(const std::string& str) {
+    if(str.length() < 1)
+    throw std::invalid_argument("Cannot parse string key '" + str + "'");
+    else {
+      const char *c_str = str.c_str();
+      c_ = c_str[0];
+      if(str.length() > 1)
+      j_ = boost::lexical_cast<size_t>(c_str+1);
+      else
+      j_ = 0;
+    }
+  }
+
+  Symbol(const char *c_str) {
+    std::string str(c_str);
+    if(str.length() < 1)
+    throw std::invalid_argument("Cannot parse string key '" + str + "'");
+    else {
+      c_ = c_str[0];
+      if(str.length() > 1)
+      j_ = boost::lexical_cast<size_t>(c_str+1);
+      else
+      j_ = 0;
+    }
+  }
+#endif
+
+  // Testable Requirements
+  void print(const std::string& s = "") const {
+    std::cout << s << ": " << (std::string) (*this) << std::endl;
+  }
+  bool equals(const Symbol& expected, double tol = 0.0) const {
+    return (*this) == expected;
+  }
+
+  /** Retrieve key character */
+  unsigned char chr() const {
+    return c_;
+  }
+
+  /** Retrieve key index */
+  size_t index() const {
+    return j_;
+  }
+
+  /** Create a string from the key */
+  operator std::string() const {
+    return str(boost::format("%c%d") % c_ % j_);
+  }
+
+  /** Comparison for use in maps */
+  bool operator<(const Symbol& comp) const {
+    return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
+  }
+  bool operator==(const Symbol& comp) const {
+    return comp.c_ == c_ && comp.j_ == j_;
+  }
+  bool operator!=(const Symbol& comp) const {
+    return comp.c_ != c_ || comp.j_ != j_;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(c_);
+    ar & BOOST_SERIALIZATION_NVP(j_);
+  }
+};
+
+} // namespace gtsam
+
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index f226007f1..bad281a9a 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -37,7 +37,7 @@
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp
index e05e5d8b5..19c3b6325 100644
--- a/gtsam/nonlinear/tests/testKey.cpp
+++ b/gtsam/nonlinear/tests/testKey.cpp
@@ -19,106 +19,11 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 	
 using namespace std;
 using namespace gtsam;
 
-class Pose3;
-
-/* ************************************************************************* */
-TEST ( TypedSymbol, basic_operations ) {
-	typedef TypedSymbol<Pose3, 'x'> Key;
-
-	Key key1(0),
-		key2(0),
-		key3(1),
-		key4(2);
-
-	CHECK(key1.index()==0);
-	CHECK(key1 == key2);
-	CHECK(assert_equal(key1, key2));
-	CHECK(!(key1 == key3));
-	CHECK(key1 < key3);
-	CHECK(key3 < key4);
-}
-
-/* ************************************************************************* */
-TEST ( TypedLabledSymbol, basic_operations ) {
-	typedef TypedSymbol<Pose3, 'x'> SimpleKey;
-	typedef TypedLabeledSymbol<Pose3, 'x', int> RobotKey;
-
-	SimpleKey key7(1);
-	RobotKey key1(0, 1),
-			 key2(0, 1),
-			 key3(1, 1),
-			 key4(2, 1),
-			 key5(0, 2),
-			 key6(1, 2),
-			 key8(1, 3),
-			 key9(key7, 3);
-
-
-	CHECK(key1.label()==1);
-	CHECK(key1.index()==0);
-	CHECK(key1 == key2);
-	CHECK(assert_equal(key1, key2));
-	CHECK(!(key1 == key3));
-	CHECK(key1 < key3);
-	CHECK(key3 < key4);
-	CHECK(!(key1 == key5));
-	CHECK(key1 < key5);
-	CHECK(key5 < key6);
-	CHECK(assert_equal(key9, key8));
-}
-
-/* ************************************************************************* */
-TEST ( TypedLabledSymbol, encoding ) {
-	typedef TypedLabeledSymbol<Pose3, 'x', char> RobotKey;
-
-	RobotKey key1(37, 'A');
-
-	// Note: calculations done in test due to possible differences between machines
-	// take the upper two bytes for the label
-	short label = key1.label();
-
-	// find the shift necessary
-	size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
-	size_t modifier = label;
-	modifier = modifier << shift;
-	size_t index = key1.index() + modifier;
-
-	// check index encoding
-	Symbol act1(key1), exp('x', index);
-	CHECK(assert_equal(exp, act1));
-
-	// check casting
-	Symbol act2 = (Symbol) key1;
-	CHECK(assert_equal(exp, act2));
-
-	// decode
-	CHECK(assert_equal(key1, RobotKey(act1)));
-}
-
-/* ************************************************************************* */
-TEST ( TypedLabledSymbol, template_reconstruction ) {
-	typedef TypedSymbol<Pose3, 'x'> Key;
-	typedef TypedLabeledSymbol<Key::Value, Key::Chr::value, char> NewKey;
-	NewKey k(1, 'A');
-}
-
-/* ************************************************************************* */
-TEST ( Key, keys2symbols )
-{
-	typedef TypedSymbol<int, 'x'> Key;
-	list<Symbol> expected;
-	expected += Key(1), Key(2), Key(3);
-
-	list<TypedSymbol<int, 'x'> > typeds;
-	typeds += 1, 2, 3;
-	CHECK(expected == keys2symbols(typeds));
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index c66e59ead..bb68ef9e6 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -20,6 +20,8 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
+#define GTSAM_MAGIC_KEY
+
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/LieVector.h>
 #include <gtsam/geometry/Pose2.h>
@@ -29,9 +31,7 @@ using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
-typedef TypedSymbol<LieVector, 'v'> VecKey;
-
-VecKey key1(1), key2(2), key3(3), key4(4);
+Symbol key1("v1"), key2("v2"), key3("v3"), key4("v4");
 
 /* ************************************************************************* */
 TEST( Values, equals1 )
@@ -114,11 +114,11 @@ TEST( Values, update_element )
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v1, cfg.at(key1)));
+  CHECK(assert_equal(v1, cfg.at<LieVector>(key1)));
 
   cfg.update(key1, v2);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v2, cfg.at(key1)));
+  CHECK(assert_equal(v2, cfg.at<LieVector>(key1)));
 }
 
 ///* ************************************************************************* */
@@ -200,10 +200,9 @@ TEST(Values, expmap_d)
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
 
-  typedef TypedSymbol<Pose2, 'p'> PoseKey;
   Values poseconfig;
-  poseconfig.insert(PoseKey(1), Pose2(1,2,3));
-  poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
+  poseconfig.insert("p1", Pose2(1,2,3));
+  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
 
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
@@ -212,16 +211,15 @@ TEST(Values, expmap_d)
 /* ************************************************************************* */
 TEST(Values, extract_keys)
 {
-	typedef TypedSymbol<Pose2, 'x'> PoseKey;
 	Values config;
 
-	config.insert(PoseKey(1), Pose2());
-	config.insert(PoseKey(2), Pose2());
-	config.insert(PoseKey(4), Pose2());
-	config.insert(PoseKey(5), Pose2());
+	config.insert("x1", Pose2());
+	config.insert("x2", Pose2());
+	config.insert("x4", Pose2());
+	config.insert("x5", Pose2());
 
 	FastList<Symbol> expected, actual;
-	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
+	expected += "x1", "x2", "x4", "x5";
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
@@ -238,7 +236,7 @@ TEST(Values, exists_)
 	config0.insert(key1, LieVector(Vector_(1, 1.)));
 	config0.insert(key2, LieVector(Vector_(1, 2.)));
 
-	boost::optional<const LieVector&> v = config0.exists(key1);
+	boost::optional<const LieVector&> v = config0.exists<LieVector>(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
 }
 
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 7917a3c79..121e0c062 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -20,7 +20,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h
index 555e5fc91..eb4eb9573 100644
--- a/gtsam/slam/pose3SLAM.h
+++ b/gtsam/slam/pose3SLAM.h
@@ -20,7 +20,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index 40adcdbe2..9cd1ed74d 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -23,7 +23,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 // \namespace
 
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 8ee8cb09a..4c13ab2b3 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -24,7 +24,7 @@
 
 using namespace std;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 8c59d63ac..1f1e616f1 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -12,7 +12,7 @@
 #include <CppUnitLite/TestHarness.h>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 6b1ace287..89dbf15ea 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -12,7 +12,7 @@
 #include <CppUnitLite/TestHarness.h>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index 29c28dbc1..4646bb5d7 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -14,7 +14,7 @@
  *  @author Frank Dellaert, Viorela Ila
  **/
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/slam/pose2SLAM.h>
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 21c0fd458..77c206918 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -28,7 +28,7 @@ using namespace boost::assign;
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 6
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/slam/pose3SLAM.h>
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index f7f804fb9..00c41aaad 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -18,7 +18,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/slam/visualSLAM.h>
diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp
index ccae629eb..fb53d4b65 100644
--- a/gtsam/slam/tests/testVSLAM.cpp
+++ b/gtsam/slam/tests/testVSLAM.cpp
@@ -22,7 +22,7 @@
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index df137ad47..40e5eafcc 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -23,7 +23,7 @@
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/StereoFactor.h>
 #include <gtsam/slam/RangeFactor.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp
index 7f746397c..dd8f49988 100644
--- a/tests/testGaussianBayesNet.cpp
+++ b/tests/testGaussianBayesNet.cpp
@@ -25,7 +25,7 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp
index 1260c51a2..f6cf7d54b 100644
--- a/tests/testGaussianFactor.cpp
+++ b/tests/testGaussianFactor.cpp
@@ -26,7 +26,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp
index d95a05830..73b1f2d63 100644
--- a/tests/testGaussianFactorGraph.cpp
+++ b/tests/testGaussianFactorGraph.cpp
@@ -28,7 +28,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp
index 3b432393d..bd592fc68 100644
--- a/tests/testGaussianISAM.cpp
+++ b/tests/testGaussianISAM.cpp
@@ -21,7 +21,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/geometry/Rot2.h>
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index ff1732ec5..19b0c676d 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -25,7 +25,7 @@
 #include <boost/assign/std/vector.hpp>
 using namespace boost::assign;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/debug.h>
diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp
index 34439b73a..2ac2ce691 100644
--- a/tests/testGraph.cpp
+++ b/tests/testGraph.cpp
@@ -23,6 +23,8 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#define GTSAM_MAGIC_KEY
+
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 3
 
@@ -37,22 +39,21 @@ using namespace gtsam;
 //		-> x3 -> x4
 //    -> x5
 TEST ( Ordering, predecessorMap2Keys ) {
-	typedef TypedSymbol<Pose2,'x'> PoseKey;
-	PredecessorMap<PoseKey> p_map;
-	p_map.insert(1,1);
-	p_map.insert(2,1);
-	p_map.insert(3,1);
-	p_map.insert(4,3);
-	p_map.insert(5,1);
+	PredecessorMap<Symbol> p_map;
+	p_map.insert("x1","x1");
+	p_map.insert("x2","x1");
+	p_map.insert("x3","x1");
+	p_map.insert("x4","x3");
+	p_map.insert("x5","x1");
 
-	list<PoseKey> expected;
-	expected += 4,5,3,2,1;//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
+	list<Symbol> expected;
+	expected += "x4","x5","x3","x2","x1";//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
 
-	list<PoseKey> actual = predecessorMap2Keys<PoseKey>(p_map);
+	list<Symbol> actual = predecessorMap2Keys<Symbol>(p_map);
 	LONGS_EQUAL(expected.size(), actual.size());
 
-	list<PoseKey>::const_iterator it1 = expected.begin();
-	list<PoseKey>::const_iterator it2 = actual.begin();
+	list<Symbol>::const_iterator it1 = expected.begin();
+	list<Symbol>::const_iterator it2 = actual.begin();
 	for(; it1!=expected.end(); it1++, it2++)
 		CHECK(*it1 == *it2)
 }
diff --git a/tests/testInference.cpp b/tests/testInference.cpp
index 5c60f08e7..d1ef745e6 100644
--- a/tests/testInference.cpp
+++ b/tests/testInference.cpp
@@ -17,7 +17,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/linear/GaussianSequentialSolver.h>
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 488d406dc..2b9037ac2 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -25,7 +25,7 @@
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 2
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 1cca4c988..938067508 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -27,7 +27,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 88d142bf6..f7a09f95f 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -26,7 +26,7 @@ using namespace boost::assign;
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
index 6ab1ad929..fa9944d1b 100644
--- a/tests/testSerialization.cpp
+++ b/tests/testSerialization.cpp
@@ -145,7 +145,7 @@ bool equalsDereferencedXML(const T& input = T()) {
 // Actual Tests
 /* ************************************************************************* */
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/geometry/Cal3DS2.h>
@@ -255,16 +255,12 @@ typedef PinholeCamera<Cal3_S2> 				PinholeCal3S2;
 typedef PinholeCamera<Cal3DS2> 				PinholeCal3DS2;
 typedef PinholeCamera<Cal3Bundler> 		PinholeCal3Bundler;
 
-typedef TypedSymbol<Cal3_S2,'a'> 				PinholeCal3S2Key;
-typedef TypedSymbol<Cal3DS2,'s'> 				PinholeCal3DS2Key;
-typedef TypedSymbol<Cal3Bundler,'d'> 		PinholeCal3BundlerKey;
-
 TEST (Serialization, TemplatedValues) {
 	Values values;
-	values.insert(PinholeCal3S2Key(0), 	PinholeCal3S2(pose3, cal1));
-	values.insert(PinholeCal3DS2Key(5), PinholeCal3DS2(pose3, cal2));
-	values.insert(PinholeCal3BundlerKey(47), PinholeCal3Bundler(pose3, cal3));
-	values.insert(PinholeCal3S2Key(5), 	PinholeCal3S2(pose3, cal1));
+	values.insert(Symbol('a',0), 	PinholeCal3S2(pose3, cal1));
+	values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
+	values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
+	values.insert(Symbol('a',5), 	PinholeCal3S2(pose3, cal1));
 	EXPECT(equalsObj(values));
 	EXPECT(equalsXML(values));
 }
diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp
index de51cb8dc..efa794a25 100644
--- a/tests/testSymbolicBayesNet.cpp
+++ b/tests/testSymbolicBayesNet.cpp
@@ -21,7 +21,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp
index 01515f079..4295a5263 100644
--- a/tests/testSymbolicFactorGraph.cpp
+++ b/tests/testSymbolicFactorGraph.cpp
@@ -20,7 +20,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/slam/smallExample.h>
diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp
index 3df49061c..06232c64e 100644
--- a/tests/testTupleValues.cpp
+++ b/tests/testTupleValues.cpp
@@ -20,7 +20,7 @@
 
 #include <gtsam/base/TestableAssertions.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/geometry/Pose2.h>
@@ -29,7 +29,7 @@
 #include <gtsam/geometry/Point3.h>
 
 #include <gtsam/base/Vector.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/TupleValues.h>
 
 using namespace gtsam;
diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp
index e6b028e6d..3af046971 100644
--- a/tests/timeGaussianFactorGraph.cpp
+++ b/tests/timeGaussianFactorGraph.cpp
@@ -15,7 +15,7 @@
  * @author  Frank Dellaert
  */
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <time.h>

From 669c7c8dc8626e88c9bf1f0a96c4c25aac282ff2 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 8 Feb 2012 21:53:02 +0000
Subject: [PATCH 48/88] Split up serialization unit tests into subdirectories
 to speed up

---
 gtsam/base/Makefile.am                        |  10 +
 gtsam/base/serializationTestHelpers.h         | 148 +++++
 gtsam/base/tests/testSerializationBase.cpp    |  41 ++
 gtsam/geometry/Makefile.am                    |  10 +
 .../tests/testSerializationGeometry.cpp       | 115 ++++
 gtsam/inference/Makefile.am                   |  10 +
 .../tests/testSerializationInference.cpp      |  91 +++
 gtsam/linear/Makefile.am                      |  10 +
 .../linear/tests/testSerializationLinear.cpp  | 161 +++++
 gtsam/nonlinear/Makefile.am                   |  10 +
 .../tests/testSerializationNonlinear.cpp      |  69 ++
 gtsam/slam/Makefile.am                        |  10 +
 gtsam/slam/tests/testSerializationSLAM.cpp    | 209 +++++++
 tests/Makefile.am                             |  10 -
 tests/testSerialization.cpp                   | 588 ------------------
 15 files changed, 894 insertions(+), 598 deletions(-)
 create mode 100644 gtsam/base/serializationTestHelpers.h
 create mode 100644 gtsam/base/tests/testSerializationBase.cpp
 create mode 100644 gtsam/geometry/tests/testSerializationGeometry.cpp
 create mode 100644 gtsam/inference/tests/testSerializationInference.cpp
 create mode 100644 gtsam/linear/tests/testSerializationLinear.cpp
 create mode 100644 gtsam/nonlinear/tests/testSerializationNonlinear.cpp
 create mode 100644 gtsam/slam/tests/testSerializationSLAM.cpp
 delete mode 100644 tests/testSerialization.cpp

diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am
index aa02ffec9..25abacbaf 100644
--- a/gtsam/base/Makefile.am
+++ b/gtsam/base/Makefile.am
@@ -36,6 +36,11 @@ headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h
 sources += DSFVector.cpp
 check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector
 
+## flag disabled to force this test to get updated properly
+if ENABLE_SERIALIZATION
+check_PROGRAMS += tests/testSerializationBase
+endif
+
 # Timing tests
 noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest
 noinst_PROGRAMS += tests/timeMatrixOps 
@@ -56,6 +61,11 @@ AM_CPPFLAGS =
 AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 
+# link to serialization library for test 
+if ENABLE_SERIALIZATION
+tests_testSerializationBase_LDFLAGS = -lboost_serialization
+endif
+
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
new file mode 100644
index 000000000..ef2e9393d
--- /dev/null
+++ b/gtsam/base/serializationTestHelpers.h
@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file serializationTestHelpers.h
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#pragma once
+
+#include <sstream>
+#include <string>
+
+// includes for standard serialization types
+#include <boost/serialization/export.hpp>
+#include <boost/serialization/optional.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/map.hpp>
+#include <boost/serialization/list.hpp>
+#include <boost/serialization/deque.hpp>
+#include <boost/serialization/weak_ptr.hpp>
+
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+
+// whether to print the serialized text to stdout
+const bool verbose = false;
+
+namespace gtsam { namespace serializationTestHelpers {
+
+  /* ************************************************************************* */
+  // Serialization testing code.
+  /* ************************************************************************* */
+
+template<class T>
+std::string serialize(const T& input) {
+  std::ostringstream out_archive_stream;
+  boost::archive::text_oarchive out_archive(out_archive_stream);
+  out_archive << input;
+  return out_archive_stream.str();
+}
+
+template<class T>
+void deserialize(const std::string& serialized, T& output) {
+  std::istringstream in_archive_stream(serialized);
+  boost::archive::text_iarchive in_archive(in_archive_stream);
+  in_archive >> output;
+}
+
+// Templated round-trip serialization
+template<class T>
+void roundtrip(const T& input, T& output) {
+  // Serialize
+  std::string serialized = serialize(input);
+  if (verbose) std::cout << serialized << std::endl << std::endl;
+
+  deserialize(serialized, output);
+}
+
+// This version requires equality operator
+template<class T>
+bool equality(const T& input = T()) {
+  T output;
+  roundtrip<T>(input,output);
+  return input==output;
+}
+
+// This version requires equals
+template<class T>
+bool equalsObj(const T& input = T()) {
+  T output;
+  roundtrip<T>(input,output);
+  return input.equals(output);
+}
+
+// De-referenced version for pointers
+template<class T>
+bool equalsDereferenced(const T& input) {
+  T output;
+  roundtrip<T>(input,output);
+  return input->equals(*output);
+}
+
+/* ************************************************************************* */
+template<class T>
+std::string serializeXML(const T& input) {
+  std::ostringstream out_archive_stream;
+  boost::archive::xml_oarchive out_archive(out_archive_stream);
+  out_archive << boost::serialization::make_nvp("data", input);
+  return out_archive_stream.str();
+}
+
+template<class T>
+void deserializeXML(const std::string& serialized, T& output) {
+  std::istringstream in_archive_stream(serialized);
+  boost::archive::xml_iarchive in_archive(in_archive_stream);
+  in_archive >> boost::serialization::make_nvp("data", output);
+}
+
+// Templated round-trip serialization using XML
+template<class T>
+void roundtripXML(const T& input, T& output) {
+  // Serialize
+  std::string serialized = serializeXML<T>(input);
+  if (verbose) std::cout << serialized << std::endl << std::endl;
+
+  // De-serialize
+  deserializeXML(serialized, output);
+}
+
+// This version requires equality operator
+template<class T>
+bool equalityXML(const T& input = T()) {
+  T output;
+  roundtripXML<T>(input,output);
+  return input==output;
+}
+
+// This version requires equals
+template<class T>
+bool equalsXML(const T& input = T()) {
+  T output;
+  roundtripXML<T>(input,output);
+  return input.equals(output);
+}
+
+// This version is for pointers
+template<class T>
+bool equalsDereferencedXML(const T& input = T()) {
+  T output;
+  roundtripXML<T>(input,output);
+  return input->equals(*output);
+}
+
+} }
diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp
new file mode 100644
index 000000000..211a1c319
--- /dev/null
+++ b/gtsam/base/tests/testSerializationBase.cpp
@@ -0,0 +1,41 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSerializationBase.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+
+/* ************************************************************************* */
+TEST (Serialization, matrix_vector) {
+  EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
+
+  EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am
index 5f9b34610..b87ba8d07 100644
--- a/gtsam/geometry/Makefile.am
+++ b/gtsam/geometry/Makefile.am
@@ -32,6 +32,11 @@ headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Ex
 sources += projectiveGeometry.cpp tensorInterface.cpp
 check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
 
+## flag disabled to force this test to get updated properly
+if ENABLE_SERIALIZATION
+check_PROGRAMS += tests/testSerializationGeometry
+endif
+
 # Timing tests
 noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera
 
@@ -53,6 +58,11 @@ libgeometry_la_SOURCES = $(allsources)
 AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 
+# link to serialization library for test 
+if ENABLE_SERIALIZATION
+tests_testSerializationGeometry_LDFLAGS = -lboost_serialization
+endif
+
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp
new file mode 100644
index 000000000..c1fba6b2c
--- /dev/null
+++ b/gtsam/geometry/tests/testSerializationGeometry.cpp
@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSerializationGeometry.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/geometry/StereoPoint2.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* ************************************************************************* */
+// Export all classes derived from Value
+BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
+BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
+BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
+BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
+BOOST_CLASS_EXPORT(gtsam::Point2)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+BOOST_CLASS_EXPORT(gtsam::Pose2)
+BOOST_CLASS_EXPORT(gtsam::Pose3)
+BOOST_CLASS_EXPORT(gtsam::Rot2)
+BOOST_CLASS_EXPORT(gtsam::Rot3)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
+BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
+
+/* ************************************************************************* */
+Point3 pt3(1.0, 2.0, 3.0);
+Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
+Pose3 pose3(rt3, pt3);
+
+Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
+Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
+Cal3Bundler cal3(1.0, 2.0, 3.0);
+Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
+CalibratedCamera cal5(Pose3(rt3, pt3));
+
+PinholeCamera<Cal3_S2> cam1(pose3, cal1);
+StereoCamera cam2(pose3, cal4ptr);
+StereoPoint2 spt(1.0, 2.0, 3.0);
+
+/* ************************************************************************* */
+TEST (Serialization, text_geometry) {
+  EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
+  EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
+  EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
+
+  EXPECT(equalsObj(pt3));
+  EXPECT(equalsObj<gtsam::Rot3>(rt3));
+  EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
+
+  EXPECT(equalsObj(cal1));
+  EXPECT(equalsObj(cal2));
+  EXPECT(equalsObj(cal3));
+  EXPECT(equalsObj(cal4));
+  EXPECT(equalsObj(cal5));
+
+  EXPECT(equalsObj(cam1));
+  EXPECT(equalsObj(cam2));
+  EXPECT(equalsObj(spt));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, xml_geometry) {
+  EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
+  EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
+  EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
+
+  EXPECT(equalsXML<gtsam::Point3>(pt3));
+  EXPECT(equalsXML<gtsam::Rot3>(rt3));
+  EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
+
+  EXPECT(equalsXML(cal1));
+  EXPECT(equalsXML(cal2));
+  EXPECT(equalsXML(cal3));
+  EXPECT(equalsXML(cal4));
+  EXPECT(equalsXML(cal5));
+
+  EXPECT(equalsXML(cam1));
+  EXPECT(equalsXML(cam2));
+  EXPECT(equalsXML(spt));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/gtsam/inference/Makefile.am b/gtsam/inference/Makefile.am
index da84df61b..cab5c5cbb 100644
--- a/gtsam/inference/Makefile.am
+++ b/gtsam/inference/Makefile.am
@@ -45,6 +45,11 @@ check_PROGRAMS += tests/testClusterTree
 check_PROGRAMS += tests/testJunctionTree
 check_PROGRAMS += tests/testPermutation
 
+## flag disabled to force this test to get updated properly
+if ENABLE_SERIALIZATION
+check_PROGRAMS += tests/testSerializationInference
+endif
+
 #----------------------------------------------------------------------------------------------------
 # Create a libtool library that is not installed
 # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
@@ -64,6 +69,11 @@ AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 AM_CXXFLAGS =
 
+# link to serialization library for test 
+if ENABLE_SERIALIZATION
+tests_testSerializationInference_LDFLAGS = -lboost_serialization
+endif
+
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp
new file mode 100644
index 000000000..2db7014f1
--- /dev/null
+++ b/gtsam/inference/tests/testSerializationInference.cpp
@@ -0,0 +1,91 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSerializationInference.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#define GTSAM_MAGIC_KEY
+
+#include <gtsam/inference/IndexConditional.h>
+#include <gtsam/inference/SymbolicFactorGraph.h>
+#include <gtsam/inference/BayesTree.h>
+#include <gtsam/nonlinear/Ordering.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* ************************************************************************* */
+TEST (Serialization, symbolic_graph) {
+  Ordering o; o += "x1","l1","x2";
+  // construct expected symbolic graph
+  SymbolicFactorGraph sfg;
+  sfg.push_factor(o["x1"]);
+  sfg.push_factor(o["x1"],o["x2"]);
+  sfg.push_factor(o["x1"],o["l1"]);
+  sfg.push_factor(o["l1"],o["x2"]);
+
+  EXPECT(equalsObj(sfg));
+  EXPECT(equalsXML(sfg));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, symbolic_bn) {
+  Ordering o; o += "x2","l1","x1";
+
+  IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
+  IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
+  IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
+
+  SymbolicBayesNet sbn;
+  sbn.push_back(x2);
+  sbn.push_back(l1);
+  sbn.push_back(x1);
+
+  EXPECT(equalsObj(sbn));
+  EXPECT(equalsXML(sbn));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, symbolic_bayes_tree ) {
+  typedef BayesTree<IndexConditional> SymbolicBayesTree;
+  static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
+  IndexConditional::shared_ptr
+  B(new IndexConditional(_B_)),
+  L(new IndexConditional(_L_, _B_)),
+  E(new IndexConditional(_E_, _L_, _B_)),
+  S(new IndexConditional(_S_, _L_, _B_)),
+  T(new IndexConditional(_T_, _E_, _L_)),
+  X(new IndexConditional(_X_, _E_));
+
+  // Bayes Tree for Asia example
+  SymbolicBayesTree bayesTree;
+  SymbolicBayesTree::insert(bayesTree, B);
+  SymbolicBayesTree::insert(bayesTree, L);
+  SymbolicBayesTree::insert(bayesTree, E);
+  SymbolicBayesTree::insert(bayesTree, S);
+  SymbolicBayesTree::insert(bayesTree, T);
+  SymbolicBayesTree::insert(bayesTree, X);
+
+  EXPECT(equalsObj(bayesTree));
+  EXPECT(equalsXML(bayesTree));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am
index 811ae7458..bd78bae52 100644
--- a/gtsam/linear/Makefile.am
+++ b/gtsam/linear/Makefile.am
@@ -41,6 +41,11 @@ sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp
 headers += IterativeSolver.h IterativeOptimizationParameters.h 
 headers += SubgraphSolver-inl.h
 
+## flag disabled to force this test to get updated properly
+if ENABLE_SERIALIZATION
+check_PROGRAMS += tests/testSerializationLinear
+endif
+
 # Timing tests
 noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike
 
@@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 AM_CXXFLAGS =
 
+# link to serialization library for test 
+if ENABLE_SERIALIZATION
+tests_testSerializationLinear_LDFLAGS = -lboost_serialization
+endif
+
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp
new file mode 100644
index 000000000..8082d3626
--- /dev/null
+++ b/gtsam/linear/tests/testSerializationLinear.cpp
@@ -0,0 +1,161 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSerializationLinear.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianISAM.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/linear/SharedNoiseModel.h>
+#include <gtsam/linear/SharedDiagonal.h>
+#include <gtsam/linear/SharedGaussian.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* ************************************************************************* */
+// Noise model components
+/* ************************************************************************* */
+
+/* ************************************************************************* */
+// Export Noisemodels
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+/* ************************************************************************* */
+// example noise models
+noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
+noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
+noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
+noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
+noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
+
+/* ************************************************************************* */
+TEST (Serialization, noiseModels) {
+  // tests using pointers to the derived class
+  EXPECT(   equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
+  EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
+
+  EXPECT(   equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
+  EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
+
+  EXPECT(   equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
+  EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
+
+  EXPECT(   equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
+  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
+
+  EXPECT(   equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
+  EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, SharedNoiseModel_noiseModels) {
+  SharedNoiseModel diag3_sg = diag3;
+  EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
+
+  EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
+
+  EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
+
+  EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
+
+  EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
+
+  EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
+  EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, SharedDiagonal_noiseModels) {
+  EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
+  EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
+
+  EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
+  EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
+
+  EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
+  EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
+
+  EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
+  EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
+}
+
+/* ************************************************************************* */
+// Linear components
+/* ************************************************************************* */
+
+/* Create GUIDs for factors */
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+
+/* ************************************************************************* */
+TEST (Serialization, linear_factors) {
+  VectorValues values;
+  values.insert(0, Vector_(1, 1.0));
+  values.insert(1, Vector_(2, 2.0,3.0));
+  values.insert(2, Vector_(2, 4.0,5.0));
+  EXPECT(equalsObj<VectorValues>(values));
+  EXPECT(equalsXML<VectorValues>(values));
+
+  Index i1 = 4, i2 = 7;
+  Matrix A1 = eye(3), A2 = -1.0 * eye(3);
+  Vector b = ones(3);
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0));
+  JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
+  EXPECT(equalsObj(jacobianfactor));
+  EXPECT(equalsXML(jacobianfactor));
+
+  HessianFactor hessianfactor(jacobianfactor);
+  EXPECT(equalsObj(hessianfactor));
+  EXPECT(equalsXML(hessianfactor));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, gaussian_conditional) {
+  Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
+  Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
+  Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
+  Vector d(2); d << 0.2, 0.5;
+  GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
+
+  EXPECT(equalsObj(cg));
+  EXPECT(equalsXML(cg));
+}
+
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 0512c36e1..15651a22a 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -44,6 +44,11 @@ headers += WhiteNoiseFactor.h
 # Kalman Filter
 headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h
 
+## flag disabled to force this test to get updated properly
+if ENABLE_SERIALIZATION
+check_PROGRAMS += tests/testSerializationNonlinear
+endif
+
 #----------------------------------------------------------------------------------------------------
 # Create a libtool library that is not installed
 # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
@@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 AM_CXXFLAGS =
 
+# link to serialization library for test 
+if ENABLE_SERIALIZATION
+tests_testSerializationNonlinear_LDFLAGS = -lboost_serialization
+endif
+
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
new file mode 100644
index 000000000..120a9fa94
--- /dev/null
+++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSerializationNonlinear.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* ************************************************************************* */
+// Export all classes derived from Value
+BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
+BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+BOOST_CLASS_EXPORT(gtsam::Pose3)
+BOOST_CLASS_EXPORT(gtsam::Rot3)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
+
+/* ************************************************************************* */
+typedef PinholeCamera<Cal3_S2>        PinholeCal3S2;
+typedef PinholeCamera<Cal3DS2>        PinholeCal3DS2;
+typedef PinholeCamera<Cal3Bundler>    PinholeCal3Bundler;
+
+/* ************************************************************************* */
+Point3 pt3(1.0, 2.0, 3.0);
+Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
+Pose3 pose3(rt3, pt3);
+
+Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
+Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
+Cal3Bundler cal3(1.0, 2.0, 3.0);
+
+TEST (Serialization, TemplatedValues) {
+  Values values;
+  values.insert(Symbol('a',0),  PinholeCal3S2(pose3, cal1));
+  values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
+  values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
+  values.insert(Symbol('a',5),  PinholeCal3S2(pose3, cal1));
+  EXPECT(equalsObj(values));
+  EXPECT(equalsXML(values));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am
index 71eb94d64..cfa37c29d 100644
--- a/gtsam/slam/Makefile.am
+++ b/gtsam/slam/Makefile.am
@@ -54,6 +54,11 @@ check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bund
 headers += StereoFactor.h
 check_PROGRAMS += tests/testStereoFactor
 
+## flag disabled to force this test to get updated properly
+if ENABLE_SERIALIZATION
+check_PROGRAMS += tests/testSerializationSLAM
+endif
+
 #----------------------------------------------------------------------------------------------------
 # Create a libtool library that is not installed
 # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
@@ -67,6 +72,11 @@ libslam_la_SOURCES = $(sources)
 AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 
+# link to serialization library for test 
+if ENABLE_SERIALIZATION
+tests_testSerializationSLAM_LDFLAGS = -lboost_serialization
+endif
+
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp
new file mode 100644
index 000000000..c75e0a148
--- /dev/null
+++ b/gtsam/slam/tests/testSerializationSLAM.cpp
@@ -0,0 +1,209 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSerializationSLAM.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 7, 2012
+ */
+
+#define GTSAM_MAGIC_KEY
+
+#include <gtsam/linear/GaussianISAM.h>
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/slam/smallExample.h>
+#include <gtsam/slam/planarSLAM.h>
+#include <gtsam/slam/visualSLAM.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* Create GUIDs for factors */
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+
+/* ************************************************************************* */
+// Export Noisemodels
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+/* ************************************************************************* */
+TEST (Serialization, smallExample_linear) {
+  using namespace example;
+
+  Ordering ordering; ordering += "x1","x2","l1";
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+  EXPECT(equalsObj(ordering));
+  EXPECT(equalsXML(ordering));
+
+  EXPECT(equalsObj(fg));
+  EXPECT(equalsXML(fg));
+
+  GaussianBayesNet cbn = createSmallGaussianBayesNet();
+  EXPECT(equalsObj(cbn));
+  EXPECT(equalsXML(cbn));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, gaussianISAM) {
+  using namespace example;
+  Ordering ordering;
+  GaussianFactorGraph smoother;
+  boost::tie(smoother, ordering) = createSmoother(7);
+  BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
+  GaussianISAM isam(bayesTree);
+
+  EXPECT(equalsObj(isam));
+  EXPECT(equalsXML(isam));
+}
+
+/* ************************************************************************* */
+/* Create GUIDs for factors in simulated2D */
+BOOST_CLASS_EXPORT_GUID(simulated2D::Prior,       "gtsam::simulated2D::Prior"      )
+BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry,    "gtsam::simulated2D::Odometry"   )
+BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
+BOOST_CLASS_EXPORT(gtsam::Point2)
+
+/* ************************************************************************* */
+TEST (Serialization, smallExample) {
+  using namespace example;
+  Graph nfg = createNonlinearFactorGraph();
+  Values c1 = createValues();
+  EXPECT(equalsObj(nfg));
+  EXPECT(equalsXML(nfg));
+
+  EXPECT(equalsObj(c1));
+  EXPECT(equalsXML(c1));
+}
+
+/* ************************************************************************* */
+/* Create GUIDs for factors */
+BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior,       "gtsam::planarSLAM::Prior");
+BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing,     "gtsam::planarSLAM::Bearing");
+BOOST_CLASS_EXPORT_GUID(planarSLAM::Range,       "gtsam::planarSLAM::Range");
+BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange");
+BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry,    "gtsam::planarSLAM::Odometry");
+BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint,  "gtsam::planarSLAM::Constraint");
+
+BOOST_CLASS_EXPORT(gtsam::Pose2)
+
+/* ************************************************************************* */
+TEST (Serialization, planar_system) {
+  using namespace planarSLAM;
+  planarSLAM::Values values;
+  values.insert(PointKey(3), Point2(1.0, 2.0));
+  values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
+
+  SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
+  SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
+  SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
+
+  Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
+  Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
+  Range range(PoseKey(2), PointKey(9), 7.0, model1);
+  BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
+  Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
+  Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
+
+  Graph graph;
+  graph.add(prior);
+  graph.add(bearing);
+  graph.add(range);
+  graph.add(bearingRange);
+  graph.add(odometry);
+  graph.add(constraint);
+
+  // text
+  EXPECT(equalsObj<Symbol>(PoseKey(2)));
+  EXPECT(equalsObj<Symbol>(PointKey(3)));
+  EXPECT(equalsObj<planarSLAM::Values>(values));
+  EXPECT(equalsObj<Prior>(prior));
+  EXPECT(equalsObj<Bearing>(bearing));
+  EXPECT(equalsObj<BearingRange>(bearingRange));
+  EXPECT(equalsObj<Range>(range));
+  EXPECT(equalsObj<Odometry>(odometry));
+  EXPECT(equalsObj<Constraint>(constraint));
+  EXPECT(equalsObj<Graph>(graph));
+
+  // xml
+  EXPECT(equalsXML<Symbol>(PoseKey(2)));
+  EXPECT(equalsXML<Symbol>(PointKey(3)));
+  EXPECT(equalsXML<planarSLAM::Values>(values));
+  EXPECT(equalsXML<Prior>(prior));
+  EXPECT(equalsXML<Bearing>(bearing));
+  EXPECT(equalsXML<BearingRange>(bearingRange));
+  EXPECT(equalsXML<Range>(range));
+  EXPECT(equalsXML<Odometry>(odometry));
+  EXPECT(equalsXML<Constraint>(constraint));
+  EXPECT(equalsXML<Graph>(graph));
+}
+
+/* ************************************************************************* */
+/* Create GUIDs for factors */
+BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint,  "gtsam::visualSLAM::PoseConstraint");
+BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint");
+BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior,       "gtsam::visualSLAM::PosePrior");
+BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior,      "gtsam::visualSLAM::PointPrior");
+BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor");
+BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor,    "gtsam::visualSLAM::StereoFactor");
+
+BOOST_CLASS_EXPORT(gtsam::Pose3)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+
+Point3 pt3(1.0, 2.0, 3.0);
+Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
+Pose3 pose3(rt3, pt3);
+Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
+
+/* ************************************************************************* */
+TEST (Serialization, visual_system) {
+  using namespace visualSLAM;
+  Values values;
+  Symbol x1('x',1), x2('x',2);
+  Symbol l1('l',1), l2('l',2);
+  Pose3 pose1 = pose3, pose2 = pose3.inverse();
+  Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0);
+  values.insert(x1, pose1);
+  values.insert(l1, pt1);
+  SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
+  SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
+  SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
+  boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
+
+  Graph graph;
+  graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K);
+  graph.addPointConstraint(1, pt1);
+  graph.addPointPrior(1, pt2, model3);
+  graph.addPoseConstraint(1, pose1);
+  graph.addPosePrior(1, pose2, model6);
+
+  EXPECT(equalsObj(values));
+  EXPECT(equalsObj(graph));
+
+  EXPECT(equalsXML(values));
+  EXPECT(equalsXML(graph));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/tests/Makefile.am b/tests/Makefile.am
index fb1172222..66044ccfc 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -24,11 +24,6 @@ check_PROGRAMS += testGaussianISAM2
 check_PROGRAMS += testExtendedKalmanFilter
 check_PROGRAMS += testRot3Optimization
 
-## flag disabled to force this test to get updated properly
-#if ENABLE_SERIALIZATION
-check_PROGRAMS += testSerialization
-#endif
-
 # Timing tests
 noinst_PROGRAMS = timeGaussianFactorGraph timeSequentialOnDataset timeMultifrontalOnDataset
 
@@ -39,11 +34,6 @@ TESTS = $(check_PROGRAMS)
 AM_LDFLAGS = $(BOOST_LDFLAGS)
 AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
 
-# link to serialization library for test 
-#if ENABLE_SERIALIZATION
-AM_LDFLAGS += -lboost_serialization
-#endif
-
 LDADD = ../gtsam/libgtsam.la ../CppUnitLite/libCppUnitLite.a  
 AM_DEFAULT_SOURCE_EXT = .cpp
 
diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp
deleted file mode 100644
index fa9944d1b..000000000
--- a/tests/testSerialization.cpp
+++ /dev/null
@@ -1,588 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @brief Unit tests for serialization of library classes
- * @author Frank Dellaert
- * @author Alex Cunningham
- */
-
-/* ************************************************************************* */
-// Serialization testing code.
-/* ************************************************************************* */
-
-#include <sstream>
-#include <string>
-
-// includes for standard serialization types
-#include <boost/serialization/export.hpp>
-#include <boost/serialization/optional.hpp>
-#include <boost/serialization/shared_ptr.hpp>
-#include <boost/serialization/vector.hpp>
-#include <boost/serialization/map.hpp>
-#include <boost/serialization/list.hpp>
-#include <boost/serialization/deque.hpp>
-#include <boost/serialization/weak_ptr.hpp>
-
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/xml_iarchive.hpp>
-#include <boost/archive/xml_oarchive.hpp>
-
-#include <gtsam/geometry/Point2.h>
-// whether to print the serialized text to stdout
-const bool verbose = false;
-
-template<class T>
-std::string serialize(const T& input) {
-	std::ostringstream out_archive_stream;
-	boost::archive::text_oarchive out_archive(out_archive_stream);
-	out_archive << input;
-	return out_archive_stream.str();
-}
-
-template<class T>
-void deserialize(const std::string& serialized, T& output) {
-	std::istringstream in_archive_stream(serialized);
-	boost::archive::text_iarchive in_archive(in_archive_stream);
-	in_archive >> output;
-}
-
-// Templated round-trip serialization
-template<class T>
-void roundtrip(const T& input, T& output) {
-	// Serialize
-	std::string serialized = serialize(input);
-	if (verbose) std::cout << serialized << std::endl << std::endl;
-
-	deserialize(serialized, output);
-}
-
-// This version requires equality operator
-template<class T>
-bool equality(const T& input = T()) {
-	T output;
-	roundtrip<T>(input,output);
-	return input==output;
-}
-
-// This version requires equals
-template<class T>
-bool equalsObj(const T& input = T()) {
-	T output;
-	roundtrip<T>(input,output);
-	return input.equals(output);
-}
-
-// De-referenced version for pointers
-template<class T>
-bool equalsDereferenced(const T& input) {
-	T output;
-	roundtrip<T>(input,output);
-	return input->equals(*output);
-}
-
-/* ************************************************************************* */
-template<class T>
-std::string serializeXML(const T& input) {
-	std::ostringstream out_archive_stream;
-	boost::archive::xml_oarchive out_archive(out_archive_stream);
-	out_archive << boost::serialization::make_nvp("data", input);
-	return out_archive_stream.str();
-}
-
-template<class T>
-void deserializeXML(const std::string& serialized, T& output) {
-	std::istringstream in_archive_stream(serialized);
-	boost::archive::xml_iarchive in_archive(in_archive_stream);
-	in_archive >> boost::serialization::make_nvp("data", output);
-}
-
-// Templated round-trip serialization using XML
-template<class T>
-void roundtripXML(const T& input, T& output) {
-	// Serialize
-	std::string serialized = serializeXML<T>(input);
-	if (verbose) std::cout << serialized << std::endl << std::endl;
-
-	// De-serialize
-	deserializeXML(serialized, output);
-}
-
-// This version requires equality operator
-template<class T>
-bool equalityXML(const T& input = T()) {
-	T output;
-	roundtripXML<T>(input,output);
-	return input==output;
-}
-
-// This version requires equals
-template<class T>
-bool equalsXML(const T& input = T()) {
-	T output;
-	roundtripXML<T>(input,output);
-	return input.equals(output);
-}
-
-// This version is for pointers
-template<class T>
-bool equalsDereferencedXML(const T& input = T()) {
-	T output;
-	roundtripXML<T>(input,output);
-	return input->equals(*output);
-}
-
-/* ************************************************************************* */
-// Actual Tests
-/* ************************************************************************* */
-
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
-#include <gtsam/geometry/Cal3DS2.h>
-#include <gtsam/geometry/Cal3Bundler.h>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
-#include <gtsam/linear/GaussianSequentialSolver.h>
-#include <gtsam/linear/GaussianISAM.h>
-#include <gtsam/slam/smallExample.h>
-#include <gtsam/slam/planarSLAM.h>
-#include <gtsam/slam/pose3SLAM.h>
-#include <gtsam/slam/visualSLAM.h>
-
-#include <CppUnitLite/TestHarness.h>
-
-using namespace std;
-using namespace gtsam;
-
-
-/* ************************************************************************* */
-TEST (Serialization, matrix_vector) {
-	EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
-	EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
-
-	EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
-	EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
-}
-
-/* ************************************************************************* */
-// Export all classes derived from Value
-BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
-BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
-BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
-BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
-BOOST_CLASS_EXPORT(gtsam::Point2)
-BOOST_CLASS_EXPORT(gtsam::Point3)
-BOOST_CLASS_EXPORT(gtsam::Pose2)
-BOOST_CLASS_EXPORT(gtsam::Pose3)
-BOOST_CLASS_EXPORT(gtsam::Rot2)
-BOOST_CLASS_EXPORT(gtsam::Rot3)
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
-BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
-
-/* ************************************************************************* */
-Point3 pt3(1.0, 2.0, 3.0);
-Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
-Pose3 pose3(rt3, pt3);
-
-Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
-Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
-Cal3Bundler cal3(1.0, 2.0, 3.0);
-Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
-Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
-CalibratedCamera cal5(Pose3(rt3, pt3));
-
-PinholeCamera<Cal3_S2> cam1(pose3, cal1);
-StereoCamera cam2(pose3, cal4ptr);
-StereoPoint2 spt(1.0, 2.0, 3.0);
-
-/* ************************************************************************* */
-TEST (Serialization, text_geometry) {
-	EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
-	EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
-	EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
-
-	EXPECT(equalsObj(pt3));
-	EXPECT(equalsObj<gtsam::Rot3>(rt3));
-	EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
-
-	EXPECT(equalsObj(cal1));
-	EXPECT(equalsObj(cal2));
-	EXPECT(equalsObj(cal3));
-	EXPECT(equalsObj(cal4));
-	EXPECT(equalsObj(cal5));
-
-	EXPECT(equalsObj(cam1));
-	EXPECT(equalsObj(cam2));
-	EXPECT(equalsObj(spt));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, xml_geometry) {
-	EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
-	EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
-	EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
-
-	EXPECT(equalsXML<gtsam::Point3>(pt3));
-	EXPECT(equalsXML<gtsam::Rot3>(rt3));
-	EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
-
-	EXPECT(equalsXML(cal1));
-	EXPECT(equalsXML(cal2));
-	EXPECT(equalsXML(cal3));
-	EXPECT(equalsXML(cal4));
-	EXPECT(equalsXML(cal5));
-
-	EXPECT(equalsXML(cam1));
-	EXPECT(equalsXML(cam2));
-	EXPECT(equalsXML(spt));
-}
-
-/* ************************************************************************* */
-typedef PinholeCamera<Cal3_S2> 				PinholeCal3S2;
-typedef PinholeCamera<Cal3DS2> 				PinholeCal3DS2;
-typedef PinholeCamera<Cal3Bundler> 		PinholeCal3Bundler;
-
-TEST (Serialization, TemplatedValues) {
-	Values values;
-	values.insert(Symbol('a',0), 	PinholeCal3S2(pose3, cal1));
-	values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
-	values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
-	values.insert(Symbol('a',5), 	PinholeCal3S2(pose3, cal1));
-	EXPECT(equalsObj(values));
-	EXPECT(equalsXML(values));
-}
-
-/* ************************************************************************* */
-// Export Noisemodels
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
-
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
-
-/* ************************************************************************* */
-// example noise models
-noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
-noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
-noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
-noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
-noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
-
-/* ************************************************************************* */
-TEST (Serialization, noiseModels) {
-	// tests using pointers to the derived class
-	EXPECT(   equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
-	EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
-
-	EXPECT(   equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
-	EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
-
-	EXPECT(   equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
-	EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
-
-	EXPECT(   equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
-	EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
-
-	EXPECT(   equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
-	EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, SharedNoiseModel_noiseModels) {
-	SharedNoiseModel diag3_sg = diag3;
-	EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
-	EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
-
-	EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
-	EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
-
-	EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
-	EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
-
-	EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
-	EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
-
-	EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
-	EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
-
-	EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
-	EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, SharedDiagonal_noiseModels) {
-	EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
-	EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
-
-	EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
-	EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
-
-	EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
-	EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
-
-	EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
-	EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
-}
-
-/* ************************************************************************* */
-// Linear components
-/* ************************************************************************* */
-
-/* ************************************************************************* */
-TEST (Serialization, linear_factors) {
-  VectorValues values;
-  values.insert(0, Vector_(1, 1.0));
-  values.insert(1, Vector_(2, 2.0,3.0));
-  values.insert(2, Vector_(2, 4.0,5.0));
-	EXPECT(equalsObj<VectorValues>(values));
-	EXPECT(equalsXML<VectorValues>(values));
-
-	Index i1 = 4, i2 = 7;
-	Matrix A1 = eye(3), A2 = -1.0 * eye(3);
-  Vector b = ones(3);
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0));
-	JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
-	EXPECT(equalsObj(jacobianfactor));
-	EXPECT(equalsXML(jacobianfactor));
-
-	HessianFactor hessianfactor(jacobianfactor);
-	EXPECT(equalsObj(hessianfactor));
-	EXPECT(equalsXML(hessianfactor));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, gaussian_conditional) {
-	Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
-	Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
-	Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
-	Vector d(2); d << 0.2, 0.5;
-	GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
-
-	EXPECT(equalsObj(cg));
-	EXPECT(equalsXML(cg));
-}
-
-/* Create GUIDs for factors */
-/* ************************************************************************* */
-BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
-BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
-/* ************************************************************************* */
-TEST (Serialization, smallExample_linear) {
-	using namespace example;
-
-  Ordering ordering; ordering += "x1","x2","l1";
-  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-  EXPECT(equalsObj(ordering));
-  EXPECT(equalsXML(ordering));
-
-  EXPECT(equalsObj(fg));
-  EXPECT(equalsXML(fg));
-
-  GaussianBayesNet cbn = createSmallGaussianBayesNet();
-  EXPECT(equalsObj(cbn));
-  EXPECT(equalsXML(cbn));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, symbolic_graph) {
-  Ordering o; o += "x1","l1","x2";
-	// construct expected symbolic graph
-	SymbolicFactorGraph sfg;
-	sfg.push_factor(o["x1"]);
-	sfg.push_factor(o["x1"],o["x2"]);
-	sfg.push_factor(o["x1"],o["l1"]);
-	sfg.push_factor(o["l1"],o["x2"]);
-
-	EXPECT(equalsObj(sfg));
-	EXPECT(equalsXML(sfg));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, symbolic_bn) {
-  Ordering o; o += "x2","l1","x1";
-
-  IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
-  IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
-  IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
-
-  SymbolicBayesNet sbn;
-  sbn.push_back(x2);
-  sbn.push_back(l1);
-  sbn.push_back(x1);
-
-	EXPECT(equalsObj(sbn));
-	EXPECT(equalsXML(sbn));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, symbolic_bayes_tree ) {
-	typedef BayesTree<IndexConditional> SymbolicBayesTree;
-	static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
-	IndexConditional::shared_ptr
-	B(new IndexConditional(_B_)),
-	L(new IndexConditional(_L_, _B_)),
-	E(new IndexConditional(_E_, _L_, _B_)),
-	S(new IndexConditional(_S_, _L_, _B_)),
-	T(new IndexConditional(_T_, _E_, _L_)),
-	X(new IndexConditional(_X_, _E_));
-
-	// Bayes Tree for Asia example
-	SymbolicBayesTree bayesTree;
-	SymbolicBayesTree::insert(bayesTree, B);
-	SymbolicBayesTree::insert(bayesTree, L);
-	SymbolicBayesTree::insert(bayesTree, E);
-	SymbolicBayesTree::insert(bayesTree, S);
-	SymbolicBayesTree::insert(bayesTree, T);
-	SymbolicBayesTree::insert(bayesTree, X);
-
-	EXPECT(equalsObj(bayesTree));
-	EXPECT(equalsXML(bayesTree));
-}
-
-/* ************************************************************************* */
-TEST (Serialization, gaussianISAM) {
-	using namespace example;
-  Ordering ordering;
-	GaussianFactorGraph smoother;
-	boost::tie(smoother, ordering) = createSmoother(7);
-	BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
-	GaussianISAM isam(bayesTree);
-
-	EXPECT(equalsObj(isam));
-	EXPECT(equalsXML(isam));
-}
-
-/* ************************************************************************* */
-/* Create GUIDs for factors in simulated2D */
-BOOST_CLASS_EXPORT_GUID(simulated2D::Prior,       "gtsam::simulated2D::Prior"      )
-BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry,    "gtsam::simulated2D::Odometry"   )
-BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
-
-/* ************************************************************************* */
-TEST (Serialization, smallExample) {
-	using namespace example;
-	Graph nfg = createNonlinearFactorGraph();
-	Values c1 = createValues();
-	EXPECT(equalsObj(nfg));
-	EXPECT(equalsXML(nfg));
-
-	EXPECT(equalsObj(c1));
-	EXPECT(equalsXML(c1));
-}
-
-/* ************************************************************************* */
-/* Create GUIDs for factors */
-BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior,       "gtsam::planarSLAM::Prior");
-BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing,     "gtsam::planarSLAM::Bearing");
-BOOST_CLASS_EXPORT_GUID(planarSLAM::Range,       "gtsam::planarSLAM::Range");
-BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange");
-BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry,    "gtsam::planarSLAM::Odometry");
-BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint,  "gtsam::planarSLAM::Constraint");
-/* ************************************************************************* */
-TEST (Serialization, planar_system) {
-	using namespace planarSLAM;
-	planarSLAM::Values values;
-	values.insert(PointKey(3), Point2(1.0, 2.0));
-	values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
-
-	SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
-	SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
-	SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
-
-	Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
-	Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
-	Range range(PoseKey(2), PointKey(9), 7.0, model1);
-	BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
-	Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
-	Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
-
-	Graph graph;
-	graph.add(prior);
-	graph.add(bearing);
-	graph.add(range);
-	graph.add(bearingRange);
-	graph.add(odometry);
-	graph.add(constraint);
-
-	// text
-	EXPECT(equalsObj<Symbol>(PoseKey(2)));
-	EXPECT(equalsObj<Symbol>(PointKey(3)));
-	EXPECT(equalsObj<planarSLAM::Values>(values));
-	EXPECT(equalsObj<Prior>(prior));
-	EXPECT(equalsObj<Bearing>(bearing));
-	EXPECT(equalsObj<BearingRange>(bearingRange));
-	EXPECT(equalsObj<Range>(range));
-	EXPECT(equalsObj<Odometry>(odometry));
-	EXPECT(equalsObj<Constraint>(constraint));
-	EXPECT(equalsObj<Graph>(graph));
-
-	// xml
-	EXPECT(equalsXML<Symbol>(PoseKey(2)));
-	EXPECT(equalsXML<Symbol>(PointKey(3)));
-	EXPECT(equalsXML<planarSLAM::Values>(values));
-	EXPECT(equalsXML<Prior>(prior));
-	EXPECT(equalsXML<Bearing>(bearing));
-	EXPECT(equalsXML<BearingRange>(bearingRange));
-	EXPECT(equalsXML<Range>(range));
-	EXPECT(equalsXML<Odometry>(odometry));
-	EXPECT(equalsXML<Constraint>(constraint));
-	EXPECT(equalsXML<Graph>(graph));
-}
-
-/* ************************************************************************* */
-/* Create GUIDs for factors */
-BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint,  "gtsam::visualSLAM::PoseConstraint");
-BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint");
-BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior,       "gtsam::visualSLAM::PosePrior");
-BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior,      "gtsam::visualSLAM::PointPrior");
-BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor");
-BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor,    "gtsam::visualSLAM::StereoFactor");
-
-/* ************************************************************************* */
-TEST (Serialization, visual_system) {
-	using namespace visualSLAM;
-	Values values;
-	Symbol x1('x',1), x2('x',2);
-	Symbol l1('l',1), l2('l',2);
-	Pose3 pose1 = pose3, pose2 = pose3.inverse();
-	Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0);
-	values.insert(x1, pose1);
-	values.insert(l1, pt1);
-	SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
-	SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
-	SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
-	boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
-
-	Graph graph;
-	graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K);
-	graph.addPointConstraint(1, pt1);
-	graph.addPointPrior(1, pt2, model3);
-	graph.addPoseConstraint(1, pose1);
-	graph.addPosePrior(1, pose2, model6);
-
-	EXPECT(equalsObj(values));
-	EXPECT(equalsObj(graph));
-
-	EXPECT(equalsXML(values));
-	EXPECT(equalsXML(graph));
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */

From 8885b331912c37b3bd95dddcac954ca1f81c2af0 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 8 Feb 2012 23:11:41 +0000
Subject: [PATCH 49/88] Added serialization functions to Fast* containers
 (wrappers around STL containers that use a boost pool allocator for speed)

---
 gtsam/base/FastList.h                      | 10 ++++
 gtsam/base/FastMap.h                       |  9 ++--
 gtsam/base/FastSet.h                       | 10 ++++
 gtsam/base/FastVector.h                    | 10 ++++
 gtsam/base/tests/testSerializationBase.cpp | 54 ++++++++++++++++++++++
 5 files changed, 89 insertions(+), 4 deletions(-)

diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h
index b34d1fad9..02f8734f6 100644
--- a/gtsam/base/FastList.h
+++ b/gtsam/base/FastList.h
@@ -20,6 +20,8 @@
 
 #include <list>
 #include <boost/pool/pool_alloc.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/list.hpp>
 
 namespace gtsam {
 
@@ -51,6 +53,14 @@ public:
   /** Copy constructor from the base map class */
   FastList(const Base& x) : Base(x) {}
 
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+  }
+
 };
 
 }
diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h
index cc5a3b4d6..c9fe30646 100644
--- a/gtsam/base/FastMap.h
+++ b/gtsam/base/FastMap.h
@@ -20,7 +20,8 @@
 
 #include <map>
 #include <boost/pool/pool_alloc.hpp>
-#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/map.hpp>
 
 namespace gtsam {
 
@@ -52,12 +53,12 @@ public:
   /** Copy constructor from the base map class */
   FastMap(const Base& x) : Base(x) {}
 
-  private:
+private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & boost::serialization::base_object<Base>(*this);
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
   }
 };
 
diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index 7d9084e5f..05fb879a1 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -25,6 +25,8 @@
 #include <boost/pool/pool_alloc.hpp>
 #include <boost/mpl/has_xxx.hpp>
 #include <boost/utility/enable_if.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/set.hpp>
 
 BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
 
@@ -74,6 +76,14 @@ public:
 
   /** Check for equality within tolerance to implement Testable */
   bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const { return FastSetTestableHelper<VALUE>::equals(*this, other, tol); }
+
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+  }
 };
 
 // This is the default Testable interface for *non*Testable elements, which
diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 57b5bb4cc..cb08871d9 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -20,6 +20,8 @@
 
 #include <vector>
 #include <boost/pool/pool_alloc.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
 
 namespace gtsam {
 
@@ -56,6 +58,14 @@ public:
   /** Copy constructor from the base map class */
   FastVector(const Base& x) : Base(x) {}
 
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+  }
+
 };
 
 }
diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp
index 211a1c319..e144b3576 100644
--- a/gtsam/base/tests/testSerializationBase.cpp
+++ b/gtsam/base/tests/testSerializationBase.cpp
@@ -18,6 +18,10 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/FastList.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/base/FastSet.h>
+#include <gtsam/base/FastVector.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
@@ -26,6 +30,53 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
+Vector v1 = Vector_(2, 1.0, 2.0);
+Vector v2 = Vector_(2, 3.0, 4.0);
+Vector v3 = Vector_(2, 5.0, 6.0);
+
+/* ************************************************************************* */
+TEST (Serialization, FastList) {
+  FastList<Vector> list;
+  list.push_back(v1);
+  list.push_back(v2);
+  list.push_back(v3);
+
+  EXPECT(equality(list));
+  EXPECT(equalityXML(list));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, FastMap) {
+  FastMap<int, Vector> map;
+  map.insert(make_pair(1, v1));
+  map.insert(make_pair(2, v2));
+  map.insert(make_pair(3, v3));
+
+  EXPECT(equality(map));
+  EXPECT(equalityXML(map));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, FastSet) {
+  FastSet<double> set;
+  set.insert(1.0);
+  set.insert(2.0);
+  set.insert(3.0);
+
+  EXPECT(equality(set));
+  EXPECT(equalityXML(set));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, FastVector) {
+  FastVector<Vector> vector;
+  vector.push_back(v1);
+  vector.push_back(v2);
+  vector.push_back(v3);
+
+  EXPECT(equality(vector));
+  EXPECT(equalityXML(vector));
+}
 
 /* ************************************************************************* */
 TEST (Serialization, matrix_vector) {
@@ -36,6 +87,9 @@ TEST (Serialization, matrix_vector) {
   EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
 }
 
+/* ************************************************************************* */
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From 263b50d85a1d627e38d2987d6aa67895713f5268 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 10 Feb 2012 15:56:01 +0000
Subject: [PATCH 50/88] Using FastVector instead of vector in most code called
 during elimination and solving

---
 gtsam/base/FastVector.h                    | 19 ++++++++++--
 gtsam/base/Lie.h                           |  2 +-
 gtsam/inference/BayesTree.h                |  4 +--
 gtsam/inference/BayesTreeCliqueBase-inl.h  |  6 ++--
 gtsam/inference/ClusterTree.h              | 11 +++----
 gtsam/inference/Conditional.h              | 12 ++++++--
 gtsam/inference/EliminationTree-inl.h      | 10 +++----
 gtsam/inference/EliminationTree.h          |  4 +--
 gtsam/inference/Factor.h                   | 21 ++++++++-----
 gtsam/inference/FactorGraph-inl.h          |  4 +--
 gtsam/inference/FactorGraph.h              |  2 +-
 gtsam/inference/IndexConditional.h         | 11 +++++++
 gtsam/inference/IndexFactor.h              |  6 ++++
 gtsam/inference/JunctionTree-inl.h         |  6 ++--
 gtsam/inference/JunctionTree.h             |  4 +--
 gtsam/inference/Permutation.h              |  8 ++---
 gtsam/inference/SymbolicFactorGraph.cpp    |  6 ++--
 gtsam/inference/SymbolicFactorGraph.h      |  2 +-
 gtsam/inference/inference.cpp              |  6 ++--
 gtsam/inference/inference.h                |  7 +++--
 gtsam/inference/tests/testJunctionTree.cpp | 16 +++++-----
 gtsam/linear/GaussianConditional.h         |  2 +-
 gtsam/linear/GaussianFactor.h              |  7 +++--
 gtsam/linear/GaussianFactorGraph.cpp       | 14 ++++-----
 gtsam/linear/GaussianJunctionTree.cpp      |  4 +--
 gtsam/linear/HessianFactor.cpp             |  4 +--
 gtsam/linear/HessianFactor.h               |  2 +-
 gtsam/linear/JacobianFactor.cpp            | 12 ++++----
 gtsam/linear/JacobianFactor.h              | 10 +++----
 gtsam/linear/NoiseModel.cpp                |  6 ++--
 gtsam/linear/NoiseModel.h                  |  9 +++---
 gtsam/nonlinear/GaussianISAM2-inl.h        |  2 +-
 gtsam/nonlinear/ISAM2-impl-inl.h           |  4 +--
 gtsam/nonlinear/ISAM2-inl.h                |  2 +-
 gtsam/nonlinear/NonlinearFactor.h          | 34 +++++++---------------
 tests/testGaussianJunctionTree.cpp         | 32 ++++++++++----------
 36 files changed, 173 insertions(+), 138 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index cb08871d9..18af63006 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -50,14 +50,29 @@ public:
 
   /** Constructor from a range, passes through to base class */
   template<typename INPUTITERATOR>
-  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
+  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
+    // This if statement works around a bug in boost pool allocator and/or
+    // STL vector where if the size is zero, the pool allocator will allocate
+    // huge amounts of memory.
+    if(first != last)
+      Base::assign(first, last);
+  }
 
   /** Copy constructor from another FastSet */
   FastVector(const FastVector<VALUE>& x) : Base(x) {}
 
-  /** Copy constructor from the base map class */
+  /** Copy constructor from the base class */
   FastVector(const Base& x) : Base(x) {}
 
+  /** Copy constructor from a standard STL container */
+  FastVector(const std::vector<VALUE>& x) {
+    // This if statement works around a bug in boost pool allocator and/or
+    // STL vector where if the size is zero, the pool allocator will allocate
+    // huge amounts of memory.
+    if(x.size() > 0)
+      Base::assign(x.begin(), x.end());
+  }
+
 private:
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index bc530a9f5..a770d9975 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
 template <class T>
 T expm(const Vector& x, int K=7) {
 	Matrix xhat = wedge<T>(x);
-	return expm(xhat,K);
+	return T(expm(xhat,K));
 }
 
 } // namespace gtsam
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 87788e3d8..3dc73519a 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -297,7 +297,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CONDITIONAL, class CLIQUE>
   void _BayesTree_dim_adder(
-      std::vector<size_t>& dims,
+      FastVector<size_t>& dims,
       const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
 
     if(clique) {
@@ -316,7 +316,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class CONDITIONAL,class CLIQUE>
 	boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
-	  std::vector<size_t> dimensions(bt.nodes().size(), 0);
+	  FastVector<size_t> dimensions(bt.nodes().size(), 0);
 	  _BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
 	  return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
 	}
diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h
index dfc8864f7..1b4d92cae 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inl.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inl.h
@@ -223,7 +223,7 @@ namespace gtsam {
 
     assertInvariants();
     GenericSequentialSolver<FactorType> solver(p_FSR);
-    return *solver.jointFactorGraph(conditional_->keys(), function);
+    return *solver.jointFactorGraph(vector<Index>(conditional_->keys().begin(), conditional_->keys().end()), function);
   }
 
   /* ************************************************************************* */
@@ -243,8 +243,8 @@ namespace gtsam {
     joint.push_back(R->conditional()->toFactor()); // P(R)
 
     // Find the keys of both C1 and C2
-    std::vector<Index> keys1(conditional_->keys());
-    std::vector<Index> keys2(C2->conditional_->keys());
+    const FastVector<Index>& keys1(conditional_->keys());
+    const FastVector<Index>& keys2(C2->conditional_->keys());
     FastSet<Index> keys12;
     keys12.insert(keys1.begin(), keys1.end());
     keys12.insert(keys2.begin(), keys2.end());
diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h
index 7544587c1..9e82aae18 100644
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@@ -20,12 +20,13 @@
 #pragma once
 
 #include <list>
-#include <vector>
 
 #include <boost/shared_ptr.hpp>
 #include <boost/weak_ptr.hpp>
 
 #include <gtsam/base/types.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/FastList.h>
 
 namespace gtsam {
 
@@ -46,13 +47,13 @@ namespace gtsam {
 			typedef typename boost::shared_ptr<Cluster> shared_ptr;
 			typedef typename boost::weak_ptr<Cluster> weak_ptr;
 
-      const std::vector<Index> frontal;                   // the frontal variables
-      const std::vector<Index> separator;                // the separator variables
+      const FastVector<Index> frontal;                   // the frontal variables
+      const FastVector<Index> separator;                // the separator variables
 
 		protected:
 
 			weak_ptr parent_;                      // the parent cluster
-			std::list<shared_ptr> children_;     // the child clusters
+			FastList<shared_ptr> children_;     // the child clusters
 			const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent
 
 		public:
@@ -82,7 +83,7 @@ namespace gtsam {
 			bool equals(const Cluster& other) const;
 
 			/// get a reference to the children
-			const std::list<shared_ptr>& children() const { return children_; }
+			const FastList<shared_ptr>& children() const { return children_; }
 
 			/// add a child
 			void addChild(shared_ptr child);
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index 2fa5f691d..a2f5bd7a1 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -45,8 +45,8 @@ private:
 
 	/** Create keys by adding key in front */
 	template<typename ITERATOR>
-	static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
-		std::vector<Key> keys((lastParent - firstParent) + 1);
+	static FastVector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
+		FastVector<Key> keys((lastParent - firstParent) + 1);
 		std::copy(firstParent, lastParent, keys.begin() + 1);
 		keys[0] = key;
 		return keys;
@@ -116,8 +116,14 @@ public:
 		assertInvariants();
 	}
 
+  /** Constructor from a frontal variable and a vector of parents */
+  Conditional(Key key, const FastVector<Key>& parents) :
+    FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
+    assertInvariants();
+  }
+
   /** Constructor from keys and nr of frontal variables */
-	Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
+	Conditional(const FastVector<Index>& keys, size_t nrFrontals) :
 		FactorType(keys), nrFrontals_(nrFrontals) {
 		assertInvariants();
 	}
diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h
index 462a2fd4a..002a5639c 100644
--- a/gtsam/inference/EliminationTree-inl.h
+++ b/gtsam/inference/EliminationTree-inl.h
@@ -58,7 +58,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
 
 /* ************************************************************************* */
 template<class FACTOR>
-vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
+FastVector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
 
   // Number of factors and variables
   const size_t m = structure.nFactors();
@@ -67,8 +67,8 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& struc
   static const Index none = numeric_limits<Index>::max();
 
   // Allocate result parent vector and vector of last factor columns
-  vector<Index> parents(n, none);
-  vector<Index> prevCol(m, none);
+  FastVector<Index> parents(n, none);
+  FastVector<Index> prevCol(m, none);
 
   // for column j \in 1 to n do
   for (Index j = 0; j < n; j++) {
@@ -100,7 +100,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   tic(1, "ET ComputeParents");
   // Compute the tree structure
-  vector<Index> parents(ComputeParents(structure));
+  FastVector<Index> parents(ComputeParents(structure));
   toc(1, "ET ComputeParents");
 
   // Number of variables
@@ -110,7 +110,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   // Create tree structure
   tic(2, "assemble tree");
-  vector<shared_ptr> trees(n);
+  FastVector<shared_ptr> trees(n);
   for (Index k = 1; k <= n; k++) {
     Index j = n - k;  // Start at the last variable and loop down to 0
     trees[j].reset(new EliminationTree(j));  // Create a new node on this variable
diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h
index 4d0ccce56..4dab46dd3 100644
--- a/gtsam/inference/EliminationTree.h
+++ b/gtsam/inference/EliminationTree.h
@@ -54,7 +54,7 @@ private:
 
   typedef FastList<sharedFactor> Factors;
   typedef FastList<shared_ptr> SubTrees;
-  typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
+  typedef FastVector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
 
   Index key_; ///< index associated with root
   Factors factors_; ///< factors associated with root
@@ -74,7 +74,7 @@ private:
    * Static internal function to build a vector of parent pointers using the
    * algorithm of Gilbert et al., 2001, BIT.
    */
-  static std::vector<Index> ComputeParents(const VariableIndex& structure);
+  static FastVector<Index> ComputeParents(const VariableIndex& structure);
 
   /** add a factor, for Create use only */
   void add(const sharedFactor& factor) { factors_.push_back(factor); }
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 5a0b1e333..f51276ddc 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -22,10 +22,10 @@
 #pragma once
 
 #include <set>
-#include <vector>
 #include <boost/serialization/nvp.hpp>
 #include <boost/foreach.hpp>
-#include <gtsam/base/FastMap.h>
+
+#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -68,15 +68,15 @@ public:
   typedef boost::shared_ptr<Factor> shared_ptr;
 
   /// Iterator over keys
-  typedef typename std::vector<Key>::iterator iterator;
+  typedef typename FastVector<Key>::iterator iterator;
 
   /// Const iterator over keys
-  typedef typename std::vector<Key>::const_iterator const_iterator;
+  typedef typename FastVector<Key>::const_iterator const_iterator;
 
 protected:
 
   /// The keys involved in this factor
-  std::vector<Key> keys_;
+  FastVector<Key> keys_;
 
   friend class JacobianFactor;
   friend class HessianFactor;
@@ -132,6 +132,11 @@ public:
 		assertInvariants();
 	}
 
+  /** Construct n-way factor */
+  Factor(const FastVector<Key>& keys) : keys_(keys) {
+    assertInvariants();
+  }
+
   /** Constructor from a collection of keys */
   template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
         keys_(beginKey, endKey) { assertInvariants(); }
@@ -165,8 +170,8 @@ public:
   /// find
   const_iterator find(Key key) const { return std::find(begin(), end(), key); }
 
-  ///TODO: comment
-  const std::vector<Key>& keys() const { return keys_; }
+  /// @return keys involved in this factor
+  const FastVector<Key>& keys() const { return keys_; }
 
   /** iterators */
   const_iterator begin() const { return keys_.begin(); }	///TODO: comment
@@ -194,7 +199,7 @@ public:
   /**
    * @return keys involved in this factor
    */
-  std::vector<Key>& keys() { return keys_; }
+  FastVector<Key>& keys() { return keys_; }
 
   /** mutable iterators */
   iterator begin() { return keys_.begin(); }	///TODO: comment
diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index 337b571c6..6244c959a 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -104,8 +104,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class DERIVED, class KEY>
 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
-			const FastMap<KEY, std::vector<KEY> >& variableSlots) {
-		typedef const pair<const KEY, std::vector<KEY> > KeySlotPair;
+			const FastMap<KEY, FastVector<KEY> >& variableSlots) {
+		typedef const pair<const KEY, FastVector<KEY> > KeySlotPair;
 		return typename DERIVED::shared_ptr(new DERIVED(
 		    boost::make_transform_iterator(variableSlots.begin(), boost::bind(&KeySlotPair::first, _1)),
 		    boost::make_transform_iterator(variableSlots.end(), boost::bind(&KeySlotPair::first, _1))));
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 458588b7b..fcbd02a29 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -232,7 +232,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
   /** Create a combined joint factor (new style for EliminationTree). */
 	template<class DERIVED, class KEY>
 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
-			const FastMap<KEY, std::vector<KEY> >& variableSlots);
+			const FastMap<KEY, FastVector<KEY> >& variableSlots);
 
 	/**
    * static function that combines two factor graphs
diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h
index c1cbbdaac..e8cb3f9c1 100644
--- a/gtsam/inference/IndexConditional.h
+++ b/gtsam/inference/IndexConditional.h
@@ -74,12 +74,23 @@ namespace gtsam {
 			assertInvariants();
 		}
 
+    /** Constructor from a frontal variable and a vector of parents (FastVector version) */
+    IndexConditional(Index j, const FastVector<Index>& parents) : Base(j, parents) {
+      assertInvariants();
+    }
+
     /** Constructor from keys and nr of frontal variables */
 		IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) :
 			Base(keys, nrFrontals) {
 			assertInvariants();
 		}
 
+    /** Constructor from keys and nr of frontal variables (FastVector version) */
+    IndexConditional(const FastVector<Index>& keys, size_t nrFrontals) :
+      Base(keys, nrFrontals) {
+      assertInvariants();
+    }
+
 		/// @}
 		/// @name Standard Interface
 		/// @{
diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h
index 9b2d39727..32488a9d5 100644
--- a/gtsam/inference/IndexFactor.h
+++ b/gtsam/inference/IndexFactor.h
@@ -114,6 +114,12 @@ namespace gtsam {
 			assertInvariants();
 		}
 
+    /** Construct n-way factor (FastVector version) */
+    IndexFactor(const FastVector<Index>& js) :
+      Base(js) {
+      assertInvariants();
+    }
+
 		/** Constructor from a collection of keys */
 		template<class KeyIterator> IndexFactor(KeyIterator beginKey,
 				KeyIterator endKey) :
diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h
index ad8b389ab..739b51048 100644
--- a/gtsam/inference/JunctionTree-inl.h
+++ b/gtsam/inference/JunctionTree-inl.h
@@ -83,7 +83,7 @@ namespace gtsam {
 
     // Two stages - first build an array of the lowest-ordered variable in each
     // factor and find the last variable to be eliminated.
-    vector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
+    FastVector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
     Index maxVar = 0;
     for(size_t i=0; i<fg.size(); ++i)
       if(fg[i]) {
@@ -96,7 +96,7 @@ namespace gtsam {
 
     // Now add each factor to the list corresponding to its lowest-ordered
     // variable.
-    vector<FastList<size_t> > targets(maxVar+1);
+    FastVector<FastList<size_t> > targets(maxVar+1);
     for(size_t i=0; i<lowestOrdered.size(); ++i)
       if(lowestOrdered[i] != numeric_limits<Index>::max())
         targets[lowestOrdered[i]].push_back(i);
@@ -108,7 +108,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class FG, class BTCLIQUE>
   typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
-      const std::vector<FastList<size_t> >& targets,
+      const FastVector<FastList<size_t> >& targets,
       const SymbolicBayesTree::sharedClique& bayesClique) {
 
     if(bayesClique) {
diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h
index 33a9aa5e4..a068be991 100644
--- a/gtsam/inference/JunctionTree.h
+++ b/gtsam/inference/JunctionTree.h
@@ -20,7 +20,7 @@
 #pragma once
 
 #include <set>
-#include <vector>
+#include <gtsam/base/FastVector.h>
 #include <gtsam/base/FastList.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/ClusterTree.h>
@@ -78,7 +78,7 @@ namespace gtsam {
 				const SymbolicBayesTree::sharedClique& clique);
 
 		/// distribute the factors along the cluster tree
-    sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
+    sharedClique distributeFactors(const FG& fg, const FastVector<FastList<size_t> >& targets,
         const SymbolicBayesTree::sharedClique& clique);
 
 		/// recursive elimination function
diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h
index 5cbd8e187..bde6dd459 100644
--- a/gtsam/inference/Permutation.h
+++ b/gtsam/inference/Permutation.h
@@ -17,12 +17,12 @@
 
 #pragma once
 
-#include <vector>
 #include <string>
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 
 #include <gtsam/base/types.h>
+#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -46,12 +46,12 @@ class Inference;
  */
 class Permutation {
 protected:
-  std::vector<Index> rangeIndices_;
+  FastVector<Index> rangeIndices_;
 
 public:
   typedef boost::shared_ptr<Permutation> shared_ptr;
-  typedef std::vector<Index>::const_iterator const_iterator;
-  typedef std::vector<Index>::iterator iterator;
+  typedef FastVector<Index>::const_iterator const_iterator;
+  typedef FastVector<Index>::iterator iterator;
 
 	/// @name Standard Constructors
 	/// @{
diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp
index 2d5190f45..233c1e21c 100644
--- a/gtsam/inference/SymbolicFactorGraph.cpp
+++ b/gtsam/inference/SymbolicFactorGraph.cpp
@@ -66,7 +66,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-					std::vector<Index> >& variableSlots) {
+			FastVector<Index> >& variableSlots) {
 		IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors,
 				variableSlots));
 //		combined->assertInvariants();
@@ -84,9 +84,11 @@ namespace gtsam {
 
 		if (keys.size() < 1) throw invalid_argument(
 				"IndexFactor::CombineAndEliminate called on factors with no variables.");
+		if(nrFrontals > keys.size()) throw invalid_argument(
+		    "Requesting to eliminate more variables than are present in the factors");
 
 		pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result;
-		std::vector<Index> newKeys(keys.begin(),keys.end());
+		FastVector<Index> newKeys(keys.begin(),keys.end());
     result.first.reset(new IndexConditional(newKeys, nrFrontals));
 		result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));
 
diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h
index 4da2baf13..e39e4a70b 100644
--- a/gtsam/inference/SymbolicFactorGraph.h
+++ b/gtsam/inference/SymbolicFactorGraph.h
@@ -84,7 +84,7 @@ namespace gtsam {
 	/** Create a combined joint factor (new style for EliminationTree). */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-					std::vector<Index> >& variableSlots);
+			FastVector<Index> >& variableSlots);
 
 	/**
 	 * CombineAndEliminate provides symbolic elimination.
diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp
index d30ea79c4..8ec67b26c 100644
--- a/gtsam/inference/inference.cpp
+++ b/gtsam/inference/inference.cpp
@@ -29,12 +29,12 @@ using namespace std;
 
 namespace gtsam {
 
-Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
+Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember) {
   size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
   // Convert to compressed column major format colamd wants it in (== MATLAB format!)
   int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
-  vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
-  vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
+  vector<int> A(Alen); /* colamd arg 4: row indices of A, of size Alen */
+  vector<int> p(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
 
   static const bool debug = false;
 
diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h
index d853ff91b..728d2c4bd 100644
--- a/gtsam/inference/inference.h
+++ b/gtsam/inference/inference.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/FastVector.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/Permutation.h>
 
@@ -48,7 +49,7 @@ namespace gtsam {
 	  /**
 	   * Compute a CCOLAMD permutation using the constraint groups in cmember.
 	   */
-    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
+    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember);
 
 	};
 
@@ -56,7 +57,7 @@ namespace gtsam {
 	template<typename CONSTRAINED>
 	Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
 
-	  std::vector<int> cmember(variableIndex.size(), 0);
+	  FastVector<int> cmember(variableIndex.size(), 0);
 
 	  // If at least some variables are not constrained to be last, constrain the
 	  // ones that should be constrained.
@@ -72,7 +73,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
-	  std::vector<int> cmember(variableIndex.size(), 0);
+	  FastVector<int> cmember(variableIndex.size(), 0);
 	  return PermutationCOLAMD_(variableIndex, cmember);
 	}
 
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 7374bbad8..9695afc56 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -56,15 +56,15 @@ TEST( JunctionTree, constructor )
 
 	SymbolicJunctionTree actual(fg);
 
-	vector<Index> frontal1; frontal1 += x3, x4;
-	vector<Index> frontal2; frontal2 += x2, x1;
-	vector<Index> sep1;
-	vector<Index> sep2; sep2 += x3;
-	CHECK(assert_equal(frontal1, actual.root()->frontal));
-	CHECK(assert_equal(sep1,     actual.root()->separator));
+	FastVector<Index> frontal1; frontal1 += x3, x4;
+	FastVector<Index> frontal2; frontal2 += x2, x1;
+	FastVector<Index> sep1;
+	FastVector<Index> sep2; sep2 += x3;
+	CHECK(assert_container_equality(frontal1, actual.root()->frontal));
+	CHECK(assert_container_equality(sep1,     actual.root()->separator));
 	LONGS_EQUAL(1,               actual.root()->size());
-	CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal));
-	CHECK(assert_equal(sep2,     actual.root()->children().front()->separator));
+	CHECK(assert_container_equality(frontal2, actual.root()->children().front()->frontal));
+	CHECK(assert_container_equality(sep2,     actual.root()->children().front()->separator));
 	LONGS_EQUAL(2,               actual.root()->children().front()->size());
 	CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
   CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index f2a77c894..af83a6f44 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -268,7 +268,7 @@ template<typename ITERATOR, class MATRIX>
 GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
 		size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
 		const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) :
-	IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_(
+	IndexConditional(FastVector<Index>(firstKey, lastKey), nrFrontals), rsd_(
 			matrix_), sigmas_(sigmas), permutation_(permutation) {
 	rsd_.assignNoalias(matrices);
 }
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index 2d889ee64..771a3de8f 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -74,6 +74,9 @@ namespace gtsam {
     /** Construct n-way factor */
     GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {}
 
+    /** Construct n-way factor */
+    GaussianFactor(const FastVector<Index>& js) : IndexFactor(js) {}
+
   public:
 
     typedef GaussianConditional ConditionalType;
@@ -110,8 +113,8 @@ namespace gtsam {
 
   /** make keys from list, vector, or map of matrices */
 	template<typename ITERATOR>
-	static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
-		std::vector<Index> keys;
+	static FastVector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
+	  FastVector<Index> keys;
 		keys.reserve(n);
 		for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
 		return keys;
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 71d111149..7efeea10d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -168,11 +168,11 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	// Helper functions for Combine
-	static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
+	static boost::tuple<FastVector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
 #ifndef NDEBUG
-		vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
+	  FastVector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
 #else
-		vector<size_t> varDims(variableSlots.size());
+	  FastVector<size_t> varDims(variableSlots.size());
 #endif
 		size_t m = 0;
 		size_t n = 0;
@@ -221,7 +221,7 @@ break;
 
 		if (debug) cout << "Determine dimensions" << endl;
 		tic(1, "countDims");
-		vector<size_t> varDims;
+		FastVector<size_t> varDims;
 		size_t m, n;
 		boost::tie(varDims, m, n) = countDims(factors, variableSlots);
 		if (debug) {
@@ -233,7 +233,7 @@ break;
 
 		if (debug) cout << "Sort rows" << endl;
 		tic(2, "sort rows");
-		vector<JacobianFactor::_RowSource> rowSources;
+		FastVector<JacobianFactor::_RowSource> rowSources;
 		rowSources.reserve(m);
 		bool anyConstrained = false;
 		for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
@@ -367,7 +367,7 @@ break;
 
 		// Pull out keys and dimensions
 		tic(2, "keys");
-		vector<size_t> dimensions(scatter.size() + 1);
+		FastVector<size_t> dimensions(scatter.size() + 1);
 		BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
 			dimensions[var_slot.second.slot] = var_slot.second.dimension;
 		}
@@ -569,7 +569,7 @@ break;
 
 		// Pull out keys and dimensions
 		tic(2, "keys");
-		vector<size_t> dimensions(scatter.size() + 1);
+		FastVector<size_t> dimensions(scatter.size() + 1);
 		BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
 			dimensions[var_slot.second.slot] = var_slot.second.dimension;
 		}
diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp
index e2e1c2951..0db558a10 100644
--- a/gtsam/linear/GaussianJunctionTree.cpp
+++ b/gtsam/linear/GaussianJunctionTree.cpp
@@ -21,8 +21,6 @@
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
 
-#include <vector>
-
 #include <boost/foreach.hpp>
 
 namespace gtsam {
@@ -68,7 +66,7 @@ namespace gtsam {
 
 		// Allocate solution vector and copy RHS
     tic(2, "allocate VectorValues");
-    vector<size_t> dims(rootClique->conditional()->back()+1, 0);
+    FastVector<size_t> dims(rootClique->conditional()->back()+1, 0);
 		countDims(rootClique, dims);
 		VectorValues result(dims);
 		btreeRHS(rootClique, result);
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 8402e4f91..eedecc4ed 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
-		const vector<size_t>& dimensions, const Scatter& scatter) :
+		const FastVector<size_t>& dimensions, const Scatter& scatter) :
 		info_(matrix_) {
 
 	const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL");
@@ -505,7 +505,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
   tic(2, "remaining factor");
   info_.blockStart() = nrFrontals;
   // Assign the keys
-  vector<Index> remainingKeys(keys_.size() - nrFrontals);
+  FastVector<Index> remainingKeys(keys_.size() - nrFrontals);
   remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
   keys_.swap(remainingKeys);
   toc(2, "remaining factor");
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 2a16c0e38..6225fcde9 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -204,7 +204,7 @@ namespace gtsam {
 
     /** Special constructor used in EliminateCholesky which combines the given factors */
     HessianFactor(const FactorGraph<GaussianFactor>& factors,
-				const std::vector<size_t>& dimensions, const Scatter& scatter);
+				const FastVector<size_t>& dimensions, const Scatter& scatter);
 
     /** Destructor */
 		virtual ~HessianFactor() {}
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 0218ecc8d..064143512 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -113,7 +113,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
+  JacobianFactor::JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
   		const Vector &b, const SharedDiagonal& model) :
   	GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
 		model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
@@ -260,7 +260,7 @@ namespace gtsam {
       sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
 
     // Build a vector of variable dimensions in the new order
-    vector<size_t> dimensions(size() + 1);
+    FastVector<size_t> dimensions(size() + 1);
     size_t j = 0;
     BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
       dimensions[j++] = Ab_(sourceSlot.second).cols();
@@ -269,7 +269,7 @@ namespace gtsam {
     dimensions.back() = 1;
 
     // Copy the variables and matrix into the new order
-    vector<Index> oldKeys(size());
+    FastVector<Index> oldKeys(size());
     keys_.swap(oldKeys);
     AbMatrix oldMatrix;
     BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
@@ -506,7 +506,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const {
+  void JacobianFactor::collectInfo(size_t index, FastVector<_RowSource>& rowSources) const {
 		assertInvariants();
 		for (size_t row = 0; row < rows(); ++row) {
 			Index firstNonzeroVar;
@@ -522,7 +522,7 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  void JacobianFactor::allocate(const VariableSlots& variableSlots, vector<
+  void JacobianFactor::allocate(const VariableSlots& variableSlots, FastVector<
 			size_t>& varDims, size_t m) {
 		keys_.resize(variableSlots.size());
 		std::transform(variableSlots.begin(), variableSlots.end(), begin(),
@@ -551,7 +551,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void JacobianFactor::copyFNZ(size_t m, size_t n,
-			vector<_RowSource>& rowSources) {
+      FastVector<_RowSource>& rowSources) {
 		Index i = 0;
 		for (size_t row = 0; row < m; ++row) {
 			while (i < n && rowSources[row].firstNonzeroVar > keys_[i])
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 079cb9292..90270ce95 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -85,7 +85,7 @@ namespace gtsam {
   protected:
 
     SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
-    std::vector<size_t> firstNonzeroBlocks_;
+    FastVector<size_t> firstNonzeroBlocks_;
     AbMatrix matrix_; // the full matrix corresponding to the factor
     BlockAb Ab_;      // the block view of the full matrix
     typedef GaussianFactor Base; // typedef to base
@@ -123,7 +123,7 @@ namespace gtsam {
         const Vector& b, const SharedDiagonal& model);
 
     /** Construct an n-ary factor */
-    JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
+    JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
         const Vector &b, const SharedDiagonal& model);
 
     JacobianFactor(const std::list<std::pair<Index, Matrix> > &terms,
@@ -268,18 +268,18 @@ namespace gtsam {
     // Many imperative, perhaps all need to be combined in constructor
 
     /** Collect information on Jacobian rows */
-    void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const;
+    void collectInfo(size_t index, FastVector<_RowSource>& rowSources) const;
 
     /** allocate space */
     void allocate(const VariableSlots& variableSlots,
-				std::vector<size_t>& varDims, size_t m);
+        FastVector<size_t>& varDims, size_t m);
 
     /** copy a slot from source */
     void copyRow(const JacobianFactor& source,
     		Index sourceRow, size_t sourceSlot, size_t row, Index slot);
 
     /** copy firstNonzeroBlocks structure */
-    void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources);
+    void copyFNZ(size_t m, size_t n, FastVector<_RowSource>& rowSources);
 
     /** set noiseModel correctly */
   	void setModel(bool anyConstrained, const Vector& sigmas);
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index fd93d39fc..37584af2e 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -163,7 +163,7 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
   return Unit::Create(maxrank);
 }
 
-void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
+void Gaussian::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
   BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
   whitenInPlace(b);
 }
@@ -513,7 +513,7 @@ Vector Base::sqrtWeight(const Vector &error) const {
  * according to their weight implementation */
 
 /** Reweight n block matrices with one error vector */
-void Base::reweight(vector<Matrix> &A, Vector &error) const {
+void Base::reweight(FastVector<Matrix> &A, Vector &error) const {
   if ( reweight_ == Block ) {
     const double w = sqrtWeight(error.norm());
     BOOST_FOREACH(Matrix& Aj, A) {
@@ -662,7 +662,7 @@ bool Robust::equals(const Base& expected, double tol) const {
   return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
 }
 
-void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
+void Robust::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
   noise_->WhitenSystem(A,b);
   robust_->reweight(A,b);
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index ee5d9e00c..640eadfba 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -20,6 +20,7 @@
 
 #include <boost/serialization/nvp.hpp>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -76,7 +77,7 @@ namespace gtsam {
 
       virtual double distance(const Vector& v) const = 0;
 
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
+      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
@@ -185,7 +186,7 @@ namespace gtsam {
 			/**
 			 * Whiten a system, in place as well
 			 */
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const ;
+      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const ;
 			virtual void WhitenSystem(Matrix& A, Vector& b) const ;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
@@ -619,7 +620,7 @@ namespace gtsam {
       Vector sqrtWeight(const Vector &error) const;
 
       /** reweight block matrices and a vector according to their weight implementation */
-      void reweight(std::vector<Matrix> &A, Vector &error) const;
+      void reweight(FastVector<Matrix> &A, Vector &error) const;
 		  void reweight(Matrix &A, Vector &error) const;
 		  void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
 		  void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
@@ -714,7 +715,7 @@ namespace gtsam {
 
       // TODO: these are really robust iterated re-weighting support functions
 
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
+      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const;
       virtual void WhitenSystem(Matrix& A, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h
index d0d37d886..b90b2c4ea 100644
--- a/gtsam/nonlinear/GaussianISAM2-inl.h
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -57,7 +57,7 @@ namespace gtsam {
     if(recalculate) {
 
       // Temporary copy of the original values, to check how much they change
-      vector<Vector> originalValues((*clique)->nrFrontals());
+      FastVector<Vector> originalValues((*clique)->nrFrontals());
       GaussianConditional::const_iterator it;
       for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
         originalValues[it - (*clique)->beginFrontals()] = delta[*it];
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index eae39bf52..4858b1900 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -126,7 +126,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
   theta.insert(newTheta);
   if(debug) newTheta.print("The new variables are: ");
   // Add the new keys onto the ordering, add zeros to the delta for the new variables
-  std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
+  FastVector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
   const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
   const size_t originalDim = delta->dim();
@@ -287,7 +287,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
   if(debug) affectedFactorsIndex.print("affectedFactorsIndex: ");
   toc(2,"variable index");
   tic(3,"ccolamd");
-  vector<int> cmember(affectedKeysSelector.size(), 0);
+  FastVector<int> cmember(affectedKeysSelector.size(), 0);
   if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
     assert(reorderingMode.constrainedKeys);
     if(keys.size() > reorderingMode.constrainedKeys->size()) {
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 5c87d1dda..6dc47131f 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -229,7 +229,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
     tic(1,"reorder");
     tic(1,"CCOLAMD");
     // Do a batch step - reorder and relinearize all variables
-    vector<int> cmember(theta_.size(), 0);
+    FastVector<int> cmember(theta_.size(), 0);
     FastSet<Index> constrainedKeysSet;
     if(constrainKeys)
       constrainedKeysSet = *constrainKeys;
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 166ad6980..512d7af91 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -24,7 +24,6 @@
 #include <limits>
 
 #include <boost/serialization/base_object.hpp>
-#include <boost/tuple/tuple.hpp>
 
 #include <gtsam/inference/Factor-inl.h>
 #include <gtsam/inference/IndexFactor.h>
@@ -36,19 +35,6 @@
 
 namespace gtsam {
 
-using boost::make_tuple;
-
-// Helper function to fill a vector from a tuple function of any length
-template<typename CONS>
-inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
-  vector[position] = tuple.get_head();
-  __fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
-}
-template<>
-inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
-  // Do nothing
-}
-
 /* ************************************************************************* */
 /**
  * Nonlinear factor base class
@@ -139,7 +125,7 @@ public:
    * variable indices.
    */
   virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
-    std::vector<Index> indices(this->size());
+    FastVector<Index> indices(this->size());
     for(size_t j=0; j<this->size(); ++j)
       indices[j] = ordering[this->keys()[j]];
     return IndexFactor::shared_ptr(new IndexFactor(indices));
@@ -228,7 +214,7 @@ public:
    * If any of the optional Matrix reference arguments are specified, it should compute
    * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
    */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const = 0;
 
   /**
    * Vector of errors, whitened
@@ -264,12 +250,12 @@ public:
     // Create the set of terms - Jacobians for each index
     Vector b;
     // Call evaluate error to get Jacobians and b vector
-    std::vector<Matrix> A(this->size());
+    FastVector<Matrix> A(this->size());
     b = -unwhitenedError(x, A);
 
     this->noiseModel_->WhitenSystem(A,b);
 
-    std::vector<std::pair<Index, Matrix> > terms(this->size());
+    FastVector<std::pair<Index, Matrix> > terms(this->size());
     // Fill in terms
     for(size_t j=0; j<this->size(); ++j) {
       terms[j].first = ordering[this->keys()[j]];
@@ -339,7 +325,7 @@ public:
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X& x1 = x.at<X>(keys_[0]);
       if(H) {
@@ -422,7 +408,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X1& x1 = x.at<X1>(keys_[0]);
       const X2& x2 = x.at<X2>(keys_[1]);
@@ -512,7 +498,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
@@ -607,7 +593,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
   			return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -707,7 +693,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -813,7 +799,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index 19b0c676d..24d8e0046 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -59,29 +59,29 @@ TEST( GaussianJunctionTree, constructor2 )
 	// create an ordering
 	GaussianJunctionTree actual(fg);
 
-	vector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
-	vector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
-	vector<Index> frontal3; frontal3 += ordering["x1"];
-	vector<Index> frontal4; frontal4 += ordering["x7"];
-	vector<Index> sep1;
-	vector<Index> sep2; sep2 += ordering["x4"];
-	vector<Index> sep3; sep3 += ordering["x2"];
-	vector<Index> sep4; sep4 += ordering["x6"];
-	EXPECT(assert_equal(frontal1, actual.root()->frontal));
-	EXPECT(assert_equal(sep1,     actual.root()->separator));
+	FastVector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
+	FastVector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
+	FastVector<Index> frontal3; frontal3 += ordering["x1"];
+	FastVector<Index> frontal4; frontal4 += ordering["x7"];
+	FastVector<Index> sep1;
+	FastVector<Index> sep2; sep2 += ordering["x4"];
+	FastVector<Index> sep3; sep3 += ordering["x2"];
+	FastVector<Index> sep4; sep4 += ordering["x6"];
+	EXPECT(assert_container_equality(frontal1, actual.root()->frontal));
+	EXPECT(assert_container_equality(sep1,     actual.root()->separator));
 	LONGS_EQUAL(5,               actual.root()->size());
 	list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
   list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
   GaussianJunctionTree::sharedClique child0 = *child0it;
   GaussianJunctionTree::sharedClique child1 = *child1it;
-	EXPECT(assert_equal(frontal2, child0->frontal));
-	EXPECT(assert_equal(sep2,     child0->separator));
+	EXPECT(assert_container_equality(frontal2, child0->frontal));
+	EXPECT(assert_container_equality(sep2,     child0->separator));
 	LONGS_EQUAL(4,               child0->size());
-	EXPECT(assert_equal(frontal3, child0->children().front()->frontal));
-	EXPECT(assert_equal(sep3,     child0->children().front()->separator));
+	EXPECT(assert_container_equality(frontal3, child0->children().front()->frontal));
+	EXPECT(assert_container_equality(sep3,     child0->children().front()->separator));
 	LONGS_EQUAL(2,               child0->children().front()->size());
-	EXPECT(assert_equal(frontal4, child1->frontal));
-	EXPECT(assert_equal(sep4,     child1->separator));
+	EXPECT(assert_container_equality(frontal4, child1->frontal));
+	EXPECT(assert_container_equality(sep4,     child1->separator));
 	LONGS_EQUAL(2,               child1->size());
 }
 

From fab16dd41f06f951e54f25e900bcff820311b65b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 10 Feb 2012 19:47:26 +0000
Subject: [PATCH 51/88] Missing include

---
 gtsam/base/Testable.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index d8829798b..a1130dfd9 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -35,6 +35,7 @@
 
 #include <boost/shared_ptr.hpp>
 #include <stdio.h>
+#include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
 

From 965417ea1b9cb02c74e120a102c7d87bc12635bf Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 10 Feb 2012 19:47:35 +0000
Subject: [PATCH 52/88] Fixed indentation

---
 gtsam/linear/GaussianFactorGraph.cpp | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 7efeea10d..16c2ce12d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -49,11 +49,10 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	void GaussianFactorGraph::permuteWithInverse(
-			const Permutation& inversePermutation) {
-		BOOST_FOREACH(const sharedFactor& factor, factors_)
-							{
-			factor->permuteWithInverse(inversePermutation);
-							}
+	    const Permutation& inversePermutation) {
+	  BOOST_FOREACH(const sharedFactor& factor, factors_) {
+	    factor->permuteWithInverse(inversePermutation);
+    }
 	}
 
 	/* ************************************************************************* */

From 6a125ed5cb00ae50a36372507ec89ef1f6d2877c Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 10 Feb 2012 19:48:06 +0000
Subject: [PATCH 53/88] Added dim function and removed commented out code

---
 gtsam/nonlinear/Values-inl.h | 6 ------
 gtsam/nonlinear/Values.cpp   | 9 +++++++++
 gtsam/nonlinear/Values.h     | 5 ++++-
 3 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 31d6c318a..7410ddade 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -58,12 +58,6 @@ namespace gtsam {
     }
   };
 
-//  /* ************************************************************************* */
-//  template<class ValueType>
-//  ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) {
-//    lhs = rhs;
-//  }
-
   /* ************************************************************************* */
   template<typename ValueType>
   const ValueType& Values::at(const Symbol& j) const {
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 2d8b72120..44245c39c 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -175,6 +175,15 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  size_t Values::dim() const {
+    size_t result = 0;
+    BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
+      result += key_value.second.dim();
+    }
+    return result;
+  }
+
   /* ************************************************************************* */
   Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
     Ordering::shared_ptr ordering(new Ordering);
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index bad281a9a..e2a69d622 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -255,9 +255,12 @@ namespace gtsam {
     /** Remove all variables from the config */
     void clear() { values_.clear(); }
 
-    /** Create an array of variable dimensions using the given ordering */
+    /** Create an array of variable dimensions using the given ordering (\f$ O(n) \f$) */
     std::vector<size_t> dims(const Ordering& ordering) const;
 
+    /** Compute the total dimensionality of all values (\f$ O(n) \f$) */
+    size_t dim() const;
+
     /**
      * Generate a default ordering, simply in key sort order.  To instead
      * create a fill-reducing ordering, use

From 75d95774e157640b9a4141a99306205208fbf323 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 10 Feb 2012 20:43:00 +0000
Subject: [PATCH 54/88] Removed duplicate header in Makefile.am

---
 gtsam/nonlinear/Makefile.am | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 15651a22a..41b6b3b8d 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -22,7 +22,6 @@ check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
 
 # Nonlinear nonlinear
 headers += Symbol.h 
-headers += NonlinearFactorGraph.h
 headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h 
 headers += NonlinearFactor.h
 sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp

From 3f2ec81e678ba41077efd15ac11d34e0dbc39ff8 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sat, 11 Feb 2012 21:59:35 +0000
Subject: [PATCH 55/88] Fixing serialization test linking

---
 gtsam/base/CMakeLists.txt      | 1 +
 gtsam/geometry/CMakeLists.txt  | 1 +
 gtsam/inference/CMakeLists.txt | 1 +
 gtsam/linear/CMakeLists.txt    | 1 +
 gtsam/nonlinear/CMakeLists.txt | 1 +
 gtsam/slam/CMakeLists.txt      | 1 +
 6 files changed, 6 insertions(+)

diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt
index e6818b787..8ca43e1c2 100644
--- a/gtsam/base/CMakeLists.txt
+++ b/gtsam/base/CMakeLists.txt
@@ -5,6 +5,7 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base)
 # Components to link tests in this subfolder against
 set(base_local_libs
     CppUnitLite
+    ${Boost_LIBRARIES}
     base
 )
 
diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt
index 2b4635638..52a691ea0 100644
--- a/gtsam/geometry/CMakeLists.txt
+++ b/gtsam/geometry/CMakeLists.txt
@@ -9,6 +9,7 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
 set(geometry_local_libs
    geometry
    base
+   ${Boost_LIBRARIES}
    CppUnitLite
 )
 
diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt
index 158ce706e..37986bb8b 100644
--- a/gtsam/inference/CMakeLists.txt
+++ b/gtsam/inference/CMakeLists.txt
@@ -12,6 +12,7 @@ set(inference_local_libs
    base
    ccolamd
    CppUnitLite
+   ${Boost_LIBRARIES}
 )
 
 # Build tests
diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt
index 49438c5c9..899de41c3 100644
--- a/gtsam/linear/CMakeLists.txt
+++ b/gtsam/linear/CMakeLists.txt
@@ -13,6 +13,7 @@ set(linear_local_libs
    base
    ccolamd
    CppUnitLite
+   ${Boost_LIBRARIES}
 )
 
 # Build tests
diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt
index 070d88da4..6d917ffc5 100644
--- a/gtsam/nonlinear/CMakeLists.txt
+++ b/gtsam/nonlinear/CMakeLists.txt
@@ -14,6 +14,7 @@ set(nonlinear_local_libs
    base
    ccolamd
    CppUnitLite
+   ${Boost_LIBRARIES}
 )
 
 # Build tests
diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt
index 7bce4f369..2354aa96c 100644
--- a/gtsam/slam/CMakeLists.txt
+++ b/gtsam/slam/CMakeLists.txt
@@ -15,6 +15,7 @@ set(slam_local_libs
    base
    ccolamd
    CppUnitLite
+   ${Boost_LIBRARIES}
 )
 
 # Build tests

From c17446463a222d7d1aac98d120917e8832af6952 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sat, 11 Feb 2012 22:00:09 +0000
Subject: [PATCH 56/88] pacifying warnings

---
 gtsam/base/Vector.cpp | 25 +++++++------------------
 1 file changed, 7 insertions(+), 18 deletions(-)

diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index 432ae5d77..5367cec09 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -42,15 +42,14 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void odprintf_(const char *format, ostream& stream, ...) {
-	char    buf[4096], *p = buf;
-	int     n;
+	char buf[4096], *p = buf;
 
 	va_list args;
 	va_start(args, stream);
 #ifdef WIN32
-	n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+	_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
 #else
-	n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+	vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
 #endif
 	va_end(args);
 
@@ -63,17 +62,15 @@ void odprintf_(const char *format, ostream& stream, ...) {
 
 /* ************************************************************************* */
 
-void odprintf(const char *format, ...)
-{
-	char    buf[4096], *p = buf;
-	int     n;
+void odprintf(const char *format, ...) {
+	char buf[4096], *p = buf;
 
 	va_list args;
 	va_start(args, format);
 #ifdef WIN32
-	n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+	_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
 #else
-	n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+	vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
 #endif
 	va_end(args);
 
@@ -86,11 +83,6 @@ void odprintf(const char *format, ...)
 
 /* ************************************************************************* */
 Vector Vector_( size_t m, const double* const data) {
-//	Vector v(m);
-//	for (size_t i=0; i<m; ++i)
-//		v(i) = data[i];
-//	return v;
-
   Vector A(m);
   copy(data, data+m, A.data());
   return A;
@@ -133,9 +125,6 @@ Vector repeat(size_t n, double value) {
 /* ************************************************************************* */
 Vector delta(size_t n, size_t i, double value) {
 	return Vector::Unit(n, i) * value;
-//	Vector v = zero(n);
-//	v(i) = value;
-//	return v;
 }
 
 /* ************************************************************************* */

From d0e9b1d51a2c883ebdc028144d247a42c735085b Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sat, 11 Feb 2012 22:38:53 +0000
Subject: [PATCH 57/88] Fixed cmake build problem to allow for excluding test
 files, switched testPose3SLAM to use a different interface to values

---
 gtsam/slam/tests/testPose3SLAM.cpp | 3 ++-
 tests/CMakeLists.txt               | 9 +++++++++
 tests/testPose2SLAMwSPCG.cpp       | 2 --
 3 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 77c206918..4fa815cdc 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -50,7 +50,8 @@ TEST(Pose3Graph, optimizeCircle) {
 	// Create a hexagon of poses
 	double radius = 10;
 	Values hexagon = pose3SLAM::circle(6,radius);
-  Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)];
+//  Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)]; // FAIL: cannot cast ValueAutomaticCasting
+  Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1)); // Works
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose3Graph> fg(new Pose3Graph);
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 240623a22..05c51f4d5 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -12,10 +12,18 @@ else (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
        gtsam-static)
 endif (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
 
+# exclude certain files
+# note the source dir on each 
+set (tests_exclude
+    "${CMAKE_CURRENT_SOURCE_DIR}/testTupleValues.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp"
+)
+
 # Build tests
 if (GTSAM_BUILD_TESTS)
     add_custom_target(check.tests COMMAND ${CMAKE_CTEST_COMMAND})
     file(GLOB tests_srcs "test*.cpp")
+    list(REMOVE_ITEM tests_srcs ${tests_exclude})
     foreach(test_src ${tests_srcs})
         get_filename_component(test_base ${test_src} NAME_WE)
         set( test_bin tests.${test_base} )
@@ -33,6 +41,7 @@ endif (GTSAM_BUILD_TESTS)
 if (GTSAM_BUILD_TIMING)
     add_custom_target(timing.tests)
     file(GLOB timing_srcs "time*.cpp") 
+    list(REMOVE_ITEM timing_srcs ${tests_exclude})
     foreach(time_src ${timing_srcs})
         get_filename_component(time_base ${time_src} NAME_WE)
         set( time_bin tests.${time_base} )
diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp
index 58020051d..439228c79 100644
--- a/tests/testPose2SLAMwSPCG.cpp
+++ b/tests/testPose2SLAMwSPCG.cpp
@@ -14,7 +14,6 @@ using namespace pose2SLAM;
 
 const double tol = 1e-5;
 
-#if ENABLE_SPCG
 /* ************************************************************************* */
 TEST(testPose2SLAMwSPCG, example1) {
 
@@ -67,7 +66,6 @@ TEST(testPose2SLAMwSPCG, example1) {
 
 	EXPECT(assert_equal(expected, actual, tol));
 }
-#endif
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

From 5a3740daebd44494304ec25c01a50cec5dafe923 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sun, 12 Feb 2012 17:41:57 +0000
Subject: [PATCH 58/88] Removed extraneous "shared" prefix from
 SharedNoiseModel named constructors

---
 examples/matlab/PlanarSLAMExample_easy.m    |  6 +++---
 examples/matlab/Pose2SLAMExample_advanced.m |  6 +++---
 examples/matlab/Pose2SLAMExample_easy.m     |  4 ++--
 gtsam.h                                     | 14 +++++++-------
 gtsam/linear/SharedNoiseModel.h             | 14 +++++++-------
 5 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/examples/matlab/PlanarSLAMExample_easy.m b/examples/matlab/PlanarSLAMExample_easy.m
index 3e165e975..54462732b 100644
--- a/examples/matlab/PlanarSLAMExample_easy.m
+++ b/examples/matlab/PlanarSLAMExample_easy.m
@@ -28,20 +28,20 @@ graph = planarSLAMGraph;
 
 %% Add prior
 % gaussian for prior
-prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
+prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
 prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
 graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
+odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
 odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
 graph.addOdometry(x1, x2, odom_measurement, odom_model);
 graph.addOdometry(x2, x3, odom_measurement, odom_model);
 
 %% Add measurements
 % general noisemodel for measurements
-meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
+meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
 
 % create the measurement values - indices are (pose id, landmark id)
 degrees = pi/180;
diff --git a/examples/matlab/Pose2SLAMExample_advanced.m b/examples/matlab/Pose2SLAMExample_advanced.m
index ce29119c6..3edb34782 100644
--- a/examples/matlab/Pose2SLAMExample_advanced.m
+++ b/examples/matlab/Pose2SLAMExample_advanced.m
@@ -28,20 +28,20 @@ graph = pose2SLAMGraph;
 
 %% Add prior
 % gaussian for prior
-prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
+prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
 prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
 graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
+odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
 odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
 graph.addOdometry(x1, x2, odom_measurement, odom_model);
 graph.addOdometry(x2, x3, odom_measurement, odom_model);
 
 %% Add measurements
 % general noisemodel for measurements
-meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
+meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
 
 % print
 graph.print('full graph');
diff --git a/examples/matlab/Pose2SLAMExample_easy.m b/examples/matlab/Pose2SLAMExample_easy.m
index 8df9c1c0e..bbafba99d 100644
--- a/examples/matlab/Pose2SLAMExample_easy.m
+++ b/examples/matlab/Pose2SLAMExample_easy.m
@@ -26,13 +26,13 @@ graph = pose2SLAMGraph;
 
 %% Add prior
 % gaussian for prior
-prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
+prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
 prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
 graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
+odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
 odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
 graph.addOdometry(x1, x2, odom_measurement, odom_model);
 graph.addOdometry(x2, x3, odom_measurement, odom_model);
diff --git a/gtsam.h b/gtsam.h
index 970980108..9b2a89e85 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -229,13 +229,13 @@ class SharedDiagonal {
 };
 
 class SharedNoiseModel {
-	static gtsam::SharedNoiseModel sharedSigmas(Vector sigmas);
-	static gtsam::SharedNoiseModel sharedSigma(size_t dim, double sigma);
-	static gtsam::SharedNoiseModel sharedPrecisions(Vector precisions);
-	static gtsam::SharedNoiseModel sharedPrecision(size_t dim, double precision);
-	static gtsam::SharedNoiseModel sharedUnit(size_t dim);
-	static gtsam::SharedNoiseModel sharedSqrtInformation(Matrix R);
-	static gtsam::SharedNoiseModel sharedCovariance(Matrix covariance);
+	static gtsam::SharedNoiseModel Sigmas(Vector sigmas);
+	static gtsam::SharedNoiseModel Sigma(size_t dim, double sigma);
+	static gtsam::SharedNoiseModel Precisions(Vector precisions);
+	static gtsam::SharedNoiseModel Precision(size_t dim, double precision);
+	static gtsam::SharedNoiseModel Unit(size_t dim);
+	static gtsam::SharedNoiseModel SqrtInformation(Matrix R);
+	static gtsam::SharedNoiseModel Covariance(Matrix covariance);
 	void print(string s) const;
 };
 
diff --git a/gtsam/linear/SharedNoiseModel.h b/gtsam/linear/SharedNoiseModel.h
index 71f2e4014..1d66e3495 100644
--- a/gtsam/linear/SharedNoiseModel.h
+++ b/gtsam/linear/SharedNoiseModel.h
@@ -50,31 +50,31 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
 
     // Static syntactic sugar functions to create noisemodels directly
     // These should only be used with the Matlab interface
-  	static inline SharedNoiseModel sharedSigmas(const Vector& sigmas, bool smart=false) {
+  	static inline SharedNoiseModel Sigmas(const Vector& sigmas, bool smart=false) {
   		return noiseModel::Diagonal::Sigmas(sigmas, smart);
   	}
 
-  	static inline SharedNoiseModel sharedSigma(size_t dim, double sigma) {
+  	static inline SharedNoiseModel Sigma(size_t dim, double sigma) {
   		return noiseModel::Isotropic::Sigma(dim, sigma);
   	}
 
-  	static inline SharedNoiseModel sharedPrecisions(const Vector& precisions) {
+  	static inline SharedNoiseModel Precisions(const Vector& precisions) {
   	  return noiseModel::Diagonal::Precisions(precisions);
   	}
 
-  	static inline SharedNoiseModel sharedPrecision(size_t dim, double precision) {
+  	static inline SharedNoiseModel Precision(size_t dim, double precision) {
   	  return noiseModel::Isotropic::Precision(dim, precision);
   	}
 
-  	static inline SharedNoiseModel sharedUnit(size_t dim) {
+  	static inline SharedNoiseModel Unit(size_t dim) {
   	  return noiseModel::Unit::Create(dim);
   	}
 
-  	static inline SharedNoiseModel sharedSqrtInformation(const Matrix& R) {
+  	static inline SharedNoiseModel SqrtInformation(const Matrix& R) {
   	  return noiseModel::Gaussian::SqrtInformation(R);
   	}
 
-  	static inline SharedNoiseModel sharedCovariance(const Matrix& covariance, bool smart=false) {
+  	static inline SharedNoiseModel Covariance(const Matrix& covariance, bool smart=false) {
   		return noiseModel::Gaussian::Covariance(covariance, smart);
   	}
 

From 051c83273715327d80e9170a072735aace71b794 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 13 Feb 2012 19:09:00 +0000
Subject: [PATCH 59/88] Fixed compile errors with gcc

---
 gtsam/geometry/CalibratedCamera.h  |  4 ++--
 gtsam/geometry/PinholeCamera.h     |  4 ++--
 gtsam/nonlinear/Values-inl.h       | 15 +++++++++++++--
 gtsam/nonlinear/Values.h           |  4 ++++
 gtsam/slam/BoundingConstraint.h    |  4 ++--
 gtsam/slam/tests/testPose3SLAM.cpp |  3 +--
 tests/testExtendedKalmanFilter.cpp | 10 +++++-----
 7 files changed, 29 insertions(+), 15 deletions(-)

diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 12cfc706d..95ff66e68 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -49,14 +49,14 @@ namespace gtsam {
 		CalibratedCamera() {}
 
 		/// construct with pose
-		CalibratedCamera(const Pose3& pose);
+		explicit CalibratedCamera(const Pose3& pose);
 
     /// @}
     /// @name Advanced Constructors
     /// @{
 
 		/// construct from vector
-		CalibratedCamera(const Vector &v);
+		explicit CalibratedCamera(const Vector &v);
 
 		/// @}
 		/// @name Testable
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index d00f0fd90..8aee12064 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -51,7 +51,7 @@ namespace gtsam {
     PinholeCamera() {}
 
     /** constructor with pose */
-    PinholeCamera(const Pose3& pose):pose_(pose){}
+    explicit PinholeCamera(const Pose3& pose):pose_(pose){}
 
     /** constructor with pose and calibration */
     PinholeCamera(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {}
@@ -63,7 +63,7 @@ namespace gtsam {
     /// @name Advanced Constructors
     /// @{
 
-    PinholeCamera(const Vector &v){
+    explicit PinholeCamera(const Vector &v){
       pose_ = Pose3::Expmap(v.head(Pose3::Dim()));
       if ( v.size() > Pose3::Dim()) {
         k_ = Calibration(v.tail(Calibration::Dim()));
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 7410ddade..c79d82a18 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -40,23 +40,30 @@ namespace gtsam {
     ValueCloneAllocator() {}
   };
 
+#if 0
   /* ************************************************************************* */
   class ValueAutomaticCasting {
     const Symbol& key_;
     const Value& value_;
+
   public:
     ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {}
 
     template<class ValueType>
-    operator const ValueType& () const {
+    class ConvertibleToValue : public ValueType {
+    };
+
+    template<class ValueType>
+    operator const ConvertibleToValue<ValueType>& () const {
       // Check the type and throw exception if incorrect
       if(typeid(ValueType) != typeid(value_))
         throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_));
 
       // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-      return static_cast<const ValueType&>(value_);
+      return static_cast<const ConvertibleToValue<ValueType>&>(value_);
     }
   };
+#endif
 
   /* ************************************************************************* */
   template<typename ValueType>
@@ -76,6 +83,7 @@ namespace gtsam {
     return static_cast<const ValueType&>(*item->second);
   }
 
+#if 0
   /* ************************************************************************* */
   inline ValueAutomaticCasting Values::at(const Symbol& j) const {
     // Find the item
@@ -87,6 +95,7 @@ namespace gtsam {
 
     return ValueAutomaticCasting(item->first, *item->second);
   }
+#endif
 
   /* ************************************************************************* */
   template<typename TypedKey>
@@ -98,10 +107,12 @@ namespace gtsam {
     return at<typename TypedKey::Value>(symbol);
   }
 
+#if 0
   /* ************************************************************************* */
   inline ValueAutomaticCasting Values::operator[](const Symbol& j) const {
     return at(j);
   }
+#endif
 
   /* ************************************************************************* */
   template<typename ValueType>
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index e2a69d622..61e11ca4e 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -139,6 +139,7 @@ namespace gtsam {
     template<typename ValueType>
     const ValueType& at(const Symbol& j) const;
 
+#if 0
     /** Retrieve a variable by key \c j.  This non-templated version returns a
      * special ValueAutomaticCasting object that may be assigned to the proper
      * value.
@@ -147,6 +148,7 @@ namespace gtsam {
      * of the proper type.
      */
     ValueAutomaticCasting at(const Symbol& j) const;
+#endif
 
     /** Retrieve a variable using a special key (typically TypedSymbol), which
      * contains the type of the value associated with the key, and which must
@@ -163,8 +165,10 @@ namespace gtsam {
     const typename TypedKey::Value& operator[](const TypedKey& j) const {
       return at(j); }
 
+#if 0
     /** operator[] syntax for at(const Symbol& j) */
     ValueAutomaticCasting operator[](const Symbol& j) const;
+#endif
 
     /** Check if a value exists with key \c j.  See exists<>(const Symbol& j)
      * and exists(const TypedKey& j) for versions that return the value if it
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index 42f42e429..13ae33cfc 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
 	/** active when constraint *NOT* met */
 	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
-		double x = value(c[this->key()]);
+		double x = value(c.at<X>(this->key()));
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
 	}
 
@@ -127,7 +127,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
 	/** active when constraint *NOT* met */
 	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
-		double x = value(c[this->key1()], c[this->key2()]);
+		double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
 	}
 
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 4fa815cdc..9eb6b3c3c 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -50,8 +50,7 @@ TEST(Pose3Graph, optimizeCircle) {
 	// Create a hexagon of poses
 	double radius = 10;
 	Values hexagon = pose3SLAM::circle(6,radius);
-//  Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)]; // FAIL: cannot cast ValueAutomaticCasting
-  Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1)); // Works
+  Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1));
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose3Graph> fg(new Pose3Graph);
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 532305f24..0b4b222be 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -181,7 +181,7 @@ public:
 
   /** Vector of errors, whitened ! */
   Vector whitenedError(const Values& c) const {
-    return QInvSqrt(c[key1()])*unwhitenedError(c);
+    return QInvSqrt(c.at<T>(key1()))*unwhitenedError(c);
   }
 
   /**
@@ -190,8 +190,8 @@ public:
    * Hence b = z - h(x1,x2) = - error_vector(x)
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
-    const X1& x1 = c[key1()];
-    const X2& x2 = c[key2()];
+    const X1& x1 = c.at<X1>(key1());
+    const X2& x2 = c.at<X2>(key2());
     Matrix A1, A2;
     Vector b = -evaluateError(x1, x2, A1, A2);
     const Index var1 = ordering[key1()], var2 = ordering[key2()];
@@ -318,7 +318,7 @@ public:
 
   /** Vector of errors, whitened ! */
   Vector whitenedError(const Values& c) const {
-    return RInvSqrt(c[key()])*unwhitenedError(c);
+    return RInvSqrt(c.at<T>(key()))*unwhitenedError(c);
   }
 
   /**
@@ -327,7 +327,7 @@ public:
    * Hence b = z - h(x1) = - error_vector(x)
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
-    const X& x1 = c[key()];
+    const X& x1 = c.at<X>(key());
     Matrix A1;
     Vector b = -evaluateError(x1, A1);
     const Index var1 = ordering[key()];

From 75428b13fe249dbf67c461a26a4f61c22cce53c4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 13 Feb 2012 20:27:54 +0000
Subject: [PATCH 60/88] Revert "Using FastVector instead of vector in most code
 called during elimination and solving"

This reverts commit 566631cb42249cb710ef01d07d583e563afccea9.
---
 gtsam/base/FastVector.h                    | 19 ++----------
 gtsam/base/Lie.h                           |  2 +-
 gtsam/inference/BayesTree.h                |  4 +--
 gtsam/inference/BayesTreeCliqueBase-inl.h  |  6 ++--
 gtsam/inference/ClusterTree.h              | 11 ++++---
 gtsam/inference/Conditional.h              | 12 ++------
 gtsam/inference/EliminationTree-inl.h      | 10 +++----
 gtsam/inference/EliminationTree.h          |  4 +--
 gtsam/inference/Factor.h                   | 21 +++++--------
 gtsam/inference/FactorGraph-inl.h          |  4 +--
 gtsam/inference/FactorGraph.h              |  2 +-
 gtsam/inference/IndexConditional.h         | 11 -------
 gtsam/inference/IndexFactor.h              |  6 ----
 gtsam/inference/JunctionTree-inl.h         |  6 ++--
 gtsam/inference/JunctionTree.h             |  4 +--
 gtsam/inference/Permutation.h              |  8 ++---
 gtsam/inference/SymbolicFactorGraph.cpp    |  6 ++--
 gtsam/inference/SymbolicFactorGraph.h      |  2 +-
 gtsam/inference/inference.cpp              |  6 ++--
 gtsam/inference/inference.h                |  7 ++---
 gtsam/inference/tests/testJunctionTree.cpp | 16 +++++-----
 gtsam/linear/GaussianConditional.h         |  2 +-
 gtsam/linear/GaussianFactor.h              |  7 ++---
 gtsam/linear/GaussianFactorGraph.cpp       | 14 ++++-----
 gtsam/linear/GaussianJunctionTree.cpp      |  4 ++-
 gtsam/linear/HessianFactor.cpp             |  4 +--
 gtsam/linear/HessianFactor.h               |  2 +-
 gtsam/linear/JacobianFactor.cpp            | 12 ++++----
 gtsam/linear/JacobianFactor.h              | 10 +++----
 gtsam/linear/NoiseModel.cpp                |  6 ++--
 gtsam/linear/NoiseModel.h                  |  9 +++---
 gtsam/nonlinear/GaussianISAM2-inl.h        |  2 +-
 gtsam/nonlinear/ISAM2-impl-inl.h           |  4 +--
 gtsam/nonlinear/ISAM2-inl.h                |  2 +-
 gtsam/nonlinear/NonlinearFactor.h          | 34 +++++++++++++++-------
 tests/testGaussianJunctionTree.cpp         | 32 ++++++++++----------
 36 files changed, 138 insertions(+), 173 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 18af63006..cb08871d9 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -50,29 +50,14 @@ public:
 
   /** Constructor from a range, passes through to base class */
   template<typename INPUTITERATOR>
-  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
-    // This if statement works around a bug in boost pool allocator and/or
-    // STL vector where if the size is zero, the pool allocator will allocate
-    // huge amounts of memory.
-    if(first != last)
-      Base::assign(first, last);
-  }
+  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
 
   /** Copy constructor from another FastSet */
   FastVector(const FastVector<VALUE>& x) : Base(x) {}
 
-  /** Copy constructor from the base class */
+  /** Copy constructor from the base map class */
   FastVector(const Base& x) : Base(x) {}
 
-  /** Copy constructor from a standard STL container */
-  FastVector(const std::vector<VALUE>& x) {
-    // This if statement works around a bug in boost pool allocator and/or
-    // STL vector where if the size is zero, the pool allocator will allocate
-    // huge amounts of memory.
-    if(x.size() > 0)
-      Base::assign(x.begin(), x.end());
-  }
-
 private:
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index a770d9975..bc530a9f5 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
 template <class T>
 T expm(const Vector& x, int K=7) {
 	Matrix xhat = wedge<T>(x);
-	return T(expm(xhat,K));
+	return expm(xhat,K);
 }
 
 } // namespace gtsam
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 3dc73519a..87788e3d8 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -297,7 +297,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CONDITIONAL, class CLIQUE>
   void _BayesTree_dim_adder(
-      FastVector<size_t>& dims,
+      std::vector<size_t>& dims,
       const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
 
     if(clique) {
@@ -316,7 +316,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class CONDITIONAL,class CLIQUE>
 	boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
-	  FastVector<size_t> dimensions(bt.nodes().size(), 0);
+	  std::vector<size_t> dimensions(bt.nodes().size(), 0);
 	  _BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
 	  return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
 	}
diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h
index 1b4d92cae..dfc8864f7 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inl.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inl.h
@@ -223,7 +223,7 @@ namespace gtsam {
 
     assertInvariants();
     GenericSequentialSolver<FactorType> solver(p_FSR);
-    return *solver.jointFactorGraph(vector<Index>(conditional_->keys().begin(), conditional_->keys().end()), function);
+    return *solver.jointFactorGraph(conditional_->keys(), function);
   }
 
   /* ************************************************************************* */
@@ -243,8 +243,8 @@ namespace gtsam {
     joint.push_back(R->conditional()->toFactor()); // P(R)
 
     // Find the keys of both C1 and C2
-    const FastVector<Index>& keys1(conditional_->keys());
-    const FastVector<Index>& keys2(C2->conditional_->keys());
+    std::vector<Index> keys1(conditional_->keys());
+    std::vector<Index> keys2(C2->conditional_->keys());
     FastSet<Index> keys12;
     keys12.insert(keys1.begin(), keys1.end());
     keys12.insert(keys2.begin(), keys2.end());
diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h
index 9e82aae18..7544587c1 100644
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@@ -20,13 +20,12 @@
 #pragma once
 
 #include <list>
+#include <vector>
 
 #include <boost/shared_ptr.hpp>
 #include <boost/weak_ptr.hpp>
 
 #include <gtsam/base/types.h>
-#include <gtsam/base/FastVector.h>
-#include <gtsam/base/FastList.h>
 
 namespace gtsam {
 
@@ -47,13 +46,13 @@ namespace gtsam {
 			typedef typename boost::shared_ptr<Cluster> shared_ptr;
 			typedef typename boost::weak_ptr<Cluster> weak_ptr;
 
-      const FastVector<Index> frontal;                   // the frontal variables
-      const FastVector<Index> separator;                // the separator variables
+      const std::vector<Index> frontal;                   // the frontal variables
+      const std::vector<Index> separator;                // the separator variables
 
 		protected:
 
 			weak_ptr parent_;                      // the parent cluster
-			FastList<shared_ptr> children_;     // the child clusters
+			std::list<shared_ptr> children_;     // the child clusters
 			const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent
 
 		public:
@@ -83,7 +82,7 @@ namespace gtsam {
 			bool equals(const Cluster& other) const;
 
 			/// get a reference to the children
-			const FastList<shared_ptr>& children() const { return children_; }
+			const std::list<shared_ptr>& children() const { return children_; }
 
 			/// add a child
 			void addChild(shared_ptr child);
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index a2f5bd7a1..2fa5f691d 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -45,8 +45,8 @@ private:
 
 	/** Create keys by adding key in front */
 	template<typename ITERATOR>
-	static FastVector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
-		FastVector<Key> keys((lastParent - firstParent) + 1);
+	static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
+		std::vector<Key> keys((lastParent - firstParent) + 1);
 		std::copy(firstParent, lastParent, keys.begin() + 1);
 		keys[0] = key;
 		return keys;
@@ -116,14 +116,8 @@ public:
 		assertInvariants();
 	}
 
-  /** Constructor from a frontal variable and a vector of parents */
-  Conditional(Key key, const FastVector<Key>& parents) :
-    FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
-    assertInvariants();
-  }
-
   /** Constructor from keys and nr of frontal variables */
-	Conditional(const FastVector<Index>& keys, size_t nrFrontals) :
+	Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
 		FactorType(keys), nrFrontals_(nrFrontals) {
 		assertInvariants();
 	}
diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h
index 002a5639c..462a2fd4a 100644
--- a/gtsam/inference/EliminationTree-inl.h
+++ b/gtsam/inference/EliminationTree-inl.h
@@ -58,7 +58,7 @@ typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminat
 
 /* ************************************************************************* */
 template<class FACTOR>
-FastVector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
+vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
 
   // Number of factors and variables
   const size_t m = structure.nFactors();
@@ -67,8 +67,8 @@ FastVector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& s
   static const Index none = numeric_limits<Index>::max();
 
   // Allocate result parent vector and vector of last factor columns
-  FastVector<Index> parents(n, none);
-  FastVector<Index> prevCol(m, none);
+  vector<Index> parents(n, none);
+  vector<Index> prevCol(m, none);
 
   // for column j \in 1 to n do
   for (Index j = 0; j < n; j++) {
@@ -100,7 +100,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   tic(1, "ET ComputeParents");
   // Compute the tree structure
-  FastVector<Index> parents(ComputeParents(structure));
+  vector<Index> parents(ComputeParents(structure));
   toc(1, "ET ComputeParents");
 
   // Number of variables
@@ -110,7 +110,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
 
   // Create tree structure
   tic(2, "assemble tree");
-  FastVector<shared_ptr> trees(n);
+  vector<shared_ptr> trees(n);
   for (Index k = 1; k <= n; k++) {
     Index j = n - k;  // Start at the last variable and loop down to 0
     trees[j].reset(new EliminationTree(j));  // Create a new node on this variable
diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h
index 4dab46dd3..4d0ccce56 100644
--- a/gtsam/inference/EliminationTree.h
+++ b/gtsam/inference/EliminationTree.h
@@ -54,7 +54,7 @@ private:
 
   typedef FastList<sharedFactor> Factors;
   typedef FastList<shared_ptr> SubTrees;
-  typedef FastVector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
+  typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
 
   Index key_; ///< index associated with root
   Factors factors_; ///< factors associated with root
@@ -74,7 +74,7 @@ private:
    * Static internal function to build a vector of parent pointers using the
    * algorithm of Gilbert et al., 2001, BIT.
    */
-  static FastVector<Index> ComputeParents(const VariableIndex& structure);
+  static std::vector<Index> ComputeParents(const VariableIndex& structure);
 
   /** add a factor, for Create use only */
   void add(const sharedFactor& factor) { factors_.push_back(factor); }
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index f51276ddc..5a0b1e333 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -22,10 +22,10 @@
 #pragma once
 
 #include <set>
+#include <vector>
 #include <boost/serialization/nvp.hpp>
 #include <boost/foreach.hpp>
-
-#include <gtsam/base/FastVector.h>
+#include <gtsam/base/FastMap.h>
 
 namespace gtsam {
 
@@ -68,15 +68,15 @@ public:
   typedef boost::shared_ptr<Factor> shared_ptr;
 
   /// Iterator over keys
-  typedef typename FastVector<Key>::iterator iterator;
+  typedef typename std::vector<Key>::iterator iterator;
 
   /// Const iterator over keys
-  typedef typename FastVector<Key>::const_iterator const_iterator;
+  typedef typename std::vector<Key>::const_iterator const_iterator;
 
 protected:
 
   /// The keys involved in this factor
-  FastVector<Key> keys_;
+  std::vector<Key> keys_;
 
   friend class JacobianFactor;
   friend class HessianFactor;
@@ -132,11 +132,6 @@ public:
 		assertInvariants();
 	}
 
-  /** Construct n-way factor */
-  Factor(const FastVector<Key>& keys) : keys_(keys) {
-    assertInvariants();
-  }
-
   /** Constructor from a collection of keys */
   template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
         keys_(beginKey, endKey) { assertInvariants(); }
@@ -170,8 +165,8 @@ public:
   /// find
   const_iterator find(Key key) const { return std::find(begin(), end(), key); }
 
-  /// @return keys involved in this factor
-  const FastVector<Key>& keys() const { return keys_; }
+  ///TODO: comment
+  const std::vector<Key>& keys() const { return keys_; }
 
   /** iterators */
   const_iterator begin() const { return keys_.begin(); }	///TODO: comment
@@ -199,7 +194,7 @@ public:
   /**
    * @return keys involved in this factor
    */
-  FastVector<Key>& keys() { return keys_; }
+  std::vector<Key>& keys() { return keys_; }
 
   /** mutable iterators */
   iterator begin() { return keys_.begin(); }	///TODO: comment
diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index 6244c959a..337b571c6 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -104,8 +104,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class DERIVED, class KEY>
 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
-			const FastMap<KEY, FastVector<KEY> >& variableSlots) {
-		typedef const pair<const KEY, FastVector<KEY> > KeySlotPair;
+			const FastMap<KEY, std::vector<KEY> >& variableSlots) {
+		typedef const pair<const KEY, std::vector<KEY> > KeySlotPair;
 		return typename DERIVED::shared_ptr(new DERIVED(
 		    boost::make_transform_iterator(variableSlots.begin(), boost::bind(&KeySlotPair::first, _1)),
 		    boost::make_transform_iterator(variableSlots.end(), boost::bind(&KeySlotPair::first, _1))));
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index fcbd02a29..458588b7b 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -232,7 +232,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
   /** Create a combined joint factor (new style for EliminationTree). */
 	template<class DERIVED, class KEY>
 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
-			const FastMap<KEY, FastVector<KEY> >& variableSlots);
+			const FastMap<KEY, std::vector<KEY> >& variableSlots);
 
 	/**
    * static function that combines two factor graphs
diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h
index e8cb3f9c1..c1cbbdaac 100644
--- a/gtsam/inference/IndexConditional.h
+++ b/gtsam/inference/IndexConditional.h
@@ -74,23 +74,12 @@ namespace gtsam {
 			assertInvariants();
 		}
 
-    /** Constructor from a frontal variable and a vector of parents (FastVector version) */
-    IndexConditional(Index j, const FastVector<Index>& parents) : Base(j, parents) {
-      assertInvariants();
-    }
-
     /** Constructor from keys and nr of frontal variables */
 		IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) :
 			Base(keys, nrFrontals) {
 			assertInvariants();
 		}
 
-    /** Constructor from keys and nr of frontal variables (FastVector version) */
-    IndexConditional(const FastVector<Index>& keys, size_t nrFrontals) :
-      Base(keys, nrFrontals) {
-      assertInvariants();
-    }
-
 		/// @}
 		/// @name Standard Interface
 		/// @{
diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h
index 32488a9d5..9b2d39727 100644
--- a/gtsam/inference/IndexFactor.h
+++ b/gtsam/inference/IndexFactor.h
@@ -114,12 +114,6 @@ namespace gtsam {
 			assertInvariants();
 		}
 
-    /** Construct n-way factor (FastVector version) */
-    IndexFactor(const FastVector<Index>& js) :
-      Base(js) {
-      assertInvariants();
-    }
-
 		/** Constructor from a collection of keys */
 		template<class KeyIterator> IndexFactor(KeyIterator beginKey,
 				KeyIterator endKey) :
diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h
index 739b51048..ad8b389ab 100644
--- a/gtsam/inference/JunctionTree-inl.h
+++ b/gtsam/inference/JunctionTree-inl.h
@@ -83,7 +83,7 @@ namespace gtsam {
 
     // Two stages - first build an array of the lowest-ordered variable in each
     // factor and find the last variable to be eliminated.
-    FastVector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
+    vector<Index> lowestOrdered(fg.size(), numeric_limits<Index>::max());
     Index maxVar = 0;
     for(size_t i=0; i<fg.size(); ++i)
       if(fg[i]) {
@@ -96,7 +96,7 @@ namespace gtsam {
 
     // Now add each factor to the list corresponding to its lowest-ordered
     // variable.
-    FastVector<FastList<size_t> > targets(maxVar+1);
+    vector<FastList<size_t> > targets(maxVar+1);
     for(size_t i=0; i<lowestOrdered.size(); ++i)
       if(lowestOrdered[i] != numeric_limits<Index>::max())
         targets[lowestOrdered[i]].push_back(i);
@@ -108,7 +108,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class FG, class BTCLIQUE>
   typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
-      const FastVector<FastList<size_t> >& targets,
+      const std::vector<FastList<size_t> >& targets,
       const SymbolicBayesTree::sharedClique& bayesClique) {
 
     if(bayesClique) {
diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h
index a068be991..33a9aa5e4 100644
--- a/gtsam/inference/JunctionTree.h
+++ b/gtsam/inference/JunctionTree.h
@@ -20,7 +20,7 @@
 #pragma once
 
 #include <set>
-#include <gtsam/base/FastVector.h>
+#include <vector>
 #include <gtsam/base/FastList.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/ClusterTree.h>
@@ -78,7 +78,7 @@ namespace gtsam {
 				const SymbolicBayesTree::sharedClique& clique);
 
 		/// distribute the factors along the cluster tree
-    sharedClique distributeFactors(const FG& fg, const FastVector<FastList<size_t> >& targets,
+    sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
         const SymbolicBayesTree::sharedClique& clique);
 
 		/// recursive elimination function
diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h
index bde6dd459..5cbd8e187 100644
--- a/gtsam/inference/Permutation.h
+++ b/gtsam/inference/Permutation.h
@@ -17,12 +17,12 @@
 
 #pragma once
 
+#include <vector>
 #include <string>
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 
 #include <gtsam/base/types.h>
-#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -46,12 +46,12 @@ class Inference;
  */
 class Permutation {
 protected:
-  FastVector<Index> rangeIndices_;
+  std::vector<Index> rangeIndices_;
 
 public:
   typedef boost::shared_ptr<Permutation> shared_ptr;
-  typedef FastVector<Index>::const_iterator const_iterator;
-  typedef FastVector<Index>::iterator iterator;
+  typedef std::vector<Index>::const_iterator const_iterator;
+  typedef std::vector<Index>::iterator iterator;
 
 	/// @name Standard Constructors
 	/// @{
diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp
index 233c1e21c..2d5190f45 100644
--- a/gtsam/inference/SymbolicFactorGraph.cpp
+++ b/gtsam/inference/SymbolicFactorGraph.cpp
@@ -66,7 +66,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-			FastVector<Index> >& variableSlots) {
+					std::vector<Index> >& variableSlots) {
 		IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors,
 				variableSlots));
 //		combined->assertInvariants();
@@ -84,11 +84,9 @@ namespace gtsam {
 
 		if (keys.size() < 1) throw invalid_argument(
 				"IndexFactor::CombineAndEliminate called on factors with no variables.");
-		if(nrFrontals > keys.size()) throw invalid_argument(
-		    "Requesting to eliminate more variables than are present in the factors");
 
 		pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result;
-		FastVector<Index> newKeys(keys.begin(),keys.end());
+		std::vector<Index> newKeys(keys.begin(),keys.end());
     result.first.reset(new IndexConditional(newKeys, nrFrontals));
 		result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));
 
diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h
index e39e4a70b..4da2baf13 100644
--- a/gtsam/inference/SymbolicFactorGraph.h
+++ b/gtsam/inference/SymbolicFactorGraph.h
@@ -84,7 +84,7 @@ namespace gtsam {
 	/** Create a combined joint factor (new style for EliminationTree). */
 	IndexFactor::shared_ptr CombineSymbolic(
 			const FactorGraph<IndexFactor>& factors, const FastMap<Index,
-			FastVector<Index> >& variableSlots);
+					std::vector<Index> >& variableSlots);
 
 	/**
 	 * CombineAndEliminate provides symbolic elimination.
diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp
index 8ec67b26c..d30ea79c4 100644
--- a/gtsam/inference/inference.cpp
+++ b/gtsam/inference/inference.cpp
@@ -29,12 +29,12 @@ using namespace std;
 
 namespace gtsam {
 
-Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember) {
+Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
   size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
   // Convert to compressed column major format colamd wants it in (== MATLAB format!)
   int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
-  vector<int> A(Alen); /* colamd arg 4: row indices of A, of size Alen */
-  vector<int> p(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
+  vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
+  vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
 
   static const bool debug = false;
 
diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h
index 728d2c4bd..d853ff91b 100644
--- a/gtsam/inference/inference.h
+++ b/gtsam/inference/inference.h
@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include <gtsam/base/FastVector.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/Permutation.h>
 
@@ -49,7 +48,7 @@ namespace gtsam {
 	  /**
 	   * Compute a CCOLAMD permutation using the constraint groups in cmember.
 	   */
-    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, FastVector<int>& cmember);
+    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
 
 	};
 
@@ -57,7 +56,7 @@ namespace gtsam {
 	template<typename CONSTRAINED>
 	Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
 
-	  FastVector<int> cmember(variableIndex.size(), 0);
+	  std::vector<int> cmember(variableIndex.size(), 0);
 
 	  // If at least some variables are not constrained to be last, constrain the
 	  // ones that should be constrained.
@@ -73,7 +72,7 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
-	  FastVector<int> cmember(variableIndex.size(), 0);
+	  std::vector<int> cmember(variableIndex.size(), 0);
 	  return PermutationCOLAMD_(variableIndex, cmember);
 	}
 
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 9695afc56..7374bbad8 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -56,15 +56,15 @@ TEST( JunctionTree, constructor )
 
 	SymbolicJunctionTree actual(fg);
 
-	FastVector<Index> frontal1; frontal1 += x3, x4;
-	FastVector<Index> frontal2; frontal2 += x2, x1;
-	FastVector<Index> sep1;
-	FastVector<Index> sep2; sep2 += x3;
-	CHECK(assert_container_equality(frontal1, actual.root()->frontal));
-	CHECK(assert_container_equality(sep1,     actual.root()->separator));
+	vector<Index> frontal1; frontal1 += x3, x4;
+	vector<Index> frontal2; frontal2 += x2, x1;
+	vector<Index> sep1;
+	vector<Index> sep2; sep2 += x3;
+	CHECK(assert_equal(frontal1, actual.root()->frontal));
+	CHECK(assert_equal(sep1,     actual.root()->separator));
 	LONGS_EQUAL(1,               actual.root()->size());
-	CHECK(assert_container_equality(frontal2, actual.root()->children().front()->frontal));
-	CHECK(assert_container_equality(sep2,     actual.root()->children().front()->separator));
+	CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal));
+	CHECK(assert_equal(sep2,     actual.root()->children().front()->separator));
 	LONGS_EQUAL(2,               actual.root()->children().front()->size());
 	CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
   CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index af83a6f44..f2a77c894 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -268,7 +268,7 @@ template<typename ITERATOR, class MATRIX>
 GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
 		size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
 		const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) :
-	IndexConditional(FastVector<Index>(firstKey, lastKey), nrFrontals), rsd_(
+	IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_(
 			matrix_), sigmas_(sigmas), permutation_(permutation) {
 	rsd_.assignNoalias(matrices);
 }
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index 771a3de8f..2d889ee64 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -74,9 +74,6 @@ namespace gtsam {
     /** Construct n-way factor */
     GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {}
 
-    /** Construct n-way factor */
-    GaussianFactor(const FastVector<Index>& js) : IndexFactor(js) {}
-
   public:
 
     typedef GaussianConditional ConditionalType;
@@ -113,8 +110,8 @@ namespace gtsam {
 
   /** make keys from list, vector, or map of matrices */
 	template<typename ITERATOR>
-	static FastVector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
-	  FastVector<Index> keys;
+	static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
+		std::vector<Index> keys;
 		keys.reserve(n);
 		for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
 		return keys;
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 16c2ce12d..56a377c61 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -167,11 +167,11 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	// Helper functions for Combine
-	static boost::tuple<FastVector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
+	static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
 #ifndef NDEBUG
-	  FastVector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
+		vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
 #else
-	  FastVector<size_t> varDims(variableSlots.size());
+		vector<size_t> varDims(variableSlots.size());
 #endif
 		size_t m = 0;
 		size_t n = 0;
@@ -220,7 +220,7 @@ break;
 
 		if (debug) cout << "Determine dimensions" << endl;
 		tic(1, "countDims");
-		FastVector<size_t> varDims;
+		vector<size_t> varDims;
 		size_t m, n;
 		boost::tie(varDims, m, n) = countDims(factors, variableSlots);
 		if (debug) {
@@ -232,7 +232,7 @@ break;
 
 		if (debug) cout << "Sort rows" << endl;
 		tic(2, "sort rows");
-		FastVector<JacobianFactor::_RowSource> rowSources;
+		vector<JacobianFactor::_RowSource> rowSources;
 		rowSources.reserve(m);
 		bool anyConstrained = false;
 		for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
@@ -366,7 +366,7 @@ break;
 
 		// Pull out keys and dimensions
 		tic(2, "keys");
-		FastVector<size_t> dimensions(scatter.size() + 1);
+		vector<size_t> dimensions(scatter.size() + 1);
 		BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
 			dimensions[var_slot.second.slot] = var_slot.second.dimension;
 		}
@@ -568,7 +568,7 @@ break;
 
 		// Pull out keys and dimensions
 		tic(2, "keys");
-		FastVector<size_t> dimensions(scatter.size() + 1);
+		vector<size_t> dimensions(scatter.size() + 1);
 		BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
 			dimensions[var_slot.second.slot] = var_slot.second.dimension;
 		}
diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp
index 0db558a10..e2e1c2951 100644
--- a/gtsam/linear/GaussianJunctionTree.cpp
+++ b/gtsam/linear/GaussianJunctionTree.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
 
+#include <vector>
+
 #include <boost/foreach.hpp>
 
 namespace gtsam {
@@ -66,7 +68,7 @@ namespace gtsam {
 
 		// Allocate solution vector and copy RHS
     tic(2, "allocate VectorValues");
-    FastVector<size_t> dims(rootClique->conditional()->back()+1, 0);
+    vector<size_t> dims(rootClique->conditional()->back()+1, 0);
 		countDims(rootClique, dims);
 		VectorValues result(dims);
 		btreeRHS(rootClique, result);
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index eedecc4ed..8402e4f91 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
-		const FastVector<size_t>& dimensions, const Scatter& scatter) :
+		const vector<size_t>& dimensions, const Scatter& scatter) :
 		info_(matrix_) {
 
 	const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL");
@@ -505,7 +505,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
   tic(2, "remaining factor");
   info_.blockStart() = nrFrontals;
   // Assign the keys
-  FastVector<Index> remainingKeys(keys_.size() - nrFrontals);
+  vector<Index> remainingKeys(keys_.size() - nrFrontals);
   remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
   keys_.swap(remainingKeys);
   toc(2, "remaining factor");
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 6225fcde9..2a16c0e38 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -204,7 +204,7 @@ namespace gtsam {
 
     /** Special constructor used in EliminateCholesky which combines the given factors */
     HessianFactor(const FactorGraph<GaussianFactor>& factors,
-				const FastVector<size_t>& dimensions, const Scatter& scatter);
+				const std::vector<size_t>& dimensions, const Scatter& scatter);
 
     /** Destructor */
 		virtual ~HessianFactor() {}
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 064143512..0218ecc8d 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -113,7 +113,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  JacobianFactor::JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
+  JacobianFactor::JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
   		const Vector &b, const SharedDiagonal& model) :
   	GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
 		model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
@@ -260,7 +260,7 @@ namespace gtsam {
       sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
 
     // Build a vector of variable dimensions in the new order
-    FastVector<size_t> dimensions(size() + 1);
+    vector<size_t> dimensions(size() + 1);
     size_t j = 0;
     BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
       dimensions[j++] = Ab_(sourceSlot.second).cols();
@@ -269,7 +269,7 @@ namespace gtsam {
     dimensions.back() = 1;
 
     // Copy the variables and matrix into the new order
-    FastVector<Index> oldKeys(size());
+    vector<Index> oldKeys(size());
     keys_.swap(oldKeys);
     AbMatrix oldMatrix;
     BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
@@ -506,7 +506,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void JacobianFactor::collectInfo(size_t index, FastVector<_RowSource>& rowSources) const {
+  void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const {
 		assertInvariants();
 		for (size_t row = 0; row < rows(); ++row) {
 			Index firstNonzeroVar;
@@ -522,7 +522,7 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  void JacobianFactor::allocate(const VariableSlots& variableSlots, FastVector<
+  void JacobianFactor::allocate(const VariableSlots& variableSlots, vector<
 			size_t>& varDims, size_t m) {
 		keys_.resize(variableSlots.size());
 		std::transform(variableSlots.begin(), variableSlots.end(), begin(),
@@ -551,7 +551,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void JacobianFactor::copyFNZ(size_t m, size_t n,
-      FastVector<_RowSource>& rowSources) {
+			vector<_RowSource>& rowSources) {
 		Index i = 0;
 		for (size_t row = 0; row < m; ++row) {
 			while (i < n && rowSources[row].firstNonzeroVar > keys_[i])
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 90270ce95..079cb9292 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -85,7 +85,7 @@ namespace gtsam {
   protected:
 
     SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
-    FastVector<size_t> firstNonzeroBlocks_;
+    std::vector<size_t> firstNonzeroBlocks_;
     AbMatrix matrix_; // the full matrix corresponding to the factor
     BlockAb Ab_;      // the block view of the full matrix
     typedef GaussianFactor Base; // typedef to base
@@ -123,7 +123,7 @@ namespace gtsam {
         const Vector& b, const SharedDiagonal& model);
 
     /** Construct an n-ary factor */
-    JacobianFactor(const FastVector<std::pair<Index, Matrix> > &terms,
+    JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
         const Vector &b, const SharedDiagonal& model);
 
     JacobianFactor(const std::list<std::pair<Index, Matrix> > &terms,
@@ -268,18 +268,18 @@ namespace gtsam {
     // Many imperative, perhaps all need to be combined in constructor
 
     /** Collect information on Jacobian rows */
-    void collectInfo(size_t index, FastVector<_RowSource>& rowSources) const;
+    void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const;
 
     /** allocate space */
     void allocate(const VariableSlots& variableSlots,
-        FastVector<size_t>& varDims, size_t m);
+				std::vector<size_t>& varDims, size_t m);
 
     /** copy a slot from source */
     void copyRow(const JacobianFactor& source,
     		Index sourceRow, size_t sourceSlot, size_t row, Index slot);
 
     /** copy firstNonzeroBlocks structure */
-    void copyFNZ(size_t m, size_t n, FastVector<_RowSource>& rowSources);
+    void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources);
 
     /** set noiseModel correctly */
   	void setModel(bool anyConstrained, const Vector& sigmas);
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 37584af2e..fd93d39fc 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -163,7 +163,7 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
   return Unit::Create(maxrank);
 }
 
-void Gaussian::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
+void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
   BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
   whitenInPlace(b);
 }
@@ -513,7 +513,7 @@ Vector Base::sqrtWeight(const Vector &error) const {
  * according to their weight implementation */
 
 /** Reweight n block matrices with one error vector */
-void Base::reweight(FastVector<Matrix> &A, Vector &error) const {
+void Base::reweight(vector<Matrix> &A, Vector &error) const {
   if ( reweight_ == Block ) {
     const double w = sqrtWeight(error.norm());
     BOOST_FOREACH(Matrix& Aj, A) {
@@ -662,7 +662,7 @@ bool Robust::equals(const Base& expected, double tol) const {
   return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
 }
 
-void Robust::WhitenSystem(FastVector<Matrix>& A, Vector& b) const {
+void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
   noise_->WhitenSystem(A,b);
   robust_->reweight(A,b);
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 640eadfba..ee5d9e00c 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -20,7 +20,6 @@
 
 #include <boost/serialization/nvp.hpp>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/base/FastVector.h>
 
 namespace gtsam {
 
@@ -77,7 +76,7 @@ namespace gtsam {
 
       virtual double distance(const Vector& v) const = 0;
 
-      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const = 0;
+      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
@@ -186,7 +185,7 @@ namespace gtsam {
 			/**
 			 * Whiten a system, in place as well
 			 */
-      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const ;
+      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const ;
 			virtual void WhitenSystem(Matrix& A, Vector& b) const ;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
@@ -620,7 +619,7 @@ namespace gtsam {
       Vector sqrtWeight(const Vector &error) const;
 
       /** reweight block matrices and a vector according to their weight implementation */
-      void reweight(FastVector<Matrix> &A, Vector &error) const;
+      void reweight(std::vector<Matrix> &A, Vector &error) const;
 		  void reweight(Matrix &A, Vector &error) const;
 		  void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
 		  void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
@@ -715,7 +714,7 @@ namespace gtsam {
 
       // TODO: these are really robust iterated re-weighting support functions
 
-      virtual void WhitenSystem(FastVector<Matrix>& A, Vector& b) const;
+      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
       virtual void WhitenSystem(Matrix& A, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h
index b90b2c4ea..d0d37d886 100644
--- a/gtsam/nonlinear/GaussianISAM2-inl.h
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -57,7 +57,7 @@ namespace gtsam {
     if(recalculate) {
 
       // Temporary copy of the original values, to check how much they change
-      FastVector<Vector> originalValues((*clique)->nrFrontals());
+      vector<Vector> originalValues((*clique)->nrFrontals());
       GaussianConditional::const_iterator it;
       for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
         originalValues[it - (*clique)->beginFrontals()] = delta[*it];
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index 4858b1900..eae39bf52 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -126,7 +126,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
   theta.insert(newTheta);
   if(debug) newTheta.print("The new variables are: ");
   // Add the new keys onto the ordering, add zeros to the delta for the new variables
-  FastVector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
+  std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
   const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
   const size_t originalDim = delta->dim();
@@ -287,7 +287,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
   if(debug) affectedFactorsIndex.print("affectedFactorsIndex: ");
   toc(2,"variable index");
   tic(3,"ccolamd");
-  FastVector<int> cmember(affectedKeysSelector.size(), 0);
+  vector<int> cmember(affectedKeysSelector.size(), 0);
   if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
     assert(reorderingMode.constrainedKeys);
     if(keys.size() > reorderingMode.constrainedKeys->size()) {
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 6dc47131f..5c87d1dda 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -229,7 +229,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
     tic(1,"reorder");
     tic(1,"CCOLAMD");
     // Do a batch step - reorder and relinearize all variables
-    FastVector<int> cmember(theta_.size(), 0);
+    vector<int> cmember(theta_.size(), 0);
     FastSet<Index> constrainedKeysSet;
     if(constrainKeys)
       constrainedKeysSet = *constrainKeys;
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 512d7af91..166ad6980 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -24,6 +24,7 @@
 #include <limits>
 
 #include <boost/serialization/base_object.hpp>
+#include <boost/tuple/tuple.hpp>
 
 #include <gtsam/inference/Factor-inl.h>
 #include <gtsam/inference/IndexFactor.h>
@@ -35,6 +36,19 @@
 
 namespace gtsam {
 
+using boost::make_tuple;
+
+// Helper function to fill a vector from a tuple function of any length
+template<typename CONS>
+inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
+  vector[position] = tuple.get_head();
+  __fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
+}
+template<>
+inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
+  // Do nothing
+}
+
 /* ************************************************************************* */
 /**
  * Nonlinear factor base class
@@ -125,7 +139,7 @@ public:
    * variable indices.
    */
   virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
-    FastVector<Index> indices(this->size());
+    std::vector<Index> indices(this->size());
     for(size_t j=0; j<this->size(); ++j)
       indices[j] = ordering[this->keys()[j]];
     return IndexFactor::shared_ptr(new IndexFactor(indices));
@@ -214,7 +228,7 @@ public:
    * If any of the optional Matrix reference arguments are specified, it should compute
    * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
    */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const = 0;
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
 
   /**
    * Vector of errors, whitened
@@ -250,12 +264,12 @@ public:
     // Create the set of terms - Jacobians for each index
     Vector b;
     // Call evaluate error to get Jacobians and b vector
-    FastVector<Matrix> A(this->size());
+    std::vector<Matrix> A(this->size());
     b = -unwhitenedError(x, A);
 
     this->noiseModel_->WhitenSystem(A,b);
 
-    FastVector<std::pair<Index, Matrix> > terms(this->size());
+    std::vector<std::pair<Index, Matrix> > terms(this->size());
     // Fill in terms
     for(size_t j=0; j<this->size(); ++j) {
       terms[j].first = ordering[this->keys()[j]];
@@ -325,7 +339,7 @@ public:
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X& x1 = x.at<X>(keys_[0]);
       if(H) {
@@ -408,7 +422,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X1& x1 = x.at<X1>(keys_[0]);
       const X2& x2 = x.at<X2>(keys_[1]);
@@ -498,7 +512,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
@@ -593,7 +607,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
   			return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -693,7 +707,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -799,7 +813,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<FastVector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index 24d8e0046..19b0c676d 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -59,29 +59,29 @@ TEST( GaussianJunctionTree, constructor2 )
 	// create an ordering
 	GaussianJunctionTree actual(fg);
 
-	FastVector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
-	FastVector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
-	FastVector<Index> frontal3; frontal3 += ordering["x1"];
-	FastVector<Index> frontal4; frontal4 += ordering["x7"];
-	FastVector<Index> sep1;
-	FastVector<Index> sep2; sep2 += ordering["x4"];
-	FastVector<Index> sep3; sep3 += ordering["x2"];
-	FastVector<Index> sep4; sep4 += ordering["x6"];
-	EXPECT(assert_container_equality(frontal1, actual.root()->frontal));
-	EXPECT(assert_container_equality(sep1,     actual.root()->separator));
+	vector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
+	vector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
+	vector<Index> frontal3; frontal3 += ordering["x1"];
+	vector<Index> frontal4; frontal4 += ordering["x7"];
+	vector<Index> sep1;
+	vector<Index> sep2; sep2 += ordering["x4"];
+	vector<Index> sep3; sep3 += ordering["x2"];
+	vector<Index> sep4; sep4 += ordering["x6"];
+	EXPECT(assert_equal(frontal1, actual.root()->frontal));
+	EXPECT(assert_equal(sep1,     actual.root()->separator));
 	LONGS_EQUAL(5,               actual.root()->size());
 	list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
   list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
   GaussianJunctionTree::sharedClique child0 = *child0it;
   GaussianJunctionTree::sharedClique child1 = *child1it;
-	EXPECT(assert_container_equality(frontal2, child0->frontal));
-	EXPECT(assert_container_equality(sep2,     child0->separator));
+	EXPECT(assert_equal(frontal2, child0->frontal));
+	EXPECT(assert_equal(sep2,     child0->separator));
 	LONGS_EQUAL(4,               child0->size());
-	EXPECT(assert_container_equality(frontal3, child0->children().front()->frontal));
-	EXPECT(assert_container_equality(sep3,     child0->children().front()->separator));
+	EXPECT(assert_equal(frontal3, child0->children().front()->frontal));
+	EXPECT(assert_equal(sep3,     child0->children().front()->separator));
 	LONGS_EQUAL(2,               child0->children().front()->size());
-	EXPECT(assert_container_equality(frontal4, child1->frontal));
-	EXPECT(assert_container_equality(sep4,     child1->separator));
+	EXPECT(assert_equal(frontal4, child1->frontal));
+	EXPECT(assert_equal(sep4,     child1->separator));
 	LONGS_EQUAL(2,               child1->size());
 }
 

From 0ae4a3f66bf4c5fb79a3a6616813257ffe0c8dd1 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 13 Feb 2012 20:27:56 +0000
Subject: [PATCH 61/88] Removed unused tuple constructor helper from
 NonlinearFactor

---
 gtsam/nonlinear/NonlinearFactor.h | 14 --------------
 1 file changed, 14 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 166ad6980..5ebe662e5 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -24,7 +24,6 @@
 #include <limits>
 
 #include <boost/serialization/base_object.hpp>
-#include <boost/tuple/tuple.hpp>
 
 #include <gtsam/inference/Factor-inl.h>
 #include <gtsam/inference/IndexFactor.h>
@@ -36,19 +35,6 @@
 
 namespace gtsam {
 
-using boost::make_tuple;
-
-// Helper function to fill a vector from a tuple function of any length
-template<typename CONS>
-inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
-  vector[position] = tuple.get_head();
-  __fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
-}
-template<>
-inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
-  // Do nothing
-}
-
 /* ************************************************************************* */
 /**
  * Nonlinear factor base class

From 7e9a4c1a2559dab93f41db4abbac29419233286a Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 13 Feb 2012 20:27:58 +0000
Subject: [PATCH 62/88] Explicit construction in expm

---
 gtsam/base/Lie.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index bc530a9f5..a770d9975 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
 template <class T>
 T expm(const Vector& x, int K=7) {
 	Matrix xhat = wedge<T>(x);
-	return expm(xhat,K);
+	return T(expm(xhat,K));
 }
 
 } // namespace gtsam

From 30bb9e61ccac634fe92af29bcaff8de19892ffec Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 13 Feb 2012 20:27:59 +0000
Subject: [PATCH 63/88] Workaround for possible bug in pool allocator

---
 gtsam/base/FastVector.h | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index cb08871d9..18af63006 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -50,14 +50,29 @@ public:
 
   /** Constructor from a range, passes through to base class */
   template<typename INPUTITERATOR>
-  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
+  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
+    // This if statement works around a bug in boost pool allocator and/or
+    // STL vector where if the size is zero, the pool allocator will allocate
+    // huge amounts of memory.
+    if(first != last)
+      Base::assign(first, last);
+  }
 
   /** Copy constructor from another FastSet */
   FastVector(const FastVector<VALUE>& x) : Base(x) {}
 
-  /** Copy constructor from the base map class */
+  /** Copy constructor from the base class */
   FastVector(const Base& x) : Base(x) {}
 
+  /** Copy constructor from a standard STL container */
+  FastVector(const std::vector<VALUE>& x) {
+    // This if statement works around a bug in boost pool allocator and/or
+    // STL vector where if the size is zero, the pool allocator will allocate
+    // huge amounts of memory.
+    if(x.size() > 0)
+      Base::assign(x.begin(), x.end());
+  }
+
 private:
   /** Serialization function */
   friend class boost::serialization::access;

From 6f09998e102075505e18c43bc14b2068bcfa43fb Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Mon, 13 Feb 2012 21:13:07 +0000
Subject: [PATCH 64/88] Removed autotools from gtsam2 branch

---
 CppUnitLite/Makefile.am     |  22 ---
 Makefile.am                 |  32 ----
 aminclude.am                | 186 ---------------------
 autogen.sh                  |   2 -
 configure.ac                | 191 ----------------------
 examples/Makefile.am        |  45 ------
 gtsam/3rdparty/Makefile.am  | 287 ---------------------------------
 gtsam/Makefile.am           |  15 --
 gtsam/base/Makefile.am      |  84 ----------
 gtsam/geometry/Makefile.am  |  81 ----------
 gtsam/inference/Makefile.am |  93 -----------
 gtsam/linear/Makefile.am    |  87 ----------
 gtsam/nonlinear/Makefile.am |  86 ----------
 gtsam/slam/Makefile.am      |  95 -----------
 m4/ac_doxygen.m4            | 312 ------------------------------------
 m4/ax_boost_base.m4         | 257 -----------------------------
 myconfigure                 |   1 -
 myconfigure.ardrone         |   1 -
 myconfigure.matlab          |   1 -
 myconfigure.overo           |   1 -
 myconfigure.profiling       |   3 -
 myconfigure.timing          |   3 -
 tests/Makefile.am           |  46 ------
 wrap/Makefile.am            | 146 -----------------
 24 files changed, 2077 deletions(-)
 delete mode 100644 CppUnitLite/Makefile.am
 delete mode 100644 Makefile.am
 delete mode 100644 aminclude.am
 delete mode 100755 autogen.sh
 delete mode 100644 configure.ac
 delete mode 100644 examples/Makefile.am
 delete mode 100644 gtsam/3rdparty/Makefile.am
 delete mode 100644 gtsam/Makefile.am
 delete mode 100644 gtsam/base/Makefile.am
 delete mode 100644 gtsam/geometry/Makefile.am
 delete mode 100644 gtsam/inference/Makefile.am
 delete mode 100644 gtsam/linear/Makefile.am
 delete mode 100644 gtsam/nonlinear/Makefile.am
 delete mode 100644 gtsam/slam/Makefile.am
 delete mode 100644 m4/ac_doxygen.m4
 delete mode 100644 m4/ax_boost_base.m4
 delete mode 100755 myconfigure
 delete mode 100755 myconfigure.ardrone
 delete mode 100755 myconfigure.matlab
 delete mode 100755 myconfigure.overo
 delete mode 100755 myconfigure.profiling
 delete mode 100755 myconfigure.timing
 delete mode 100644 tests/Makefile.am
 delete mode 100644 wrap/Makefile.am

diff --git a/CppUnitLite/Makefile.am b/CppUnitLite/Makefile.am
deleted file mode 100644
index 3e4dbdb05..000000000
--- a/CppUnitLite/Makefile.am
+++ /dev/null
@@ -1,22 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# CppUnitLite
-# replaced Makefile with automake for easy linking
-#----------------------------------------------------------------------------------------------------
-
-headers = TestHarness.h
-sources = Failure.cpp SimpleString.cpp Test.cpp TestRegistry.cpp TestResult.cpp
-headers += $(sources:.cpp=.h)
-
-if ENABLE_INSTALL_CPPUNITLITE
-CppUnitLitedir = $(includedir)/CppUnitLite
-lib_LIBRARIES = libCppUnitLite.a
-CppUnitLite_HEADERS = $(headers) 
-libCppUnitLite_a_SOURCES = $(sources)
-else 
-noinst_LIBRARIES = libCppUnitLite.a
-noinst_HEADERS = $(headers)
-libCppUnitLite_a_SOURCES = $(sources)
-endif
-
-
-
diff --git a/Makefile.am b/Makefile.am
deleted file mode 100644
index 8e68e7d6d..000000000
--- a/Makefile.am
+++ /dev/null
@@ -1,32 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM top-level automake file
-#----------------------------------------------------------------------------------------------------
-
-#The option -I m4 tells Autoconf to look for additional Autoconf macros in the m4 subdirectory.
-ACLOCAL_AMFLAGS = -I m4
-
-# make automake install some standard but missing files
-# also use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = foreign nostdinc
-
-# For Doxygen integration
-#include aminclude.am
-
-# All the sub-directories that need to be built
-SUBDIRS = CppUnitLite gtsam tests examples
-
-if ENABLE_BUILD_TOOLBOX
-SUBDIRS += wrap
-endif
-
-# Add these files to make sure they're in the distribution
-EXTRA_DIST = autogen.sh configure.ac COPYING INSTALL LGPL LICENSE README THANKS USAGE doc
-
-# For Doxygen integration
-#MOSTLYCLEANFILES = $(DX_CLEANFILES)
-#EXTRA_DIST += $(DX_CONFIG)
-
-# Remove .svn directories from dist
-dist-hook:
-	rm -rf `find $(distdir)/doc -type d -name .svn`
diff --git a/aminclude.am b/aminclude.am
deleted file mode 100644
index 09e6bf065..000000000
--- a/aminclude.am
+++ /dev/null
@@ -1,186 +0,0 @@
-# Copyright (C) 2004 Oren Ben-Kiki
-# This file is distributed under the same terms as the Automake macro files.
-
-# Generate automatic documentation using Doxygen. Goals and variables values
-# are controlled by the various DX_COND_??? conditionals set by autoconf.
-#
-# The provided goals are:
-# doc:         Generate all doxygen documentation.
-# doxygen-run: Run doxygen, which will generate some of the documentation
-#              (HTML, CHM, CHI, MAN, RTF, XML) but will not do the post
-#              processing required for the rest of it (PS, PDF, and some MAN).
-# doxygen-man: Rename some doxygen generated man pages.
-# doxygen-ps: Generate doxygen PostScript documentation.
-# doxygen-pdf: Generate doxygen PDF documentation.
-#
-# Note that by default these are not integrated into the automake goals. If
-# doxygen is used to generate man pages, you can achieve this integration by
-# setting man3_MANS to the list of man pages generated and then adding the
-# dependency:
-#
-#   $(man3_MANS): doxygen-doc
-#
-# This will cause make to run doxygen and generate all the documentation.
-#
-# The following variable is intended for use in Makefile.am:
-#
-# DX_CLEANFILES = everything to clean.
-#
-# This is usually added to MOSTLYCLEANFILES.
-
-## --------------------------------- ##
-## Format-independent Doxygen rules. ##
-## --------------------------------- ##
-
-if DX_COND_doc
-
-## ------------------------------- ##
-## Rules specific for HTML output. ##
-## ------------------------------- ##
-
-if DX_COND_html
-
-DX_CLEAN_HTML = @DX_DOCDIR@/html
-
-endif DX_COND_html
-
-## ------------------------------ ##
-## Rules specific for CHM output. ##
-## ------------------------------ ##
-
-if DX_COND_chm
-
-DX_CLEAN_CHM = @DX_DOCDIR@/chm
-
-if DX_COND_chi
-
-DX_CLEAN_CHI = @DX_DOCDIR@/@PACKAGE@.chi
-
-endif DX_COND_chi
-
-endif DX_COND_chm
-
-## ------------------------------ ##
-## Rules specific for MAN output. ##
-## ------------------------------ ##
-
-if DX_COND_man
-
-DX_CLEAN_MAN = @DX_DOCDIR@/man
-
-endif DX_COND_man
-
-## ------------------------------ ##
-## Rules specific for RTF output. ##
-## ------------------------------ ##
-
-if DX_COND_rtf
-
-DX_CLEAN_RTF = @DX_DOCDIR@/rtf
-
-endif DX_COND_rtf
-
-## ------------------------------ ##
-## Rules specific for XML output. ##
-## ------------------------------ ##
-
-if DX_COND_xml
-
-DX_CLEAN_XML = @DX_DOCDIR@/xml
-
-endif DX_COND_xml
-
-## ----------------------------- ##
-## Rules specific for PS output. ##
-## ----------------------------- ##
-
-if DX_COND_ps
-
-DX_CLEAN_PS = @DX_DOCDIR@/@PACKAGE@.ps
-
-DX_PS_GOAL = doxygen-ps
-
-doxygen-ps: @DX_DOCDIR@/@PACKAGE@.ps
-
-@DX_DOCDIR@/@PACKAGE@.ps: @DX_DOCDIR@/@PACKAGE@.tag
-	cd @DX_DOCDIR@/latex; \
-	rm -f *.aux *.toc *.idx *.ind *.ilg *.log *.out; \
-	$(DX_LATEX) refman.tex; \
-	$(MAKEINDEX_PATH) refman.idx; \
-	$(DX_LATEX) refman.tex; \
-	countdown=5; \
-	while $(DX_EGREP) 'Rerun (LaTeX|to get cross-references right)' \
-	                  refman.log > /dev/null 2>&1 \
-	   && test $$countdown -gt 0; do \
-	    $(DX_LATEX) refman.tex; \
-	    countdown=`expr $$countdown - 1`; \
-	done; \
-	$(DX_DVIPS) -o ../@PACKAGE@.ps refman.dvi
-
-endif DX_COND_ps
-
-## ------------------------------ ##
-## Rules specific for PDF output. ##
-## ------------------------------ ##
-
-if DX_COND_pdf
-
-DX_CLEAN_PDF = @DX_DOCDIR@/@PACKAGE@.pdf
-
-DX_PDF_GOAL = doxygen-pdf
-
-doxygen-pdf: @DX_DOCDIR@/@PACKAGE@.pdf
-
-@DX_DOCDIR@/@PACKAGE@.pdf: @DX_DOCDIR@/@PACKAGE@.tag
-	cd @DX_DOCDIR@/latex; \
-	rm -f *.aux *.toc *.idx *.ind *.ilg *.log *.out; \
-	$(DX_PDFLATEX) refman.tex; \
-	$(DX_MAKEINDEX) refman.idx; \
-	$(DX_PDFLATEX) refman.tex; \
-	countdown=5; \
-	while $(DX_EGREP) 'Rerun (LaTeX|to get cross-references right)' \
-	                  refman.log > /dev/null 2>&1 \
-	   && test $$countdown -gt 0; do \
-	    $(DX_PDFLATEX) refman.tex; \
-	    countdown=`expr $$countdown - 1`; \
-	done; \
-	mv refman.pdf ../@PACKAGE@.pdf
-
-endif DX_COND_pdf
-
-## ------------------------------------------------- ##
-## Rules specific for LaTeX (shared for PS and PDF). ##
-## ------------------------------------------------- ##
-
-if DX_COND_latex
-
-DX_CLEAN_LATEX = @DX_DOCDIR@/latex
-
-endif DX_COND_latex
-
-.PHONY: doxygen-run doc $(DX_PS_GOAL) $(DX_PDF_GOAL)
-
-.INTERMEDIATE: doxygen-run $(DX_PS_GOAL) $(DX_PDF_GOAL)
-
-doxygen-run: @DX_DOCDIR@/@PACKAGE@.tag
-
-doc: doxygen-run $(DX_PS_GOAL) $(DX_PDF_GOAL)
-
-@DX_DOCDIR@/@PACKAGE@.tag: $(DX_CONFIG) $(pkginclude_HEADERS)
-	rm -rf @DX_DOCDIR@
-	$(DX_ENV) $(DX_DOXYGEN) $(srcdir)/$(DX_CONFIG)
-
-DX_CLEANFILES = \
-    @DX_DOCDIR@/@PACKAGE@.tag \
-    -r \
-    $(DX_CLEAN_HTML) \
-    $(DX_CLEAN_CHM) \
-    $(DX_CLEAN_CHI) \
-    $(DX_CLEAN_MAN) \
-    $(DX_CLEAN_RTF) \
-    $(DX_CLEAN_XML) \
-    $(DX_CLEAN_PS) \
-    $(DX_CLEAN_PDF) \
-    $(DX_CLEAN_LATEX)
-
-endif DX_COND_doc
diff --git a/autogen.sh b/autogen.sh
deleted file mode 100755
index fd0be89ce..000000000
--- a/autogen.sh
+++ /dev/null
@@ -1,2 +0,0 @@
-#!/bin/sh
-autoreconf --force --install -I config -I m4
diff --git a/configure.ac b/configure.ac
deleted file mode 100644
index 6bcbfe015..000000000
--- a/configure.ac
+++ /dev/null
@@ -1,191 +0,0 @@
-#                                               -*- Autoconf -*-
-# Process this file with autoconf to produce a configure script.
-
-AC_PREREQ(2.59)
-AC_INIT(gtsam, 0.9.3, dellaert@cc.gatech.edu)
-AM_INIT_AUTOMAKE(gtsam, 0.9.3)
-AC_CONFIG_MACRO_DIR([m4])
-AC_CONFIG_HEADER([config.h])
-AC_CONFIG_SRCDIR([CppUnitLite/Test.cpp])
-AC_CONFIG_SRCDIR([wrap/wrap.cpp])
-AC_CONFIG_SRCDIR([gtsam/base/DSFVector.cpp])
-AC_CONFIG_SRCDIR([gtsam/geometry/Cal3_S2.cpp])
-AC_CONFIG_SRCDIR([gtsam/inference/SymbolicFactorGraph.cpp])
-AC_CONFIG_SRCDIR([gtsam/linear/GaussianFactor.cpp])
-AC_CONFIG_SRCDIR([gtsam/nonlinear/NonlinearOptimizer.cpp])
-AC_CONFIG_SRCDIR([gtsam/slam/pose2SLAM.cpp])
-AC_CONFIG_SRCDIR([tests/testTupleValues.cpp])
-AC_CONFIG_SRCDIR([examples/SimpleRotation.cpp])
-AC_CONFIG_SRCDIR([gtsam/3rdparty/Makefile.am])
-
-# For doxygen support
-#DX_HTML_FEATURE(ON)
-#DX_CHM_FEATURE(OFF)
-#DX_CHI_FEATURE(OFF)
-#DX_MAN_FEATURE(OFF)
-#DX_RTF_FEATURE(OFF)
-#DX_XML_FEATURE(OFF)
-#DX_PDF_FEATURE(OFF)
-#DX_PS_FEATURE(OFF)
-#DX_INIT_DOXYGEN(gtsam, Doxyfile, doc)
-
-# Check for OS
-# needs to be called at some point earlier
-AC_CANONICAL_HOST    
-AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac])
-AM_CONDITIONAL([LINUX], [case $host_os in linux*) true;; *) false;; esac])
-AM_CONDITIONAL([IS_64BIT], [case $host_cpu in *x86_64*) true;; *) false;; esac])
-
-# enable debug variable
-AC_ARG_ENABLE([debug],
-     [  --enable-debug    Turn on debugging],
-     [case "${enableval}" in
-       yes) debug=true ;;
-       no)  debug=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-debug]) ;;
-     esac],[debug=false])
-
-AM_CONDITIONAL([DEBUG], [test x$debug = xtrue])
-
-# enable profiling
-AC_ARG_ENABLE([profiling],
-     [  --enable-profiling    Enable profiling],
-     [case "${enableval}" in
-       yes) profiling=true ;;
-       no)  profiling=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-profiling]) ;;
-     esac],[profiling=false])
-
-AM_CONDITIONAL([USE_PROFILING], [test x$profiling = xtrue])
-
-# enable serialization in serialization test
-AC_ARG_ENABLE([serialization],
-     [  --enable-serialization    Enable serialization with boost serialization],
-     [case "${enableval}" in
-       yes) serialization=true ;;
-       no)  serialization=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-serialization]) ;;
-     esac],[serialization=false])
-
-AM_CONDITIONAL([ENABLE_SERIALIZATION], [test x$serialization = xtrue])
-
-# enable installation of CppUnitLite with gtsam
-AC_ARG_ENABLE([install_cppunitlite],
-     [  --enable-install-cppunitlite    Enable installation of CppUnitLite],
-     [case "${enableval}" in
-       yes) install_cppunitlite=true ;;
-       no)  install_cppunitlite=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-cppunitlite]) ;;
-     esac],[install_cppunitlite=false])
-
-AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue])
-
-# enable matlab toolbox generation
-AC_ARG_ENABLE([build_toolbox],
-     [  --enable-build-toolbox    Enable building of the Matlab toolbox],
-     [case "${enableval}" in
-       yes) build_toolbox=true ;;
-       no)  build_toolbox=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-build-toolbox]) ;;
-     esac],[build_toolbox=false])
-
-AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue])
-
-# enable installation of matlab tests
-AC_ARG_ENABLE([install_matlab_tests],
-     [  --enable-install-matlab-tests    Enable installation of tests for the Matlab toolbox],
-     [case "${enableval}" in
-       yes) install_matlab_tests=true ;;
-       no)  install_matlab_tests=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-tests]) ;;
-     esac],[install_matlab_tests=true])
-
-AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue])
-
-# enable installation of matlab examples
-AC_ARG_ENABLE([install_matlab_examples],
-     [  --enable-install-matlab-examples    Enable installation of examples for the Matlab toolbox],
-     [case "${enableval}" in
-       yes) install_matlab_examples=true ;;
-       no)  install_matlab_examples=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-examples]) ;;
-     esac],[install_matlab_examples=true])
-
-AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_EXAMPLES], [test x$install_matlab_examples = xtrue])
-
-# Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix
-AC_ARG_WITH([toolbox],
-            [AS_HELP_STRING([--with-toolbox],
-              [specify the matlab toolbox directory for installation])],
-            [toolbox=$withval],
-            [toolbox=$prefix])
-AC_SUBST([toolbox])
-
-# enable installation of the wrap utility
-AC_ARG_ENABLE([install_wrap],
-     [  --enable-install-wrap    Enable installation of the wrap tool for generating matlab interfaces],
-     [case "${enableval}" in
-       yes) install_wrap=true ;;
-       no)  install_wrap=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-wrap]) ;;
-     esac],[install_wrap=false])
-
-AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue])
-
-# enable unsafe mode for wrap
-AC_ARG_ENABLE([unsafe_wrap],
-     [  --enable-unsafe-wrap    Enable using unsafe mode in wrap],
-     [case "${enableval}" in
-       yes) unsafe_wrap=true ;;
-       no)  unsafe_wrap=false ;;
-       *) AC_MSG_ERROR([bad value ${enableval} for --enable-unsafe-wrap]) ;;
-     esac],[unsafe_wrap=false])
-
-AM_CONDITIONAL([ENABLE_UNSAFE_WRAP], [test x$unsafe_wrap = xtrue])
-
-# wrap install path: optional flag to change location of wrap, defaults to install prefix/bin
-AC_ARG_WITH([wrap],
-            [AS_HELP_STRING([--with-wrap],
-              [specify the wrap directory for installation])],
-            [wrap=$withval],
-            [wrap=$prefix/bin])
-AC_SUBST([wrap])
-
-# Checks for programs.
-AC_PROG_CXX
-AC_PROG_CC
-
-# Checks for libraries.
-LT_INIT
-
-# Checks for header files.
-AC_HEADER_STDC
-AC_CHECK_HEADERS([string.h])
-
-# Checks for typedefs, structures, and compiler characteristics.
-AC_HEADER_STDBOOL
-AC_C_CONST
-AC_C_INLINE
-AC_TYPE_SIZE_T
-
-# Checks for library functions.
-AC_FUNC_ERROR_AT_LINE
-AC_CHECK_FUNCS([pow sqrt])
-
-# Check for boost
-AX_BOOST_BASE([1.40])
-
-AC_CONFIG_FILES([CppUnitLite/Makefile \
-wrap/Makefile \
-gtsam/3rdparty/Makefile \
-gtsam/base/Makefile \
-gtsam/geometry/Makefile \
-gtsam/inference/Makefile \
-gtsam/linear/Makefile \
-gtsam/nonlinear/Makefile \
-gtsam/slam/Makefile gtsam/Makefile \
-tests/Makefile \
-examples/Makefile \
-examples/vSLAMexample/Makefile \
-Makefile])
-AC_OUTPUT
diff --git a/examples/Makefile.am b/examples/Makefile.am
deleted file mode 100644
index d7ec2aaea..000000000
--- a/examples/Makefile.am
+++ /dev/null
@@ -1,45 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM Examples
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS =
-
-# Examples
-noinst_PROGRAMS  = SimpleRotation		      		# Optimizes a single nonlinear rotation variable
-noinst_PROGRAMS += PlanarSLAMExample_easy			# Solves SLAM example from tutorial by using planarSLAM
-noinst_PROGRAMS += PlanarSLAMSelfContained_advanced	# Solves SLAM example from tutorial with all typedefs in the script
-noinst_PROGRAMS += Pose2SLAMExample_easy 			# Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
-noinst_PROGRAMS += Pose2SLAMExample_advanced		# Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
-#noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
-#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
-noinst_PROGRAMS += elaboratePoint2KalmanFilter 		# simple linear Kalman filter on a moving 2D point, but done using factor graphs
-noinst_PROGRAMS += easyPoint2KalmanFilter 		    # uses the cool generic templated Kalman filter class to do the same
-noinst_PROGRAMS += CameraResectioning
-
-EXTRA_DIST = Data
-dist-hook:
-	rm -rf $(distdir)/Data/.svn
-
-SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-LDADD = ../gtsam/libgtsam.la
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./%
-#----------------------------------------------------------------------------------------------------
diff --git a/gtsam/3rdparty/Makefile.am b/gtsam/3rdparty/Makefile.am
deleted file mode 100644
index e5a930ca5..000000000
--- a/gtsam/3rdparty/Makefile.am
+++ /dev/null
@@ -1,287 +0,0 @@
-# 3rd Party libraries to be built and installed along with gtsam
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-# set up the folders for includes
-3rdpartydir = $(pkgincludedir)/3rdparty
-3rdparty_includedir = $(includedir)/gtsam/3rdparty
-nobase_3rdparty_HEADERS  =
-
-# CCOLAMD (with UFconfig files included)
-# FIXME: ccolamd requires a -I setting for every header file
-ccolamd_inc = $(top_srcdir)/gtsam/3rdparty/CCOLAMD/Include
-ufconfig_inc = $(top_srcdir)/gtsam/3rdparty/UFconfig
-headers = CCOLAMD/Include/ccolamd.h UFconfig/UFconfig.h
-sources = CCOLAMD/Source/ccolamd.c CCOLAMD/Source/ccolamd_global.c UFconfig/UFconfig.c 
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam/3rdparty: 
-#----------------------------------------------------------------------------------------------------
-nobase_3rdparty_HEADERS += $(headers)
-noinst_LTLIBRARIES = libccolamd.la
-libccolamd_la_SOURCES = $(sources)
-
-AM_CPPFLAGS = 
-AM_CPPFLAGS += -I$(ccolamd_inc) -I$(ufconfig_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir) 
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-
-# Eigen Installation - just copies the headers
-eigen_path = 
-eigen_path += Eigen/Eigen  
-nobase_3rdparty_HEADERS += $(eigen_path)/Array         $(eigen_path)/LeastSquares
-nobase_3rdparty_HEADERS += $(eigen_path)/Cholesky      $(eigen_path)/LU
-nobase_3rdparty_HEADERS += $(eigen_path)/Core          $(eigen_path)/QR
-nobase_3rdparty_HEADERS += $(eigen_path)/Dense         $(eigen_path)/QtAlignedMalloc
-nobase_3rdparty_HEADERS += $(eigen_path)/Eigen         $(eigen_path)/Sparse
-nobase_3rdparty_HEADERS += $(eigen_path)/Eigen2Support $(eigen_path)/StdDeque
-nobase_3rdparty_HEADERS += $(eigen_path)/Eigenvalues   $(eigen_path)/StdList
-nobase_3rdparty_HEADERS += $(eigen_path)/Geometry      $(eigen_path)/StdVector
-nobase_3rdparty_HEADERS += $(eigen_path)/Householder   $(eigen_path)/SVD
-nobase_3rdparty_HEADERS += $(eigen_path)/Jacobi
-
-##./src/Cholesky:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Cholesky/LDLT.h  $(eigen_path)/src/Cholesky/LLT.h
-
-##./src/Core:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ArrayBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Array.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ArrayWrapper.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Assign.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/BandMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Block.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/BooleanRedux.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CommaInitializer.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseBinaryOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseNullaryOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseUnaryOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseUnaryView.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseCoeffsBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseStorage.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Diagonal.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DiagonalMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DiagonalProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Dot.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/EigenBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Flagged.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ForceAlignedAccess.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Functors.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Fuzzy.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/GenericPacketMath.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/GlobalFunctions.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/IO.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MapBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Map.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MathFunctions.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MatrixBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Matrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NestByValue.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NoAlias.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NumTraits.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/PermutationMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/PlainObjectBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ProductBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Product.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Random.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Redux.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Replicate.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ReturnByValue.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Reverse.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Select.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SelfAdjointView.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SelfCwiseBinaryOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SolveTriangular.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/StableNorm.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Stride.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Swap.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Transpose.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Transpositions.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/TriangularMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/VectorBlock.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/VectorwiseOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Visitor.h
-
-##./src/Core/arch/AltiVec:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/AltiVec/Complex.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/AltiVec/PacketMath.h
-
-##./src/Core/arch/Default:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/Default/Settings.h
-
-##./src/Core/arch/NEON:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/NEON/Complex.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/NEON/PacketMath.h
-
-##./src/Core/arch/SSE:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/Complex.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/MathFunctions.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/PacketMath.h
-
-##./src/Core/products:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/CoeffBasedProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralBlockPanelKernel.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixMatrixTriangular.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixVector.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/Parallelizer.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointMatrixMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointMatrixVector.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointRank2Update.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularMatrixMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularMatrixVector.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularSolverMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularSolverVector.h
-
-##./src/Core/util:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/BlasUtil.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Constants.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/DisableStupidWarnings.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/ForwardDeclarations.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Macros.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Memory.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Meta.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/ReenableStupidWarnings.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/StaticAssert.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/XprHelper.h
-
-##./src/Eigen2Support:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Block.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Cwise.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/CwiseOperators.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Lazy.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/LeastSquares.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/LU.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Macros.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/MathFunctions.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Memory.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Meta.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Minor.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/QR.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/SVD.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/TriangularSolver.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/VectorBlock.h
-
-##./src/Eigen2Support/Geometry:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/AlignedBox.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/All.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/AngleAxis.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Hyperplane.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/ParametrizedLine.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Quaternion.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Rotation2D.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/RotationBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Scaling.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Transform.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Translation.h
-
-##./src/Eigenvalues:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/ComplexEigenSolver.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/ComplexSchur.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/EigenSolver.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/EigenvaluesCommon.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/HessenbergDecomposition.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/MatrixBaseEigenvalues.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/RealSchur.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/SelfAdjointEigenSolver.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/Tridiagonalization.h
-
-##./src/Geometry:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/AlignedBox.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/AngleAxis.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/EulerAngles.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Homogeneous.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Hyperplane.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/OrthoMethods.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/ParametrizedLine.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Quaternion.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Rotation2D.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/RotationBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Scaling.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Transform.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Translation.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Umeyama.h
-
-##./src/Geometry/arch:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/arch/Geometry_SSE.h
-
-##./src/Householder:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/BlockHouseholder.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/Householder.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/HouseholderSequence.h
-
-##./src/Jacobi:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Jacobi/Jacobi.h
-
-##./src/LU:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/Determinant.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/FullPivLU.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/Inverse.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/PartialPivLU.h
-
-##./src/LU/arch:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/arch/Inverse_SSE.h
-
-##./src/misc:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Image.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Kernel.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Solve.h
-
-##./src/plugins:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/ArrayCwiseBinaryOps.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/ArrayCwiseUnaryOps.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/BlockMethods.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/CommonCwiseBinaryOps.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/CommonCwiseUnaryOps.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/MatrixCwiseBinaryOps.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/MatrixCwiseUnaryOps.h
-
-##./src/QR:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/ColPivHouseholderQR.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/FullPivHouseholderQR.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/HouseholderQR.h
-
-##./src/Sparse:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/AmbiVector.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/CompressedStorage.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/CoreIterators.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/DynamicSparseMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/MappedSparseMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseAssign.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseBlock.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseCwiseBinaryOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseCwiseUnaryOp.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDenseProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDiagonalProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDot.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseFuzzy.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseMatrixBase.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseMatrix.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseRedux.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseSelfAdjointView.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseSparseProduct.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseTranspose.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseTriangularView.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseUtil.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseVector.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseView.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/TriangularSolver.h
-
-##./src/StlSupport:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/details.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdDeque.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdList.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdVector.h
-
-##./src/SVD:
-nobase_3rdparty_HEADERS += $(eigen_path)/src/SVD/JacobiSVD.h
-nobase_3rdparty_HEADERS += $(eigen_path)/src/SVD/UpperBidiagonalization.h
-
-
-
diff --git a/gtsam/Makefile.am b/gtsam/Makefile.am
deleted file mode 100644
index 1a09b5b63..000000000
--- a/gtsam/Makefile.am
+++ /dev/null
@@ -1,15 +0,0 @@
-
-# All the sub-directories that need to be built
-SUBDIRS = 3rdparty base geometry inference linear nonlinear slam 
-
-# And the corresponding libraries produced
-SUBLIBS = 3rdparty/libccolamd.la base/libbase.la geometry/libgeometry.la \
-					inference/libinference.la linear/liblinear.la nonlinear/libnonlinear.la \
-					 slam/libslam.la 
-
-# The following lines specify the actual shared library to be built with libtool
-lib_LTLIBRARIES = libgtsam.la
-libgtsam_la_SOURCES = 
-nodist_EXTRA_libgtsam_la_SOURCES = dummy.cxx
-libgtsam_la_LIBADD = $(SUBLIBS) $(BOOST_LDFLAGS) 
-libgtsam_la_LDFLAGS = -no-undefined -version-info 0:0:0
diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am
deleted file mode 100644
index 25abacbaf..000000000
--- a/gtsam/base/Makefile.am
+++ /dev/null
@@ -1,84 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM base
-# provides some base Math and data structures, as well as test-related utilities
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS = 
-
-# base Math
-
-headers += FixedVector.h types.h blockMatrices.h 
-sources += Vector.cpp Matrix.cpp 
-sources += cholesky.cpp
-check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices 
-check_PROGRAMS += tests/testCholesky
-check_PROGRAMS += tests/testNumericalDerivative
-
-# Testing
-headers += Testable.h TestableAssertions.h numericalDerivative.h
-sources += timing.cpp debug.cpp
-check_PROGRAMS += tests/testDebug tests/testTestableAssertions
-
-# Manifolds and Lie Groups
-headers += DerivedValue.h Value.h Manifold.h Group.h
-headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
-sources += LieVector.cpp
-check_PROGRAMS += tests/testLieVector tests/testLieScalar
-
-# Data structures
-headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h
-sources += DSFVector.cpp
-check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector
-
-## flag disabled to force this test to get updated properly
-if ENABLE_SERIALIZATION
-check_PROGRAMS += tests/testSerializationBase
-endif
-
-# Timing tests
-noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest
-noinst_PROGRAMS += tests/timeMatrixOps 
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam: 
-#----------------------------------------------------------------------------------------------------
-headers += $(sources:.cpp=.h)
-basedir = $(pkgincludedir)/base
-base_HEADERS = $(headers)
-noinst_LTLIBRARIES = libbase.la
-libbase_la_SOURCES = $(sources)
-
-AM_CPPFLAGS =
-
-AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-
-# link to serialization library for test 
-if ENABLE_SERIALIZATION
-tests_testSerializationBase_LDFLAGS = -lboost_serialization
-endif
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_DEFAULT_SOURCE_EXT = .cpp
-AM_LDFLAGS += $(boost_serialization) 
-LDADD = libbase.la ../../CppUnitLite/libCppUnitLite.a 
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-#----------------------------------------------------------------------------------------------------
diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am
deleted file mode 100644
index b87ba8d07..000000000
--- a/gtsam/geometry/Makefile.am
+++ /dev/null
@@ -1,81 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM geometry
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS = 
-
-# Concept check
-headers += concepts.h
-
-# Points and poses
-sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Pose3.cpp
-check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3
-
-# Cameras
-headers += GeneralCameraT.h Cal3_S2Stereo.h PinholeCamera.h
-sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp
-check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera
-
-# Stereo
-sources += StereoPoint2.cpp StereoCamera.cpp
-check_PROGRAMS += tests/testStereoCamera tests/testStereoPoint2
-
-# Tensors
-headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h
-headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h
-sources += projectiveGeometry.cpp tensorInterface.cpp
-check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
-
-## flag disabled to force this test to get updated properly
-if ENABLE_SERIALIZATION
-check_PROGRAMS += tests/testSerializationGeometry
-endif
-
-# Timing tests
-noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera
-
-# Rot3M and Rot3Q both use Rot3.h, they do not have individual header files
-allsources = $(sources)
-allsources += Rot3M.cpp Rot3Q.cpp
-headers += Rot3.h
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam: 
-#----------------------------------------------------------------------------------------------------
-headers += $(sources:.cpp=.h)
-geometrydir = $(pkgincludedir)/geometry
-geometry_HEADERS = $(headers)
-noinst_LTLIBRARIES = libgeometry.la
-libgeometry_la_SOURCES = $(allsources)
-AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-
-# link to serialization library for test 
-if ENABLE_SERIALIZATION
-tests_testSerializationGeometry_LDFLAGS = -lboost_serialization
-endif
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_LDFLAGS += $(boost_serialization) 
-LDADD = libgeometry.la ../base/libbase.la ../../CppUnitLite/libCppUnitLite.a
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-#----------------------------------------------------------------------------------------------------
diff --git a/gtsam/inference/Makefile.am b/gtsam/inference/Makefile.am
deleted file mode 100644
index cab5c5cbb..000000000
--- a/gtsam/inference/Makefile.am
+++ /dev/null
@@ -1,93 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM core functionality: base classes for inference, as well as symbolic and discrete
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS = 
-
-#----------------------------------------------------------------------------------------------------
-# base
-#----------------------------------------------------------------------------------------------------
-
-# GTSAM core
-headers += Factor.h Factor-inl.h Conditional.h 
-
-# Symbolic Inference
-sources += SymbolicFactorGraph.cpp SymbolicMultifrontalSolver.cpp SymbolicSequentialSolver.cpp
-check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional
-check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots
-
-# Inference
-headers += GenericMultifrontalSolver.h GenericMultifrontalSolver-inl.h GenericSequentialSolver.h GenericSequentialSolver-inl.h
-sources += inference.cpp VariableSlots.cpp Permutation.cpp VariableIndex.cpp
-sources += IndexFactor.cpp IndexConditional.cpp
-headers += graph.h graph-inl.h
-headers += FactorGraph.h FactorGraph-inl.h
-headers += ClusterTree.h ClusterTree-inl.h
-headers += JunctionTree.h JunctionTree-inl.h
-headers += EliminationTree.h EliminationTree-inl.h
-headers += BayesNet.h BayesNet-inl.h
-headers += BayesTree.h BayesTree-inl.h
-headers += BayesTreeCliqueBase.h BayesTreeCliqueBase-inl.h
-headers += ISAM.h ISAM-inl.h
-check_PROGRAMS += tests/testInference
-check_PROGRAMS += tests/testFactorGraph
-check_PROGRAMS += tests/testFactorGraph
-check_PROGRAMS += tests/testBayesTree 
-check_PROGRAMS += tests/testISAM
-check_PROGRAMS += tests/testEliminationTree
-check_PROGRAMS += tests/testClusterTree
-check_PROGRAMS += tests/testJunctionTree
-check_PROGRAMS += tests/testPermutation
-
-## flag disabled to force this test to get updated properly
-if ENABLE_SERIALIZATION
-check_PROGRAMS += tests/testSerializationInference
-endif
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam: 
-#----------------------------------------------------------------------------------------------------
-
-# CCOLAMD include flags are needed due to the bad include paths within the library
-# but will not be exposed to users.  
-ccolamd_inc = -I$(top_srcdir)/gtsam/3rdparty/CCOLAMD/Include -I$(top_srcdir)/gtsam/3rdparty/UFconfig
-
-headers += $(sources:.cpp=.h)
-inferencedir = $(pkgincludedir)/inference
-inference_HEADERS = $(headers)
-noinst_LTLIBRARIES = libinference.la
-libinference_la_SOURCES = $(sources)
-AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-AM_CXXFLAGS =
-
-# link to serialization library for test 
-if ENABLE_SERIALIZATION
-tests_testSerializationInference_LDFLAGS = -lboost_serialization
-endif
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_LDFLAGS += $(boost_serialization)
-LDADD = libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
-LDADD += ../../CppUnitLite/libCppUnitLite.a 
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-
diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am
deleted file mode 100644
index bd78bae52..000000000
--- a/gtsam/linear/Makefile.am
+++ /dev/null
@@ -1,87 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM linear: inference in Gaussian factor graphs
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS = 
-
-# Noise Model
-headers += SharedGaussian.h SharedDiagonal.h  SharedNoiseModel.h
-sources += NoiseModel.cpp Errors.cpp Sampler.cpp
-check_PROGRAMS += tests/testNoiseModel tests/testErrors tests/testSampler
-
-# Vector Configurations
-sources += VectorValues.cpp 
-check_PROGRAMS += tests/testVectorValues
-
-# Solvers
-sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp
-
-# Gaussian Factor Graphs
-sources += JacobianFactor.cpp HessianFactor.cpp
-sources += GaussianFactor.cpp GaussianFactorGraph.cpp
-sources += GaussianJunctionTree.cpp 
-sources += GaussianConditional.cpp GaussianDensity.cpp GaussianBayesNet.cpp
-sources += GaussianISAM.cpp
-check_PROGRAMS += tests/testHessianFactor tests/testJacobianFactor tests/testGaussianConditional
-check_PROGRAMS +=  tests/testGaussianDensity tests/testGaussianFactorGraph tests/testGaussianJunctionTree
-
-# Kalman Filter
-sources += KalmanFilter.cpp
-check_PROGRAMS += tests/testKalmanFilter
-
-# Iterative Methods
-headers += iterative-inl.h
-sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp
-headers += IterativeSolver.h IterativeOptimizationParameters.h 
-headers += SubgraphSolver-inl.h
-
-## flag disabled to force this test to get updated properly
-if ENABLE_SERIALIZATION
-check_PROGRAMS += tests/testSerializationLinear
-endif
-
-# Timing tests
-noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam: 
-#----------------------------------------------------------------------------------------------------
-headers += $(sources:.cpp=.h)
-lineardir = $(pkgincludedir)/linear
-linear_HEADERS = $(headers)
-noinst_LTLIBRARIES = liblinear.la
-liblinear_la_SOURCES = $(sources)
-AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-AM_CXXFLAGS =
-
-# link to serialization library for test 
-if ENABLE_SERIALIZATION
-tests_testSerializationLinear_LDFLAGS = -lboost_serialization
-endif
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_LDFLAGS += $(boost_serialization)
-LDADD = liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
-LDADD += ../../CppUnitLite/libCppUnitLite.a
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
deleted file mode 100644
index 41b6b3b8d..000000000
--- a/gtsam/nonlinear/Makefile.am
+++ /dev/null
@@ -1,86 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM nonlinear
-# Non-linear optimization
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS = 
-
-#----------------------------------------------------------------------------------------------------
-# nonlinear
-#----------------------------------------------------------------------------------------------------
-
-# Lie Groups
-headers += Values-inl.h
-sources += Values.cpp
-check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
-
-# Nonlinear nonlinear
-headers += Symbol.h 
-headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h 
-headers += NonlinearFactor.h
-sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp
-headers += DoglegOptimizer.h DoglegOptimizer-inl.h
-
-# Nonlinear iSAM(2)
-headers += NonlinearISAM.h NonlinearISAM-inl.h
-headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h
-sources += GaussianISAM2.cpp
-headers += GaussianISAM2-inl.h
-
-# Nonlinear constraints 
-headers += NonlinearEquality.h 
-
-# White noise factor
-headers += WhiteNoiseFactor.h
-#check_PROGRAMS += tests/testWhiteFactor
-
-# Kalman Filter
-headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h
-
-## flag disabled to force this test to get updated properly
-if ENABLE_SERIALIZATION
-check_PROGRAMS += tests/testSerializationNonlinear
-endif
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam: 
-#----------------------------------------------------------------------------------------------------
-headers += $(sources:.cpp=.h)
-nonlineardir = $(pkgincludedir)/nonlinear
-nonlinear_HEADERS = $(headers)
-noinst_LTLIBRARIES = libnonlinear.la
-libnonlinear_la_SOURCES = $(sources)
-AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-AM_CXXFLAGS =
-
-# link to serialization library for test 
-if ENABLE_SERIALIZATION
-tests_testSerializationNonlinear_LDFLAGS = -lboost_serialization
-endif
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_LDFLAGS += $(boost_serialization)
-LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la
-LDADD += ../../CppUnitLite/libCppUnitLite.a
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-
diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am
deleted file mode 100644
index cfa37c29d..000000000
--- a/gtsam/slam/Makefile.am
+++ /dev/null
@@ -1,95 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# SLAM and SFM sources
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS = 
-
-# simulated2D example
-headers += simulated2DConstraints.h
-sources += simulated2D.cpp smallExample.cpp
-check_PROGRAMS += tests/testSimulated2D 
-
-# simulated2DOriented example
-sources +=  simulated2DOriented.cpp 
-check_PROGRAMS += tests/testSimulated2DOriented
-
-# simulated3D example
-sources += simulated3D.cpp 
-check_PROGRAMS += tests/testSimulated3D 
-
-# Generic SLAM headers
-headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h
-headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h
-headers += AntiFactor.h
-check_PROGRAMS += tests/testAntiFactor 
-
-# Generic constraint headers
-headers += BoundingConstraint.h
-
-# 2D Pose SLAM
-sources += pose2SLAM.cpp dataset.cpp
-check_PROGRAMS += tests/testPose2SLAM 
-
-# 2D SLAM using Bearing and Range
-sources += planarSLAM.cpp
-check_PROGRAMS += tests/testPlanarSLAM
-
-# 3D Pose constraints
-sources += pose3SLAM.cpp 
-check_PROGRAMS += tests/testPose3SLAM 
-
-# Visual SLAM
-headers += GeneralSFMFactor.h ProjectionFactor.h
-sources += visualSLAM.cpp
-check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM 
-check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler
-
-# StereoFactor
-headers += StereoFactor.h
-check_PROGRAMS += tests/testStereoFactor
-
-## flag disabled to force this test to get updated properly
-if ENABLE_SERIALIZATION
-check_PROGRAMS += tests/testSerializationSLAM
-endif
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am 
-# The headers are installed in $(includedir)/gtsam: 
-#----------------------------------------------------------------------------------------------------
-headers += $(sources:.cpp=.h)
-slamdir = $(pkgincludedir)/slam
-slam_HEADERS = $(headers)
-noinst_LTLIBRARIES = libslam.la
-libslam_la_SOURCES = $(sources)
-AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-
-# link to serialization library for test 
-if ENABLE_SERIALIZATION
-tests_testSerializationSLAM_LDFLAGS = -lboost_serialization
-endif
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_DEFAULT_SOURCE_EXT = .cpp
-AM_LDFLAGS += $(boost_serialization) 
-LDADD = libslam.la ../geometry/libgeometry.la  ../nonlinear/libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la 
-LDADD += ../../CppUnitLite/libCppUnitLite.a
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
diff --git a/m4/ac_doxygen.m4 b/m4/ac_doxygen.m4
deleted file mode 100644
index e9c56c206..000000000
--- a/m4/ac_doxygen.m4
+++ /dev/null
@@ -1,312 +0,0 @@
-# This file is part of Autoconf.                       -*- Autoconf -*-
-
-# Copyright (C) 2004 Oren Ben-Kiki
-# This file is distributed under the same terms as the Autoconf macro files.
-
-# Generate automatic documentation using Doxygen. Works in concert with the
-# aminclude.m4 file and a compatible doxygen configuration file. Defines the
-# following public macros:
-#
-# DX_???_FEATURE(ON|OFF) - control the default setting fo a Doxygen feature.
-# Supported features are 'DOXYGEN' itself, 'DOT' for generating graphics,
-# 'HTML' for plain HTML, 'CHM' for compressed HTML help (for MS users), 'CHI'
-# for generating a seperate .chi file by the .chm file, and 'MAN', 'RTF',
-# 'XML', 'PDF' and 'PS' for the appropriate output formats. The environment
-# variable DOXYGEN_PAPER_SIZE may be specified to override the default 'a4wide'
-# paper size.
-#
-# By default, HTML, PDF and PS documentation is generated as this seems to be
-# the most popular and portable combination. MAN pages created by Doxygen are
-# usually problematic, though by picking an appropriate subset and doing some
-# massaging they might be better than nothing. CHM and RTF are specific for MS
-# (note that you can't generate both HTML and CHM at the same time). The XML is
-# rather useless unless you apply specialized post-processing to it.
-#
-# The macro mainly controls the default state of the feature. The use can
-# override the default by specifying --enable or --disable. The macros ensure
-# that contradictory flags are not given (e.g., --enable-doxygen-html and
-# --enable-doxygen-chm, --enable-doxygen-anything with --disable-doxygen, etc.)
-# Finally, each feature will be automatically disabled (with a warning) if the
-# required programs are missing.
-#
-# Once all the feature defaults have been specified, call DX_INIT_DOXYGEN with
-# the following parameters: a one-word name for the project for use as a
-# filename base etc., an optional configuration file name (the default is
-# 'Doxyfile', the same as Doxygen's default), and an optional output directory
-# name (the default is 'doxygen-doc').
-
-## ----------##
-## Defaults. ##
-## ----------##
-
-DX_ENV=""
-AC_DEFUN([DX_FEATURE_doc],  ON)
-AC_DEFUN([DX_FEATURE_dot],  ON)
-AC_DEFUN([DX_FEATURE_man],  OFF)
-AC_DEFUN([DX_FEATURE_html], ON)
-AC_DEFUN([DX_FEATURE_chm],  OFF)
-AC_DEFUN([DX_FEATURE_chi],  OFF)
-AC_DEFUN([DX_FEATURE_rtf],  OFF)
-AC_DEFUN([DX_FEATURE_xml],  OFF)
-AC_DEFUN([DX_FEATURE_pdf],  ON)
-AC_DEFUN([DX_FEATURE_ps],   ON)
-
-## --------------- ##
-## Private macros. ##
-## --------------- ##
-
-# DX_ENV_APPEND(VARIABLE, VALUE)
-# ------------------------------
-# Append VARIABLE="VALUE" to DX_ENV for invoking doxygen.
-AC_DEFUN([DX_ENV_APPEND], [AC_SUBST([DX_ENV], ["$DX_ENV $1='$2'"])])
-
-# DX_DIRNAME_EXPR
-# ---------------
-# Expand into a shell expression prints the directory part of a path.
-AC_DEFUN([DX_DIRNAME_EXPR],
-         [[expr ".$1" : '\(\.\)[^/]*$' \| "x$1" : 'x\(.*\)/[^/]*$']])
-
-# DX_IF_FEATURE(FEATURE, IF-ON, IF-OFF)
-# -------------------------------------
-# Expands according to the M4 (static) status of the feature.
-AC_DEFUN([DX_IF_FEATURE], [ifelse(DX_FEATURE_$1, ON, [$2], [$3])])
-
-# DX_REQUIRE_PROG(VARIABLE, PROGRAM)
-# ----------------------------------
-# Require the specified program to be found for the DX_CURRENT_FEATURE to work.
-AC_DEFUN([DX_REQUIRE_PROG], [
-AC_PATH_TOOL([$1], [$2])
-if test "$DX_FLAG_[]DX_CURRENT_FEATURE$$1" = 1; then
-    AC_MSG_WARN([$2 not found - will not DX_CURRENT_DESCRIPTION])
-    AC_SUBST([DX_FLAG_]DX_CURRENT_FEATURE, 0)
-fi
-])
-
-# DX_TEST_FEATURE(FEATURE)
-# ------------------------
-# Expand to a shell expression testing whether the feature is active.
-AC_DEFUN([DX_TEST_FEATURE], [test "$DX_FLAG_$1" = 1])
-
-# DX_CHECK_DEPEND(REQUIRED_FEATURE, REQUIRED_STATE)
-# -------------------------------------------------
-# Verify that a required features has the right state before trying to turn on
-# the DX_CURRENT_FEATURE.
-AC_DEFUN([DX_CHECK_DEPEND], [
-test "$DX_FLAG_$1" = "$2" \
-|| AC_MSG_ERROR([doxygen-DX_CURRENT_FEATURE ifelse([$2], 1,
-                            requires, contradicts) doxygen-DX_CURRENT_FEATURE])
-])
-
-# DX_CLEAR_DEPEND(FEATURE, REQUIRED_FEATURE, REQUIRED_STATE)
-# ----------------------------------------------------------
-# Turn off the DX_CURRENT_FEATURE if the required feature is off.
-AC_DEFUN([DX_CLEAR_DEPEND], [
-test "$DX_FLAG_$1" = "$2" || AC_SUBST([DX_FLAG_]DX_CURRENT_FEATURE, 0)
-])
-
-# DX_FEATURE_ARG(FEATURE, DESCRIPTION,
-#                CHECK_DEPEND, CLEAR_DEPEND,
-#                REQUIRE, DO-IF-ON, DO-IF-OFF)
-# --------------------------------------------
-# Parse the command-line option controlling a feature. CHECK_DEPEND is called
-# if the user explicitly turns the feature on (and invokes DX_CHECK_DEPEND),
-# otherwise CLEAR_DEPEND is called to turn off the default state if a required
-# feature is disabled (using DX_CLEAR_DEPEND). REQUIRE performs additional
-# requirement tests (DX_REQUIRE_PROG). Finally, an automake flag is set and
-# DO-IF-ON or DO-IF-OFF are called according to the final state of the feature.
-AC_DEFUN([DX_ARG_ABLE], [
-    AC_DEFUN([DX_CURRENT_FEATURE], [$1])
-    AC_DEFUN([DX_CURRENT_DESCRIPTION], [$2])
-    AC_ARG_ENABLE(doxygen-$1,
-                  [AS_HELP_STRING(DX_IF_FEATURE([$1], [--disable-doxygen-$1],
-                                                      [--enable-doxygen-$1]),
-                                  DX_IF_FEATURE([$1], [don't $2], [$2]))],
-                  [
-case "$enableval" in
-#(
-y|Y|yes|Yes|YES)
-    AC_SUBST([DX_FLAG_$1], 1)
-    $3
-;; #(
-n|N|no|No|NO)
-    AC_SUBST([DX_FLAG_$1], 0)
-;; #(
-*)
-    AC_MSG_ERROR([invalid value '$enableval' given to doxygen-$1])
-;;
-esac
-], [
-AC_SUBST([DX_FLAG_$1], [DX_IF_FEATURE([$1], 1, 0)])
-$4
-])
-if DX_TEST_FEATURE([$1]); then
-    $5
-    :
-fi
-if DX_TEST_FEATURE([$1]); then
-    AM_CONDITIONAL(DX_COND_$1, :)
-    $6
-    :
-else
-    AM_CONDITIONAL(DX_COND_$1, false)
-    $7
-    :
-fi
-])
-
-## -------------- ##
-## Public macros. ##
-## -------------- ##
-
-# DX_XXX_FEATURE(DEFAULT_STATE)
-# -----------------------------
-AC_DEFUN([DX_DOXYGEN_FEATURE], [AC_DEFUN([DX_FEATURE_doc],  [$1])])
-AC_DEFUN([DX_MAN_FEATURE],     [AC_DEFUN([DX_FEATURE_man],  [$1])])
-AC_DEFUN([DX_HTML_FEATURE],    [AC_DEFUN([DX_FEATURE_html], [$1])])
-AC_DEFUN([DX_CHM_FEATURE],     [AC_DEFUN([DX_FEATURE_chm],  [$1])])
-AC_DEFUN([DX_CHI_FEATURE],     [AC_DEFUN([DX_FEATURE_chi],  [$1])])
-AC_DEFUN([DX_RTF_FEATURE],     [AC_DEFUN([DX_FEATURE_rtf],  [$1])])
-AC_DEFUN([DX_XML_FEATURE],     [AC_DEFUN([DX_FEATURE_xml],  [$1])])
-AC_DEFUN([DX_XML_FEATURE],     [AC_DEFUN([DX_FEATURE_xml],  [$1])])
-AC_DEFUN([DX_PDF_FEATURE],     [AC_DEFUN([DX_FEATURE_pdf],  [$1])])
-AC_DEFUN([DX_PS_FEATURE],      [AC_DEFUN([DX_FEATURE_ps],   [$1])])
-
-# DX_INIT_DOXYGEN(PROJECT, [CONFIG-FILE], [OUTPUT-DOC-DIR])
-# ---------------------------------------------------------
-# PROJECT also serves as the base name for the documentation files.
-# The default CONFIG-FILE is "Doxyfile" and OUTPUT-DOC-DIR is "doxygen-doc".
-AC_DEFUN([DX_INIT_DOXYGEN], [
-
-# Files:
-AC_SUBST([DX_PROJECT], [$1])
-AC_SUBST([DX_CONFIG], [ifelse([$2], [], Doxyfile, [$2])])
-AC_SUBST([DX_DOCDIR], [ifelse([$3], [], doxygen-doc, [$3])])
-
-# Environment variables used inside doxygen.cfg:
-DX_ENV_APPEND(SRCDIR, $srcdir)
-DX_ENV_APPEND(PROJECT, $DX_PROJECT)
-DX_ENV_APPEND(DOCDIR, $DX_DOCDIR)
-DX_ENV_APPEND(VERSION, $PACKAGE_VERSION)
-
-# Doxygen itself:
-DX_ARG_ABLE(doc, [generate any doxygen documentation],
-            [],
-            [],
-            [DX_REQUIRE_PROG([DX_DOXYGEN], doxygen)
-             DX_REQUIRE_PROG([DX_PERL], perl)],
-            [DX_ENV_APPEND(PERL_PATH, $DX_PERL)])
-
-# Dot for graphics:
-DX_ARG_ABLE(dot, [generate graphics for doxygen documentation],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [DX_REQUIRE_PROG([DX_DOT], dot)],
-            [DX_ENV_APPEND(HAVE_DOT, YES)
-             DX_ENV_APPEND(DOT_PATH, [`DX_DIRNAME_EXPR($DX_DOT)`])],
-            [DX_ENV_APPEND(HAVE_DOT, NO)])
-
-# Man pages generation:
-DX_ARG_ABLE(man, [generate doxygen manual pages],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [],
-            [DX_ENV_APPEND(GENERATE_MAN, YES)],
-            [DX_ENV_APPEND(GENERATE_MAN, NO)])
-
-# RTF file generation:
-DX_ARG_ABLE(rtf, [generate doxygen RTF documentation],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [],
-            [DX_ENV_APPEND(GENERATE_RTF, YES)],
-            [DX_ENV_APPEND(GENERATE_RTF, NO)])
-
-# XML file generation:
-DX_ARG_ABLE(xml, [generate doxygen XML documentation],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [],
-            [DX_ENV_APPEND(GENERATE_XML, YES)],
-            [DX_ENV_APPEND(GENERATE_XML, NO)])
-
-# (Compressed) HTML help generation:
-DX_ARG_ABLE(chm, [generate doxygen compressed HTML help documentation],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [DX_REQUIRE_PROG([DX_HHC], hhc)],
-            [DX_ENV_APPEND(HHC_PATH, $DX_HHC)
-             DX_ENV_APPEND(GENERATE_HTML, YES)
-             DX_ENV_APPEND(GENERATE_HTMLHELP, YES)],
-            [DX_ENV_APPEND(GENERATE_HTMLHELP, NO)])
-
-# Seperate CHI file generation.
-DX_ARG_ABLE(chi, [generate doxygen seperate compressed HTML help index file],
-            [DX_CHECK_DEPEND(chm, 1)],
-            [DX_CLEAR_DEPEND(chm, 1)],
-            [],
-            [DX_ENV_APPEND(GENERATE_CHI, YES)],
-            [DX_ENV_APPEND(GENERATE_CHI, NO)])
-
-# Plain HTML pages generation:
-DX_ARG_ABLE(html, [generate doxygen plain HTML documentation],
-            [DX_CHECK_DEPEND(doc, 1) DX_CHECK_DEPEND(chm, 0)],
-            [DX_CLEAR_DEPEND(doc, 1) DX_CLEAR_DEPEND(chm, 0)],
-            [],
-            [DX_ENV_APPEND(GENERATE_HTML, YES)],
-            [DX_TEST_FEATURE(chm) || DX_ENV_APPEND(GENERATE_HTML, NO)])
-
-# PostScript file generation:
-DX_ARG_ABLE(ps, [generate doxygen PostScript documentation],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [DX_REQUIRE_PROG([DX_LATEX], latex)
-             DX_REQUIRE_PROG([DX_MAKEINDEX], makeindex)
-             DX_REQUIRE_PROG([DX_DVIPS], dvips)
-             DX_REQUIRE_PROG([DX_EGREP], egrep)])
-
-# PDF file generation:
-DX_ARG_ABLE(pdf, [generate doxygen PDF documentation],
-            [DX_CHECK_DEPEND(doc, 1)],
-            [DX_CLEAR_DEPEND(doc, 1)],
-            [DX_REQUIRE_PROG([DX_PDFLATEX], pdflatex)
-             DX_REQUIRE_PROG([DX_MAKEINDEX], makeindex)
-             DX_REQUIRE_PROG([DX_EGREP], egrep)])
-
-# LaTeX generation for PS and/or PDF:
-if DX_TEST_FEATURE(ps) || DX_TEST_FEATURE(pdf); then
-    AM_CONDITIONAL(DX_COND_latex, :)
-    DX_ENV_APPEND(GENERATE_LATEX, YES)
-else
-    AM_CONDITIONAL(DX_COND_latex, false)
-    DX_ENV_APPEND(GENERATE_LATEX, NO)
-fi
-
-# Paper size for PS and/or PDF:
-AC_ARG_VAR(DOXYGEN_PAPER_SIZE,
-           [a4wide (default), a4, letter, legal or executive])
-case "$DOXYGEN_PAPER_SIZE" in
-#(
-"")
-    AC_SUBST(DOXYGEN_PAPER_SIZE, "")
-;; #(
-a4wide|a4|letter|legal|executive)
-    DX_ENV_APPEND(PAPER_SIZE, $DOXYGEN_PAPER_SIZE)
-;; #(
-*)
-    AC_MSG_ERROR([unknown DOXYGEN_PAPER_SIZE='$DOXYGEN_PAPER_SIZE'])
-;;
-esac
-
-#For debugging:
-#echo DX_FLAG_doc=$DX_FLAG_doc
-#echo DX_FLAG_dot=$DX_FLAG_dot
-#echo DX_FLAG_man=$DX_FLAG_man
-#echo DX_FLAG_html=$DX_FLAG_html
-#echo DX_FLAG_chm=$DX_FLAG_chm
-#echo DX_FLAG_chi=$DX_FLAG_chi
-#echo DX_FLAG_rtf=$DX_FLAG_rtf
-#echo DX_FLAG_xml=$DX_FLAG_xml
-#echo DX_FLAG_pdf=$DX_FLAG_pdf
-#echo DX_FLAG_ps=$DX_FLAG_ps
-#echo DX_ENV=$DX_ENV
-])
diff --git a/m4/ax_boost_base.m4 b/m4/ax_boost_base.m4
deleted file mode 100644
index ec23a0abb..000000000
--- a/m4/ax_boost_base.m4
+++ /dev/null
@@ -1,257 +0,0 @@
-# ===========================================================================
-#       http://www.gnu.org/software/autoconf-archive/ax_boost_base.html
-# ===========================================================================
-#
-# SYNOPSIS
-#
-#   AX_BOOST_BASE([MINIMUM-VERSION], [ACTION-IF-FOUND], [ACTION-IF-NOT-FOUND])
-#
-# DESCRIPTION
-#
-#   Test for the Boost C++ libraries of a particular version (or newer)
-#
-#   If no path to the installed boost library is given the macro searchs
-#   under /usr, /usr/local, /opt and /opt/local and evaluates the
-#   $BOOST_ROOT environment variable. Further documentation is available at
-#   <http://randspringer.de/boost/index.html>.
-#
-#   This macro calls:
-#
-#     AC_SUBST(BOOST_CPPFLAGS) / AC_SUBST(BOOST_LDFLAGS)
-#
-#   And sets:
-#
-#     HAVE_BOOST
-#
-# LICENSE
-#
-#   Copyright (c) 2008 Thomas Porschberg <thomas@randspringer.de>
-#   Copyright (c) 2009 Peter Adolphs
-#
-#   Copying and distribution of this file, with or without modification, are
-#   permitted in any medium without royalty provided the copyright notice
-#   and this notice are preserved. This file is offered as-is, without any
-#   warranty.
-
-#serial 18
-
-AC_DEFUN([AX_BOOST_BASE],
-[
-AC_ARG_WITH([boost],
-  [AS_HELP_STRING([--with-boost@<:@=ARG@:>@],
-    [use Boost library from a standard location (ARG=yes),
-     from the specified location (ARG=<path>),
-     or disable it (ARG=no)
-     @<:@ARG=yes@:>@ ])],
-    [
-    if test "$withval" = "no"; then
-        want_boost="no"
-    elif test "$withval" = "yes"; then
-        want_boost="yes"
-        ac_boost_path=""
-    else
-        want_boost="yes"
-        ac_boost_path="$withval"
-    fi
-    ],
-    [want_boost="yes"])
-
-
-AC_ARG_WITH([boost-libdir],
-        AS_HELP_STRING([--with-boost-libdir=LIB_DIR],
-        [Force given directory for boost libraries. Note that this will overwrite library path detection, so use this parameter only if default library detection fails and you know exactly where your boost libraries are located.]),
-        [
-        if test -d "$withval"
-        then
-                ac_boost_lib_path="$withval"
-        else
-                AC_MSG_ERROR(--with-boost-libdir expected directory name)
-        fi
-        ],
-        [ac_boost_lib_path=""]
-)
-
-if test "x$want_boost" = "xyes"; then
-    boost_lib_version_req=ifelse([$1], ,1.20.0,$1)
-    boost_lib_version_req_shorten=`expr $boost_lib_version_req : '\([[0-9]]*\.[[0-9]]*\)'`
-    boost_lib_version_req_major=`expr $boost_lib_version_req : '\([[0-9]]*\)'`
-    boost_lib_version_req_minor=`expr $boost_lib_version_req : '[[0-9]]*\.\([[0-9]]*\)'`
-    boost_lib_version_req_sub_minor=`expr $boost_lib_version_req : '[[0-9]]*\.[[0-9]]*\.\([[0-9]]*\)'`
-    if test "x$boost_lib_version_req_sub_minor" = "x" ; then
-        boost_lib_version_req_sub_minor="0"
-        fi
-    WANT_BOOST_VERSION=`expr $boost_lib_version_req_major \* 100000 \+  $boost_lib_version_req_minor \* 100 \+ $boost_lib_version_req_sub_minor`
-    AC_MSG_CHECKING(for boostlib >= $boost_lib_version_req)
-    succeeded=no
-
-    dnl On x86_64 systems check for system libraries in both lib64 and lib.
-    dnl The former is specified by FHS, but e.g. Debian does not adhere to
-    dnl this (as it rises problems for generic multi-arch support).
-    dnl The last entry in the list is chosen by default when no libraries
-    dnl are found, e.g. when only header-only libraries are installed!
-    libsubdirs="lib"
-    if test `uname -m` = x86_64; then
-        libsubdirs="lib64 lib lib64"
-    fi
-
-    dnl first we check the system location for boost libraries
-    dnl this location ist chosen if boost libraries are installed with the --layout=system option
-    dnl or if you install boost with RPM
-    if test "$ac_boost_path" != ""; then
-        BOOST_CPPFLAGS="-I$ac_boost_path/include"
-        for ac_boost_path_tmp in $libsubdirs; do
-                if test -d "$ac_boost_path"/"$ac_boost_path_tmp" ; then
-                        BOOST_LDFLAGS="-L$ac_boost_path/$ac_boost_path_tmp"
-                        break
-                fi
-        done
-    elif test "$cross_compiling" != yes; then
-        for ac_boost_path_tmp in /usr /usr/local /opt /opt/local ; do
-            if test -d "$ac_boost_path_tmp/include/boost" && test -r "$ac_boost_path_tmp/include/boost"; then
-                for libsubdir in $libsubdirs ; do
-                    if ls "$ac_boost_path_tmp/$libsubdir/libboost_"* >/dev/null 2>&1 ; then break; fi
-                done
-                BOOST_LDFLAGS="-L$ac_boost_path_tmp/$libsubdir"
-                BOOST_CPPFLAGS="-I$ac_boost_path_tmp/include"
-                break;
-            fi
-        done
-    fi
-
-    dnl overwrite ld flags if we have required special directory with
-    dnl --with-boost-libdir parameter
-    if test "$ac_boost_lib_path" != ""; then
-       BOOST_LDFLAGS="-L$ac_boost_lib_path"
-    fi
-
-    CPPFLAGS_SAVED="$CPPFLAGS"
-    CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS"
-    export CPPFLAGS
-
-    LDFLAGS_SAVED="$LDFLAGS"
-    LDFLAGS="$LDFLAGS $BOOST_LDFLAGS"
-    export LDFLAGS
-
-    AC_REQUIRE([AC_PROG_CXX])
-    AC_LANG_PUSH(C++)
-        AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[
-    @%:@include <boost/version.hpp>
-    ]], [[
-    #if BOOST_VERSION >= $WANT_BOOST_VERSION
-    // Everything is okay
-    #else
-    #  error Boost version is too old
-    #endif
-    ]])],[
-        AC_MSG_RESULT(yes)
-    succeeded=yes
-    found_system=yes
-        ],[
-        ])
-    AC_LANG_POP([C++])
-
-
-
-    dnl if we found no boost with system layout we search for boost libraries
-    dnl built and installed without the --layout=system option or for a staged(not installed) version
-    if test "x$succeeded" != "xyes"; then
-        _version=0
-        if test "$ac_boost_path" != ""; then
-            if test -d "$ac_boost_path" && test -r "$ac_boost_path"; then
-                for i in `ls -d $ac_boost_path/include/boost-* 2>/dev/null`; do
-                    _version_tmp=`echo $i | sed "s#$ac_boost_path##" | sed 's/\/include\/boost-//' | sed 's/_/./'`
-                    V_CHECK=`expr $_version_tmp \> $_version`
-                    if test "$V_CHECK" = "1" ; then
-                        _version=$_version_tmp
-                    fi
-                    VERSION_UNDERSCORE=`echo $_version | sed 's/\./_/'`
-                    BOOST_CPPFLAGS="-I$ac_boost_path/include/boost-$VERSION_UNDERSCORE"
-                done
-            fi
-        else
-            if test "$cross_compiling" != yes; then
-                for ac_boost_path in /usr /usr/local /opt /opt/local ; do
-                    if test -d "$ac_boost_path" && test -r "$ac_boost_path"; then
-                        for i in `ls -d $ac_boost_path/include/boost-* 2>/dev/null`; do
-                            _version_tmp=`echo $i | sed "s#$ac_boost_path##" | sed 's/\/include\/boost-//' | sed 's/_/./'`
-                            V_CHECK=`expr $_version_tmp \> $_version`
-                            if test "$V_CHECK" = "1" ; then
-                                _version=$_version_tmp
-                                best_path=$ac_boost_path
-                            fi
-                        done
-                    fi
-                done
-
-                VERSION_UNDERSCORE=`echo $_version | sed 's/\./_/'`
-                BOOST_CPPFLAGS="-I$best_path/include/boost-$VERSION_UNDERSCORE"
-                if test "$ac_boost_lib_path" = ""; then
-                    for libsubdir in $libsubdirs ; do
-                        if ls "$best_path/$libsubdir/libboost_"* >/dev/null 2>&1 ; then break; fi
-                    done
-                    BOOST_LDFLAGS="-L$best_path/$libsubdir"
-                fi
-            fi
-
-            if test "x$BOOST_ROOT" != "x"; then
-                for libsubdir in $libsubdirs ; do
-                    if ls "$BOOST_ROOT/stage/$libsubdir/libboost_"* >/dev/null 2>&1 ; then break; fi
-                done
-                if test -d "$BOOST_ROOT" && test -r "$BOOST_ROOT" && test -d "$BOOST_ROOT/stage/$libsubdir" && test -r "$BOOST_ROOT/stage/$libsubdir"; then
-                    version_dir=`expr //$BOOST_ROOT : '.*/\(.*\)'`
-                    stage_version=`echo $version_dir | sed 's/boost_//' | sed 's/_/./g'`
-                        stage_version_shorten=`expr $stage_version : '\([[0-9]]*\.[[0-9]]*\)'`
-                    V_CHECK=`expr $stage_version_shorten \>\= $_version`
-                    if test "$V_CHECK" = "1" -a "$ac_boost_lib_path" = "" ; then
-                        AC_MSG_NOTICE(We will use a staged boost library from $BOOST_ROOT)
-                        BOOST_CPPFLAGS="-I$BOOST_ROOT"
-                        BOOST_LDFLAGS="-L$BOOST_ROOT/stage/$libsubdir"
-                    fi
-                fi
-            fi
-        fi
-
-        CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS"
-        export CPPFLAGS
-        LDFLAGS="$LDFLAGS $BOOST_LDFLAGS"
-        export LDFLAGS
-
-        AC_LANG_PUSH(C++)
-            AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[
-        @%:@include <boost/version.hpp>
-        ]], [[
-        #if BOOST_VERSION >= $WANT_BOOST_VERSION
-        // Everything is okay
-        #else
-        #  error Boost version is too old
-        #endif
-        ]])],[
-            AC_MSG_RESULT(yes)
-        succeeded=yes
-        found_system=yes
-            ],[
-            ])
-        AC_LANG_POP([C++])
-    fi
-
-    if test "$succeeded" != "yes" ; then
-        if test "$_version" = "0" ; then
-            AC_MSG_NOTICE([[We could not detect the boost libraries (version $boost_lib_version_req_shorten or higher). If you have a staged boost library (still not installed) please specify \$BOOST_ROOT in your environment and do not give a PATH to --with-boost option.  If you are sure you have boost installed, then check your version number looking in <boost/version.hpp>. See http://randspringer.de/boost for more documentation.]])
-        else
-            AC_MSG_NOTICE([Your boost libraries seems to old (version $_version).])
-        fi
-        # execute ACTION-IF-NOT-FOUND (if present):
-        ifelse([$3], , :, [$3])
-    else
-        AC_SUBST(BOOST_CPPFLAGS)
-        AC_SUBST(BOOST_LDFLAGS)
-        AC_DEFINE(HAVE_BOOST,,[define if the Boost library is available])
-        # execute ACTION-IF-FOUND (if present):
-        ifelse([$2], , :, [$2])
-    fi
-
-    CPPFLAGS="$CPPFLAGS_SAVED"
-    LDFLAGS="$LDFLAGS_SAVED"
-fi
-
-])
diff --git a/myconfigure b/myconfigure
deleted file mode 100755
index 06c23ff71..000000000
--- a/myconfigure
+++ /dev/null
@@ -1 +0,0 @@
-../configure --prefix=$HOME --enable-install-cppunitlite CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static  --disable-fast-install
diff --git a/myconfigure.ardrone b/myconfigure.ardrone
deleted file mode 100755
index 117bcdadf..000000000
--- a/myconfigure.ardrone
+++ /dev/null
@@ -1 +0,0 @@
-../configure --build=i686-pc-linux-gnu --host=arm-none-linux-gnueabi -prefix=/usr CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static 
diff --git a/myconfigure.matlab b/myconfigure.matlab
deleted file mode 100755
index d8f33f7fe..000000000
--- a/myconfigure.matlab
+++ /dev/null
@@ -1 +0,0 @@
-../configure --prefix=$HOME --with-toolbox=$HOME/toolbox  --enable-build-toolbox  --enable-install-matlab-tests  --enable-install-matlab-examples  --enable-install-wrap  --with-wrap=$HOME/bin --enable-install-cppunitlite CFLAGS="-fno-inline -g -Wall"  CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall"  --disable-static   --disable-fast-install
diff --git a/myconfigure.overo b/myconfigure.overo
deleted file mode 100755
index c3889973f..000000000
--- a/myconfigure.overo
+++ /dev/null
@@ -1 +0,0 @@
-GCC=/home/uuv/overo-oe/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-gcc CXX=$OVEROTOP/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-g++ CC=$OVEROTOP/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-gcc LD=$OVEROTOP/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-ld ./configure --prefix=$HOME --build=i686-pc-linux-gnu --host=arm-angstrom-linux-gnueabi --with-boost=$OVERO_ROOT/usr/include CXXFLAGS="--sysroot=$OVEROTOP/tmp/staging/armv7a-angstrom-linux-gnueabi -g -O3 -DNDEBUG" CCFLAGS="--sysroot=$OVEROTOP/tmp/staging/armv7a-angstrom-linux-gnueabi -g -O3 -DNDEBUG" --disable-static
diff --git a/myconfigure.profiling b/myconfigure.profiling
deleted file mode 100755
index 76fd19208..000000000
--- a/myconfigure.profiling
+++ /dev/null
@@ -1,3 +0,0 @@
-
-#../configure --disable-fast-install --prefix=$HOME/borg-simplelinear  CXXFLAGS="-fno-inline -g -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g " --disable-static
-../configure --prefix=$HOME/borg CPPFLAGS="-g -DNDEBUG -O3" LDFLAGS="-g -DNDEBUG -O3" --disable-static
diff --git a/myconfigure.timing b/myconfigure.timing
deleted file mode 100755
index 972b35e12..000000000
--- a/myconfigure.timing
+++ /dev/null
@@ -1,3 +0,0 @@
-../configure --prefix=$HOME CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static
- 
- 
diff --git a/tests/Makefile.am b/tests/Makefile.am
deleted file mode 100644
index 66044ccfc..000000000
--- a/tests/Makefile.am
+++ /dev/null
@@ -1,46 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM tests
-# More elaborate unit tests that test functionality with slam examples
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-check_PROGRAMS =
-check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGraph
-check_PROGRAMS += testGaussianISAM 
-check_PROGRAMS += testGraph
-check_PROGRAMS += testInference
-check_PROGRAMS += testGaussianJunctionTree 
-check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
-check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer
-check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
-#check_PROGRAMS += testTupleValues
-check_PROGRAMS += testNonlinearISAM
-check_PROGRAMS += testBoundingConstraint
-#check_PROGRAMS += testPose2SLAMwSPCG
-check_PROGRAMS += testGaussianISAM2
-check_PROGRAMS += testExtendedKalmanFilter
-check_PROGRAMS += testRot3Optimization
-
-# Timing tests
-noinst_PROGRAMS = timeGaussianFactorGraph timeSequentialOnDataset timeMultifrontalOnDataset
-
-#----------------------------------------------------------------------------------------------------
-# rules to build unit tests
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
-
-LDADD = ../gtsam/libgtsam.la ../CppUnitLite/libCppUnitLite.a  
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
diff --git a/wrap/Makefile.am b/wrap/Makefile.am
deleted file mode 100644
index 2cb426bae..000000000
--- a/wrap/Makefile.am
+++ /dev/null
@@ -1,146 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM Matlab wrap toolset
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-headers =
-sources = 
-check_PROGRAMS =
-noinst_PROGRAMS =  
-wrap_PROGRAMS = 
-wrapdir = $(includedir)/wrap
-
-# disable all of matlab toolbox build by default
-if ENABLE_BUILD_TOOLBOX
-
-# Build a library from the core sources
-sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp 
-sources += Method.cpp StaticMethod.cpp Class.cpp Module.cpp FileWriter.cpp
-check_PROGRAMS += tests/testSpirit tests/testWrap
-
-# Manually install wrap later
-noinst_PROGRAMS += wrap
-
-
-#----------------------------------------------------------------------------------------------------
-# Create a libtool library that is not installed
-# The headers are installed in $(includedir)/wrap: 
-#----------------------------------------------------------------------------------------------------
-# Only install the header necessary for wrap interfaces to build with mex
-headers += matlab.h 
-wrap_HEADERS = $(headers)
-noinst_LTLIBRARIES = libwrap.la
-libwrap_la_SOURCES = $(sources)
-AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -DTOPSRCDIR="\"$(top_srcdir)\""
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-TESTS = $(check_PROGRAMS)
-AM_LDFLAGS += $(boost_serialization) 
-LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a  
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./$^
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-
-# generate local toolbox dir
-interfacePath = $(top_srcdir)
-moduleName = gtsam
-toolboxpath = ../toolbox  
-
-# Set flags to pass to mex
-mexFlags = 
-if ENABLE_UNSAFE_WRAP
-mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam"
-else
-mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam"
-endif
-
-# Find the extension for mex binaries 
-# this should be done with mexext with matlab
-mexextension = 
-if LINUX
-if IS_64BIT
-mexextension += mexa64
-else
-mexextension += mexglx
-endif
-else # Linux
-if DARWIN
-mexextension += mexmaci64
-else
-mexextension += mex_bin   	
-endif
-endif # Linux
-
-# Choose correct cp command by OS
-# In linux, this will only copy the file if it is an update
-# Macs use an older version of cp that doesn't support the -u flag
-cp_install =
-if LINUX
-cp_install += cp -ru
-endif
-if DARWIN
-cp_install += cp -Rf
-endif
-
-all: generate_toolbox
-
-generate_toolbox: $(top_srcdir)/gtsam.h wrap
-	./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${mexFlags}
-
-source_mode = -m 644
-
-wrap-install-matlab-toolbox: generate_toolbox
-	install -d ${toolbox}/gtsam
-	${cp_install} ../toolbox/*.m ${toolbox}/gtsam
-	${cp_install} ../toolbox/*.cpp ${toolbox}/gtsam
-	${cp_install} ../toolbox/Makefile ${toolbox}/gtsam
-	${cp_install} ../toolbox/@* ${toolbox}/gtsam
-
-wrap-install-bin: wrap
-	install -d ${wrap} 
-	install -c ./wrap ${wrap}
-
-wrap-install-matlab-tests: 
-	install -d ${toolbox}/gtsam/tests
-	install ${source_mode} -c ../../tests/matlab/*.m ${toolbox}/gtsam/tests
-
-wrap-install-matlab-examples: 
-	install -d ${toolbox}/gtsam/examples 
-	install ${source_mode} -c ../../examples/matlab/*.m ${toolbox}/gtsam/examples
-
-wrap_install_targets =
-wrap_install_targets += wrap-install-matlab-toolbox 
-
-if ENABLE_INSTALL_WRAP
-wrap_install_targets += wrap-install-bin
-endif
-
-if ENABLE_INSTALL_MATLAB_TESTS
-wrap_install_targets += wrap-install-matlab-tests
-endif
-
-if ENABLE_INSTALL_MATLAB_EXAMPLES
-wrap_install_targets += wrap-install-matlab-examples
-endif
-
-install-exec-hook: ${wrap_install_targets}
-
-# clean local toolbox dir
-clean:
-	@test -z "wrap" || rm -f wrap
-	@test -z "../toolbox" || rm -rf ../toolbox
-
-endif
-#----------------------------------------------------------------------------------------------------

From 9856a7c7aaac344f91ebd609e2cbaeb741bab0a6 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Mon, 13 Feb 2012 21:37:10 +0000
Subject: [PATCH 65/88] Removing extraneous make targets

---
 .cproject | 2270 +++++++++++++++++++++++------------------------------
 .project  |    4 +-
 2 files changed, 999 insertions(+), 1275 deletions(-)

diff --git a/.cproject b/.cproject
index d0206ed4b..3dbe58c87 100644
--- a/.cproject
+++ b/.cproject
@@ -21,7 +21,7 @@
 					<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
-							<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
+							<builder arguments="-j2" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
 								<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@@ -286,7 +286,39 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 		<buildTargets>
-			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testTestableAssertions.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -294,15 +326,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSPQRUtil.run</buildTarget>
+				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testBlockMatrices.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -310,6 +350,110 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCholesky.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testLieConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -336,7 +480,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -344,7 +487,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -392,7 +534,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -400,7 +541,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -408,7 +548,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -424,290 +563,15 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianISAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testIterative.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearEquality.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearOptimizer.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSubgraphPreconditioner.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testTupleConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testInference.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSymbolicBayesNet.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianJunctionTree" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianJunctionTree</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSerialization.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSerialization.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>vSFMexample.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -752,7 +616,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -760,6 +624,22 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -792,15 +672,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -808,7 +680,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -816,15 +696,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>vSFMexample.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -832,10 +704,34 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMGraph</buildTarget>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -992,31 +888,79 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>SimpleRotation.run</buildTarget>
+				<buildTarget>testRot3.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildTarget>testRot2.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>testPose3.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCal3_S2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSimpleCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testHomography2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1024,15 +968,135 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianConditional.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVectorValues.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testHessianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testKalmanFilter.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianDensity.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>testGaussianFactor.run</buildTarget>
@@ -1040,62 +1104,54 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeGaussianFactor.run</buildTarget>
+				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeVectorConfig.run</buildTarget>
+				<buildTarget>install</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorBTree.run</buildTarget>
+				<buildTarget>tests/testSpirit.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorMap.run</buildTarget>
+				<buildTarget>tests/testWrap.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNoiseModel.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>testErrors.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1136,7 +1192,39 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testRot3M.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1144,7 +1232,79 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timePose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j1</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1152,71 +1312,359 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
+				<buildTarget>install</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
 				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5 VERBOSE=1</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>base</buildTarget>
+				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="base.testMatrix.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>base.testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base.testVector.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>base.testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base.timeMatrix.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>base.timeMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.base" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check.base</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.geometry" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.geometry</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.inference" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.inference</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.linear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.linear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.nonlinear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.nonlinear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.slam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.slam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.tests" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check.tests</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.wrap" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check.wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check install j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>..</buildArguments>
+				<buildTarget/>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="examples" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>examples</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>geometry</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testHomography2.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j1 VERBOSE=1</buildArguments>
+				<buildTarget>geometry.testHomography2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testPoint2.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>geometry.testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testPose2.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>geometry.testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testPose3.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>geometry.testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="gtsam-shared" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>gtsam-shared</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="gtsam-static" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>gtsam-static</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="inference" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>inference</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="linear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>linear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="nonlinear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>nonlinear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="slam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>slam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests.testSerialization.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>tests.testSerialization.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.base" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.base</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.geometry" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.geometry</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.inference" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.inference</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.linear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.linear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.nonlinear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.nonlinear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.slam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.slam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.tests" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timing.tests</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap_gtsam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>wrap_gtsam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap.testWrap.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>wrap.testWrap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>testGaussianFactor.run</buildTarget>
@@ -1224,6 +1672,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMGraph</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1320,23 +1776,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -1344,7 +1784,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1352,7 +1792,86 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeVectorConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testErrors.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -1360,7 +1879,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1368,7 +1887,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1376,90 +1911,106 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testStereoCamera.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testRot3M.run</buildTarget>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
+				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
 				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
+				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
+				<buildTarget>SimpleRotation.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
+				<buildTarget>CameraResectioning</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint3.run</buildTarget>
+				<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="easyPoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeRot3.run</buildTarget>
+				<buildTarget>easyPoint2KalmanFilter.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="elaboratePoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timePose3.run</buildTarget>
+				<buildTarget>elaboratePoint2KalmanFilter.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeStereoCamera.run</buildTarget>
+				<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="Pose2SLAMExample_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint2.run</buildTarget>
+				<buildTarget>Pose2SLAMExample_advanced.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1546,6 +2097,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1585,6 +2137,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1592,6 +2145,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1605,406 +2159,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="cmake" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>cmake</buildCommand>
-				<buildArguments>..</buildArguments>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5 VERBOSE=1</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j1</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base.testMatrix.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>base.testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.base" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check.base</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.base" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.base</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>base</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base.testVector.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>base.testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base.timeMatrix.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>base.timeMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.geometry" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.geometry</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.geometry" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.geometry</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>geometry</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testPoint2.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>geometry.testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testPose2.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>geometry.testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testPose3.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>geometry.testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testHomography2.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j1 VERBOSE=1</buildArguments>
-				<buildTarget>geometry.testHomography2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.inference" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.inference</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.linear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.linear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.nonlinear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.nonlinear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.slam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.slam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="gtsam-static" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>gtsam-static</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="gtsam-shared" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>gtsam-shared</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.tests" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check.tests</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests.testSerialization.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests.testSerialization.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.inference" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.inference</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.linear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.linear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.nonlinear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.nonlinear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.slam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.slam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.tests" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timing.tests</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="inference" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>inference</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="linear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>linear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="nonlinear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>nonlinear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="slam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>slam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="examples" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>examples</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>wrap</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.wrap" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check.wrap</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap_gtsam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>wrap_gtsam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check install j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check install" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap.testWrap.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>wrap.testWrap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testTestableAssertions.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -2012,151 +2167,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testBlockMatrices.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCholesky.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVectorValues.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNoiseModel.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testHessianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testKalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianDensity.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>SimpleRotation.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>CameraResectioning</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -2164,94 +2175,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="easyPoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>easyPoint2KalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="elaboratePoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>elaboratePoint2KalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="Pose2SLAMExample_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>Pose2SLAMExample_advanced.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>tests/testGaussianISAM2</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testLieConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -2259,206 +2183,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>dist</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCal3_S2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSimpleCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testHomography2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSpirit.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testWrap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>
diff --git a/.project b/.project
index 6ab04ab22..8ab25e91b 100644
--- a/.project
+++ b/.project
@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j5</value>
+					<value>-j2  -j5</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>
@@ -31,7 +31,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildLocation</key>
-					<value>${workspace_loc:/gtsam/build}</value>
+					<value>${ProjDirPath}/build</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>

From 0409c1c7ee32f2c687da5aff0fa366c37cc82adf Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Tue, 14 Feb 2012 18:03:29 +0000
Subject: [PATCH 66/88] Adding faster compilation options for distribution in
 CMake

---
 .cproject                      |  2 +-
 .project                       |  2 +-
 CMakeLists.txt                 | 11 +++++----
 gtsam/CMakeLists.txt           | 10 ++++++--
 gtsam/base/CMakeLists.txt      | 26 +++++++++++++-------
 gtsam/geometry/CMakeLists.txt  | 34 ++++++++++++++++++---------
 gtsam/inference/CMakeLists.txt | 36 ++++++++++++++++++----------
 gtsam/linear/CMakeLists.txt    | 39 +++++++++++++++++++-----------
 gtsam/nonlinear/CMakeLists.txt | 40 +++++++++++++++++++------------
 gtsam/slam/CMakeLists.txt      | 43 +++++++++++++++++++++-------------
 wrap/CMakeLists.txt            | 30 ++++++++++++------------
 11 files changed, 173 insertions(+), 100 deletions(-)

diff --git a/.cproject b/.cproject
index 3dbe58c87..b87d5e5a2 100644
--- a/.cproject
+++ b/.cproject
@@ -21,7 +21,7 @@
 					<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
-							<builder arguments="-j2" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
+							<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
 								<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
diff --git a/.project b/.project
index 8ab25e91b..9856df2ea 100644
--- a/.project
+++ b/.project
@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j2  -j5</value>
+					<value>-j5</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0888638c2..b1cc13234 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,7 @@ option(GTSAM_BUILD_TESTS    "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_TIMING   "Enable/Disable building of timing scripts" ON)
 option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples"       ON)
 option(GTSAM_BUILD_WRAP     "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
+option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable using convenience librares for tests" ON)
 option(GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS    
     "Enable/Disable linking tests against the convenince libraries for faster debugging" ON)
 
@@ -46,10 +47,12 @@ option(GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS
 # FIXME: breaks generation and install of matlab toolbox
 #set(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY TRUE)
 
-# Pull in tests
-enable_testing()
-include(Dart)
-include(CTest)
+# Pull in infrastructure
+if (GTSAM_BUILD_TESTS)
+    enable_testing()
+    include(Dart)
+    include(CTest)
+endif()
 
 # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
 add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND})
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 71cf8448b..0ca316b36 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -20,14 +20,20 @@ set (3rdparty_srcs
  3rdparty/CCOLAMD/Source/ccolamd.c 
  3rdparty/CCOLAMD/Source/ccolamd_global.c
  3rdparty/UFconfig/UFconfig.c) 
-add_library(ccolamd STATIC ${3rdparty_srcs})
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    message(STATUS "Building Convenience Library: ccolamd")
+    add_library(ccolamd STATIC ${3rdparty_srcs})
+endif()
 
 # assemble core libaries
 foreach(subdir ${gtsam_subdirs})
     # Build convenience libraries
     file(GLOB subdir_srcs "${subdir}/*.cpp")
-    add_library(${subdir} STATIC ${subdir_srcs})
     set(${subdir}_srcs ${subdir_srcs})
+    if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+        message(STATUS "Building Convenience Library: ${subdir}")
+        add_library(${subdir} STATIC ${subdir_srcs})
+    endif()
 
     # Build local library and tests
     message(STATUS "Building ${subdir}") 
diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt
index 8ca43e1c2..f13e0b4f5 100644
--- a/gtsam/base/CMakeLists.txt
+++ b/gtsam/base/CMakeLists.txt
@@ -3,11 +3,15 @@ file(GLOB base_headers "*.h")
 install(FILES ${base_headers} DESTINATION include/gtsam/base)
 
 # Components to link tests in this subfolder against
-set(base_local_libs
-    CppUnitLite
-    ${Boost_LIBRARIES}
-    base
-)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    set(base_local_libs
+        CppUnitLite
+        base)
+else()
+    set(base_local_libs
+        CppUnitLite
+        gtsam-static)
+endif()
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
@@ -20,8 +24,11 @@ if (GTSAM_BUILD_TESTS)
         add_executable(${test_bin} ${test_src})
         add_dependencies(check.base ${test_bin})
         add_dependencies(check ${test_bin})
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${base_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} ${base_local_libs})
+        endif()
+        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin} )
+        target_link_libraries(${test_bin} ${base_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif(GTSAM_BUILD_TESTS)
@@ -37,7 +44,10 @@ if (GTSAM_BUILD_TIMING)
         add_executable(${time_bin} ${time_src})
         add_dependencies(timing.base ${time_bin})
         add_dependencies(timing ${time_bin})
-        target_link_libraries(${time_bin} ${base_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} ${base_local_libs})
+        endif()
+        target_link_libraries(${time_bin} ${base_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
     endforeach(time_src)
 endif(GTSAM_BUILD_TIMING)
diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt
index 52a691ea0..46a9e17ac 100644
--- a/gtsam/geometry/CMakeLists.txt
+++ b/gtsam/geometry/CMakeLists.txt
@@ -1,17 +1,23 @@
-# link back to base
-add_dependencies(geometry base)
-
 # Install headers
 file(GLOB geometry_headers "*.h")
 install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
 
 # Components to link tests in this subfolder against
-set(geometry_local_libs
-   geometry
-   base
-   ${Boost_LIBRARIES}
-   CppUnitLite
-)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    set(geometry_local_libs
+       geometry
+       base
+       CppUnitLite
+    )
+    # link back to base
+    add_dependencies(geometry base)
+else()
+    set(geometry_local_libs
+        CppUnitLite
+        gtsam-static
+    )
+endif()
+
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
@@ -24,8 +30,11 @@ if (GTSAM_BUILD_TESTS)
         add_executable(${test_bin} ${test_src})
         add_dependencies(check.geometry ${test_bin})
         add_dependencies(check ${test_bin})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} gtsam-static)
+        endif()
         add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${geometry_local_libs})
+        target_link_libraries(${test_bin} ${geometry_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif(GTSAM_BUILD_TESTS)
@@ -41,7 +50,10 @@ if (GTSAM_BUILD_TIMING)
         add_executable(${time_bin} ${time_src})
         add_dependencies(timing.geometry ${time_bin})
         add_dependencies(timing ${time_bin})
-        target_link_libraries(${time_bin} ${geometry_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} gtsam-static)
+        endif()
+        target_link_libraries(${time_bin} ${geometry_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
     endforeach(time_src)
 endif(GTSAM_BUILD_TIMING)
diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt
index 37986bb8b..0c72d647b 100644
--- a/gtsam/inference/CMakeLists.txt
+++ b/gtsam/inference/CMakeLists.txt
@@ -1,19 +1,23 @@
-# link back to previous convenience library
-add_dependencies(inference base)
-
 # Install headers
 file(GLOB inference_headers "*.h")
 install(FILES ${inference_headers} DESTINATION include/gtsam/inference)
 
 # Components to link tests in this subfolder against
-set(inference_local_libs
-   inference
-   geometry
-   base
-   ccolamd
-   CppUnitLite
-   ${Boost_LIBRARIES}
-)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    set(inference_local_libs
+       inference
+       geometry
+       base
+       ccolamd
+       CppUnitLite)
+    # link back to previous convenience library
+    add_dependencies(inference base)
+else()
+    set(inference_local_libs
+        CppUnitLite
+        gtsam-static
+    )
+endif()
 
 # Build tests
 if(GTSAM_BUILD_TESTS)
@@ -26,8 +30,11 @@ if(GTSAM_BUILD_TESTS)
         add_executable(${test_bin} ${test_src})
         add_dependencies(check.inference ${test_bin})
         add_dependencies(check ${test_bin})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} ${inference_local_libs})
+        endif()
         add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${inference_local_libs})
+        target_link_libraries(${test_bin} ${inference_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif(GTSAM_BUILD_TESTS)
@@ -43,7 +50,10 @@ if(GTSAM_BUILD_TIMING)
         add_executable(${time_bin} ${time_src})
         add_dependencies(timing.inference ${time_bin})
         add_dependencies(timing ${time_bin})
-        target_link_libraries(${time_bin} ${inference_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} ${inference_local_libs})
+        endif()
+        target_link_libraries(${time_bin} ${inference_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
     endforeach(time_src)
 endif(GTSAM_BUILD_TIMING)
diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt
index 899de41c3..ad9157775 100644
--- a/gtsam/linear/CMakeLists.txt
+++ b/gtsam/linear/CMakeLists.txt
@@ -1,20 +1,25 @@
-# link back to base
-add_dependencies(linear inference)
-
 # Install headers
 file(GLOB linear_headers "*.h")
 install(FILES ${linear_headers} DESTINATION include/gtsam/linear)
 
 # Components to link tests in this subfolder against
-set(linear_local_libs
-   linear
-   inference
-   geometry
-   base
-   ccolamd
-   CppUnitLite
-   ${Boost_LIBRARIES}
-)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    set(linear_local_libs
+       linear
+       inference
+       geometry
+       base
+       ccolamd
+       CppUnitLite)
+    # link back to base
+    add_dependencies(linear inference)
+    
+else()
+    set(linear_local_libs
+        CppUnitLite
+        gtsam-static
+    )
+endif()
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
@@ -27,8 +32,11 @@ if (GTSAM_BUILD_TESTS)
         add_executable(${test_bin} ${test_src})
         add_dependencies(check.linear ${test_bin})
         add_dependencies(check ${test_bin})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} ${linear_local_libs})
+        endif()
         add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${linear_local_libs})
+        target_link_libraries(${test_bin} ${linear_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif (GTSAM_BUILD_TESTS)
@@ -44,7 +52,10 @@ if (GTSAM_BUILD_TIMING)
         add_executable(${time_bin} ${time_src})
         add_dependencies(timing.linear ${time_bin})
         add_dependencies(timing ${time_bin})
-        target_link_libraries(${time_bin} ${linear_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} ${linear_local_libs})
+        endif()
+        target_link_libraries(${time_bin} ${linear_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
     endforeach(time_src)
 endif (GTSAM_BUILD_TIMING)
diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt
index 6d917ffc5..7762437e7 100644
--- a/gtsam/nonlinear/CMakeLists.txt
+++ b/gtsam/nonlinear/CMakeLists.txt
@@ -1,21 +1,25 @@
-# link back to base
-add_dependencies(nonlinear linear)
-
 # Install headers
 file(GLOB nonlinear_headers "*.h")
 install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear)
 
 # Components to link tests in this subfolder against
-set(nonlinear_local_libs
-   nonlinear
-   linear
-   inference
-   geometry
-   base
-   ccolamd
-   CppUnitLite
-   ${Boost_LIBRARIES}
-)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    set(nonlinear_local_libs
+       nonlinear
+       linear
+       inference
+       geometry
+       base
+       ccolamd
+       CppUnitLite)
+    # link back to base
+    add_dependencies(nonlinear linear)
+else()
+    set(nonlinear_local_libs
+        CppUnitLite
+        gtsam-static
+    )
+endif()
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
@@ -28,8 +32,11 @@ if (GTSAM_BUILD_TESTS)
         add_executable(${test_bin} ${test_src})
         add_dependencies(check.nonlinear ${test_bin})
         add_dependencies(check ${test_bin})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} ${nonlinear_local_libs} )
+        endif()
         add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${nonlinear_local_libs})
+        target_link_libraries(${test_bin} ${nonlinear_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif (GTSAM_BUILD_TESTS)
@@ -45,7 +52,10 @@ if (GTSAM_BUILD_TIMING)
         add_executable(${time_bin} ${time_src})
         add_dependencies(timing.nonlinear ${time_bin})
         add_dependencies(timing ${time_bin})
-        target_link_libraries(${time_bin} ${nonlinear_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} ${nonlinear_local_libs} )
+        endif()
+        target_link_libraries(${time_bin} ${nonlinear_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
     endforeach(time_src)
 endif (GTSAM_BUILD_TIMING)
diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt
index 2354aa96c..d78dc0d5a 100644
--- a/gtsam/slam/CMakeLists.txt
+++ b/gtsam/slam/CMakeLists.txt
@@ -1,22 +1,27 @@
-# link back to base
-add_dependencies(slam nonlinear)
-
 # Install headers
 file(GLOB slam_headers "*.h")
 install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
 
 # Components to link tests in this subfolder against
-set(slam_local_libs
-   slam
-   nonlinear
-   linear
-   inference
-   geometry
-   base
-   ccolamd
-   CppUnitLite
-   ${Boost_LIBRARIES}
-)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+    set(slam_local_libs
+       slam
+       nonlinear
+       linear
+       inference
+       geometry
+       base
+       ccolamd
+       CppUnitLite)
+       
+    # link back to base
+    add_dependencies(slam nonlinear geometry)
+else()
+    set(slam_local_libs
+        CppUnitLite
+        gtsam-static
+    )
+endif()
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
@@ -30,7 +35,10 @@ if (GTSAM_BUILD_TESTS)
         add_dependencies(check.slam ${test_bin})
         add_dependencies(check ${test_bin})
         add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${slam_local_libs})
+        target_link_libraries(${test_bin} ${slam_local_libs} ${Boost_LIBRARIES})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} ${slam_local_libs})
+        endif()
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif (GTSAM_BUILD_TESTS)
@@ -46,7 +54,10 @@ if (GTSAM_BUILD_TIMING)
         add_executable(${time_bin} ${time_src})
         add_dependencies(timing.slam ${time_bin})
         add_dependencies(timing ${time_bin})
-        target_link_libraries(${time_bin} ${slam_local_libs})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} ${slam_local_libs})
+        endif()
+        target_link_libraries(${time_bin} ${slam_local_libs} ${Boost_LIBRARIES})
         add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
     endforeach(time_src)
 endif (GTSAM_BUILD_TIMING)
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 6b97822d2..6794a601f 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -63,7 +63,7 @@ set(moduleName gtsam)
 
 # only support 64-bit apple
 if(CMAKE_HOST_APPLE)
-    set(mex_bin_extension_default mexmaci64)
+    set(GTSAM_MEX_BIN_EXTENSION_default mexmaci64)
 endif(CMAKE_HOST_APPLE)
 
 if(NOT CMAKE_HOST_APPLE)
@@ -79,56 +79,56 @@ if(NOT CMAKE_HOST_APPLE)
     # Check for linux machines
     if (CMAKE_HOST_UNIX)
         if (HAVE_64_BIT)
-            set(mex_bin_extension_default mexa64)
+            set(GTSAM_MEX_BIN_EXTENSION_default mexa64)
         else (HAVE_64_BIT)
-            set(mex_bin_extension_default mexglx)
+            set(GTSAM_MEX_BIN_EXTENSION_default mexglx)
         endif (HAVE_64_BIT)
     endif(CMAKE_HOST_UNIX)
     
     # Check for windows machines
     if (CMAKE_HOST_WIN32)
         if (HAVE_64_BIT)
-            set(mex_bin_extension_default mexw64)
+            set(GTSAM_MEX_BIN_EXTENSION_default mexw64)
         else (HAVE_64_BIT)
-            set(mex_bin_extension_default mexw32)
+            set(GTSAM_MEX_BIN_EXTENSION_default mexw32)
         endif (HAVE_64_BIT)
     endif(CMAKE_HOST_WIN32)
 endif(NOT CMAKE_HOST_APPLE)
 
 # Allow for setting mex extension manually
-set(mex_bin_extension ${mex_bin_extension_default} CACHE DOCSTRING "Extension for matlab mex files")
-message(STATUS "Detected Matlab mex extension: ${mex_bin_extension_default}")
-message(STATUS "Current Matlab mex extension: ${mex_bin_extension}")
+set(GTSAM_MEX_BIN_EXTENSION ${GTSAM_MEX_BIN_EXTENSION_default} CACHE DOCSTRING "Extension for matlab mex files")
+message(STATUS "Detected Matlab mex extension: ${GTSAM_MEX_BIN_EXTENSION_default}")
+message(STATUS "Current Matlab mex extension: ${GTSAM_MEX_BIN_EXTENSION}")
 
 # Actual build commands - separated by OS
 add_custom_target(wrap_gtsam ALL COMMAND 
-    ./wrap ${mex_bin_extension} ${CMAKE_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}"
+    ./wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}"
     DEPENDS wrap)
 
 option(GTSAM_INSTALL_MATLAB_TOOLBOX  "Enable/Disable installation of matlab toolbox"  ON)
 option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON)
 option(GTSAM_INSTALL_MATLAB_TESTS    "Enable/Disable installation of matlab tests"    ON)
 
-set(toolbox_install_path ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
+set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
 
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Primary toolbox files
-    message(STATUS "Installing Matlab Toolbox to ${toolbox_install_path}")
-    install(DIRECTORY DESTINATION ${toolbox_install_path}) # make an empty folder
+    message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
+    install(DIRECTORY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}) # make an empty folder
     # exploit need for trailing slash to specify a full folder, rather than just its contents to copy
-    install(DIRECTORY ${toolbox_path} DESTINATION ${toolbox_install_path})
+    install(DIRECTORY ${toolbox_path} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH})
     
     # Examples
     if (GTSAM_INSTALL_MATLAB_EXAMPLES)
         message(STATUS "Installing Matlab Toolbox Examples")
         file(GLOB matlab_examples "${CMAKE_SOURCE_DIR}/examples/matlab/*.m")
-        install(FILES ${matlab_examples} DESTINATION ${toolbox_install_path}/gtsam/examples)
+        install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples)
     endif (GTSAM_INSTALL_MATLAB_EXAMPLES)
     
     # Tests
     if (GTSAM_INSTALL_MATLAB_TESTS)
         message(STATUS "Installing Matlab Toolbox Tests")
         file(GLOB matlab_tests "${CMAKE_SOURCE_DIR}/tests/matlab/*.m")
-        install(FILES ${matlab_tests} DESTINATION ${toolbox_install_path}/gtsam/tests)
+        install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/tests)
     endif (GTSAM_INSTALL_MATLAB_TESTS)
 endif (GTSAM_INSTALL_MATLAB_TOOLBOX)

From 23971aa044d97cdca967628b105af467979b30f5 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Tue, 14 Feb 2012 20:00:42 +0000
Subject: [PATCH 67/88] Added macros to remove copy/paste in cmake.  Added
 support for building without convenience libraries.

---
 CMakeLists.txt                 |  4 +++
 cmake/GtsamTesting.cmake       | 65 ++++++++++++++++++++++++++++++++++
 gtsam/3rdparty/CMakeLists.txt  |  7 ----
 gtsam/base/CMakeLists.txt      | 45 +++--------------------
 gtsam/geometry/CMakeLists.txt  | 52 ++++-----------------------
 gtsam/inference/CMakeLists.txt | 58 ++++++------------------------
 gtsam/linear/CMakeLists.txt    | 61 ++++++-------------------------
 gtsam/nonlinear/CMakeLists.txt | 62 +++++++-------------------------
 gtsam/slam/CMakeLists.txt      | 65 +++++++---------------------------
 wrap/CMakeLists.txt            | 18 +++++-----
 10 files changed, 135 insertions(+), 302 deletions(-)
 create mode 100644 cmake/GtsamTesting.cmake

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b1cc13234..bf94c2454 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -54,6 +54,10 @@ if (GTSAM_BUILD_TESTS)
     include(CTest)
 endif()
 
+# Use macros for creating tests/timing scripts
+set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+include(GtsamTesting)
+
 # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
 add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND})
 add_custom_target(timing)
diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake
new file mode 100644
index 000000000..f2f345f11
--- /dev/null
+++ b/cmake/GtsamTesting.cmake
@@ -0,0 +1,65 @@
+# Build macros for using tests
+
+# Collects all tests in an adjacent tests folder and builds them
+macro(gtsam_add_tests subdir libs)
+    add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND})
+    file(GLOB tests_srcs "tests/test*.cpp")
+    foreach(test_src ${tests_srcs})
+        get_filename_component(test_base ${test_src} NAME_WE)
+        set( test_bin ${subdir}.${test_base} )
+        message(STATUS "Adding Test ${test_bin}") 
+        add_executable(${test_bin} ${test_src})
+        add_dependencies(check.${subdir} ${test_bin})
+        add_dependencies(check ${test_bin})
+        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin} )
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${test_bin} ${libs} CppUnitLite)
+            target_link_libraries(${test_bin} ${libs} ${Boost_LIBRARIES} CppUnitLite)
+        else()
+            add_dependencies(${test_bin} gtsam-static)
+            target_link_libraries(${test_bin} ${Boost_LIBRARIES} gtsam-static CppUnitLite)
+        endif()
+        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
+    endforeach(test_src)
+endmacro()
+
+# Collects all tests in an adjacent tests folder and builds them
+# This version forces the use of libs, as necessary for wrap/tests
+macro(gtsam_add_external_tests subdir libs)
+    add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND})
+    file(GLOB tests_srcs "tests/test*.cpp")
+    foreach(test_src ${tests_srcs})
+        get_filename_component(test_base ${test_src} NAME_WE)
+        set( test_bin ${subdir}.${test_base} )
+        message(STATUS "Adding Test ${test_bin}") 
+        add_executable(${test_bin} ${test_src})
+        add_dependencies(check.${subdir} ${test_bin})
+        add_dependencies(check ${test_bin})
+        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin} )
+        add_dependencies(${test_bin} ${libs} CppUnitLite)
+        target_link_libraries(${test_bin} ${libs} ${Boost_LIBRARIES} CppUnitLite)
+        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
+    endforeach(test_src)
+endmacro()
+
+# Collects timing scripts and builds them
+macro(gtsam_add_timing subdir libs)
+    add_custom_target(timing.${subdir})
+    file(GLOB base_timing_srcs "tests/time*.cpp") 
+    foreach(time_src ${base_timing_srcs})
+        get_filename_component(time_base ${time_src} NAME_WE)
+        set( time_bin ${subdir}.${time_base} )
+        message(STATUS "Adding Timing Benchmark ${time_bin}")
+        add_executable(${time_bin} ${time_src})
+        add_dependencies(timing.${subdir} ${time_bin})
+        add_dependencies(timing ${time_bin})
+        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+            add_dependencies(${time_bin} ${libs})
+            target_link_libraries(${time_bin} ${libs} ${Boost_LIBRARIES} CppUnitLite)
+        else()
+            add_dependencies(${time_bin} gtsam-static)
+            target_link_libraries(${time_bin} ${Boost_LIBRARIES} gtsam-static CppUnitLite)
+        endif()
+        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
+    endforeach(time_src)
+endmacro()
\ No newline at end of file
diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt
index 45118d7b8..673411a58 100644
--- a/gtsam/3rdparty/CMakeLists.txt
+++ b/gtsam/3rdparty/CMakeLists.txt
@@ -15,10 +15,3 @@ foreach(eigen_dir ${eigen_dir_headers_all})
         install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/Eigen)     
     endif()
 endforeach(eigen_dir)
-
-## build convenience library
-#set (3rdparty_srcs 
-# CCOLAMD/Source/ccolamd.c 
-# CCOLAMD/Source/ccolamd_global.c
-# UFconfig/UFconfig.c) 
-#add_library(ccolamd STATIC ${3rdparty_srcs}) 
\ No newline at end of file
diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt
index f13e0b4f5..bd2b5df96 100644
--- a/gtsam/base/CMakeLists.txt
+++ b/gtsam/base/CMakeLists.txt
@@ -3,52 +3,17 @@ file(GLOB base_headers "*.h")
 install(FILES ${base_headers} DESTINATION include/gtsam/base)
 
 # Components to link tests in this subfolder against
-if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-    set(base_local_libs
-        CppUnitLite
-        base)
-else()
-    set(base_local_libs
-        CppUnitLite
-        gtsam-static)
-endif()
+set(base_local_libs
+    base
+)
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
-    add_custom_target(check.base COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB base_tests_srcs "tests/test*.cpp")
-    foreach(test_src ${base_tests_srcs})
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin base.${test_base} )
-        message(STATUS "Adding Test ${test_bin}") 
-        add_executable(${test_bin} ${test_src})
-        add_dependencies(check.base ${test_bin})
-        add_dependencies(check ${test_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${test_bin} ${base_local_libs})
-        endif()
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin} )
-        target_link_libraries(${test_bin} ${base_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
+    gtsam_add_tests(base "${base_local_libs}")
 endif(GTSAM_BUILD_TESTS)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
-    add_custom_target(timing.base)
-    file(GLOB base_timing_srcs "tests/time*.cpp") 
-    foreach(time_src ${base_timing_srcs})
-        get_filename_component(time_base ${time_src} NAME_WE)
-        set( time_bin base.${time_base} )
-        message(STATUS "Adding Timing Benchmark ${time_bin}")
-        add_executable(${time_bin} ${time_src})
-        add_dependencies(timing.base ${time_bin})
-        add_dependencies(timing ${time_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${time_bin} ${base_local_libs})
-        endif()
-        target_link_libraries(${time_bin} ${base_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
-    endforeach(time_src)
+    gtsam_add_timing(base "${base_local_libs}")
 endif(GTSAM_BUILD_TIMING)
 
diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt
index 46a9e17ac..8f1e8fb3a 100644
--- a/gtsam/geometry/CMakeLists.txt
+++ b/gtsam/geometry/CMakeLists.txt
@@ -3,58 +3,18 @@ file(GLOB geometry_headers "*.h")
 install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
 
 # Components to link tests in this subfolder against
-if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-    set(geometry_local_libs
-       geometry
-       base
-       CppUnitLite
-    )
-    # link back to base
-    add_dependencies(geometry base)
-else()
-    set(geometry_local_libs
-        CppUnitLite
-        gtsam-static
-    )
-endif()
-
+set(geometry_local_libs
+    base
+    geometry
+)
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
-    add_custom_target(check.geometry COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB geometry_tests_srcs "tests/test*.cpp")
-    foreach(test_src ${geometry_tests_srcs})
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin geometry.${test_base} )
-        message(STATUS "Adding Test ${test_bin}") 
-        add_executable(${test_bin} ${test_src})
-        add_dependencies(check.geometry ${test_bin})
-        add_dependencies(check ${test_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${test_bin} gtsam-static)
-        endif()
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${geometry_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
+    gtsam_add_tests(geometry "${geometry_local_libs}")
 endif(GTSAM_BUILD_TESTS)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
-    add_custom_target(timing.geometry)
-    file(GLOB geometry_timing_srcs "tests/time*.cpp") 
-    foreach(time_src ${geometry_timing_srcs})
-        get_filename_component(time_base ${time_src} NAME_WE)
-        set( time_bin geometry.${time_base} )
-        message(STATUS "Adding Timing Benchmark ${time_bin}")
-        add_executable(${time_bin} ${time_src})
-        add_dependencies(timing.geometry ${time_bin})
-        add_dependencies(timing ${time_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${time_bin} gtsam-static)
-        endif()
-        target_link_libraries(${time_bin} ${geometry_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
-    endforeach(time_src)
+    gtsam_add_timing(geometry "${geometry_local_libs}")
 endif(GTSAM_BUILD_TIMING)
 
diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt
index 0c72d647b..c7006d698 100644
--- a/gtsam/inference/CMakeLists.txt
+++ b/gtsam/inference/CMakeLists.txt
@@ -3,58 +3,20 @@ file(GLOB inference_headers "*.h")
 install(FILES ${inference_headers} DESTINATION include/gtsam/inference)
 
 # Components to link tests in this subfolder against
-if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-    set(inference_local_libs
-       inference
-       geometry
-       base
-       ccolamd
-       CppUnitLite)
-    # link back to previous convenience library
-    add_dependencies(inference base)
-else()
-    set(inference_local_libs
-        CppUnitLite
-        gtsam-static
-    )
-endif()
+set(inference_local_libs
+   inference
+   geometry
+   base
+   ccolamd
+)
 
 # Build tests
-if(GTSAM_BUILD_TESTS)
-    add_custom_target(check.inference COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB inference_tests_srcs "tests/test*.cpp")
-    foreach(test_src ${inference_tests_srcs})
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin inference.${test_base} )
-        message(STATUS "Adding Test ${test_bin}") 
-        add_executable(${test_bin} ${test_src})
-        add_dependencies(check.inference ${test_bin})
-        add_dependencies(check ${test_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${test_bin} ${inference_local_libs})
-        endif()
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${inference_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
+if (GTSAM_BUILD_TESTS)
+    gtsam_add_tests(inference "${inference_local_libs}")
 endif(GTSAM_BUILD_TESTS)
 
 # Build timing scripts
-if(GTSAM_BUILD_TIMING)
-    add_custom_target(timing.inference)
-    file(GLOB inference_timing_srcs "tests/time*.cpp") 
-    foreach(time_src ${inference_timing_srcs})
-        get_filename_component(time_base ${time_src} NAME_WE)
-        set( time_bin inference.${time_base} )
-        message(STATUS "Adding Timing Benchmark ${time_bin}")
-        add_executable(${time_bin} ${time_src})
-        add_dependencies(timing.inference ${time_bin})
-        add_dependencies(timing ${time_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${time_bin} ${inference_local_libs})
-        endif()
-        target_link_libraries(${time_bin} ${inference_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
-    endforeach(time_src)
+if (GTSAM_BUILD_TIMING)
+    gtsam_add_timing(inference "${inference_local_libs}")
 endif(GTSAM_BUILD_TIMING)
 
diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt
index ad9157775..02c300565 100644
--- a/gtsam/linear/CMakeLists.txt
+++ b/gtsam/linear/CMakeLists.txt
@@ -3,59 +3,20 @@ file(GLOB linear_headers "*.h")
 install(FILES ${linear_headers} DESTINATION include/gtsam/linear)
 
 # Components to link tests in this subfolder against
-if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-    set(linear_local_libs
-       linear
-       inference
-       geometry
-       base
-       ccolamd
-       CppUnitLite)
-    # link back to base
-    add_dependencies(linear inference)
-    
-else()
-    set(linear_local_libs
-        CppUnitLite
-        gtsam-static
-    )
-endif()
+set(linear_local_libs
+   linear
+   inference
+   geometry
+   base
+   ccolamd
+)
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
-    add_custom_target(check.linear COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB linear_tests_srcs "tests/test*.cpp")
-    foreach(test_src ${linear_tests_srcs})
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin linear.${test_base} )
-        message(STATUS "Adding Test ${test_bin}") 
-        add_executable(${test_bin} ${test_src})
-        add_dependencies(check.linear ${test_bin})
-        add_dependencies(check ${test_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${test_bin} ${linear_local_libs})
-        endif()
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${linear_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
-endif (GTSAM_BUILD_TESTS)
+    gtsam_add_tests(linear "${linear_local_libs}")
+endif(GTSAM_BUILD_TESTS)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
-    add_custom_target(timing.linear)
-    file(GLOB linear_timing_srcs "tests/time*.cpp") 
-    foreach(time_src ${linear_timing_srcs})
-        get_filename_component(time_base ${time_src} NAME_WE)
-        set( time_bin linear.${time_base} )
-        message(STATUS "Adding Timing Benchmark ${time_bin}")
-        add_executable(${time_bin} ${time_src})
-        add_dependencies(timing.linear ${time_bin})
-        add_dependencies(timing ${time_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${time_bin} ${linear_local_libs})
-        endif()
-        target_link_libraries(${time_bin} ${linear_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
-    endforeach(time_src)
-endif (GTSAM_BUILD_TIMING)
+    gtsam_add_timing(linear "${linear_local_libs}")
+endif(GTSAM_BUILD_TIMING)
diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt
index 7762437e7..69459e3d4 100644
--- a/gtsam/nonlinear/CMakeLists.txt
+++ b/gtsam/nonlinear/CMakeLists.txt
@@ -3,60 +3,22 @@ file(GLOB nonlinear_headers "*.h")
 install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear)
 
 # Components to link tests in this subfolder against
-if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-    set(nonlinear_local_libs
-       nonlinear
-       linear
-       inference
-       geometry
-       base
-       ccolamd
-       CppUnitLite)
-    # link back to base
-    add_dependencies(nonlinear linear)
-else()
-    set(nonlinear_local_libs
-        CppUnitLite
-        gtsam-static
-    )
-endif()
+set(nonlinear_local_libs
+   nonlinear
+   linear
+   inference
+   geometry
+   base
+   ccolamd
+)
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
-    add_custom_target(check.nonlinear COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB nonlinear_tests_srcs "tests/test*.cpp")
-    foreach(test_src ${nonlinear_tests_srcs})
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin nonlinear.${test_base} )
-        message(STATUS "Adding Test ${test_bin}") 
-        add_executable(${test_bin} ${test_src})
-        add_dependencies(check.nonlinear ${test_bin})
-        add_dependencies(check ${test_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${test_bin} ${nonlinear_local_libs} )
-        endif()
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${nonlinear_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
-endif (GTSAM_BUILD_TESTS)
+    gtsam_add_tests(nonlinear "${nonlinear_local_libs}")
+endif(GTSAM_BUILD_TESTS)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
-    add_custom_target(timing.nonlinear)
-    file(GLOB nonlinear_timing_srcs "tests/time*.cpp") 
-    foreach(time_src ${nonlinear_timing_srcs})
-        get_filename_component(time_base ${time_src} NAME_WE)
-        set( time_bin nonlinear.${time_base} )
-        message(STATUS "Adding Timing Benchmark ${time_bin}")
-        add_executable(${time_bin} ${time_src})
-        add_dependencies(timing.nonlinear ${time_bin})
-        add_dependencies(timing ${time_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${time_bin} ${nonlinear_local_libs} )
-        endif()
-        target_link_libraries(${time_bin} ${nonlinear_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
-    endforeach(time_src)
-endif (GTSAM_BUILD_TIMING)
+    gtsam_add_timing(nonlinear "${nonlinear_local_libs}")
+endif(GTSAM_BUILD_TIMING)
 
diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt
index d78dc0d5a..6c2ee61d5 100644
--- a/gtsam/slam/CMakeLists.txt
+++ b/gtsam/slam/CMakeLists.txt
@@ -3,61 +3,22 @@ file(GLOB slam_headers "*.h")
 install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
 
 # Components to link tests in this subfolder against
-if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-    set(slam_local_libs
-       slam
-       nonlinear
-       linear
-       inference
-       geometry
-       base
-       ccolamd
-       CppUnitLite)
-       
-    # link back to base
-    add_dependencies(slam nonlinear geometry)
-else()
-    set(slam_local_libs
-        CppUnitLite
-        gtsam-static
-    )
-endif()
+set(slam_local_libs
+   slam
+   nonlinear
+   linear
+   inference
+   geometry
+   base
+   ccolamd
+)
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
-    add_custom_target(check.slam COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB slam_tests_srcs "tests/test*.cpp")
-    foreach(test_src ${slam_tests_srcs})
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin slam.${test_base} )
-        message(STATUS "Adding Test ${test_bin}") 
-        add_executable(${test_bin} ${test_src})
-        add_dependencies(check.slam ${test_bin})
-        add_dependencies(check ${test_bin})
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        target_link_libraries(${test_bin} ${slam_local_libs} ${Boost_LIBRARIES})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${test_bin} ${slam_local_libs})
-        endif()
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
-endif (GTSAM_BUILD_TESTS)
+    gtsam_add_tests(slam "${slam_local_libs}")
+endif(GTSAM_BUILD_TESTS)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
-    add_custom_target(timing.slam)
-    file(GLOB slam_timing_srcs "tests/time*.cpp") 
-    foreach(time_src ${slam_timing_srcs})
-        get_filename_component(time_base ${time_src} NAME_WE)
-        set( time_bin slam.${time_base} )
-        message(STATUS "Adding Timing Benchmark ${time_bin}")
-        add_executable(${time_bin} ${time_src})
-        add_dependencies(timing.slam ${time_bin})
-        add_dependencies(timing ${time_bin})
-        if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-            add_dependencies(${time_bin} ${slam_local_libs})
-        endif()
-        target_link_libraries(${time_bin} ${slam_local_libs} ${Boost_LIBRARIES})
-        add_custom_target(${time_bin}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN})
-    endforeach(time_src)
-endif (GTSAM_BUILD_TIMING)
+    gtsam_add_timing(slam "${slam_local_libs}")
+endif(GTSAM_BUILD_TIMING)
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 6794a601f..951d7bca2 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -16,17 +16,17 @@ endif(GTSAM_INSTALL_WRAP)
 # Install matlab header
 install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/wrap)
 
-if (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
-    set(convenience_libs
-       base
-       )
-else (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
-    set(convenience_libs
-       gtsam-static)
-endif (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
+set(wrap_local_libs
+    base
+#    wrap_lib 
+#    gtsam-static # FIXME: find a way to avoid this inclusion
+)
 
 # Build tests
 if (GTSAM_BUILD_TESTS)
+    # FIXME: need to add way to force linking against local libs when convenience libraries are disabled 
+#    gtsam_add_external_tests(wrap "${wrap_local_libs}")
+    
     add_custom_target(check.wrap COMMAND ${CMAKE_CTEST_COMMAND})
     file(GLOB wrap_test_srcs "tests/test*.cpp")
     add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}")
@@ -37,7 +37,7 @@ if (GTSAM_BUILD_TESTS)
         add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
         add_dependencies(check ${test_bin})
         add_dependencies(check.wrap ${test_bin})
-        target_link_libraries(${test_bin} CppUnitLite ${convenience_libs} wrap_lib)
+        target_link_libraries(${test_bin} CppUnitLite ${wrap_local_libs} wrap_lib)
         add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
     endforeach(test_src)
 endif(GTSAM_BUILD_TESTS)

From d0bb9855f1b0bcab11759edaa84da1750b22ef75 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Tue, 14 Feb 2012 21:57:16 +0000
Subject: [PATCH 68/88] Switched wrap test targets to using macro

---
 wrap/CMakeLists.txt | 24 ++----------------------
 1 file changed, 2 insertions(+), 22 deletions(-)

diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 951d7bca2..de89ca5ee 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -16,30 +16,10 @@ endif(GTSAM_INSTALL_WRAP)
 # Install matlab header
 install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/wrap)
 
-set(wrap_local_libs
-    base
-#    wrap_lib 
-#    gtsam-static # FIXME: find a way to avoid this inclusion
-)
-
 # Build tests
 if (GTSAM_BUILD_TESTS)
-    # FIXME: need to add way to force linking against local libs when convenience libraries are disabled 
-#    gtsam_add_external_tests(wrap "${wrap_local_libs}")
-    
-    add_custom_target(check.wrap COMMAND ${CMAKE_CTEST_COMMAND})
-    file(GLOB wrap_test_srcs "tests/test*.cpp")
-    add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}")
-    foreach(test_src ${wrap_test_srcs} )
-        get_filename_component(test_base ${test_src} NAME_WE)
-        set( test_bin wrap.${test_base} )
-        add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src})
-        add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin})
-        add_dependencies(check ${test_bin})
-        add_dependencies(check.wrap ${test_bin})
-        target_link_libraries(${test_bin} CppUnitLite ${wrap_local_libs} wrap_lib)
-        add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN})
-    endforeach(test_src)
+    add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") 
+    gtsam_add_external_tests(wrap wrap_lib)
 endif(GTSAM_BUILD_TESTS)
 
 # Wrap codegen

From c59e1bd8d9baf1d4ad03d4715b02d4f90a4cf235 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 14 Feb 2012 23:42:39 +0000
Subject: [PATCH 69/88] Added missing header

---
 gtsam/inference/ClusterTree.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h
index 7544587c1..c5301f030 100644
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@@ -21,6 +21,7 @@
 
 #include <list>
 #include <vector>
+#include <string>
 
 #include <boost/shared_ptr.hpp>
 #include <boost/weak_ptr.hpp>

From d4875ae8e57d9cae7cad3ae2440e04caac48d930 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 14 Feb 2012 23:42:40 +0000
Subject: [PATCH 70/88] Fixed variable name in CMakeLists.txt

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index bf94c2454..0e7e2ad8d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,7 +71,7 @@ include_directories(
   gtsam/3rdparty/CCOLAMD/Include
   ${CMAKE_SOURCE_DIR}
   CppUnitLite
-  ${BOOST_INCLUDE_DIR})
+  ${Boost_INCLUDE_DIR})
 link_directories(${Boost_LIBRARY_DIRS})
 
 # Build CppUnitLite

From 4e008377bd52a4ac5eb75addd71f9a324fb15606 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 15 Feb 2012 15:31:28 +0000
Subject: [PATCH 71/88] Updated project (testing)

---
 .cproject | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.cproject b/.cproject
index b87d5e5a2..b6034e167 100644
--- a/.cproject
+++ b/.cproject
@@ -71,7 +71,7 @@
 					<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890." name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.1257341773" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.1756820285" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
-							<builder arguments="" buildPath="${workspace_loc:/gtsam/build-timing}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.404194139" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
+							<builder arguments="" buildPath="${workspace_loc:/gtsam/build-timing}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.404194139" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.879403713" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
 							<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.48676242" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
 								<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1326666213" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">

From 3ee1b88c3daafdb55922b0507c7cb46649332347 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 15 Feb 2012 21:25:13 +0000
Subject: [PATCH 72/88] Updating branch to Eigen 3.0.5

---
 gtsam/3rdparty/Eigen/Eigen/SVD                |  4 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h   |  2 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h     |  4 +-
 .../Core/products/GeneralBlockPanelKernel.h   | 17 ++--
 .../Eigen/Eigen/src/Core/util/Macros.h        |  3 +-
 .../Eigen/Eigen/src/Core/util/XprHelper.h     |  2 +-
 .../Eigen/Eigen/src/Eigenvalues/EigenSolver.h |  9 +-
 .../src/Eigenvalues/SelfAdjointEigenSolver.h  |  4 -
 .../Eigen/Eigen/src/Geometry/Hyperplane.h     |  2 +-
 .../Eigen/Eigen/src/Geometry/Quaternion.h     |  5 +-
 .../Eigen/Eigen/src/Geometry/Rotation2D.h     |  2 +-
 .../Eigen/Eigen/src/LU/arch/Inverse_SSE.h     |  2 +-
 .../3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h  |  7 ++
 .../3rdparty/Eigen/doc/C09_TutorialSparse.dox |  4 +-
 .../Eigen/doc/C10_TutorialMapClass.dox        | 96 +++++++++++++++++++
 .../doc/I13_FunctionsTakingEigenTypes.dox     | 21 +++-
 gtsam/3rdparty/Eigen/doc/Overview.dox         |  1 +
 gtsam/3rdparty/Eigen/doc/QuickReference.dox   |  6 +-
 .../doc/snippets/Tutorial_Map_rowmajor.cpp    |  7 ++
 .../Eigen/doc/snippets/Tutorial_Map_using.cpp | 21 ++++
 .../Eigen/doc/unsupported_modules.dox         | 64 -------------
 gtsam/3rdparty/Eigen/scripts/eigen_gen_docs   |  2 +-
 gtsam/3rdparty/Eigen/test/geo_quaternion.cpp  | 49 +++++++++-
 23 files changed, 230 insertions(+), 104 deletions(-)
 create mode 100644 gtsam/3rdparty/Eigen/doc/C10_TutorialMapClass.dox
 create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_rowmajor.cpp
 create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_using.cpp
 delete mode 100644 gtsam/3rdparty/Eigen/doc/unsupported_modules.dox

diff --git a/gtsam/3rdparty/Eigen/Eigen/SVD b/gtsam/3rdparty/Eigen/Eigen/SVD
index d24471fd7..7c987a9dd 100644
--- a/gtsam/3rdparty/Eigen/Eigen/SVD
+++ b/gtsam/3rdparty/Eigen/Eigen/SVD
@@ -13,9 +13,9 @@ namespace Eigen {
   *
   *
   *
-  * This module provides SVD decomposition for (currently) real matrices.
+  * This module provides SVD decomposition for matrices (both real and complex).
   * This decomposition is accessible via the following MatrixBase method:
-  *  - MatrixBase::svd()
+  *  - MatrixBase::jacobiSvd()
   *
   * \code
   * #include <Eigen/SVD>
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
index fe3e449bf..d508a60ac 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
@@ -94,7 +94,7 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel, HasDirectAccess>
     MaskPacketAccessBit = (InnerSize == Dynamic || (InnerSize % packet_traits<Scalar>::size) == 0)
                        && (InnerStrideAtCompileTime == 1)
                         ? PacketAccessBit : 0,
-    MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * sizeof(Scalar)) % 16) == 0)) ? AlignedBit : 0,
+    MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
     FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
     FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
     FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
index dd0673609..2bf80b3af 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
@@ -102,7 +102,7 @@ struct traits<Map<PlainObjectType, MapOptions, StrideType> >
                            || HasNoOuterStride
                            || ( OuterStrideAtCompileTime!=Dynamic
                            && ((static_cast<int>(sizeof(Scalar))*OuterStrideAtCompileTime)%16)==0 ) ),
-    Flags0 = TraitsBase::Flags,
+    Flags0 = TraitsBase::Flags & (~NestByRefBit),
     Flags1 = IsAligned ? (int(Flags0) | AlignedBit) : (int(Flags0) & ~AlignedBit),
     Flags2 = (bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime))
            ? int(Flags1) : int(Flags1 & ~LinearAccessBit),
@@ -120,7 +120,6 @@ template<typename PlainObjectType, int MapOptions, typename StrideType> class Ma
   public:
 
     typedef MapBase<Map> Base;
-
     EIGEN_DENSE_PUBLIC_INTERFACE(Map)
 
     typedef typename Base::PointerType PointerType;
@@ -181,7 +180,6 @@ template<typename PlainObjectType, int MapOptions, typename StrideType> class Ma
       PlainObjectType::Base::_check_template_params();
     }
 
-
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
 
   protected:
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
index c601f4771..cd1c37c78 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
@@ -30,19 +30,16 @@ namespace internal {
 template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs=false, bool _ConjRhs=false>
 class gebp_traits;
 
+inline std::ptrdiff_t manage_caching_sizes_second_if_negative(std::ptrdiff_t a, std::ptrdiff_t b)
+{
+  return a<=0 ? b : a;
+}
+
 /** \internal */
 inline void manage_caching_sizes(Action action, std::ptrdiff_t* l1=0, std::ptrdiff_t* l2=0)
 {
-  static std::ptrdiff_t m_l1CacheSize = 0;
-  static std::ptrdiff_t m_l2CacheSize = 0;
-  if(m_l1CacheSize==0)
-  {
-    m_l1CacheSize = queryL1CacheSize();
-    m_l2CacheSize = queryTopLevelCacheSize();
-
-    if(m_l1CacheSize<=0) m_l1CacheSize = 8 * 1024;
-    if(m_l2CacheSize<=0) m_l2CacheSize = 1 * 1024 * 1024;
-  }
+  static std::ptrdiff_t m_l1CacheSize = manage_caching_sizes_second_if_negative(queryL1CacheSize(),8 * 1024);
+  static std::ptrdiff_t m_l2CacheSize = manage_caching_sizes_second_if_negative(queryTopLevelCacheSize(),1*1024*1024);
 
   if(action==SetAction)
   {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
index e6c5ef425..b7c2b79af 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
@@ -1,3 +1,4 @@
+
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
@@ -28,7 +29,7 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 0
-#define EIGEN_MINOR_VERSION 4
+#define EIGEN_MINOR_VERSION 5
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h
index fcd3b093f..c2078f137 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h
@@ -127,7 +127,7 @@ class compute_matrix_flags
             ((Options&DontAlign)==0)
         && (
 #if EIGEN_ALIGN_STATICALLY
-             ((!is_dynamic_size_storage) && (((MaxCols*MaxRows*sizeof(Scalar)) % 16) == 0))
+             ((!is_dynamic_size_storage) && (((MaxCols*MaxRows*int(sizeof(Scalar))) % 16) == 0))
 #else
              0
 #endif
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h
index ac4c4242d..f57353c06 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h
@@ -291,7 +291,7 @@ template<typename _MatrixType> class EigenSolver
 
     ComputationInfo info() const
     {
-      eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized.");
+      eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
       return m_realSchur.info();
     }
 
@@ -339,7 +339,7 @@ typename EigenSolver<MatrixType>::EigenvectorsType EigenSolver<MatrixType>::eige
   EigenvectorsType matV(n,n);
   for (Index j=0; j<n; ++j)
   {
-    if (internal::isMuchSmallerThan(internal::imag(m_eivalues.coeff(j)), internal::real(m_eivalues.coeff(j))))
+    if (internal::isMuchSmallerThan(internal::imag(m_eivalues.coeff(j)), internal::real(m_eivalues.coeff(j))) || j+1==n)
     {
       // we have a real eigen value
       matV.col(j) = m_eivec.col(j).template cast<ComplexScalar>();
@@ -570,10 +570,13 @@ void EigenSolver<MatrixType>::doComputeEigenvectors()
 
         }
       }
+      
+      // We handled a pair of complex conjugate eigenvalues, so need to skip them both
+      n--;
     }
     else
     {
-      eigen_assert("Internal bug in EigenSolver"); // this should not happen
+      eigen_assert(0 && "Internal bug in EigenSolver"); // this should not happen
     }
   }
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
index c8945a848..ad107c632 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
@@ -220,7 +220,6 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
     const MatrixType& eigenvectors() const
     {
       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(info() == Success && "Eigenvalue computation did not converge.");
       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
       return m_eivec;
     }
@@ -243,7 +242,6 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
     const RealVectorType& eigenvalues() const
     {
       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(info() == Success && "Eigenvalue computation did not converge.");
       return m_eivalues;
     }
 
@@ -268,7 +266,6 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
     MatrixType operatorSqrt() const
     {
       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(info() == Success && "Eigenvalue computation did not converge.");
       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
       return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint();
     }
@@ -294,7 +291,6 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
     MatrixType operatorInverseSqrt() const
     {
       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
-      eigen_assert(info() == Success && "Eigenvalue computation did not converge.");
       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
       return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint();
     }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h
index eb0a58771..d85d3e553 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h
@@ -225,7 +225,7 @@ public:
       normal() = mat * normal();
     else
     {
-      eigen_assert("invalid traits value in Hyperplane::transform()");
+      eigen_assert(0 && "invalid traits value in Hyperplane::transform()");
     }
     return *this;
   }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
index b2f127b6a..9180db67d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
@@ -682,7 +682,7 @@ QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& oth
   Scalar scale0;
   Scalar scale1;
 
-  if (absD>=one)
+  if(absD>=one)
   {
     scale0 = Scalar(1) - t;
     scale1 = t;
@@ -695,9 +695,8 @@ QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& oth
 
     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
     scale1 = internal::sin( ( t * theta) ) / sinTheta;
-    if (d<0)
-      scale1 = -scale1;
   }
+  if(d<0) scale1 = -scale1;
 
   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
 }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h
index e1214bd3e..cf36da1c5 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h
@@ -89,7 +89,7 @@ public:
 
   /** Concatenates two rotations */
   inline Rotation2D& operator*=(const Rotation2D& other)
-  { return m_angle += other.m_angle; return *this; }
+  { m_angle += other.m_angle; return *this; }
 
   /** Applies the rotation to a 2D vector */
   Vector2 operator* (const Vector2& vec) const
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h
index 176c349ce..4c6153f0a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h
@@ -55,7 +55,7 @@ struct compute_inverse_size4<Architecture::SSE, float, MatrixType, ResultType>
   
   static void run(const MatrixType& matrix, ResultType& result)
   {
-    EIGEN_ALIGN16 const  int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 };
+    EIGEN_ALIGN16 const unsigned int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 };
 
     // Load the full matrix into registers
     __m128 _L1 = matrix.template packet<MatrixAlignment>( 0);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h
index c2326d752..3c423095c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h
@@ -708,6 +708,13 @@ struct solve_retval<JacobiSVD<_MatrixType, QRPreconditioner>, Rhs>
 };
 } // end namespace internal
 
+/** \svd_module
+  *
+  * \return the singular value decomposition of \c *this computed by two-sided
+  * Jacobi transformations.
+  *
+  * \sa class JacobiSVD
+  */
 template<typename Derived>
 JacobiSVD<typename MatrixBase<Derived>::PlainObject>
 MatrixBase<Derived>::jacobiSvd(unsigned int computationOptions) const
diff --git a/gtsam/3rdparty/Eigen/doc/C09_TutorialSparse.dox b/gtsam/3rdparty/Eigen/doc/C09_TutorialSparse.dox
index 047ba7af2..737f4cc09 100644
--- a/gtsam/3rdparty/Eigen/doc/C09_TutorialSparse.dox
+++ b/gtsam/3rdparty/Eigen/doc/C09_TutorialSparse.dox
@@ -4,7 +4,7 @@ namespace Eigen {
     \ingroup Tutorial
 
 \li \b Previous: \ref TutorialGeometry
-\li \b Next: TODO
+\li \b Next: \ref TutorialMapClass
 
 \b Table \b of \b contents \n
   - \ref TutorialSparseIntro
@@ -254,7 +254,7 @@ if(!lu_of_A.solve(b,&x)) {
 
 See also the class SparseLLT, class SparseLU, and class SparseLDLT.
 
-\li \b Next: TODO
+\li \b Next: \ref TutorialMapClass
 
 */
 
diff --git a/gtsam/3rdparty/Eigen/doc/C10_TutorialMapClass.dox b/gtsam/3rdparty/Eigen/doc/C10_TutorialMapClass.dox
new file mode 100644
index 000000000..09e792792
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/doc/C10_TutorialMapClass.dox
@@ -0,0 +1,96 @@
+namespace Eigen {
+
+/** \page TutorialMapClass Tutorial page 10 - Interfacing with C/C++ arrays and external libraries: the %Map class
+
+\ingroup Tutorial
+
+\li \b Previous: \ref TutorialSparse
+\li \b Next: \ref TODO
+
+This tutorial page explains how to work with "raw" C++ arrays.  This can be useful in a variety of contexts, particularly when "importing" vectors and matrices from other libraries into Eigen.
+
+\b Table \b of \b contents
+  - \ref TutorialMapIntroduction
+  - \ref TutorialMapTypes
+  - \ref TutorialMapUsing
+  - \ref TutorialMapPlacementNew
+
+\section TutorialMapIntroduction Introduction
+
+Occasionally you may have a pre-defined array of numbers that you want to use within Eigen as a vector or matrix. While one option is to make a copy of the data, most commonly you probably want to re-use this memory as an Eigen type. Fortunately, this is very easy with the Map class.
+
+\section TutorialMapTypes Map types and declaring Map variables
+
+A Map object has a type defined by its Eigen equivalent:
+\code
+Map<Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime> >
+\endcode
+Note that, in this default case, a Map requires just a single template parameter.  
+
+To construct a Map variable, you need two other pieces of information: a pointer to the region of memory defining the array of coefficients, and the desired shape of the matrix or vector.  For example, to define a matrix of \c float with sizes determined at compile time, you might do the following:
+\code
+Map<MatrixXf> mf(pf,rows,columns);
+\endcode
+where \c pf is a \c float \c * pointing to the array of memory.  A fixed-size read-only vector of integers might be declared as
+\code
+Map<const Vector4i> mi(pi);
+\endcode
+where \c pi is an \c int \c *. In this case the size does not have to be passed to the constructor, because it is already specified by the Matrix/Array type.
+
+Note that Map does not have a default constructor; you \em must pass a pointer to intialize the object. However, you can work around this requirement (see \ref TutorialMapPlacementNew).
+
+Map is flexible enough to accomodate a variety of different data representations.  There are two other (optional) template parameters:
+\code
+Map<typename MatrixType,
+    int MapOptions,
+    typename StrideType>
+\endcode
+\li \c MapOptions specifies whether the pointer is \c #Aligned, or \c #Unaligned.  The default is \c #Unaligned.
+\li \c StrideType allows you to specify a custom layout for the memory array, using the Stride class.  One example would be to specify that the data array is organized in row-major format:
+<table class="example">
+<tr><th>Example:</th><th>Output:</th></tr>
+<tr>
+<td>\include Tutorial_Map_rowmajor.cpp </td>
+<td>\verbinclude Tutorial_Map_rowmajor.out </td>
+</table>
+However, Stride is even more flexible than this; for details, see the documentation for the Map and Stride classes.
+
+\section TutorialMapUsing Using Map variables
+
+You can use a Map object just like any other Eigen type:
+<table class="example">
+<tr><th>Example:</th><th>Output:</th></tr>
+<tr>
+<td>\include Tutorial_Map_using.cpp </td>
+<td>\verbinclude Tutorial_Map_using.out </td>
+</table>
+
+However, when writing functions taking Eigen types, it is important to realize that a Map type is \em not identical to its Dense equivalent.  See \ref TopicFunctionTakingEigenTypesMultiarguments for details.
+
+\section TutorialMapPlacementNew Changing the mapped array
+
+It is possible to change the array of a Map object after declaration, using the C++ "placement new" syntax:
+<table class="example">
+<tr><th>Example:</th><th>Output:</th></tr>
+<tr>
+<td>\include Map_placement_new.cpp </td>
+<td>\verbinclude Map_placement_new.out </td>
+</table>
+Despite appearances, this does not invoke the memory allocator, because the syntax specifies the location for storing the result.
+
+This syntax makes it possible to declare a Map object without first knowing the mapped array's location in memory:
+\code
+Map<Matrix3f> A(NULL);  // don't try to use this matrix yet!
+VectorXf b(n_matrices);
+for (int i = 0; i < n_matrices; i++)
+{
+  new (&A) Map<Matrix3f>(get_matrix_pointer(i));
+  b(i) = A.trace();
+}
+\endcode
+
+\li \b Next: \ref TODO
+
+*/
+
+}
diff --git a/gtsam/3rdparty/Eigen/doc/I13_FunctionsTakingEigenTypes.dox b/gtsam/3rdparty/Eigen/doc/I13_FunctionsTakingEigenTypes.dox
index ef71e6f19..f9e6fafed 100644
--- a/gtsam/3rdparty/Eigen/doc/I13_FunctionsTakingEigenTypes.dox
+++ b/gtsam/3rdparty/Eigen/doc/I13_FunctionsTakingEigenTypes.dox
@@ -47,9 +47,9 @@ void print_block(const DenseBase<Derived>& b, int x, int y, int r, int c)
 Prints the maximum coefficient of the array or array-expression.
 \code
 template <typename Derived>
-void print_max(const ArrayBase<Derived>& a, const ArrayBase<Derived>& b)
+void print_max_coeff(const ArrayBase<Derived> &a)
 {
-  std::cout << "max: " << (a.max(b)).maxCoeff() << std::endl;
+  std::cout << "max: " << a.maxCoeff() << std::endl;
 }
 \endcode
 <b> %MatrixBase Example </b><br/><br/>
@@ -63,6 +63,21 @@ void print_inv_cond(const MatrixBase<Derived>& a)
   std::cout << "inv cond: " << sing_vals(sing_vals.size()-1) / sing_vals(0) << std::endl;
 }
 \endcode
+<b> Multiple templated arguments example </b><br/><br/>
+Calculate the Euclidean distance between two points.
+\code
+template <typename DerivedA,typename DerivedB>
+typename DerivedA::Scalar squaredist(const MatrixBase<DerivedA>& p1,const MatrixBase<DerivedB>& p2)
+{
+  return (p1-p2).squaredNorm();
+}
+\endcode
+Notice that we used two template parameters, one per argument. This permits the function to handle inputs of different types, e.g.,
+\code
+squaredist(v1,2*v2)
+\endcode
+where the first argument \c v1 is a vector and the second argument \c 2*v2 is an expression.
+<br/><br/>
 
 These examples are just intended to give the reader a first impression of how functions can be written which take a plain and constant Matrix or Array argument. They are also intended to give the reader an idea about the most common base classes being the optimal candidates for functions. In the next section we will look in more detail at an example and the different ways it can be implemented, while discussing each implementation's problems and advantages. For the discussion below, Matrix and Array as well as MatrixBase and ArrayBase can be exchanged and all arguments still hold.
 
@@ -128,6 +143,8 @@ The implementation above does now not only work with temporary expressions but i
 
 \b Note: The const cast hack will only work with templated functions. It will not work with the MatrixXf implementation because it is not possible to cast a Block expression to a Matrix reference!
 
+
+
 \section TopicResizingInGenericImplementations How to resize matrices in generic implementations?
 
 One might think we are done now, right? This is not completely true because in order for our covariance function to be generically applicable, we want the follwing code to work
diff --git a/gtsam/3rdparty/Eigen/doc/Overview.dox b/gtsam/3rdparty/Eigen/doc/Overview.dox
index 598a96b4d..68e07ad63 100644
--- a/gtsam/3rdparty/Eigen/doc/Overview.dox
+++ b/gtsam/3rdparty/Eigen/doc/Overview.dox
@@ -27,6 +27,7 @@ For a first contact with Eigen, the best place is to have a look at the \ref Get
     - \ref TutorialReductionsVisitorsBroadcasting
     - \ref TutorialGeometry
     - \ref TutorialSparse
+    - \ref TutorialMapClass
   - \ref QuickRefPage
   - <b>Advanced topics</b>
     - \ref TopicAliasing
diff --git a/gtsam/3rdparty/Eigen/doc/QuickReference.dox b/gtsam/3rdparty/Eigen/doc/QuickReference.dox
index 89a3c4322..e23ff7ce5 100644
--- a/gtsam/3rdparty/Eigen/doc/QuickReference.dox
+++ b/gtsam/3rdparty/Eigen/doc/QuickReference.dox
@@ -645,11 +645,11 @@ m3 -= s1 * m3.adjoint() * m1.selfadjointView<Eigen::Lower>();\endcode
 </td></tr>
 <tr><td>
 Rank 1 and rank K update: \n
-\f$ upper(M_1) \mathrel{{+}{=}} s_1 M_2^* M_2 \f$ \n
-\f$ lower(M_1) \mathbin{{-}{=}} M_2 M_2^* \f$
+\f$ upper(M_1) \mathrel{{+}{=}} s_1 M_2 M_2^* \f$ \n
+\f$ lower(M_1) \mathbin{{-}{=}} M_2^* M_2 \f$
 </td><td>\n \code
 M1.selfadjointView<Eigen::Upper>().rankUpdate(M2,s1);
-m1.selfadjointView<Eigen::Lower>().rankUpdate(m2.adjoint(),-1); \endcode
+M1.selfadjointView<Eigen::Lower>().rankUpdate(M2.adjoint(),-1); \endcode
 </td></tr>
 <tr><td>
 Rank 2 update: (\f$ M \mathrel{{+}{=}} s u v^* + s v u^* \f$)
diff --git a/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_rowmajor.cpp b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_rowmajor.cpp
new file mode 100644
index 000000000..fd45ace03
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_rowmajor.cpp
@@ -0,0 +1,7 @@
+int array[8];
+for(int i = 0; i < 8; ++i) array[i] = i;
+cout << "Column-major:\n" << Map<Matrix<int,2,4> >(array) << endl;
+cout << "Row-major:\n" << Map<Matrix<int,2,4,RowMajor> >(array) << endl;
+cout << "Row-major using stride:\n" <<
+  Map<Matrix<int,2,4>, Unaligned, Stride<1,4> >(array) << endl;
+
diff --git a/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_using.cpp b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_using.cpp
new file mode 100644
index 000000000..e5e499f1f
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_using.cpp
@@ -0,0 +1,21 @@
+typedef Matrix<float,1,Dynamic> MatrixType;
+typedef Map<MatrixType> MapType;
+typedef Map<const MatrixType> MapTypeConst;   // a read-only map
+const int n_dims = 5;
+  
+MatrixType m1(n_dims), m2(n_dims);
+m1.setRandom();
+m2.setRandom();
+float *p = &m2(0);  // get the address storing the data for m2
+MapType m2map(p,m2.size());   // m2map shares data with m2
+MapTypeConst m2mapconst(p,m2.size());  // a read-only accessor for m2
+
+cout << "m1: " << m1 << endl;
+cout << "m2: " << m2 << endl;
+cout << "Squared euclidean distance: " << (m1-m2).squaredNorm() << endl;
+cout << "Squared euclidean distance, using map: " <<
+  (m1-m2map).squaredNorm() << endl;
+m2map(3) = 7;   // this will change m2, since they share the same array
+cout << "Updated m2: " << m2 << endl;
+cout << "m2 coefficient 2, constant accessor: " << m2mapconst(2) << endl;
+/* m2mapconst(2) = 5; */   // this yields a compile-time error
diff --git a/gtsam/3rdparty/Eigen/doc/unsupported_modules.dox b/gtsam/3rdparty/Eigen/doc/unsupported_modules.dox
deleted file mode 100644
index 1d303c25e..000000000
--- a/gtsam/3rdparty/Eigen/doc/unsupported_modules.dox
+++ /dev/null
@@ -1,64 +0,0 @@
-
-namespace Eigen {
-
-/** \defgroup Unsupported_modules Unsupported modules
-  *
-  * The unsupported modules are contributions from various users. They are
-  * provided "as is", without any support. Nevertheless, some of them are
-  * subject to be included in Eigen in the future.
-  */
-
-// please list here all unsupported modules
-
-/** \ingroup Unsupported_modules
-  * \defgroup AdolcForward_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup AlignedVector3_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup AutoDiff_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup BVH_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup FFT_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup IterativeSolvers_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup MatrixFunctions_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup MoreVectorization */
-
-/** \ingroup Unsupported_modules
-  * \defgroup NonLinearOptimization_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup NumericalDiff_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup Polynomials_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup Skyline_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup CholmodSupport_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup SuperLUSupport_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup UmfPackSupport_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup SparseExtra_Module */
-
-/** \ingroup Unsupported_modules
-  * \defgroup MpfrcxxSupport_Module */
-
-} // namespace Eigen
diff --git a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs
index 539abb1cd..e97e9ab8f 100644
--- a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs
+++ b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs
@@ -15,7 +15,7 @@ mkdir build -p
 
 #step 2 : upload
 # (the '/' at the end of path are very important, see rsync documentation)
-rsync -az build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/dox-3.0/ || { echo "upload failed"; exit 1; }
+rsync -az --no-p build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/dox-3.0/ || { echo "upload failed"; exit 1; }
 
 echo "Uploaded successfully"
 
diff --git a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp
index 1e7b2cba0..7adbe0b3d 100644
--- a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp
@@ -28,6 +28,35 @@
 #include <Eigen/LU>
 #include <Eigen/SVD>
 
+template<typename T> T bounded_acos(T v)
+{
+  using std::acos;
+  using std::min;
+  using std::max;
+  return acos((max)(T(-1),(min)(v,T(1))));
+}
+
+template<typename QuatType> void check_slerp(const QuatType& q0, const QuatType& q1)
+{
+  typedef typename QuatType::Scalar Scalar;
+  typedef Matrix<Scalar,3,1> VectorType;
+  typedef AngleAxis<Scalar> AA;
+
+  Scalar largeEps = test_precision<Scalar>();
+
+  Scalar theta_tot = AA(q1*q0.inverse()).angle();
+  if(theta_tot>M_PI)
+    theta_tot = 2.*M_PI-theta_tot;
+  for(Scalar t=0; t<=1.001; t+=0.1)
+  {
+    QuatType q = q0.slerp(t,q1);
+    Scalar theta = AA(q*q0.inverse()).angle();
+    VERIFY(internal::abs(q.norm() - 1) < largeEps);
+    if(theta_tot==0)  VERIFY(theta_tot==0);
+    else              VERIFY(internal::abs(theta/theta_tot - t) < largeEps);
+  }
+}
+
 template<typename Scalar, int Options> void quaternion(void)
 {
   /* this test covers the following files:
@@ -36,6 +65,7 @@ template<typename Scalar, int Options> void quaternion(void)
 
   typedef Matrix<Scalar,3,3> Matrix3;
   typedef Matrix<Scalar,3,1> Vector3;
+  typedef Matrix<Scalar,4,1> Vector4;
   typedef Quaternion<Scalar,Options> Quaternionx;
   typedef AngleAxis<Scalar> AngleAxisx;
 
@@ -50,7 +80,8 @@ template<typename Scalar, int Options> void quaternion(void)
           v2 = Vector3::Random(),
           v3 = Vector3::Random();
 
-  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
+  Scalar  a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)),
+          b = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
 
   // Quaternion: Identity(), setIdentity();
   Quaternionx q1, q2;
@@ -124,6 +155,22 @@ template<typename Scalar, int Options> void quaternion(void)
   // test bug 369 - improper alignment.
   Quaternionx *q = new Quaternionx;
   delete q;
+
+  q1 = AngleAxisx(a, v0.normalized());
+  q2 = AngleAxisx(b, v1.normalized());
+  check_slerp(q1,q2);
+
+  q1 = AngleAxisx(b, v1.normalized());
+  q2 = AngleAxisx(b+M_PI, v1.normalized());
+  check_slerp(q1,q2);
+
+  q1 = AngleAxisx(b,  v1.normalized());
+  q2 = AngleAxisx(-b, -v1.normalized());
+  check_slerp(q1,q2);
+
+  q1.coeffs() = Vector4::Random().normalized();
+  q2.coeffs() = -q1.coeffs();
+  check_slerp(q1,q2);
 }
 
 template<typename Scalar> void mapQuaternion(void){

From 0592b71ac46b79386b089b950ee65f4488c601b6 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sun, 19 Feb 2012 01:02:07 +0000
Subject: [PATCH 74/88] Switched Values, Ordering, and factors to int 'Key'
 instead of 'Symbol', still more code changes required to compile

---
 gtsam/base/types.h                         |  3 +
 gtsam/inference/Conditional.h              | 28 ++++----
 gtsam/inference/Factor-inl.h               |  4 +-
 gtsam/inference/Factor.h                   | 36 +++++-----
 gtsam/inference/IndexConditional.cpp       | 10 +--
 gtsam/inference/tests/timeSymbolMaps.cpp   | 24 +++----
 gtsam/linear/SubgraphSolver-inl.h          |  6 +-
 gtsam/nonlinear/ExtendedKalmanFilter-inl.h |  8 +--
 gtsam/nonlinear/ExtendedKalmanFilter.h     |  2 +-
 gtsam/nonlinear/GaussianISAM2-inl.h        |  2 +-
 gtsam/nonlinear/ISAM2-impl-inl.h           |  2 +-
 gtsam/nonlinear/ISAM2-inl.h                |  6 +-
 gtsam/nonlinear/ISAM2.h                    |  2 +-
 gtsam/nonlinear/NonlinearEquality.h        | 10 +--
 gtsam/nonlinear/NonlinearFactor.h          | 61 ++++++++--------
 gtsam/nonlinear/NonlinearFactorGraph.cpp   |  4 +-
 gtsam/nonlinear/NonlinearFactorGraph.h     |  2 +-
 gtsam/nonlinear/NonlinearISAM-inl.h        |  4 +-
 gtsam/nonlinear/NonlinearISAM.h            |  4 +-
 gtsam/nonlinear/NonlinearOptimizer.h       |  2 +-
 gtsam/nonlinear/Ordering.cpp               |  6 +-
 gtsam/nonlinear/Ordering.h                 | 42 ++++++-----
 gtsam/nonlinear/Symbol.h                   | 24 +++++++
 gtsam/nonlinear/Values-inl.h               | 32 ++-------
 gtsam/nonlinear/Values.cpp                 | 18 ++---
 gtsam/nonlinear/Values.h                   | 81 ++++++++--------------
 gtsam/nonlinear/tests/testValues.cpp       |  6 +-
 gtsam/slam/BearingFactor.h                 |  2 +-
 gtsam/slam/BearingRangeFactor.h            |  2 +-
 gtsam/slam/BetweenFactor.h                 |  4 +-
 gtsam/slam/BoundingConstraint.h            |  4 +-
 gtsam/slam/GeneralSFMFactor.h              |  2 +-
 gtsam/slam/PartialPriorFactor.h            |  6 +-
 gtsam/slam/PriorFactor.h                   |  2 +-
 gtsam/slam/ProjectionFactor.h              |  2 +-
 gtsam/slam/RangeFactor.h                   |  2 +-
 gtsam/slam/StereoFactor.h                  |  2 +-
 gtsam/slam/simulated2D.h                   |  6 +-
 gtsam/slam/simulated2DConstraints.h        |  6 +-
 gtsam/slam/simulated2DOriented.h           |  4 +-
 gtsam/slam/simulated3D.h                   |  4 +-
 gtsam/slam/smallExample.cpp                |  2 +-
 tests/testNonlinearFactorGraph.cpp         |  4 +-
 tests/testNonlinearISAM.cpp                |  4 +-
 44 files changed, 233 insertions(+), 254 deletions(-)

diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index b6c5617a5..f3f1ae465 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -26,6 +26,9 @@ namespace gtsam {
 	/// Integer variable index type
 	typedef size_t Index;
 
+	/// Integer nonlinear key type
+	typedef size_t Key;
+
 	/**
 	 * Helper class that uses templates to select between two types based on
 	 * whether TEST_TYPE is const or not.
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index 2fa5f691d..0b5fca660 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -46,7 +46,7 @@ private:
 	/** Create keys by adding key in front */
 	template<typename ITERATOR>
 	static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
-		std::vector<Key> keys((lastParent - firstParent) + 1);
+		std::vector<KeyType> keys((lastParent - firstParent) + 1);
 		std::copy(firstParent, lastParent, keys.begin() + 1);
 		keys[0] = key;
 		return keys;
@@ -62,16 +62,16 @@ protected:
 
 public:
 
-  typedef KEY Key;
-  typedef Conditional<Key> This;
-  typedef Factor<Key> Base;
+  typedef KEY KeyType;
+  typedef Conditional<KeyType> This;
+  typedef Factor<KeyType> Base;
 
   /**
    * Typedef to the factor type that produces this conditional and that this
    * conditional can be converted to using a factor constructor. Derived
    * classes must redefine this.
    */
-  typedef gtsam::Factor<Key> FactorType;
+  typedef gtsam::Factor<KeyType> FactorType;
 
   /** A shared_ptr to this class.  Derived classes must redefine this. */
   typedef boost::shared_ptr<This> shared_ptr;
@@ -95,23 +95,23 @@ public:
   Conditional() : nrFrontals_(0) { assertInvariants(); }
 
   /** No parents */
-  Conditional(Key key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
 
   /** Single parent */
-  Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
 
   /** Two parents */
-  Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
 
   /** Three parents */
-  Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
 
 	/// @}
 	/// @name Advanced Constructors
 	/// @{
 
   /** Constructor from a frontal variable and a vector of parents */
-	Conditional(Key key, const std::vector<Key>& parents) :
+	Conditional(KeyType key, const std::vector<KeyType>& parents) :
 		FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
 		assertInvariants();
 	}
@@ -145,8 +145,8 @@ public:
 	size_t nrParents() const { return FactorType::size() - nrFrontals_; }
 
 	/** Special accessor when there is only one frontal variable. */
-	Key firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
-	Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
+	KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
+	KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
 
   /** return a view of the frontal keys */
   Frontals frontals() const {
@@ -198,9 +198,9 @@ private:
 template<typename KEY>
 void Conditional<KEY>::print(const std::string& s) const {
   std::cout << s << " P(";
-  BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
+  BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key;
   if (nrParents()>0) std::cout << " |";
-  BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
+  BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent;
   std::cout << ")" << std::endl;
 }
 
diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h
index 09336186e..d05cc1de3 100644
--- a/gtsam/inference/Factor-inl.h
+++ b/gtsam/inference/Factor-inl.h
@@ -45,8 +45,8 @@ namespace gtsam {
 	void Factor<KEY>::assertInvariants() const {
 #ifndef NDEBUG
 		// Check that keys are all unique
-		std::multiset < Key > nonunique(keys_.begin(), keys_.end());
-		std::set < Key > unique(keys_.begin(), keys_.end());
+		std::multiset < KeyType > nonunique(keys_.begin(), keys_.end());
+		std::set < KeyType > unique(keys_.begin(), keys_.end());
 		bool correct = (nonunique.size() == unique.size())
 				&& std::equal(nonunique.begin(), nonunique.end(), unique.begin());
 		if (!correct) throw std::logic_error(
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 5a0b1e333..962bf44d1 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -55,28 +55,28 @@ class Factor {
 
 public:
 
-  typedef KEY Key; ///< The KEY template parameter
-  typedef Factor<Key> This; ///< This class
+  typedef KEY KeyType; ///< The KEY template parameter
+  typedef Factor<KeyType> This; ///< This class
 
   /**
    * Typedef to the conditional type obtained by eliminating this factor,
    * derived classes must redefine this.
    */
-  typedef Conditional<Key> ConditionalType;
+  typedef Conditional<KeyType> ConditionalType;
 
   /// A shared_ptr to this class, derived classes must redefine this.
   typedef boost::shared_ptr<Factor> shared_ptr;
 
   /// Iterator over keys
-  typedef typename std::vector<Key>::iterator iterator;
+  typedef typename std::vector<KeyType>::iterator iterator;
 
   /// Const iterator over keys
-  typedef typename std::vector<Key>::const_iterator const_iterator;
+  typedef typename std::vector<KeyType>::const_iterator const_iterator;
 
 protected:
 
   /// The keys involved in this factor
-  std::vector<Key> keys_;
+  std::vector<KeyType> keys_;
 
   friend class JacobianFactor;
   friend class HessianFactor;
@@ -102,19 +102,19 @@ public:
   Factor() {}
 
   /** Construct unary factor */
-  Factor(Key key) : keys_(1) {
+  Factor(KeyType key) : keys_(1) {
     keys_[0] = key; assertInvariants(); }
 
   /** Construct binary factor */
-  Factor(Key key1, Key key2) : keys_(2) {
+  Factor(KeyType key1, KeyType key2) : keys_(2) {
     keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
 
   /** Construct ternary factor */
-  Factor(Key key1, Key key2, Key key3) : keys_(3) {
+  Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
     keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
 
   /** Construct 4-way factor */
-  Factor(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
+  Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
     keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
 
 	/// @}
@@ -122,13 +122,13 @@ public:
 	/// @{
 
   /** Construct n-way factor */
-	Factor(const std::set<Key>& keys) {
-		BOOST_FOREACH(const Key& key, keys) keys_.push_back(key);
+	Factor(const std::set<KeyType>& keys) {
+		BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
 		assertInvariants();
 	}
 
 	/** Construct n-way factor */
-	Factor(const std::vector<Key>& keys) : keys_(keys) {
+	Factor(const std::vector<KeyType>& keys) : keys_(keys) {
 		assertInvariants();
 	}
 
@@ -157,16 +157,16 @@ public:
 	/// @{
 
   /// First key
-  Key front() const { return keys_.front(); }
+  KeyType front() const { return keys_.front(); }
 
   /// Last key
-  Key back() const { return keys_.back(); }
+  KeyType back() const { return keys_.back(); }
 
   /// find
-  const_iterator find(Key key) const { return std::find(begin(), end(), key); }
+  const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
 
   ///TODO: comment
-  const std::vector<Key>& keys() const { return keys_; }
+  const std::vector<KeyType>& keys() const { return keys_; }
 
   /** iterators */
   const_iterator begin() const { return keys_.begin(); }	///TODO: comment
@@ -194,7 +194,7 @@ public:
   /**
    * @return keys involved in this factor
    */
-  std::vector<Key>& keys() { return keys_; }
+  std::vector<KeyType>& keys() { return keys_; }
 
   /** mutable iterators */
   iterator begin() { return keys_.begin(); }	///TODO: comment
diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp
index eca7a220d..58d5f93d1 100644
--- a/gtsam/inference/IndexConditional.cpp
+++ b/gtsam/inference/IndexConditional.cpp
@@ -43,11 +43,11 @@ namespace gtsam {
   /* ************************************************************************* */
   bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
   #ifndef NDEBUG
-    BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
+    BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
   #endif
     bool parentChanged = false;
-    BOOST_FOREACH(Key& parent, parents()) {
-      Key newParent = inversePermutation[parent];
+    BOOST_FOREACH(KeyType& parent, parents()) {
+      KeyType newParent = inversePermutation[parent];
       if(parent != newParent) {
         parentChanged = true;
         parent = newParent;
@@ -61,8 +61,8 @@ namespace gtsam {
   void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
     // The permutation may not move the separators into the frontals
   #ifndef NDEBUG
-    BOOST_FOREACH(const Key frontal, this->frontals()) {
-      BOOST_FOREACH(const Key separator, this->parents()) {
+    BOOST_FOREACH(const KeyType frontal, this->frontals()) {
+      BOOST_FOREACH(const KeyType separator, this->parents()) {
         assert(inversePermutation[frontal] < inversePermutation[separator]);
       }
     }
diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp
index 61ce1b30c..1912f34d3 100644
--- a/gtsam/inference/tests/timeSymbolMaps.cpp
+++ b/gtsam/inference/tests/timeSymbolMaps.cpp
@@ -37,11 +37,11 @@ private:
 	Map values_;
 
 public:
-	typedef pair<Symbol, T> value_type;
+	typedef pair<Key, T> value_type;
 
 	SymbolMapExp() {}
 
-	T& at(const Symbol& key) {
+	T& at(Key key) {
 		typename Map::iterator it = values_.find(key.chr());
 		if(it != values_.end())
 			return it->second.at(key.index());
@@ -49,7 +49,7 @@ public:
 			throw invalid_argument("Key " + (string)key + " not present");
 	}
 
-	void set(const Symbol& key, const T& value) {
+	void set(Key key, const T& value) {
 		Vec& vec(values_[key.chr()]);
 		//vec.reserve(10000);
 		if(key.index() >= vec.size()) {
@@ -62,13 +62,13 @@ public:
 };
 
 template<class T>
-class SymbolMapBinary : public std::map<Symbol, T> {
+class SymbolMapBinary : public std::map<Key, T> {
 private:
-	typedef std::map<Symbol, T> Base;
+	typedef std::map<Key, T> Base;
 public:
-	SymbolMapBinary() : std::map<Symbol, T>() {}
+	SymbolMapBinary() : std::map<Key, T>() {}
 
-	T& at(const Symbol& key) {
+	T& at(Key key) {
 		typename Base::iterator it = Base::find(key);
 		if (it == Base::end())
 			throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
@@ -76,8 +76,8 @@ public:
 	}
 };
 
-struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
-	std::size_t operator()(Symbol const& x) const {
+struct SymbolHash : public std::unary_function<Key, std::size_t> {
+	std::size_t operator()(Key const& x) const {
 		std::size_t seed = 0;
 		boost::hash_combine(seed, x.chr());
 		boost::hash_combine(seed, x.index());
@@ -86,9 +86,9 @@ struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
 };
 
 template<class T>
-class SymbolMapHash : public boost::unordered_map<Symbol, T, SymbolHash> {
+class SymbolMapHash : public boost::unordered_map<Key, T, SymbolHash> {
 public:
-	SymbolMapHash() : boost::unordered_map<Symbol, T, SymbolHash>(60000) {}
+	SymbolMapHash() : boost::unordered_map<Key, T, SymbolHash>(60000) {}
 };
 
 struct Value {
@@ -107,7 +107,7 @@ int main(int argc, char *argv[]) {
 
 	// pre-allocate
 	cout << "Generating test data ..." << endl;
-	vector<pair<Symbol, Value> > values;
+	vector<pair<Key, Value> > values;
 	for(size_t i=0; i<ELEMS; i++) {
 		values.push_back(make_pair(Symbol('a',i), (double)i));
 		values.push_back(make_pair(Symbol('b',i), (double)i));
diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h
index 9d53c5741..d2dc7cee2 100644
--- a/gtsam/linear/SubgraphSolver-inl.h
+++ b/gtsam/linear/SubgraphSolver-inl.h
@@ -67,14 +67,14 @@ void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values&
 
 	// make the ordering
 	list<KEY> keys = predecessorMap2Keys(tree_);
-	ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
+	ordering_ = boost::make_shared<Ordering>(list<Key>(keys.begin(), keys.end()));
 
 	// build factor pairs
 	pairs_.clear();
 	typedef pair<KEY,KEY> EG ;
 	BOOST_FOREACH( const EG &eg, tree_ ) {
-		Symbol key1 = Symbol(eg.first),
-				key2 = Symbol(eg.second) ;
+		Key key1 = Key(eg.first),
+				key2 = Key(eg.second) ;
 		pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
 	}
 }
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 7cd67f6d2..06a24d473 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -30,7 +30,7 @@ namespace gtsam {
 	template<class VALUE>
 	typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
 			const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
-			const Values& linearizationPoints, const Symbol& lastKey,
+			const Values& linearizationPoints, Key lastKey,
 			JacobianFactor::shared_ptr& newPrior) const {
 
 		// Extract the Index of the provided last key
@@ -91,8 +91,8 @@ namespace gtsam {
 		// different keys will still compute as if a common key-set was used
 
 		// Create Keys
-		Symbol x0 = motionFactor.key1();
-		Symbol x1 = motionFactor.key2();
+		Key x0 = motionFactor.key1();
+		Key x1 = motionFactor.key2();
 
 		// Create an elimination ordering
 		Ordering ordering;
@@ -131,7 +131,7 @@ namespace gtsam {
 		// different keys will still compute as if a common key-set was used
 
 		// Create Keys
-		Symbol x0 = measurementFactor.key();
+		Key x0 = measurementFactor.key();
 
 		// Create an elimination ordering
 		Ordering ordering;
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h
index bdfaa7651..37d7d216b 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter.h
@@ -55,7 +55,7 @@ namespace gtsam {
 
 		T solve_(const GaussianFactorGraph& linearFactorGraph,
 				const Ordering& ordering, const Values& linearizationPoints,
-				const Symbol& x, JacobianFactor::shared_ptr& newPrior) const;
+				Key x, JacobianFactor::shared_ptr& newPrior) const;
 
 	public:
 
diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h
index d0d37d886..de3992d28 100644
--- a/gtsam/nonlinear/GaussianISAM2-inl.h
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -121,7 +121,7 @@ namespace gtsam {
   ///* ************************************************************************* */
   //boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
   //	boost::shared_ptr<VectorValues> delta(new VectorValues());
-  //	set<Symbol> changed;
+  //	set<Key> changed;
   //	// starting from the root, call optimize on each conditional
   //	optimize2(root, delta);
   //	return delta;
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index eae39bf52..0abea8e7f 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -155,7 +155,7 @@ template<class CONDITIONAL, class GRAPH>
 FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
   FastSet<Index> indices;
   BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
-    BOOST_FOREACH(const Symbol& key, factor->keys()) {
+    BOOST_FOREACH(Key key, factor->keys()) {
       indices.insert(ordering[key]);
     }
   }
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 5c87d1dda..a054eb179 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -105,7 +105,7 @@ ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& aff
   tic(3,"check candidates");
   BOOST_FOREACH(size_t idx, candidates) {
     bool inside = true;
-    BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
+    BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
       Index var = ordering_[key];
       if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
         inside = false;
@@ -398,7 +398,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
 template<class CONDITIONAL, class GRAPH>
 ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
     const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
-    const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
+    const boost::optional<FastSet<Key> >& constrainedKeys, bool force_relinearize) {
 
   static const bool debug = ISDEBUG("ISAM2 update");
   static const bool verbose = ISDEBUG("ISAM2 update verbose");
@@ -519,7 +519,7 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   boost::optional<FastSet<Index> > constrainedIndices;
   if(constrainedKeys) {
     constrainedIndices.reset(FastSet<Index>());
-    BOOST_FOREACH(const Symbol& key, *constrainedKeys) {
+    BOOST_FOREACH(Key key, *constrainedKeys) {
       constrainedIndices->insert(ordering_[key]);
     }
   }
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 00d07a040..505aa2de5 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -369,7 +369,7 @@ public:
    */
   ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
       const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
-      const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
+      const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
       bool force_relinearize = false);
 
   /** Access the current linearization point */
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 1a1fce839..9c34069f5 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -89,7 +89,7 @@ namespace gtsam {
 		/**
 		 * Constructor - forces exact evaluation
 		 */
-		NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
+		NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
 			Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
 			allow_error_(false), error_gain_(0.0),
 			compare_(_compare) {
@@ -98,7 +98,7 @@ namespace gtsam {
 		/**
 		 * Constructor - allows inexact evaluation
 		 */
-		NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
+		NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
 			Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
 			allow_error_(true), error_gain_(error_gain),
 			compare_(_compare) {
@@ -109,7 +109,7 @@ namespace gtsam {
 		/// @{
 
 		virtual void print(const std::string& s = "") const {
-			std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n";
+			std::cout << "Constraint: " << s << " on [" << (std::string)this->key() << "]\n";
 			gtsam::print(feasible_,"Feasible Point");
 			std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
 		}
@@ -204,7 +204,7 @@ namespace gtsam {
 		typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
 
 		///TODO: comment
-		NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0)
+		NonlinearEquality1(const X& value, Key key1, double mu = 1000.0)
 			: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
 
 		virtual ~NonlinearEquality1() {}
@@ -259,7 +259,7 @@ namespace gtsam {
 		typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
 
 		///TODO: comment
-		NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0)
+		NonlinearEquality2(Key key1, Key key2, double mu = 1000.0)
 			: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
 		virtual ~NonlinearEquality2() {}
 
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 5ebe662e5..db1de6740 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -32,6 +32,7 @@
 
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 namespace gtsam {
 
@@ -44,12 +45,12 @@ namespace gtsam {
  * which are objects in non-linear manifolds (Lie groups).
  * \nosubgrouping
  */
-class NonlinearFactor: public Factor<Symbol> {
+class NonlinearFactor: public Factor<Key> {
 
 protected:
 
   // Some handy typedefs
-  typedef Factor<Symbol> Base;
+  typedef Factor<Key> Base;
   typedef NonlinearFactor This;
 
 public:
@@ -187,7 +188,7 @@ public:
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NoiseModelFactor\n";
     std::cout << "  ";
-    BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
+    BOOST_FOREACH(Key key, this->keys()) { std::cout << (std::string)key << " "; }
     std::cout << "\n";
     this->noiseModel_->print("  noise model: ");
   }
@@ -310,14 +311,14 @@ public:
 
   virtual ~NonlinearFactor1() {}
 
-  inline const Symbol& key() const { return keys_[0]; }
+  inline Key key() const { return keys_[0]; }
 
   /**
    *  Constructor
    *  @param z measurement
    *  @param key by which to look up X value in Values
    */
-  NonlinearFactor1(const SharedNoiseModel& noiseModel, const Symbol& key1) :
+  NonlinearFactor1(const SharedNoiseModel& noiseModel, Key key1) :
     Base(noiseModel) {
     keys_.resize(1);
     keys_[0] = key1;
@@ -393,7 +394,7 @@ public:
    * @param j1 key of the first variable
    * @param j2 key of the second variable
    */
-  NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
+  NonlinearFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
     Base(noiseModel) {
     keys_.resize(2);
     keys_[0] = j1;
@@ -403,8 +404,8 @@ public:
   virtual ~NonlinearFactor2() {}
 
   /** methods to retrieve both keys */
-  inline const Symbol& key1() const { return keys_[0];	}
-  inline const Symbol& key2() const {	return keys_[1];	}
+  inline Key key1() const { return keys_[0];	}
+  inline Key key2() const {	return keys_[1];	}
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
@@ -481,7 +482,7 @@ public:
    * @param j2 key of the second variable
    * @param j3 key of the third variable
    */
-  NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) :
+  NonlinearFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
     Base(noiseModel) {
     keys_.resize(3);
     keys_[0] = j1;
@@ -492,9 +493,9 @@ public:
   virtual ~NonlinearFactor3() {}
 
   /** methods to retrieve keys */
-  inline const Symbol& key1() const { return keys_[0]; }
-  inline const Symbol& key2() const { return keys_[1]; }
-  inline const Symbol& key3() const { return keys_[2]; }
+  inline Key key1() const { return keys_[0]; }
+  inline Key key2() const { return keys_[1]; }
+  inline Key key3() const { return keys_[2]; }
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
@@ -574,7 +575,7 @@ public:
    * @param j3 key of the third variable
    * @param j4 key of the fourth variable
    */
-  NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) :
+  NonlinearFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
     Base(noiseModel) {
     keys_.resize(4);
     keys_[0] = j1;
@@ -586,10 +587,10 @@ public:
   virtual ~NonlinearFactor4() {}
 
   /** methods to retrieve keys */
-  inline const Symbol& key1() const { return keys_[0]; }
-  inline const Symbol& key2() const { return keys_[1]; }
-  inline const Symbol& key3() const { return keys_[2]; }
-  inline const Symbol& key4() const { return keys_[3]; }
+  inline Key key1() const { return keys_[0]; }
+  inline Key key2() const { return keys_[1]; }
+  inline Key key3() const { return keys_[2]; }
+  inline Key key4() const { return keys_[3]; }
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
@@ -672,7 +673,7 @@ public:
    * @param j4 key of the fourth variable
    * @param j5 key of the fifth variable
    */
-  NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) :
+  NonlinearFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
     Base(noiseModel) {
     keys_.resize(5);
     keys_[0] = j1;
@@ -685,11 +686,11 @@ public:
   virtual ~NonlinearFactor5() {}
 
   /** methods to retrieve keys */
-  inline const Symbol& key1() const { return keys_[0]; }
-  inline const Symbol& key2() const { return keys_[1]; }
-  inline const Symbol& key3() const { return keys_[2]; }
-  inline const Symbol& key4() const { return keys_[3]; }
-  inline const Symbol& key5() const { return keys_[4]; }
+  inline Key key1() const { return keys_[0]; }
+  inline Key key2() const { return keys_[1]; }
+  inline Key key3() const { return keys_[2]; }
+  inline Key key4() const { return keys_[3]; }
+  inline Key key5() const { return keys_[4]; }
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
@@ -776,7 +777,7 @@ public:
    * @param j5 key of the fifth variable
    * @param j6 key of the fifth variable
    */
-  NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) :
+  NonlinearFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
     Base(noiseModel) {
     keys_.resize(6);
     keys_[0] = j1;
@@ -790,12 +791,12 @@ public:
   virtual ~NonlinearFactor6() {}
 
   /** methods to retrieve keys */
-  inline const Symbol& key1() const { return keys_[0]; }
-  inline const Symbol& key2() const { return keys_[1]; }
-  inline const Symbol& key3() const { return keys_[2]; }
-  inline const Symbol& key4() const { return keys_[3]; }
-  inline const Symbol& key5() const { return keys_[4]; }
-  inline const Symbol& key6() const { return keys_[5]; }
+  inline Key key1() const { return keys_[0]; }
+  inline Key key2() const { return keys_[1]; }
+  inline Key key3() const { return keys_[2]; }
+  inline Key key4() const { return keys_[3]; }
+  inline Key key5() const { return keys_[4]; }
+  inline Key key6() const { return keys_[5]; }
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 11f8e84d9..0c005ce3e 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -49,8 +49,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	std::set<Symbol> NonlinearFactorGraph::keys() const {
-		std::set<Symbol> keys;
+	std::set<Key> NonlinearFactorGraph::keys() const {
+		std::set<Key> keys;
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
 		  if(factor)
 		    keys.insert(factor->begin(), factor->end());
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index fa87baedf..c2cd844be 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -47,7 +47,7 @@ namespace gtsam {
     void print(const std::string& str = "NonlinearFactorGraph: ") const;
 
     /** return keys in some random order */
-    std::set<Symbol> keys() const;
+    std::set<Key> keys() const;
 
 		/** unnormalized error */
 		double error(const Values& c) const;
diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h
index 03d2832ce..e054ecacb 100644
--- a/gtsam/nonlinear/NonlinearISAM-inl.h
+++ b/gtsam/nonlinear/NonlinearISAM-inl.h
@@ -50,7 +50,7 @@ void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
     // Augment ordering
     // FIXME: should just loop over new values
     BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors)
-      BOOST_FOREACH(const Symbol& key, factor->keys())
+      BOOST_FOREACH(Key key, factor->keys())
         ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
 
     boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
@@ -99,7 +99,7 @@ Values NonlinearISAM<GRAPH>::estimate() const {
 
 /* ************************************************************************* */
 template<class GRAPH>
-Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
+Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
 	return isam_.marginalCovariance(ordering_[key]);
 }
 
diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h
index f74cff5ea..16547770b 100644
--- a/gtsam/nonlinear/NonlinearISAM.h
+++ b/gtsam/nonlinear/NonlinearISAM.h
@@ -70,7 +70,7 @@ public:
 	Values estimate() const;
 
 	/** find the marginal covariance for a single variable */
-	Matrix marginalCovariance(const Symbol& key) const;
+	Matrix marginalCovariance(Key key) const;
 
 	// access
 
@@ -104,7 +104,7 @@ public:
 	void reorder_relinearize();
 
 	/** manually add a key to the end of the ordering */
-	void addKey(const Symbol& key) { ordering_.push_back(key); }
+	void addKey(Key key) { ordering_.push_back(key); }
 
 	/** replace the current ordering */
 	void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index 5a2caed64..4e41649e6 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -242,7 +242,7 @@ public:
 	/**
 	 * Return mean and covariance on a single variable
 	 */
-	Matrix marginalCovariance(Symbol j) const {
+	Matrix marginalCovariance(Key j) const {
 		return createSolver()->marginalCovariance((*ordering_)[j]);
 	}
 
diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp
index d8b5eac7e..c35b07f43 100644
--- a/gtsam/nonlinear/Ordering.cpp
+++ b/gtsam/nonlinear/Ordering.cpp
@@ -26,9 +26,9 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-Ordering::Ordering(const std::list<Symbol> & L):nVars_(0) {
+Ordering::Ordering(const std::list<Key> & L):nVars_(0) {
 	int i = 0;
-	BOOST_FOREACH( const Symbol& s, L )
+	BOOST_FOREACH( Key s, L )
 	  insert(s, i++) ;
 }
 
@@ -70,7 +70,7 @@ Ordering::value_type Ordering::pop_back() {
 }
 
 /* ************************************************************************* */
-Index Ordering::pop_back(const Symbol& key) {
+Index Ordering::pop_back(Key key) {
   Map::iterator item = order_.find(key);
   if(item == order_.end()) {
     throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 47d048f2b..084d69ebd 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -17,9 +17,8 @@
 
 #pragma once
 
-#include <map>
 #include <set>
-#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/base/FastMap.h>
 #include <gtsam/inference/inference.h>
 
 #include <boost/foreach.hpp>
@@ -34,8 +33,7 @@ namespace gtsam {
  */
 class Ordering {
 protected:
-  typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
-  typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
+  typedef FastMap<Key, Index> Map;
   Map order_;
   Index nVars_;
 
@@ -43,7 +41,7 @@ public:
 
   typedef boost::shared_ptr<Ordering> shared_ptr;
 
-  typedef std::pair<const Symbol, Index> value_type;
+  typedef std::pair<const Key, Index> value_type;
   typedef Map::iterator iterator;
   typedef Map::const_iterator const_iterator;
 
@@ -54,7 +52,7 @@ public:
   Ordering() : nVars_(0) {}
 
   /// Construct from list, assigns order indices sequentially to list items.
-  Ordering(const std::list<Symbol> & L) ;
+  Ordering(const std::list<Key> & L) ;
 
 	/// @}
 	/// @name Standard Interface
@@ -69,7 +67,7 @@ public:
   const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
   const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
 
-  Index at(const Symbol& key) const { return operator[](key); } ///< Synonym for operator[](const Symbol&) const
+  Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const
 
   /** Assigns the ordering index of the requested \c key into \c index if the symbol
    * is present in the ordering, otherwise does not modify \c index.  The
@@ -79,7 +77,7 @@ public:
    * @param [out] index Reference into which to write the index of the requested key, if the key is present.
    * @return true if the key is present and \c index was modified, false otherwise.
    */
-  bool tryAt(const Symbol& key, Index& index) const {
+  bool tryAt(Key key, Index& index) const {
     const_iterator i = order_.find(key);
     if(i != order_.end()) {
       index = i->second;
@@ -91,7 +89,7 @@ public:
   /// Access the index for the requested key, throws std::out_of_range if the
   /// key is not present in the ordering (note that this differs from the
   /// behavior of std::map)
-  Index& operator[](const Symbol& key) {
+  Index& operator[](Key key) {
     iterator i=order_.find(key);
     if(i == order_.end())  throw std::out_of_range(std::string());
     else                   return i->second; }
@@ -99,7 +97,7 @@ public:
   /// Access the index for the requested key, throws std::out_of_range if the
   /// key is not present in the ordering (note that this differs from the
   /// behavior of std::map)
-  Index operator[](const Symbol& key) const {
+  Index operator[](Key key) const {
     const_iterator i=order_.find(key);
     if(i == order_.end())  throw std::out_of_range(std::string());
     else                   return i->second; }
@@ -110,7 +108,7 @@ public:
    * @return An iterator pointing to the symbol/index pair with the requested,
    * or the end iterator if it does not exist.
    */
-  iterator find(const Symbol& key) { return order_.find(key); }
+  iterator find(Key key) { return order_.find(key); }
 
   /** Returns an iterator pointing to the symbol/index pair with the requested,
    * or the end iterator if it does not exist.
@@ -118,7 +116,7 @@ public:
    * @return An iterator pointing to the symbol/index pair with the requested,
    * or the end iterator if it does not exist.
    */
-  const_iterator find(const Symbol& key) const { return order_.find(key); }
+  const_iterator find(Key key) const { return order_.find(key); }
 
   /**
    * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
@@ -153,22 +151,22 @@ public:
   iterator end() { return order_.end(); }
 
   /// Test if the key exists in the ordering.
-  bool exists(const Symbol& key) const { return order_.count(key); }
+  bool exists(Key key) const { return order_.count(key); }
 
   ///TODO: comment
-  std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
+  std::pair<iterator,bool> tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); }
 
   ///TODO: comment
-  iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
+  iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
 
   /// Adds a new key to the ordering with an index of one greater than the current highest index.
-  Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
+  Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; }
 
   /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair
    * from the ordering (this version is \f$ O(n) \f$, use it when you do not
    * know the last-ordered key).
    *
-   * If you already know the last-ordered symbol, call popback(const Symbol&)
+   * If you already know the last-ordered symbol, call popback(Key)
    * that accepts this symbol as an argument.
    *
    * @return The symbol and index that were removed.
@@ -183,15 +181,15 @@ public:
    *
    * @return The index of the symbol that was removed.
    */
-  Index pop_back(const Symbol& key);
+  Index pop_back(Key key);
 
   /**
    * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
    * very useful for unit tests.  This functionality is courtesy of
    * boost::assign.
    */
-  inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Symbol>
-  operator+=(const Symbol& key) {
+  inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Key>
+  operator+=(Key key) {
     return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<Ordering>(*this))(key); }
 
   /**
@@ -201,8 +199,8 @@ public:
    */
   void permuteWithInverse(const Permutation& inversePermutation);
 
-  /// Synonym for operator[](const Symbol&)
-  Index& at(const Symbol& key) { return operator[](key); }
+  /// Synonym for operator[](Key)
+  Index& at(Key key) { return operator[](key); }
 
 	/// @}
 	/// @name Testable
diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h
index f2963dc32..a59e864a3 100644
--- a/gtsam/nonlinear/Symbol.h
+++ b/gtsam/nonlinear/Symbol.h
@@ -87,6 +87,30 @@ public:
   }
 #endif
 
+  /** Constructor that decodes an integer Key */
+  Symbol(Key key) {
+    const size_t keyBytes = sizeof(Key);
+    const size_t chrBytes = sizeof(unsigned char);
+    const size_t indexBytes = keyBytes - chrBytes;
+    const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
+    const Key indexMask = ~chrMask;
+    c_ = key & chrMask;
+    j_ = key & indexMask;
+  }
+
+  /** Cast to integer */
+  operator Key() const {
+    const size_t keyBytes = sizeof(Key);
+    const size_t chrBytes = sizeof(unsigned char);
+    const size_t indexBytes = keyBytes - chrBytes;
+    const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
+    const Key indexMask = ~chrMask;
+    if(j_ > indexMask)
+      throw std::invalid_argument("Symbol index is too large");
+    Key key = (c_ << indexBytes) | j_;
+    return key;
+  }
+
   // Testable Requirements
   void print(const std::string& s = "") const {
     std::cout << s << ": " << (std::string) (*this) << std::endl;
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index c79d82a18..dd5b29fbd 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -43,11 +43,11 @@ namespace gtsam {
 #if 0
   /* ************************************************************************* */
   class ValueAutomaticCasting {
-    const Symbol& key_;
+    Key key_;
     const Value& value_;
 
   public:
-    ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {}
+    ValueAutomaticCasting(Key key, const Value& value) : key_(key), value_(value) {}
 
     template<class ValueType>
     class ConvertibleToValue : public ValueType {
@@ -67,7 +67,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<typename ValueType>
-  const ValueType& Values::at(const Symbol& j) const {
+  const ValueType& Values::at(Key j) const {
     // Find the item
     KeyValueMap::const_iterator item = values_.find(j);
 
@@ -85,7 +85,7 @@ namespace gtsam {
 
 #if 0
   /* ************************************************************************* */
-  inline ValueAutomaticCasting Values::at(const Symbol& j) const {
+  inline ValueAutomaticCasting Values::at(Key j) const {
     // Find the item
     KeyValueMap::const_iterator item = values_.find(j);
 
@@ -97,26 +97,16 @@ namespace gtsam {
   }
 #endif
 
-  /* ************************************************************************* */
-  template<typename TypedKey>
-  const typename TypedKey::Value& Values::at(const TypedKey& j) const {
-    // Convert to Symbol
-    const Symbol symbol(j.symbol());
-
-    // Call at with the Value type from the key
-    return at<typename TypedKey::Value>(symbol);
-  }
-
 #if 0
   /* ************************************************************************* */
-  inline ValueAutomaticCasting Values::operator[](const Symbol& j) const {
+  inline ValueAutomaticCasting Values::operator[](Key j) const {
     return at(j);
   }
 #endif
 
   /* ************************************************************************* */
   template<typename ValueType>
-  boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
+  boost::optional<const ValueType&> Values::exists(Key j) const {
     // Find the item
     KeyValueMap::const_iterator item = values_.find(j);
 
@@ -132,14 +122,4 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
-  template<class TypedKey>
-  boost::optional<const typename TypedKey::Value&> Values::exists(const TypedKey& j) const {
-    // Convert to Symbol
-    const Symbol symbol(j.symbol());
-
-    // Call exists with the Value type from the key
-    return exists<typename TypedKey::Value>(symbol);
-  }
-
 }
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 44245c39c..4c8d03d9d 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -64,7 +64,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool Values::exists(const Symbol& j) const {
+  bool Values::exists(Key j) const {
     return values_.find(j) != values_.end();
   }
 
@@ -79,7 +79,7 @@ namespace gtsam {
 
     for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
       const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
-      Symbol key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
+      Key key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
       Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
       result.values_.insert(key, retractedValue); // Add retracted result directly to result values
     }
@@ -107,8 +107,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void Values::insert(const Symbol& j, const Value& val) {
-  	Symbol key = j; // Non-const duplicate to deal with non-const insert argument
+  void Values::insert(Key j, const Value& val) {
+  	Key key = j; // Non-const duplicate to deal with non-const insert argument
   	std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
   	if(!insertResult.second)
   		throw ValuesKeyAlreadyExists(j);
@@ -117,13 +117,13 @@ namespace gtsam {
   /* ************************************************************************* */
   void Values::insert(const Values& values) {
     for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
-      Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
+      Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument
       insert(key, key_value->second);
     }
   }
 
   /* ************************************************************************* */
-  void Values::update(const Symbol& j, const Value& val) {
+  void Values::update(Key j, const Value& val) {
   	// Find the value to update
   	KeyValueMap::iterator item = values_.find(j);
   	if(item == values_.end())
@@ -144,7 +144,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void Values::erase(const Symbol& j) {
+  void Values::erase(Key j) {
     KeyValueMap::iterator item = values_.find(j);
     if(item == values_.end())
       throw ValuesKeyDoesNotExist("erase", j);
@@ -152,8 +152,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  FastList<Symbol> Values::keys() const {
-    FastList<Symbol> result;
+  FastList<Key> Values::keys() const {
+    FastList<Key> result;
     for(const_iterator key_value = begin(); key_value != end(); ++key_value)
       result.push_back(key_value->first);
     return result;
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 61e11ca4e..5074c2360 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -37,7 +37,6 @@
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
@@ -64,11 +63,11 @@ namespace gtsam {
     // fast_pool_allocator to allocate map nodes.  In this way, all memory is
     // allocated in a boost memory pool.
     typedef boost::ptr_map<
-        Symbol,
+        Key,
         Value,
-        std::less<Symbol>,
+        std::less<Key>,
         ValueCloneAllocator,
-        boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
+        boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;
 
     // The member to store the values, see just above
     KeyValueMap values_;
@@ -84,16 +83,16 @@ namespace gtsam {
 
     /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
     struct ConstKeyValuePair {
-      const Symbol& first;
+      const Key first;
       const Value& second;
-      ConstKeyValuePair(const Symbol& key, const Value& value) : first(key), second(value) {}
+      ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {}
     };
 
     /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
     struct KeyValuePair {
-      const Symbol& first;
+      const Key first;
       Value& second;
-      KeyValuePair(const Symbol& key, Value& value) : first(key), second(value) {}
+      KeyValuePair(Key key, Value& value) : first(key), second(value) {}
     };
 
     /// Mutable forward iterator, with value type KeyValuePair
@@ -137,7 +136,7 @@ namespace gtsam {
      * @return A const reference to the stored value
      */
     template<typename ValueType>
-    const ValueType& at(const Symbol& j) const;
+    const ValueType& at(Key j) const;
 
 #if 0
     /** Retrieve a variable by key \c j.  This non-templated version returns a
@@ -147,51 +146,25 @@ namespace gtsam {
      * @return A ValueAutomaticCasting object that may be assignmed to a Value
      * of the proper type.
      */
-    ValueAutomaticCasting at(const Symbol& j) const;
+    ValueAutomaticCasting at(Key j) const;
 #endif
 
-    /** Retrieve a variable using a special key (typically TypedSymbol), which
-     * contains the type of the value associated with the key, and which must
-     * be conversion constructible to a Symbol, e.g.
-     * <tt>Symbol(const TypedKey&)</tt>.  Throws DynamicValuesKeyDoesNotExist
-     * the key is not found, and DynamicValuesIncorrectType if the value type
-     * associated with the requested key does not match the stored value type.
-     */
-    template<class TypedKey>
-    const typename TypedKey::Value& at(const TypedKey& j) const;
-
-    /** operator[] syntax for at(const TypedKey& j) */
-    template<class TypedKey>
-    const typename TypedKey::Value& operator[](const TypedKey& j) const {
-      return at(j); }
-
 #if 0
-    /** operator[] syntax for at(const Symbol& j) */
-    ValueAutomaticCasting operator[](const Symbol& j) const;
+    /** operator[] syntax for at(Key j) */
+    ValueAutomaticCasting operator[](Key j) const;
 #endif
 
-    /** Check if a value exists with key \c j.  See exists<>(const Symbol& j)
+    /** Check if a value exists with key \c j.  See exists<>(Key j)
      * and exists(const TypedKey& j) for versions that return the value if it
      * exists. */
-    bool exists(const Symbol& j) const;
+    bool exists(Key j) const;
 
     /** Check if a value with key \c j exists, returns the value with type
      * \c Value if the key does exist, or boost::none if it does not exist.
      * Throws DynamicValuesIncorrectType if the value type associated with the
      * requested key does not match the stored value type. */
     template<typename ValueType>
-    boost::optional<const ValueType&> exists(const Symbol& j) const;
-
-    /** Check if a value with key \c j exists, returns the value with type
-     * \c Value if the key does exist, or boost::none if it does not exist.
-     * Uses a special key (typically TypedSymbol), which contains the type of
-     * the value associated with the key, and which must be conversion
-     * constructible to a Symbol, e.g. <tt>Symbol(const TypedKey&)</tt>. Throws
-     * DynamicValuesIncorrectType if the value type associated with the
-     * requested key does not match the stored value type.
-     */
-    template<class TypedKey>
-    boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const;
+    boost::optional<const ValueType&> exists(Key j) const;
 
     /** The number of variables in this config */
     size_t size() const { return values_.size(); }
@@ -233,25 +206,25 @@ namespace gtsam {
     ///@}
 
     /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
-    void insert(const Symbol& j, const Value& val);
+    void insert(Key j, const Value& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
     void insert(const Values& values);
 
     /** single element change of existing element */
-    void update(const Symbol& j, const Value& val);
+    void update(Key j, const Value& val);
 
     /** update the current available values without adding new ones */
     void update(const Values& values);
 
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
-    void erase(const Symbol& j);
+    void erase(Key j);
 
     /**
      * Returns a set of keys in the config
      * Note: by construction, the list is ordered
      */
-    FastList<Symbol> keys() const;
+    FastList<Key> keys() const;
 
     /** Replace all keys and variables */
     Values& operator=(const Values& rhs);
@@ -286,20 +259,20 @@ namespace gtsam {
   /* ************************************************************************* */
   class ValuesKeyAlreadyExists : public std::exception {
   protected:
-    const Symbol key_; ///< The key that already existed
+    const Key key_; ///< The key that already existed
 
   private:
     mutable std::string message_;
 
   public:
     /// Construct with the key-value pair attemped to be added
-    ValuesKeyAlreadyExists(const Symbol& key) throw() :
+    ValuesKeyAlreadyExists(Key key) throw() :
       key_(key) {}
 
     virtual ~ValuesKeyAlreadyExists() throw() {}
 
     /// The duplicate key that was attemped to be added
-    const Symbol& key() const throw() { return key_; }
+    Key key() const throw() { return key_; }
 
     /// The message to be displayed to the user
     virtual const char* what() const throw();
@@ -309,20 +282,20 @@ namespace gtsam {
   class ValuesKeyDoesNotExist : public std::exception {
   protected:
     const char* operation_; ///< The operation that attempted to access the key
-    const Symbol key_; ///< The key that does not exist
+    const Key key_; ///< The key that does not exist
 
   private:
     mutable std::string message_;
 
   public:
     /// Construct with the key that does not exist in the values
-    ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
+    ValuesKeyDoesNotExist(const char* operation, Key key) throw() :
       operation_(operation), key_(key) {}
 
     virtual ~ValuesKeyDoesNotExist() throw() {}
 
     /// The key that was attempted to be accessed that does not exist
-    const Symbol& key() const throw() { return key_; }
+    Key key() const throw() { return key_; }
 
     /// The message to be displayed to the user
     virtual const char* what() const throw();
@@ -331,7 +304,7 @@ namespace gtsam {
   /* ************************************************************************* */
   class ValuesIncorrectType : public std::exception {
   protected:
-    const Symbol key_; ///< The key requested
+    const Key key_; ///< The key requested
     const std::type_info& storedTypeId_;
     const std::type_info& requestedTypeId_;
 
@@ -340,14 +313,14 @@ namespace gtsam {
 
   public:
     /// Construct with the key that does not exist in the values
-    ValuesIncorrectType(const Symbol& key,
+    ValuesIncorrectType(Key key,
         const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
       key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
 
     virtual ~ValuesIncorrectType() throw() {}
 
     /// The key that was attempted to be accessed that does not exist
-    const Symbol& key() const throw() { return key_; }
+    Key key() const throw() { return key_; }
 
     /// The typeid of the value stores in the Values
     const std::type_info& storedTypeId() const { return storedTypeId_; }
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index bb68ef9e6..77e1155a7 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -31,7 +31,7 @@ using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
-Symbol key1("v1"), key2("v2"), key3("v3"), key4("v4");
+Key key1("v1"), key2("v2"), key3("v3"), key4("v4");
 
 /* ************************************************************************* */
 TEST( Values, equals1 )
@@ -218,12 +218,12 @@ TEST(Values, extract_keys)
 	config.insert("x4", Pose2());
 	config.insert("x5", Pose2());
 
-	FastList<Symbol> expected, actual;
+	FastList<Key> expected, actual;
 	expected += "x1", "x2", "x4", "x5";
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
-	FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	FastList<Key>::const_iterator itAct = actual.begin(), itExp = expected.begin();
 	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
 		CHECK(assert_equal(*itExp, *itAct));
 	}
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index 70d19061e..b114d14b8 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -48,7 +48,7 @@ namespace gtsam {
 		BearingFactor() {}
 
 		/** primary constructor */
-		BearingFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measured,
+		BearingFactor(Key poseKey, Key pointKey, const Rot& measured,
 				const SharedNoiseModel& model) :
 					Base(model, poseKey, pointKey), measured_(measured) {
 		}
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 4e442ad86..785b4c488 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -50,7 +50,7 @@ namespace gtsam {
 	public:
 
 		BearingRangeFactor() {} /* Default constructor */
-		BearingRangeFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measuredBearing, const double measuredRange,
+		BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double measuredRange,
 				const SharedNoiseModel& model) :
 					Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) {
 		}
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 313241eee..9454984b2 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -55,7 +55,7 @@ namespace gtsam {
 		BetweenFactor() {}
 
 		/** Constructor */
-		BetweenFactor(const Symbol& key1, const Symbol& key2, const VALUE& measured,
+		BetweenFactor(Key key1, Key key2, const VALUE& measured,
 				const SharedNoiseModel& model) :
 			Base(model, key1, key2), measured_(measured) {
 		}
@@ -123,7 +123,7 @@ namespace gtsam {
 		typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
 
 		/** Syntactic sugar for constrained version */
-		BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
+		BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
 		  BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
 
 	private:
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index 13ae33cfc..0decb3290 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -37,7 +37,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
 
-	BoundingConstraint1(const Symbol& key, double threshold,
+	BoundingConstraint1(Key key, double threshold,
 			bool isGreaterThan, double mu = 1000.0) :
 				Base(noiseModel::Constrained::All(1, mu), key),
 				threshold_(threshold), isGreaterThan_(isGreaterThan) {
@@ -106,7 +106,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
 
-	BoundingConstraint2(const Symbol& key1, const Symbol& key2, double threshold,
+	BoundingConstraint2(Key key1, Key key2, double threshold,
 			bool isGreaterThan, double mu = 1000.0)
 	: Base(noiseModel::Constrained::All(1, mu), key1, key2),
 	  threshold_(threshold), isGreaterThan_(isGreaterThan) {}
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 868b064d7..ac5d07f75 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -52,7 +52,7 @@ namespace gtsam {
 		 * @param i is basically the frame number
 		 * @param j is the index of the landmark
 		 */
-		GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
+		GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
 		  Base(model, cameraKey, landmarkKey), measured_(measured) {}
 
 		GeneralSFMFactor():measured_(0.0,0.0) {} 							///< default constructor
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index e29893e77..91c71b1ad 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -60,7 +60,7 @@ namespace gtsam {
 		 * constructor with just minimum requirements for a factor - allows more
 		 * computation in the constructor.  This should only be used by subclasses
 		 */
-		PartialPriorFactor(const Symbol& key, const SharedNoiseModel& model)
+		PartialPriorFactor(Key key, const SharedNoiseModel& model)
 		  : Base(model, key) {}
 
 	public:
@@ -71,14 +71,14 @@ namespace gtsam {
 		virtual ~PartialPriorFactor() {}
 
 		/** Single Element Constructor: acts on a single parameter specified by idx */
-		PartialPriorFactor(const Symbol& key, size_t idx, double prior, const SharedNoiseModel& model) :
+		PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
 			Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
 			assert(model->dim() == 1);
 			this->fillH();
 		}
 
 		/** Indices Constructor: specify the mask with a set of indices */
-		PartialPriorFactor(const Symbol& key, const std::vector<size_t>& mask, const Vector& prior,
+		PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
 				const SharedNoiseModel& model) :
 			Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
 			assert((size_t)prior_.size() == mask.size());
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 43a33a9fe..1f8436b02 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -52,7 +52,7 @@ namespace gtsam {
 		virtual ~PriorFactor() {}
 
 		/** Constructor */
-		PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) :
+		PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model) :
 			Base(model, key), prior_(prior) {
 		}
 
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index f654e5d33..805a991cd 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -63,7 +63,7 @@ namespace gtsam {
 		 * @param K shared pointer to the constant calibration
 		 */
 		GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-				const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) :
+				const Symbol poseKey, Key pointKey, const shared_ptrK& K) :
 				  Base(model, poseKey, pointKey), measured_(measured), K_(K) {
 		}
 
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index 43cc3da57..9edc9693f 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -44,7 +44,7 @@ namespace gtsam {
 
 		RangeFactor() {} /* Default constructor */
 
-		RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
+		RangeFactor(Key poseKey, Key pointKey, double measured,
 				const SharedNoiseModel& model) :
 					Base(model, poseKey, pointKey), measured_(measured) {
 		}
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 76359d8c9..a69f4189b 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -50,7 +50,7 @@ public:
 	 * @param landmarkKey the landmark variable key
 	 * @param K the constant calibration
 	 */
-	GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) :
+	GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, const shared_ptrKStereo& K) :
 		Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {
 	}
 
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index bb9f143c5..cdcc2663a 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -129,7 +129,7 @@ namespace simulated2D {
     Pose measured_; ///< prior mean
 
     /// Create generic prior
-    GenericPrior(const Pose& z, const SharedNoiseModel& model, const Symbol& key) :
+    GenericPrior(const Pose& z, const SharedNoiseModel& model, Key key) :
       Base(model, key), measured_(z) {
     }
 
@@ -166,7 +166,7 @@ namespace simulated2D {
     Pose measured_; ///< odometry measurement
 
     /// Create odometry
-    GenericOdometry(const Pose& measured, const SharedNoiseModel& model, const Symbol& key1, const Symbol& key2) :
+    GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key key1, Key key2) :
           Base(model, key1, key2), measured_(measured) {
     }
 
@@ -206,7 +206,7 @@ namespace simulated2D {
     Landmark measured_; ///< Measurement
 
     /// Create measurement factor
-    GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :
+    GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey) :
           Base(model, poseKey, landmarkKey), measured_(measured) {
     }
 
diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h
index 5a6b39861..b44062933 100644
--- a/gtsam/slam/simulated2DConstraints.h
+++ b/gtsam/slam/simulated2DConstraints.h
@@ -65,7 +65,7 @@ namespace simulated2D {
        * @param isGreaterThan is a flag to set inequality as greater than or less than
        * @param mu is the penalty function gain
        */
-      ScalarCoordConstraint1(const Symbol& key, double c,
+      ScalarCoordConstraint1(Key key, double c,
           bool isGreaterThan, double mu = 1000.0) :
             Base(key, c, isGreaterThan, mu) {
       }
@@ -127,7 +127,7 @@ namespace simulated2D {
        * @param range_bound is the maximum range allowed between the variables
        * @param mu is the gain for the penalty function
        */
-      MaxDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) :
+      MaxDistanceConstraint(Key key1, Key key2, double range_bound, double mu = 1000.0) :
         Base(key1, key2, range_bound, false, mu) {}
 
       /**
@@ -170,7 +170,7 @@ namespace simulated2D {
        * @param range_bound is the minimum range allowed between the variables
        * @param mu is the gain for the penalty function
        */
-      MinDistanceConstraint(const Symbol& key1, const Symbol& key2,
+      MinDistanceConstraint(Key key1, Key key2,
           double range_bound, double mu = 1000.0)
       : Base(key1, key2, range_bound, true, mu) {}
 
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index f38e9c34d..47314f8cf 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -83,7 +83,7 @@ namespace simulated2DOriented {
     Pose2 measured_; ///< measurement
 
     /// Create generic pose prior
-    GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, const Symbol& key) :
+    GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) :
       NonlinearFactor1<VALUE>(model, key), measured_(measured) {
     }
 
@@ -106,7 +106,7 @@ namespace simulated2DOriented {
      * Creates an odometry factor between two poses
      */
     GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
-        const Symbol& i1, const Symbol& i2) :
+        Key i1, Key i2) :
           NonlinearFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
     }
 
diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index 9cd1ed74d..6a2ce8d26 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -74,7 +74,7 @@ struct PointPrior3D: public NonlinearFactor1<Point3> {
 	 * @param model is the measurement model for the factor (Dimension: 3)
 	 * @param key is the key for the pose
 	 */
-	PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) :
+	PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
 	  NonlinearFactor1<Point3> (model, key), measured_(measured) {
 	}
 
@@ -106,7 +106,7 @@ struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
 	 * @param pointKey is the point key for the landmark
 	 */
 	Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
-	    const Symbol& poseKey, const Symbol& pointKey) :
+	    Key poseKey, Key pointKey) :
 	      NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
 
 	/**
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 4c13ab2b3..7c5b333ad 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -202,7 +202,7 @@ namespace example {
 
 			Point2 z_;
 
-			UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) :
+			UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
 				gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
 			}
 
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 938067508..da8d8a114 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -64,9 +64,9 @@ TEST( Graph, error )
 TEST( Graph, keys )
 {
 	Graph fg = createNonlinearFactorGraph();
-	set<Symbol> actual = fg.keys();
+	set<Key> actual = fg.keys();
 	LONGS_EQUAL(3, actual.size());
-	set<Symbol>::const_iterator it = actual.begin();
+	set<Key>::const_iterator it = actual.begin();
 	CHECK(assert_equal(Symbol('l', 1), *(it++)));
 	CHECK(assert_equal(Symbol('x', 1), *(it++)));
 	CHECK(assert_equal(Symbol('x', 2), *(it++)));
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index 09a3d4db0..80ed7b6b6 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -48,8 +48,8 @@ TEST(testNonlinearISAM, markov_chain ) {
 			Ordering ordering = isam.getOrdering();
 
 			// swap last two elements
-			Symbol last = ordering.pop_back().first;
-			Symbol secondLast = ordering.pop_back().first;
+			Key last = ordering.pop_back().first;
+			Key secondLast = ordering.pop_back().first;
 			ordering.push_back(last);
 			ordering.push_back(secondLast);
 			isam.setOrdering(ordering);

From 3bd1aa13fc97ee1d7a54064047dbc5e07313b0ed Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 20 Feb 2012 17:02:59 +0000
Subject: [PATCH 75/88] Started print formatter

---
 .cproject                         | 2002 +++++++++++++++--------------
 gtsam/base/StringFormatter.cpp    |   23 +
 gtsam/base/StringFormatter.h      |   36 +
 gtsam/nonlinear/NonlinearFactor.h |    4 +-
 gtsam/nonlinear/Symbol.h          |    5 +
 5 files changed, 1073 insertions(+), 997 deletions(-)
 create mode 100644 gtsam/base/StringFormatter.cpp
 create mode 100644 gtsam/base/StringFormatter.h

diff --git a/.cproject b/.cproject
index b6034e167..c47c94ecf 100644
--- a/.cproject
+++ b/.cproject
@@ -286,39 +286,7 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 		<buildTargets>
-			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testTestableAssertions.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -326,23 +294,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
+				<buildTarget>tests/testSPQRUtil.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testBlockMatrices.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -350,110 +310,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCholesky.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testLieConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -480,6 +336,7 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -487,6 +344,7 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -534,6 +392,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -541,6 +400,7 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -548,6 +408,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -563,15 +424,120 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>vSFMexample.run</buildTarget>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -616,7 +582,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -624,22 +590,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -672,7 +622,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -680,58 +630,18 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildTarget>vSFMexample.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>tests/testGaussianISAM2</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>testVSLAMGraph</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -888,6 +798,890 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeVectorConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testErrors.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianJunctionTree.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactor.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianConditional.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeSLAMlike.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeSLAMlike.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testClusterTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testClusterTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testJunctionTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testEliminationTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testEliminationTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testSymbolicFactor.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVariableSlots.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVariableSlots.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testConditional.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testSymbolicFactorGraph.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSymbolicFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearConstraint.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearConstraint.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testLieConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testConstraintOptimizer.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testConstraintOptimizer.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testRot3M.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timePose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPlanarSLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPlanarSLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2Config.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2Factor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2Prior.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2Prior.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2SLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3Config.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3SLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testSimulated2DOriented.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMConfig.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMFactor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMGraph.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3Factor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testSimulated2D.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testSimulated3D.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianISAM2" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testTestableAssertions.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testBlockMatrices.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCholesky.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVectorValues.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testHessianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testKalmanFilter.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianDensity.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>SimpleRotation.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>CameraResectioning</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="easyPoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>easyPoint2KalmanFilter.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="elaboratePoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>elaboratePoint2KalmanFilter.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="Pose2SLAMExample_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>Pose2SLAMExample_advanced.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testLieConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -984,126 +1778,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVectorValues.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNoiseModel.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testHessianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testKalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianDensity.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1152,150 +1826,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianJunctionTree.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactor.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianConditional.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeSLAMlike.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeSLAMlike.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testStereoCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testRot3M.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timePose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeStereoCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j1</buildArguments>
@@ -1459,7 +1989,6 @@
 			<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cmake</buildCommand>
 				<buildArguments>..</buildArguments>
-				<buildTarget/>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1664,525 +2193,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMGraph</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testClusterTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testClusterTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testJunctionTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testEliminationTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testEliminationTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSymbolicFactor.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVariableSlots.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVariableSlots.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testConditional.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSymbolicFactorGraph.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSymbolicFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearConstraint.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearConstraint.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testLieConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testConstraintOptimizer.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testConstraintOptimizer.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeVectorConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNoiseModel.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testErrors.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>SimpleRotation.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>CameraResectioning</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="easyPoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>easyPoint2KalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="elaboratePoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>elaboratePoint2KalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="Pose2SLAMExample_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>Pose2SLAMExample_advanced.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPlanarSLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPlanarSLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2Config.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2Factor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2Prior.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2Prior.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2SLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3Config.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3SLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>testSimulated2DOriented.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMConfig.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMFactor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMGraph.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3Factor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>testSimulated2D.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>testSimulated3D.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianISAM2" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianISAM2</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>
diff --git a/gtsam/base/StringFormatter.cpp b/gtsam/base/StringFormatter.cpp
new file mode 100644
index 000000000..6674d3f5a
--- /dev/null
+++ b/gtsam/base/StringFormatter.cpp
@@ -0,0 +1,23 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file StringFormatter.cpp
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 19, 2012
+ */
+
+#include "StringFormatter.h"
+
+namespace gtsam {
+
+} /* namespace gtsam */
diff --git a/gtsam/base/StringFormatter.h b/gtsam/base/StringFormatter.h
new file mode 100644
index 000000000..26cea8881
--- /dev/null
+++ b/gtsam/base/StringFormatter.h
@@ -0,0 +1,36 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file StringFormatter.h
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 19, 2012
+ */
+
+#pragma once
+
+namespace gtsam {
+
+  /**
+   *
+   */
+  class StringFormatter {
+
+  public:
+
+    virtual ~StringFormatter() {}
+
+    virtual std::string keyToString(Key key) = 0;
+
+  };
+
+} /* namespace gtsam */
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index db1de6740..760514821 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -24,6 +24,7 @@
 #include <limits>
 
 #include <boost/serialization/base_object.hpp>
+#include <boost/function.hpp>
 
 #include <gtsam/inference/Factor-inl.h>
 #include <gtsam/inference/IndexFactor.h>
@@ -78,7 +79,8 @@ public:
 	/// @{
 
   /** print */
-  virtual void print(const std::string& s = "") const {
+  virtual void print(const std::string& s = "",
+      const boost::function<std::string(Key)>& keyFormatter = &Symbol::format) const {
     std::cout << s << ": NonlinearFactor\n";
   }
 
diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h
index a59e864a3..6a9d612f7 100644
--- a/gtsam/nonlinear/Symbol.h
+++ b/gtsam/nonlinear/Symbol.h
@@ -119,6 +119,11 @@ public:
     return (*this) == expected;
   }
 
+  /** Format function that can be passed to print functions in nonlinear */
+  static std::string format(Key key) {
+    return (std::string)Symbol(key);
+  }
+
   /** Retrieve key character */
   unsigned char chr() const {
     return c_;

From 0a81c4e57abd8096070fd1a75158f9c43961c4df Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 20 Feb 2012 21:52:47 +0000
Subject: [PATCH 76/88] Renamed NonlinearFactor[1-6] to NoiseModelFactor[1-6]

---
 examples/CameraResectioning.cpp        |  4 +-
 gtsam/nonlinear/ExtendedKalmanFilter.h |  4 +-
 gtsam/nonlinear/NonlinearEquality.h    | 18 +++----
 gtsam/nonlinear/NonlinearFactor.h      | 72 +++++++++++++-------------
 gtsam/slam/BearingFactor.h             |  6 +--
 gtsam/slam/BearingRangeFactor.h        |  6 +--
 gtsam/slam/BetweenFactor.h             |  6 +--
 gtsam/slam/BoundingConstraint.h        | 12 ++---
 gtsam/slam/GeneralSFMFactor.h          |  4 +-
 gtsam/slam/PartialPriorFactor.h        |  6 +--
 gtsam/slam/PriorFactor.h               |  6 +--
 gtsam/slam/ProjectionFactor.h          |  4 +-
 gtsam/slam/RangeFactor.h               |  6 +--
 gtsam/slam/StereoFactor.h              |  4 +-
 gtsam/slam/simulated2D.h               | 12 ++---
 gtsam/slam/simulated2DOriented.h       |  8 +--
 gtsam/slam/simulated3D.h               |  8 +--
 gtsam/slam/smallExample.cpp            |  4 +-
 tests/testExtendedKalmanFilter.cpp     |  8 +--
 tests/testNonlinearFactor.cpp          | 18 +++----
 20 files changed, 108 insertions(+), 108 deletions(-)

diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 1bfd6c785..f00509c3b 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -29,8 +29,8 @@ using namespace gtsam;
 /**
  * Unary factor for the pose.
  */
-class ResectioningFactor: public NonlinearFactor1<Pose3> {
-  typedef NonlinearFactor1<Pose3> Base;
+class ResectioningFactor: public NoiseModelFactor1<Pose3> {
+  typedef NoiseModelFactor1<Pose3> Base;
 
   shared_ptrK K_; // camera's intrinsic parameters
   Point3 P_; // 3D point on the calibration rig
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h
index 37d7d216b..13a612f84 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter.h
@@ -46,8 +46,8 @@ namespace gtsam {
 
 		typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
 		typedef VALUE T;
-		typedef NonlinearFactor2<VALUE, VALUE> MotionFactor;
-		typedef NonlinearFactor1<VALUE> MeasurementFactor;
+		typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
+		typedef NoiseModelFactor1<VALUE> MeasurementFactor;
 
 	protected:
 		T x_; // linearization point
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 9c34069f5..3a37fa53d 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -48,7 +48,7 @@ namespace gtsam {
 	 * \nosubgrouping
 	 */
 	template<class VALUE>
-	class NonlinearEquality: public NonlinearFactor1<VALUE> {
+	class NonlinearEquality: public NoiseModelFactor1<VALUE> {
 
 	public:
 		typedef VALUE T;
@@ -68,7 +68,7 @@ namespace gtsam {
 		typedef NonlinearEquality<VALUE> This;
 
 		// typedef to base class
-		typedef NonlinearFactor1<VALUE> Base;
+		typedef NoiseModelFactor1<VALUE> Base;
 
 	public:
 
@@ -169,7 +169,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor1",
+			ar & boost::serialization::make_nvp("NoiseModelFactor1",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(feasible_);
 			ar & BOOST_SERIALIZATION_NVP(allow_error_);
@@ -183,13 +183,13 @@ namespace gtsam {
 	 * Simple unary equality constraint - fixes a value for a variable
 	 */
 	template<class VALUE>
-	class NonlinearEquality1 : public NonlinearFactor1<VALUE> {
+	class NonlinearEquality1 : public NoiseModelFactor1<VALUE> {
 
 	public:
 		typedef VALUE X;
 
 	protected:
-		typedef NonlinearFactor1<VALUE> Base;
+		typedef NoiseModelFactor1<VALUE> Base;
 
 		/** default constructor to allow for serialization */
 		NonlinearEquality1() {}
@@ -230,7 +230,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor1",
+			ar & boost::serialization::make_nvp("NoiseModelFactor1",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(value_);
 		}
@@ -242,12 +242,12 @@ namespace gtsam {
 	 * be the same.
 	 */
 	template<class VALUE>
-	class NonlinearEquality2 : public NonlinearFactor2<VALUE, VALUE> {
+	class NonlinearEquality2 : public NoiseModelFactor2<VALUE, VALUE> {
 	public:
 		typedef VALUE X;
 
 	protected:
-		typedef NonlinearFactor2<VALUE, VALUE> Base;
+		typedef NoiseModelFactor2<VALUE, VALUE> Base;
 
 		GTSAM_CONCEPT_MANIFOLD_TYPE(X);
 
@@ -279,7 +279,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor2",
+			ar & boost::serialization::make_nvp("NoiseModelFactor2",
 					boost::serialization::base_object<Base>(*this));
 		}
 	}; // \NonlinearEquality2
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 760514821..60f29cbf6 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -294,7 +294,7 @@ private:
 /** A convenient base class for creating your own NoiseModelFactor with 1
  * variable.  To derive from this class, implement evaluateError(). */
 template<class VALUE>
-class NonlinearFactor1: public NoiseModelFactor {
+class NoiseModelFactor1: public NoiseModelFactor {
 
 public:
 
@@ -304,14 +304,14 @@ public:
 protected:
 
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor1<VALUE> This;
+  typedef NoiseModelFactor1<VALUE> This;
 
 public:
 
   /** Default constructor for I/O only */
-  NonlinearFactor1() {}
+  NoiseModelFactor1() {}
 
-  virtual ~NonlinearFactor1() {}
+  virtual ~NoiseModelFactor1() {}
 
   inline Key key() const { return keys_[0]; }
 
@@ -320,7 +320,7 @@ public:
    *  @param z measurement
    *  @param key by which to look up X value in Values
    */
-  NonlinearFactor1(const SharedNoiseModel& noiseModel, Key key1) :
+  NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
     Base(noiseModel) {
     keys_.resize(1);
     keys_[0] = key1;
@@ -364,14 +364,14 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
   }
-};// \class NonlinearFactor1
+};// \class NoiseModelFactor1
 
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 2
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2>
-class NonlinearFactor2: public NoiseModelFactor {
+class NoiseModelFactor2: public NoiseModelFactor {
 
 public:
 
@@ -382,28 +382,28 @@ public:
 protected:
 
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor2<VALUE1, VALUE2> This;
+  typedef NoiseModelFactor2<VALUE1, VALUE2> This;
 
 public:
 
   /**
    * Default Constructor for I/O
    */
-  NonlinearFactor2() {}
+  NoiseModelFactor2() {}
 
   /**
    * Constructor
    * @param j1 key of the first variable
    * @param j2 key of the second variable
    */
-  NonlinearFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
+  NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
     Base(noiseModel) {
     keys_.resize(2);
     keys_[0] = j1;
     keys_[1] = j2;
   }
 
-  virtual ~NonlinearFactor2() {}
+  virtual ~NoiseModelFactor2() {}
 
   /** methods to retrieve both keys */
   inline Key key1() const { return keys_[0];	}
@@ -451,13 +451,13 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
   }
-}; // \class NonlinearFactor2
+}; // \class NoiseModelFactor2
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 3
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3>
-class NonlinearFactor3: public NoiseModelFactor {
+class NoiseModelFactor3: public NoiseModelFactor {
 
 public:
 
@@ -469,14 +469,14 @@ public:
 protected:
 
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor3<VALUE1, VALUE2, VALUE3> This;
+  typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> This;
 
 public:
 
   /**
    * Default Constructor for I/O
    */
-  NonlinearFactor3() {}
+  NoiseModelFactor3() {}
 
   /**
    * Constructor
@@ -484,7 +484,7 @@ public:
    * @param j2 key of the second variable
    * @param j3 key of the third variable
    */
-  NonlinearFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
+  NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
     Base(noiseModel) {
     keys_.resize(3);
     keys_[0] = j1;
@@ -492,7 +492,7 @@ public:
     keys_[2] = j3;
   }
 
-  virtual ~NonlinearFactor3() {}
+  virtual ~NoiseModelFactor3() {}
 
   /** methods to retrieve keys */
   inline Key key1() const { return keys_[0]; }
@@ -542,13 +542,13 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
   }
-}; // \class NonlinearFactor3
+}; // \class NoiseModelFactor3
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 4
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
-class NonlinearFactor4: public NoiseModelFactor {
+class NoiseModelFactor4: public NoiseModelFactor {
 
 public:
 
@@ -561,14 +561,14 @@ public:
 protected:
 
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
+  typedef NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
 
 public:
 
   /**
    * Default Constructor for I/O
    */
-  NonlinearFactor4() {}
+  NoiseModelFactor4() {}
 
   /**
    * Constructor
@@ -577,7 +577,7 @@ public:
    * @param j3 key of the third variable
    * @param j4 key of the fourth variable
    */
-  NonlinearFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
+  NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
     Base(noiseModel) {
     keys_.resize(4);
     keys_[0] = j1;
@@ -586,7 +586,7 @@ public:
     keys_[3] = j4;
   }
 
-  virtual ~NonlinearFactor4() {}
+  virtual ~NoiseModelFactor4() {}
 
   /** methods to retrieve keys */
   inline Key key1() const { return keys_[0]; }
@@ -638,13 +638,13 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
   }
-}; // \class NonlinearFactor4
+}; // \class NoiseModelFactor4
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 5
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
-class NonlinearFactor5: public NoiseModelFactor {
+class NoiseModelFactor5: public NoiseModelFactor {
 
 public:
 
@@ -658,14 +658,14 @@ public:
 protected:
 
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
+  typedef NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
 
 public:
 
   /**
    * Default Constructor for I/O
    */
-  NonlinearFactor5() {}
+  NoiseModelFactor5() {}
 
   /**
    * Constructor
@@ -675,7 +675,7 @@ public:
    * @param j4 key of the fourth variable
    * @param j5 key of the fifth variable
    */
-  NonlinearFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
+  NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
     Base(noiseModel) {
     keys_.resize(5);
     keys_[0] = j1;
@@ -685,7 +685,7 @@ public:
     keys_[4] = j5;
   }
 
-  virtual ~NonlinearFactor5() {}
+  virtual ~NoiseModelFactor5() {}
 
   /** methods to retrieve keys */
   inline Key key1() const { return keys_[0]; }
@@ -740,13 +740,13 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
   }
-}; // \class NonlinearFactor5
+}; // \class NoiseModelFactor5
 
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 6
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
-class NonlinearFactor6: public NoiseModelFactor {
+class NoiseModelFactor6: public NoiseModelFactor {
 
 public:
 
@@ -761,14 +761,14 @@ public:
 protected:
 
   typedef NoiseModelFactor Base;
-  typedef NonlinearFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
+  typedef NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
 
 public:
 
   /**
    * Default Constructor for I/O
    */
-  NonlinearFactor6() {}
+  NoiseModelFactor6() {}
 
   /**
    * Constructor
@@ -779,7 +779,7 @@ public:
    * @param j5 key of the fifth variable
    * @param j6 key of the fifth variable
    */
-  NonlinearFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
+  NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
     Base(noiseModel) {
     keys_.resize(6);
     keys_[0] = j1;
@@ -790,7 +790,7 @@ public:
     keys_[5] = j6;
   }
 
-  virtual ~NonlinearFactor6() {}
+  virtual ~NoiseModelFactor6() {}
 
   /** methods to retrieve keys */
   inline Key key1() const { return keys_[0]; }
@@ -848,7 +848,7 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor",
         boost::serialization::base_object<Base>(*this));
   }
-}; // \class NonlinearFactor6
+}; // \class NoiseModelFactor6
 
 /* ************************************************************************* */
 
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index b114d14b8..7c208436a 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -26,7 +26,7 @@ namespace gtsam {
 	 * Binary factor for a bearing measurement
 	 */
 	template<class POSE, class POINT>
-	class BearingFactor: public NonlinearFactor2<POSE, POINT> {
+	class BearingFactor: public NoiseModelFactor2<POSE, POINT> {
 	private:
 
 		typedef POSE Pose;
@@ -34,7 +34,7 @@ namespace gtsam {
 		typedef POINT Point;
 
 		typedef BearingFactor<POSE, POINT> This;
-		typedef NonlinearFactor2<POSE, POINT> Base;
+		typedef NoiseModelFactor2<POSE, POINT> Base;
 
 		Rot measured_; /** measurement */
 
@@ -79,7 +79,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor2",
+			ar & boost::serialization::make_nvp("NoiseModelFactor2",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(measured_);
 		}
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 785b4c488..4ff792e23 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -28,7 +28,7 @@ namespace gtsam {
 	 * Binary factor for a bearing measurement
 	 */
 	template<class POSE, class POINT>
-	class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> {
+	class BearingRangeFactor: public NoiseModelFactor2<POSE, POINT> {
 	private:
 
 		typedef POSE Pose;
@@ -36,7 +36,7 @@ namespace gtsam {
 		typedef POINT Point;
 
 		typedef BearingRangeFactor<POSE, POINT> This;
-		typedef NonlinearFactor2<POSE, POINT> Base;
+		typedef NoiseModelFactor2<POSE, POINT> Base;
 
 		// the measurement
 		Rot measuredBearing_;
@@ -106,7 +106,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor2",
+			ar & boost::serialization::make_nvp("NoiseModelFactor2",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(measuredBearing_);
 			ar & BOOST_SERIALIZATION_NVP(measuredRange_);
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 9454984b2..c88af0d42 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -29,7 +29,7 @@ namespace gtsam {
 	 * @tparam VALUE the Value type
 	 */
 	template<class VALUE>
-	class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> {
+	class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> {
 
 	public:
 
@@ -38,7 +38,7 @@ namespace gtsam {
 	private:
 
 		typedef BetweenFactor<VALUE> This;
-		typedef NonlinearFactor2<VALUE, VALUE> Base;
+		typedef NoiseModelFactor2<VALUE, VALUE> Base;
 
 		VALUE measured_; /** The measurement */
 
@@ -106,7 +106,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor2",
+			ar & boost::serialization::make_nvp("NoiseModelFactor2",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(measured_);
 		}
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index 0decb3290..0032d9a05 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -29,9 +29,9 @@ namespace gtsam {
  * a scalar for comparison.
  */
 template<class VALUE>
-struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
+struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
 	typedef VALUE X;
-	typedef NonlinearFactor1<VALUE> Base;
+	typedef NoiseModelFactor1<VALUE> Base;
 	typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
 
 	double threshold_;
@@ -84,7 +84,7 @@ private:
 	friend class boost::serialization::access;
 	template<class ARCHIVE>
 	void serialize(ARCHIVE & ar, const unsigned int version) {
-		ar & boost::serialization::make_nvp("NonlinearFactor1",
+		ar & boost::serialization::make_nvp("NoiseModelFactor1",
 				boost::serialization::base_object<Base>(*this));
 		ar & BOOST_SERIALIZATION_NVP(threshold_);
 		ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
@@ -96,11 +96,11 @@ private:
  * to implement for specific systems
  */
 template<class VALUE1, class VALUE2>
-struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
+struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
 	typedef VALUE1 X1;
 	typedef VALUE2 X2;
 
-	typedef NonlinearFactor2<VALUE1, VALUE2> Base;
+	typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
 	typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
 
 	double threshold_;
@@ -157,7 +157,7 @@ private:
 	friend class boost::serialization::access;
 	template<class ARCHIVE>
 	void serialize(ARCHIVE & ar, const unsigned int version) {
-		ar & boost::serialization::make_nvp("NonlinearFactor2",
+		ar & boost::serialization::make_nvp("NoiseModelFactor2",
 				boost::serialization::base_object<Base>(*this));
 		ar & BOOST_SERIALIZATION_NVP(threshold_);
 		ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index ac5d07f75..4a8b76739 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -31,7 +31,7 @@ namespace gtsam {
 	 */
 	template <class CAMERA, class LANDMARK>
 	class GeneralSFMFactor:
-	public NonlinearFactor2<CAMERA, LANDMARK> {
+	public NoiseModelFactor2<CAMERA, LANDMARK> {
 	protected:
 		Point2 measured_;			///< the 2D measurement
 
@@ -39,7 +39,7 @@ namespace gtsam {
 
 		typedef CAMERA Cam;					            					///< typedef for camera type
 		typedef GeneralSFMFactor<CAMERA, LANDMARK> This;	///< typedef for this object
-		typedef NonlinearFactor2<CAMERA, LANDMARK> Base;	///< typedef for the base class
+		typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;	///< typedef for the base class
 		typedef Point2 Measurement;												///< typedef for the measurement
 
 		// shorthand for a smart pointer to a factor
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index 91c71b1ad..a7c39cf77 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -39,14 +39,14 @@ namespace gtsam {
 	 * construct the mask.
 	 */
 	template<class VALUE>
-	class PartialPriorFactor: public NonlinearFactor1<VALUE> {
+	class PartialPriorFactor: public NoiseModelFactor1<VALUE> {
 
 	public:
 		typedef VALUE T;
 
 	protected:
 
-		typedef NonlinearFactor1<VALUE> Base;
+		typedef NoiseModelFactor1<VALUE> Base;
 		typedef PartialPriorFactor<VALUE> This;
 
 		Vector prior_;             ///< measurement on logmap parameters, in compressed form
@@ -133,7 +133,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor1",
+			ar & boost::serialization::make_nvp("NoiseModelFactor1",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(prior_);
 			ar & BOOST_SERIALIZATION_NVP(mask_);
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 1f8436b02..fd0cc590e 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -24,14 +24,14 @@ namespace gtsam {
 	 * A class for a soft prior on any Value type
 	 */
 	template<class VALUE>
-	class PriorFactor: public NonlinearFactor1<VALUE> {
+	class PriorFactor: public NoiseModelFactor1<VALUE> {
 
 	public:
 		typedef VALUE T;
 
 	private:
 
-		typedef NonlinearFactor1<VALUE> Base;
+		typedef NoiseModelFactor1<VALUE> Base;
 
 		VALUE prior_; /** The measurement */
 
@@ -86,7 +86,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor1",
+			ar & boost::serialization::make_nvp("NoiseModelFactor1",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(prior_);
 		}
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 805a991cd..1eb298134 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -30,7 +30,7 @@ namespace gtsam {
 	 * i.e. the main building block for visual SLAM.
 	 */
 	template<class POSE, class LANDMARK>
-	class GenericProjectionFactor: public NonlinearFactor2<POSE, LANDMARK> {
+	class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
 	protected:
 
 		// Keep a copy of measurement and calibration for I/O
@@ -40,7 +40,7 @@ namespace gtsam {
 	public:
 
 		/// shorthand for base class type
-		typedef NonlinearFactor2<POSE, LANDMARK> Base;
+		typedef NoiseModelFactor2<POSE, LANDMARK> Base;
 
 		/// shorthand for this class
 		typedef GenericProjectionFactor<POSE, LANDMARK> This;
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index 9edc9693f..29fdb9160 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -26,13 +26,13 @@ namespace gtsam {
 	 * Binary factor for a range measurement
 	 */
 	template<class POSE, class POINT>
-	class RangeFactor: public NonlinearFactor2<POSE, POINT> {
+	class RangeFactor: public NoiseModelFactor2<POSE, POINT> {
 	private:
 
 		double measured_; /** measurement */
 
 		typedef RangeFactor<POSE, POINT> This;
-		typedef NonlinearFactor2<POSE, POINT> Base;
+		typedef NoiseModelFactor2<POSE, POINT> Base;
 
 		typedef POSE Pose;
 		typedef POINT Point;
@@ -79,7 +79,7 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			ar & boost::serialization::make_nvp("NonlinearFactor2",
+			ar & boost::serialization::make_nvp("NoiseModelFactor2",
 					boost::serialization::base_object<Base>(*this));
 			ar & BOOST_SERIALIZATION_NVP(measured_);
 		}
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index a69f4189b..d18f2ed22 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -23,7 +23,7 @@
 namespace gtsam {
 
 template<class POSE, class LANDMARK>
-class GenericStereoFactor: public NonlinearFactor2<POSE, LANDMARK> {
+class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {
 private:
 
 	// Keep a copy of measurement and calibration for I/O
@@ -33,7 +33,7 @@ private:
 public:
 
 	// shorthand for base class type
-	typedef NonlinearFactor2<POSE, LANDMARK> Base;		     			///< typedef for base class
+	typedef NoiseModelFactor2<POSE, LANDMARK> Base;		     			///< typedef for base class
 	typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
 	typedef POSE CamPose;												///< typedef for Pose Lie Value type
 
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index cdcc2663a..ca9c0cf15 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -120,9 +120,9 @@ namespace simulated2D {
    *  Unary factor encoding a soft prior on a vector
    */
   template<class VALUE = Point2>
-  class GenericPrior: public NonlinearFactor1<VALUE> {
+  class GenericPrior: public NoiseModelFactor1<VALUE> {
   public:
-    typedef NonlinearFactor1<VALUE> Base;  ///< base class
+    typedef NoiseModelFactor1<VALUE> Base;  ///< base class
     typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
     typedef VALUE Pose; ///< shortcut to Pose type
 
@@ -157,9 +157,9 @@ namespace simulated2D {
    * Binary factor simulating "odometry" between two Vectors
    */
   template<class VALUE = Point2>
-  class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
+  class GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
   public:
-    typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class
+    typedef NoiseModelFactor2<VALUE, VALUE> Base; ///< base class
     typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
     typedef VALUE Pose; ///< shortcut to Pose type
 
@@ -196,9 +196,9 @@ namespace simulated2D {
    * Binary factor simulating "measurement" between two Vectors
    */
   template<class POSE, class LANDMARK>
-  class GenericMeasurement: public NonlinearFactor2<POSE, LANDMARK> {
+  class GenericMeasurement: public NoiseModelFactor2<POSE, LANDMARK> {
   public:
-    typedef NonlinearFactor2<POSE, LANDMARK> Base;  ///< base class
+    typedef NoiseModelFactor2<POSE, LANDMARK> Base;  ///< base class
     typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
     typedef POSE Pose; ///< shortcut to Pose type
     typedef LANDMARK Landmark; ///< shortcut to Landmark type
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index 47314f8cf..242aab94b 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -78,13 +78,13 @@ namespace simulated2DOriented {
 
   /// Unary factor encoding a soft prior on a vector
   template<class VALUE = Pose2>
-  struct GenericPosePrior: public NonlinearFactor1<VALUE> {
+  struct GenericPosePrior: public NoiseModelFactor1<VALUE> {
 
     Pose2 measured_; ///< measurement
 
     /// Create generic pose prior
     GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) :
-      NonlinearFactor1<VALUE>(model, key), measured_(measured) {
+      NoiseModelFactor1<VALUE>(model, key), measured_(measured) {
     }
 
     /// Evaluate error and optionally derivative
@@ -99,7 +99,7 @@ namespace simulated2DOriented {
    * Binary factor simulating "odometry" between two Vectors
    */
   template<class VALUE = Pose2>
-  struct GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
+  struct GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
     Pose2 measured_;   ///< Between measurement for odometry factor
 
     /**
@@ -107,7 +107,7 @@ namespace simulated2DOriented {
      */
     GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
         Key i1, Key i2) :
-          NonlinearFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
+          NoiseModelFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
     }
 
     /// Evaluate error and optionally derivative
diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index 6a2ce8d26..a386736a1 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -64,7 +64,7 @@ Point3 mea(const Point3& x, const Point3& l,
 /**
  * A prior factor on a single linear robot pose
  */
-struct PointPrior3D: public NonlinearFactor1<Point3> {
+struct PointPrior3D: public NoiseModelFactor1<Point3> {
 
 	Point3 measured_; ///< The prior pose value for the variable attached to this factor
 
@@ -75,7 +75,7 @@ struct PointPrior3D: public NonlinearFactor1<Point3> {
 	 * @param key is the key for the pose
 	 */
 	PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
-	  NonlinearFactor1<Point3> (model, key), measured_(measured) {
+	  NoiseModelFactor1<Point3> (model, key), measured_(measured) {
 	}
 
 	/**
@@ -94,7 +94,7 @@ struct PointPrior3D: public NonlinearFactor1<Point3> {
 /**
  * Models a linear 3D measurement between 3D points
  */
-struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
+struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
 
 	Point3 measured_; ///< Linear displacement between a pose and landmark
 
@@ -107,7 +107,7 @@ struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
 	 */
 	Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
 	    Key poseKey, Key pointKey) :
-	      NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
+	      NoiseModelFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
 
 	/**
 	 * Error function with optional derivatives
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 7c5b333ad..722e5ff2a 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -198,12 +198,12 @@ namespace example {
 					 0.0, cos(v.y()));
 		}
 
-		struct UnaryFactor: public gtsam::NonlinearFactor1<Point2> {
+		struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
 
 			Point2 z_;
 
 			UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
-				gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
+				gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
 			}
 
 			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 0b4b222be..4d3279250 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -88,12 +88,12 @@ TEST( ExtendedKalmanFilter, linear ) {
 
 
 // Create Motion Model Factor
-class NonlinearMotionModel : public NonlinearFactor2<Point2,Point2> {
+class NonlinearMotionModel : public NoiseModelFactor2<Point2,Point2> {
 public:
   typedef Point2 T;
 
 private:
-  typedef NonlinearFactor2<Point2, Point2> Base;
+  typedef NoiseModelFactor2<Point2, Point2> Base;
   typedef NonlinearMotionModel This;
 
 protected:
@@ -235,13 +235,13 @@ public:
 };
 
 // Create Measurement Model Factor
-class NonlinearMeasurementModel : public NonlinearFactor1<Point2> {
+class NonlinearMeasurementModel : public NoiseModelFactor1<Point2> {
 public:
   typedef Point2 T;
 
 private:
 
-  typedef NonlinearFactor1<Point2> Base;
+  typedef NoiseModelFactor1<Point2> Base;
   typedef NonlinearMeasurementModel This;
 
 protected:
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 2b9037ac2..5b4050997 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -238,9 +238,9 @@ TEST( NonlinearFactor, linearize_constraint2 )
 }
 
 /* ************************************************************************* */
-class TestFactor4 : public NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> {
+class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> Base;
+  typedef NoiseModeFactor4<LieVector, LieVector, LieVector, LieVector> Base;
   TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {}
 
   virtual Vector
@@ -260,7 +260,7 @@ public:
 };
 
 /* ************************************ */
-TEST(NonlinearFactor, NonlinearFactor4) {
+TEST(NonlinearFactor, NoiseModelFactor4) {
   TestFactor4 tf;
   Values tv;
   tv.insert("x1", LieVector(1, 1.0));
@@ -283,9 +283,9 @@ TEST(NonlinearFactor, NonlinearFactor4) {
 }
 
 /* ************************************************************************* */
-class TestFactor5 : public NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
+class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
+  typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
   TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5") {}
 
   virtual Vector
@@ -307,7 +307,7 @@ public:
 };
 
 /* ************************************ */
-TEST(NonlinearFactor, NonlinearFactor5) {
+TEST(NonlinearFactor, NoiseModelFactor5) {
   TestFactor5 tf;
   Values tv;
   tv.insert("x1", LieVector(1, 1.0));
@@ -333,9 +333,9 @@ TEST(NonlinearFactor, NonlinearFactor5) {
 }
 
 /* ************************************************************************* */
-class TestFactor6 : public NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
+class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
+  typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
   TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5", "x6") {}
 
   virtual Vector
@@ -359,7 +359,7 @@ public:
 };
 
 /* ************************************ */
-TEST(NonlinearFactor, NonlinearFactor6) {
+TEST(NonlinearFactor, NoiseModelFactor6) {
   TestFactor6 tf;
   Values tv;
   tv.insert("x1", LieVector(1, 1.0));

From a3797e3cdbf0e9168fa0874fcbef57c576ddeda4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 21 Feb 2012 00:53:35 +0000
Subject: [PATCH 77/88] Int keys with formatter objects, all unit tests pass

---
 gtsam/base/StringFormatter.cpp                |  23 --
 gtsam/base/StringFormatter.h                  |  36 ---
 gtsam/base/Testable.h                         |   2 +-
 gtsam/base/types.h                            |   3 -
 gtsam/inference/tests/testJunctionTree.cpp    |   4 -
 .../tests/testSerializationInference.cpp      |  20 +-
 gtsam/nonlinear/ISAM2-impl-inl.h              |  29 ++-
 gtsam/nonlinear/ISAM2.h                       |  15 +-
 gtsam/nonlinear/Key.cpp                       |  34 +++
 gtsam/nonlinear/Key.h                         |  40 +++
 gtsam/nonlinear/NonlinearEquality.h           |  10 +-
 gtsam/nonlinear/NonlinearFactor.h             |  71 +-----
 gtsam/nonlinear/NonlinearFactorGraph.cpp      |   9 +-
 gtsam/nonlinear/NonlinearFactorGraph.h        |   2 +-
 gtsam/nonlinear/Ordering.cpp                  |   4 +-
 gtsam/nonlinear/Ordering.h                    |   3 +-
 gtsam/nonlinear/Symbol.h                      |   9 +-
 gtsam/nonlinear/Values.cpp                    |  10 +-
 gtsam/nonlinear/Values.h                      |   2 +-
 gtsam/nonlinear/tests/testKey.cpp             |   9 +
 gtsam/nonlinear/tests/testValues.cpp          |  19 +-
 gtsam/slam/AntiFactor.h                       |   4 +-
 gtsam/slam/BearingRangeFactor.h               |   6 +-
 gtsam/slam/BetweenFactor.h                    |   6 +-
 gtsam/slam/GeneralSFMFactor.h                 |   4 +-
 gtsam/slam/PartialPriorFactor.h               |   4 +-
 gtsam/slam/PriorFactor.h                      |   4 +-
 gtsam/slam/ProjectionFactor.h                 |   8 +-
 gtsam/slam/RangeFactor.h                      |   4 +-
 gtsam/slam/StereoFactor.h                     |   4 +-
 gtsam/slam/dataset.cpp                        |   4 +-
 gtsam/slam/smallExample.cpp                   |  22 +-
 gtsam/slam/tests/testGeneralSFMFactor.cpp     |  13 +-
 .../testGeneralSFMFactor_Cal3Bundler.cpp      |  13 +-
 gtsam/slam/tests/testPose2SLAM.cpp            |  24 +-
 gtsam/slam/tests/testPose3SLAM.cpp            |   4 +-
 gtsam/slam/tests/testProjectionFactor.cpp     |  10 +-
 gtsam/slam/tests/testSerializationSLAM.cpp    |   2 +-
 gtsam/slam/tests/testVSLAM.cpp                |  12 +-
 gtsam/slam/visualSLAM.h                       |   4 +-
 tests/testExtendedKalmanFilter.cpp            |   6 +-
 tests/testGaussianFactor.cpp                  |  78 +++---
 tests/testGaussianFactorGraph.cpp             | 235 +++++++++---------
 tests/testGaussianISAM.cpp                    |  95 +++----
 tests/testGaussianISAM2.cpp                   |   2 +-
 tests/testGaussianJunctionTree.cpp            |  25 +-
 tests/testGraph.cpp                           |  44 ++--
 tests/testNonlinearFactor.cpp                 |  50 ++--
 tests/testNonlinearFactorGraph.cpp            |  11 +-
 tests/testNonlinearOptimizer.cpp              |  27 +-
 tests/testSymbolicBayesNet.cpp                |  13 +-
 tests/testSymbolicFactorGraph.cpp             |  39 +--
 52 files changed, 563 insertions(+), 568 deletions(-)
 delete mode 100644 gtsam/base/StringFormatter.cpp
 delete mode 100644 gtsam/base/StringFormatter.h
 create mode 100644 gtsam/nonlinear/Key.cpp
 create mode 100644 gtsam/nonlinear/Key.h

diff --git a/gtsam/base/StringFormatter.cpp b/gtsam/base/StringFormatter.cpp
deleted file mode 100644
index 6674d3f5a..000000000
--- a/gtsam/base/StringFormatter.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file StringFormatter.cpp
- * @brief 
- * @author Richard Roberts
- * @date Feb 19, 2012
- */
-
-#include "StringFormatter.h"
-
-namespace gtsam {
-
-} /* namespace gtsam */
diff --git a/gtsam/base/StringFormatter.h b/gtsam/base/StringFormatter.h
deleted file mode 100644
index 26cea8881..000000000
--- a/gtsam/base/StringFormatter.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file StringFormatter.h
- * @brief 
- * @author Richard Roberts
- * @date Feb 19, 2012
- */
-
-#pragma once
-
-namespace gtsam {
-
-  /**
-   *
-   */
-  class StringFormatter {
-
-  public:
-
-    virtual ~StringFormatter() {}
-
-    virtual std::string keyToString(Key key) = 0;
-
-  };
-
-} /* namespace gtsam */
diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index a1130dfd9..bcc6c8957 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -23,7 +23,7 @@
  * 		void print(const std::string& name) const = 0;
  * 
  * equality up to tolerance
- * tricky to implement, see NonlinearFactor1 for an example
+ * tricky to implement, see NoiseModelFactor1 for an example
  * equals is not supposed to print out *anything*, just return true|false
  * 		bool equals(const Derived& expected, double tol) const = 0;
  * 
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index f3f1ae465..b6c5617a5 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -26,9 +26,6 @@ namespace gtsam {
 	/// Integer variable index type
 	typedef size_t Index;
 
-	/// Integer nonlinear key type
-	typedef size_t Key;
-
 	/**
 	 * Helper class that uses templates to select between two types based on
 	 * whether TEST_TYPE is const or not.
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 7374bbad8..16309abda 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -24,10 +24,6 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
-#include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/inference/ClusterTree.h>
diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp
index 2db7014f1..d4c8ea62a 100644
--- a/gtsam/inference/tests/testSerializationInference.cpp
+++ b/gtsam/inference/tests/testSerializationInference.cpp
@@ -16,12 +16,9 @@
  * @date Feb 7, 2012
  */
 
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/inference/IndexConditional.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/inference/BayesTree.h>
-#include <gtsam/nonlinear/Ordering.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
@@ -32,13 +29,12 @@ using namespace gtsam::serializationTestHelpers;
 
 /* ************************************************************************* */
 TEST (Serialization, symbolic_graph) {
-  Ordering o; o += "x1","l1","x2";
   // construct expected symbolic graph
   SymbolicFactorGraph sfg;
-  sfg.push_factor(o["x1"]);
-  sfg.push_factor(o["x1"],o["x2"]);
-  sfg.push_factor(o["x1"],o["l1"]);
-  sfg.push_factor(o["l1"],o["x2"]);
+  sfg.push_factor(0);
+  sfg.push_factor(0,1);
+  sfg.push_factor(0,2);
+  sfg.push_factor(2,1);
 
   EXPECT(equalsObj(sfg));
   EXPECT(equalsXML(sfg));
@@ -46,11 +42,9 @@ TEST (Serialization, symbolic_graph) {
 
 /* ************************************************************************* */
 TEST (Serialization, symbolic_bn) {
-  Ordering o; o += "x2","l1","x1";
-
-  IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
-  IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
-  IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
+  IndexConditional::shared_ptr x2(new IndexConditional(1, 2, 0));
+  IndexConditional::shared_ptr l1(new IndexConditional(2, 0));
+  IndexConditional::shared_ptr x1(new IndexConditional(0));
 
   SymbolicBayesNet sbn;
   sbn.push_back(x2);
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index 0abea8e7f..c2483488c 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -44,8 +44,10 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
    * @param delta Current linear delta to be augmented with zeros
    * @param ordering Current ordering to be augmented with new variables
    * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
+   * @param keyFormatter Formatter for printing nonlinear keys during debugging
    */
-  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
+  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering,
+      typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
@@ -61,10 +63,12 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
    * Any variables in the VectorValues delta whose vector magnitude is greater than
    * or equal to relinearizeThreshold are returned.
    * @param delta The linear delta to check against the threshold
+   * @param keyFormatter Formatter for printing nonlinear keys during debugging
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
+  static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
+      const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
    * Recursively search this clique and its children for marked keys appearing
@@ -94,10 +98,12 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
    * expmapped deltas will be set to an invalid value (infinity) to catch bugs
    * where we might expmap something twice, or expmap it but then not
    * recalculate its delta.
+   * @param keyFormatter Formatter for printing nonlinear keys during debugging
    */
   static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
       const Ordering& ordering, const std::vector<bool>& mask,
-      boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
+      boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
    * Reorder and eliminate factors.  These factors form a subset of the full
@@ -120,7 +126,7 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
-    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
+    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
   theta.insert(newTheta);
@@ -139,7 +145,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
       delta.permutation()[nextVar] = nextVar;
       ordering.insert(key_value.first, nextVar);
-      if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl;
+      if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl;
       ++ nextVar;
     }
     assert(delta.permutation().size() == delta.container().size());
@@ -164,7 +170,8 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
+FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
+    const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
   FastSet<Index> relinKeys;
 
   if(relinearizeThreshold.type() == typeid(double)) {
@@ -178,7 +185,7 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permut
   } else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
     const FastMap<char,Vector>& thresholds = boost::get<FastMap<char,Vector> >(relinearizeThreshold);
     BOOST_FOREACH(const Ordering::value_type& key_index, ordering) {
-      const Vector& threshold = thresholds.find(key_index.first.chr())->second;
+      const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second;
       Index j = key_index.second;
       if(threshold.rows() != delta[j].rows())
         throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality");
@@ -213,8 +220,8 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
-void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
-    const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
+void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
+    const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
   // If debugging, invalidate if requested, otherwise do not invalidate.
   // Invalidating means setting expmapped entries to Inf, to trigger assertions
   // if we try to re-use them.
@@ -231,9 +238,9 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
     const Index var = key_index->second;
     if(ISDEBUG("ISAM2 update verbose")) {
       if(mask[var])
-        cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+        cout << "expmap " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
       else
-        cout << "       " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+        cout << "       " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
     }
     assert(delta[var].size() == (int)key_value->second.dim());
     assert(delta[var].unaryExpr(&isfinite<double>).all());
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 505aa2de5..4c8df2997 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -106,16 +106,19 @@ struct ISAM2Params {
 
   bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
 
+  KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
+
   /** Specify parameters as constructor arguments */
   ISAM2Params(
-      OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params public variables, ISAM2Params::optimizationParams
-      RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold
-      int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip
-      bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization
-      bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError
+      OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
+      RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold
+      int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
+      bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
+      bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
+      const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
   ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
       relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
-      evaluateNonlinearError(_evaluateNonlinearError) {}
+      evaluateNonlinearError(_evaluateNonlinearError), keyFormatter(_keyFormatter) {}
 };
 
 /**
diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/nonlinear/Key.cpp
new file mode 100644
index 000000000..821285558
--- /dev/null
+++ b/gtsam/nonlinear/Key.cpp
@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Key.h
+ * @brief
+ * @author Richard Roberts
+ * @date Feb 20, 2012
+ */
+
+#include <gtsam/nonlinear/Key.h>
+
+#include <boost/lexical_cast.hpp>
+#include <gtsam/nonlinear/Symbol.h>
+
+namespace gtsam {
+
+std::string _defaultKeyFormatter(Key key) {
+    const Symbol asSymbol(key);
+    if(asSymbol.chr() > 0)
+      return (std::string)asSymbol;
+    else
+      return boost::lexical_cast<std::string>(key);
+  }
+
+}
diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h
new file mode 100644
index 000000000..65a65c6d4
--- /dev/null
+++ b/gtsam/nonlinear/Key.h
@@ -0,0 +1,40 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Key.h
+ * @brief 
+ * @author Richard Roberts
+ * @date Feb 20, 2012
+ */
+#pragma once
+
+#include <boost/function.hpp>
+#include <string>
+
+namespace gtsam {
+
+  /// Integer nonlinear key type
+  typedef size_t Key;
+
+  /// Typedef for a function to format a key, i.e. to convert it to a string
+  typedef boost::function<std::string(Key)> KeyFormatter;
+
+  // Helper function for DefaultKeyFormatter
+  std::string _defaultKeyFormatter(Key key);
+
+  /// The default KeyFormatter, which is used if no KeyFormatter is passed to
+  /// a nonlinear 'print' function.  Automatically detects plain integer keys
+  /// and Symbol keys.
+  static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
+
+}
+
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 3a37fa53d..a1c685668 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -108,8 +108,8 @@ namespace gtsam {
 		/// @name Testable
 		/// @{
 
-		virtual void print(const std::string& s = "") const {
-			std::cout << "Constraint: " << s << " on [" << (std::string)this->key() << "]\n";
+		virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+			std::cout << "Constraint: " << s << " on [" << keyFormatter(this->key()) << "]\n";
 			gtsam::print(feasible_,"Feasible Point");
 			std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
 		}
@@ -147,7 +147,7 @@ namespace gtsam {
 				return zero(nj); // set error to zero if equal
 			} else {
 				if (H) throw std::invalid_argument(
-						"Linearization point not feasible for " + (std::string)(this->key()) + "!");
+						"Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!");
 				return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
 			}
 		}
@@ -217,9 +217,9 @@ namespace gtsam {
 		}
 
 		/** Print */
-	  virtual void print(const std::string& s = "") const {
+	  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
 	    std::cout << s << ": NonlinearEquality1("
-	    		<< (std::string) this->key() << "),"<< "\n";
+	    		<< keyFormatter(this->key()) << "),"<< "\n";
 	    this->noiseModel_->print();
 	    value_.print("Value");
 	  }
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 60f29cbf6..e260f24db 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -79,9 +79,10 @@ public:
 	/// @{
 
   /** print */
-  virtual void print(const std::string& s = "",
-      const boost::function<std::string(Key)>& keyFormatter = &Symbol::format) const {
-    std::cout << s << ": NonlinearFactor\n";
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "keys = { ";
+    BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
+    std::cout << "}" << endl;
   }
 
   /** Check if two factors are equal */
@@ -187,11 +188,8 @@ protected:
 public:
 
   /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NoiseModelFactor\n";
-    std::cout << "  ";
-    BOOST_FOREACH(Key key, this->keys()) { std::cout << (std::string)key << " "; }
-    std::cout << "\n";
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    Base::print(s, keyFormatter);
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -341,12 +339,6 @@ public:
     }
   }
 
-  /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor1(" << (std::string) keys_[0] << ")\n";
-    this->noiseModel_->print("  noise model: ");
-  }
-
   /**
    *  Override this method to finish implementing a unary factor.
    *  If the optional Matrix reference argument is specified, it should compute
@@ -425,14 +417,6 @@ public:
     }
   }
 
-  /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor2("
-    		<< (std::string) keys_[0] << ","
-    		<< (std::string) keys_[1] << ")\n";
-    this->noiseModel_->print("  noise model: ");
-  }
-
   /**
    *  Override this method to finish implementing a binary factor.
    *  If any of the optional Matrix reference arguments are specified, it should compute
@@ -512,16 +496,6 @@ public:
     }
   }
 
-  /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor3("
-    		<< (std::string) this->keys_[0] << ","
-    		<< (std::string) this->keys_[1] << ","
-    		<< (std::string) this->keys_[2] << ")\n";
-    this->noiseModel_->print("  noise model: ");
-  }
-
-
   /**
    *  Override this method to finish implementing a trinary factor.
    *  If any of the optional Matrix reference arguments are specified, it should compute
@@ -607,16 +581,6 @@ public:
   	}
   }
 
-  /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor4("
-    		<< (std::string) this->keys_[0] << ","
-    		<< (std::string) this->keys_[1] << ","
-     		<< (std::string) this->keys_[2] << ","
-    		<< (std::string) this->keys_[3] << ")\n";
-    this->noiseModel_->print("  noise model: ");
-  }
-
   /**
    *  Override this method to finish implementing a 4-way factor.
    *  If any of the optional Matrix reference arguments are specified, it should compute
@@ -707,17 +671,6 @@ public:
   	}
   }
 
-  /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor5("
-    		<< (std::string) this->keys_[0] << ","
-    		<< (std::string) this->keys_[1] << ","
-     		<< (std::string) this->keys_[2] << ","
-    		<< (std::string) this->keys_[3] << ","
-    		<< (std::string) this->keys_[4] << ")\n";
-    this->noiseModel_->print("  noise model: ");
-  }
-
   /**
    *  Override this method to finish implementing a 5-way factor.
    *  If any of the optional Matrix reference arguments are specified, it should compute
@@ -813,18 +766,6 @@ public:
   	}
   }
 
-  /** Print */
-  virtual void print(const std::string& s = "") const {
-    std::cout << s << ": NonlinearFactor6("
-    		<< (std::string) this->keys_[0] << ","
-    		<< (std::string) this->keys_[1] << ","
-     		<< (std::string) this->keys_[2] << ","
-    		<< (std::string) this->keys_[3] << ","
-    		<< (std::string) this->keys_[4] << ","
-    		<< (std::string) this->keys_[5] << ")\n";
-    this->noiseModel_->print("  noise model: ");
-  }
-
   /**
    *  Override this method to finish implementing a 6-way factor.
    *  If any of the optional Matrix reference arguments are specified, it should compute
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 0c005ce3e..647afa2d5 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -33,8 +33,13 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	void NonlinearFactorGraph::print(const std::string& str) const {
-		Base::print(str);
+	void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
+    cout << str << "size: " << size() << endl;
+    for (size_t i = 0; i < factors_.size(); i++) {
+      stringstream ss;
+      ss << "factor " << i << ": ";
+      if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
+    }
 	}
 
 	/* ************************************************************************* */
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index c2cd844be..3caeec680 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -44,7 +44,7 @@ namespace gtsam {
 		typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
 
     /** print just calls base class */
-    void print(const std::string& str = "NonlinearFactorGraph: ") const;
+    void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /** return keys in some random order */
     std::set<Key> keys() const;
diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp
index c35b07f43..10c3370cf 100644
--- a/gtsam/nonlinear/Ordering.cpp
+++ b/gtsam/nonlinear/Ordering.cpp
@@ -40,12 +40,12 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
 }
 
 /* ************************************************************************* */
-void Ordering::print(const string& str) const {
+void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
   cout << str << " ";
   BOOST_FOREACH(const Ordering::value_type& key_order, *this) {
     if(key_order != *begin())
       cout << ", ";
-    cout << (string)key_order.first << ":" << key_order.second;
+    cout << keyFormatter(key_order.first) << ":" << key_order.second;
   }
   cout << endl;
 }
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 084d69ebd..fcb3d5bfb 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -20,6 +20,7 @@
 #include <set>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/inference.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 #include <boost/foreach.hpp>
 #include <boost/assign/list_inserter.hpp>
@@ -207,7 +208,7 @@ public:
 	/// @{
 
   /** print (from Testable) for testing and debugging */
-  void print(const std::string& str = "Ordering:") const;
+  void print(const std::string& str = "Ordering:", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
   /** equals (from Testable) for testing and debugging */
   bool equals(const Ordering& rhs, double tol = 0.0) const;
diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h
index 6a9d612f7..854e18203 100644
--- a/gtsam/nonlinear/Symbol.h
+++ b/gtsam/nonlinear/Symbol.h
@@ -27,6 +27,8 @@
 #include <boost/lexical_cast.hpp>
 #endif
 
+#include <gtsam/nonlinear/Key.h>
+
 #define ALPHA '\224'
 
 namespace gtsam {
@@ -94,7 +96,7 @@ public:
     const size_t indexBytes = keyBytes - chrBytes;
     const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
     const Key indexMask = ~chrMask;
-    c_ = key & chrMask;
+    c_ = (key & chrMask) >> indexBytes;
     j_ = key & indexMask;
   }
 
@@ -119,11 +121,6 @@ public:
     return (*this) == expected;
   }
 
-  /** Format function that can be passed to print functions in nonlinear */
-  static std::string format(Key key) {
-    return (std::string)Symbol(key);
-  }
-
   /** Retrieve key character */
   unsigned char chr() const {
     return c_;
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 4c8d03d9d..00b42730b 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -40,10 +40,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void Values::print(const string& str) const {
+  void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
     cout << str << "Values with " << size() << " values:\n" << endl;
     for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      cout << "  " << (string)key_value->first << ": ";
+      cout << "  " << keyFormatter(key_value->first) << ": ";
       key_value->second.print("");
     }
   }
@@ -197,7 +197,7 @@ namespace gtsam {
   const char* ValuesKeyAlreadyExists::what() const throw() {
     if(message_.empty())
       message_ =
-          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
+          "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
     return message_.c_str();
   }
 
@@ -206,7 +206,7 @@ namespace gtsam {
     if(message_.empty())
       message_ =
           "Attempting to " + std::string(operation_) + " the key \"" +
-          (std::string)key_ + "\", which does not exist in the Values.";
+          DefaultKeyFormatter(key_) + "\", which does not exist in the Values.";
     return message_.c_str();
   }
 
@@ -214,7 +214,7 @@ namespace gtsam {
   const char* ValuesIncorrectType::what() const throw() {
     if(message_.empty())
       message_ =
-          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in Values is " +
+          "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " +
           std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
     return message_.c_str();
   }
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 5074c2360..659779858 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -121,7 +121,7 @@ namespace gtsam {
     /// @{
 
     /** print method for testing and debugging */
-    void print(const std::string& str = "") const;
+    void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /** Test whether the sets of keys and values are identical */
     bool equals(const Values& other, double tol=1e-9) const;
diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp
index 19c3b6325..78688192f 100644
--- a/gtsam/nonlinear/tests/testKey.cpp
+++ b/gtsam/nonlinear/tests/testKey.cpp
@@ -24,6 +24,15 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
+/* ************************************************************************* */
+TEST(Key, KeySymbolConversion) {
+  Symbol expected('j', 4);
+  Key key(expected);
+  Symbol actual(key);
+
+  EXPECT(assert_equal(expected, actual))
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 77e1155a7..1ea70e213 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -23,6 +23,7 @@ using namespace boost::assign;
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/LieVector.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>
@@ -31,7 +32,7 @@ using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
-Key key1("v1"), key2("v2"), key3("v3"), key4("v4");
+Key key1(Symbol("v1")), key2(Symbol("v2")), key3(Symbol("v3")), key4(Symbol("v4"));
 
 /* ************************************************************************* */
 TEST( Values, equals1 )
@@ -201,8 +202,8 @@ TEST(Values, expmap_d)
   CHECK(config0.equals(config0));
 
   Values poseconfig;
-  poseconfig.insert("p1", Pose2(1,2,3));
-  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
+  poseconfig.insert(key1, Pose2(1,2,3));
+  poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5));
 
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
@@ -213,19 +214,19 @@ TEST(Values, extract_keys)
 {
 	Values config;
 
-	config.insert("x1", Pose2());
-	config.insert("x2", Pose2());
-	config.insert("x4", Pose2());
-	config.insert("x5", Pose2());
+	config.insert(key1, Pose2());
+	config.insert(key2, Pose2());
+	config.insert(key3, Pose2());
+	config.insert(key4, Pose2());
 
 	FastList<Key> expected, actual;
-	expected += "x1", "x2", "x4", "x5";
+	expected += key1, key2, key3, key4;
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
 	FastList<Key>::const_iterator itAct = actual.begin(), itExp = expected.begin();
 	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
-		CHECK(assert_equal(*itExp, *itAct));
+		LONGS_EQUAL(*itExp, *itAct);
 	}
 }
 
diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h
index 8ed7f284f..633360fed 100644
--- a/gtsam/slam/AntiFactor.h
+++ b/gtsam/slam/AntiFactor.h
@@ -53,9 +53,9 @@ namespace gtsam {
 		/** implement functions needed for Testable */
 
 		/** print */
-		virtual void print(const std::string& s) const {
+		virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
 	    std::cout << s << "AntiFactor version of:" << std::endl;
-	    factor_->print(s);
+	    factor_->print(s, keyFormatter);
 		}
 
 		/** equals */
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 4ff792e23..4c170c811 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -58,10 +58,10 @@ namespace gtsam {
 		virtual ~BearingRangeFactor() {}
 
 	  /** Print */
-	  virtual void print(const std::string& s = "") const {
+	  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
 	    std::cout << s << ": BearingRangeFactor("
-	    		<< (std::string) this->key1() << ","
-	    		<< (std::string) this->key2() << ")\n";
+	    		<< keyFormatter(this->key1()) << ","
+	    		<< keyFormatter(this->key2()) << ")\n";
 	    measuredBearing_.print("  measured bearing");
 	    std::cout << "  measured range: " << measuredRange_ << std::endl;
 	    this->noiseModel_->print("  noise model");
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index c88af0d42..2df0b98de 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -65,10 +65,10 @@ namespace gtsam {
 		/** implement functions needed for Testable */
 
 		/** print */
-		virtual void print(const std::string& s) const {
+		virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
 	    std::cout << s << "BetweenFactor("
-	    		<< (std::string) this->key1() << ","
-	    		<< (std::string) this->key2() << ")\n";
+	    		<< keyFormatter(this->key1()) << ","
+	    		<< keyFormatter(this->key2()) << ")\n";
 			measured_.print("  measured");
 	    this->noiseModel_->print("  noise model");
 		}
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 4a8b76739..333465b71 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -65,8 +65,8 @@ namespace gtsam {
 		 * print
 		 * @param s optional string naming the factor
 		 */
-		void print(const std::string& s = "SFMFactor") const {
-			Base::print(s);
+		void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+			Base::print(s, keyFormatter);
 			measured_.print(s + ".z");
 		}
 
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index a7c39cf77..4c7069174 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -89,8 +89,8 @@ namespace gtsam {
 		/** implement functions needed for Testable */
 
 		/** print */
-		virtual void print(const std::string& s) const {
-			Base::print(s);
+		virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+			Base::print(s, keyFormatter);
 			gtsam::print(prior_, "prior");
 		}
 
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index fd0cc590e..2ab011bf5 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -59,8 +59,8 @@ namespace gtsam {
 		/** implement functions needed for Testable */
 
 		/** print */
-		virtual void print(const std::string& s) const {
-			std::cout << s << "PriorFactor(" << (std::string) this->key() << ")\n";
+		virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+			std::cout << s << "PriorFactor(" << keyFormatter(this->key()) << ")\n";
 			prior_.print("  prior");
 			this->noiseModel_->print("  noise model");
 		}
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 1eb298134..fcde33629 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -71,8 +71,8 @@ namespace gtsam {
 		 * print
 		 * @param s optional string naming the factor
 		 */
-		void print(const std::string& s = "ProjectionFactor") const {
-			Base::print(s);
+		void print(const std::string& s = "ProjectionFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+			Base::print(s, keyFormatter);
 			measured_.print(s + ".z");
 		}
 
@@ -92,8 +92,8 @@ namespace gtsam {
 			} catch( CheiralityException& e) {
 			  if (H1) *H1 = zeros(2,6);
 			  if (H2) *H2 = zeros(2,3);
-			  cout << e.what() << ": Landmark "<< this->key2().index() <<
-			      " moved behind camera " << this->key1().index() << endl;
+			  cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+			      " moved behind camera " << DefaultKeyFormatter(this->key1()) << endl;
 			  return ones(2) * 2.0 * K_->fx();
 			}
 		}
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index 29fdb9160..489f751db 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -69,8 +69,8 @@ namespace gtsam {
 		}
 
 		/** print contents */
-		void print(const std::string& s="") const {
-			Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(measured_));
+		void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+			Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(measured_), keyFormatter);
 		}
 
 	private:
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index d18f2ed22..195056615 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -60,8 +60,8 @@ public:
 	 * print
 	 * @param s optional string naming the factor
 	 */
-	void print(const std::string& s) const {
-		Base::print(s);
+	void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+		Base::print(s, keyFormatter);
 		measured_.print(s + ".z");
 	}
 
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 3f0551863..dae437b79 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -156,7 +156,7 @@ void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDia
 	// save poses
 	BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
 	  const Pose2& pose = dynamic_cast<const Pose2&>(key_value.second);
-		stream << "VERTEX2 " << key_value.first.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
+		stream << "VERTEX2 " << key_value.first << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
 	}
 
 	// save edges
@@ -167,7 +167,7 @@ void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDia
 		if (!factor) continue;
 
 		Pose2 pose = factor->measured().inverse();
-		stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
+		stream << "EDGE2 " << factor->key2() << " " << factor->key1()
 				<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
 				<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
 				<< " " << RR(0, 2) << " " << RR(1, 2) << endl;
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 722e5ff2a..f48d363b5 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -24,7 +24,7 @@
 
 using namespace std;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like Symbol("x3") to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
@@ -121,18 +121,18 @@ namespace example {
 	/* ************************************************************************* */
 	VectorValues createCorrectDelta(const Ordering& ordering) {
 		VectorValues c(vector<size_t>(3,2));
-		c[ordering["l1"]] = Vector_(2, -0.1, 0.1);
-		c[ordering["x1"]] = Vector_(2, -0.1, -0.1);
-		c[ordering["x2"]] = Vector_(2, 0.1, -0.2);
+		c[ordering[Symbol(Symbol("l1"))]] = Vector_(2, -0.1, 0.1);
+		c[ordering[Symbol("x1")]] = Vector_(2, -0.1, -0.1);
+		c[ordering[Symbol("x2")]] = Vector_(2, 0.1, -0.2);
 		return c;
 	}
 
 	/* ************************************************************************* */
 	VectorValues createZeroDelta(const Ordering& ordering) {
 		VectorValues c(vector<size_t>(3,2));
-		c[ordering["l1"]] = zero(2);
-		c[ordering["x1"]] = zero(2);
-		c[ordering["x2"]] = zero(2);
+		c[ordering[Symbol(Symbol("l1"))]] = zero(2);
+		c[ordering[Symbol("x1")]] = zero(2);
+		c[ordering[Symbol("x2")]] = zero(2);
 		return c;
 	}
 
@@ -144,16 +144,16 @@ namespace example {
 		SharedDiagonal unit2 = noiseModel::Unit::Create(2);
 
 		// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
-		fg.add(ordering["x1"], 10*eye(2), -1.0*ones(2), unit2);
+		fg.add(ordering[Symbol("x1")], 10*eye(2), -1.0*ones(2), unit2);
 
 		// odometry between x1 and x2: x2-x1=[0.2;-0.1]
-		fg.add(ordering["x1"], -10*eye(2),ordering["x2"], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
+		fg.add(ordering[Symbol("x1")], -10*eye(2),ordering[Symbol("x2")], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
 
     // measurement between x1 and l1: l1-x1=[0.0;0.2]
-		fg.add(ordering["x1"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
+		fg.add(ordering[Symbol("x1")], -5*eye(2), ordering[Symbol("l1")], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
 
 		// measurement between x2 and l1: l1-x2=[-0.2;0.3]
-		fg.add(ordering["x2"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
+		fg.add(ordering[Symbol("x2")], -5*eye(2), ordering[Symbol("l1")], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
 
 		return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
 	}
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 1f1e616f1..7833bce18 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -89,14 +89,14 @@ TEST( GeneralSFMFactor, equals )
 TEST( GeneralSFMFactor, error ) {
 	Point2 z(3.,0.);
 	const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
-	boost::shared_ptr<Projection>	factor(new Projection(z, sigma, "x1", "l1"));
+	boost::shared_ptr<Projection>	factor(new Projection(z, sigma, Symbol("x1"), Symbol("l1")));
 	// For the following configuration, the factor predicts 320,240
 	Values values;
 	Rot3 R;
 	Point3 t1(0,0,-6);
 	Pose3 x1(R,t1);
-	values.insert("x1", GeneralCamera(x1));
-	Point3 l1;  values.insert("l1", l1);
+	values.insert(Symbol("x1"), GeneralCamera(x1));
+	Point3 l1;  values.insert(Symbol("l1"), l1);
 	EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -137,10 +137,9 @@ vector<GeneralCamera> genCameraVariableCalibration() {
 }
 
 shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
-  list<Symbol> keys;
-  for ( size_t i = 0 ; i < L.size() ; ++i ) keys.push_back(Symbol('l', i)) ;
-  for ( size_t i = 0 ; i < X.size() ; ++i ) keys.push_back(Symbol('x', i)) ;
-  shared_ptr<Ordering> ordering(new Ordering(keys));
+  shared_ptr<Ordering> ordering(new Ordering);
+  for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ;
+  for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ;
   return ordering ;
 }
 
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 89dbf15ea..3f6d698d5 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -91,14 +91,14 @@ TEST( GeneralSFMFactor, error ) {
   Point2 z(3.,0.);
   const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
   boost::shared_ptr<Projection>
-  factor(new Projection(z, sigma, "x1", "l1"));
+  factor(new Projection(z, sigma, Symbol("x1"), Symbol("l1")));
   // For the following configuration, the factor predicts 320,240
   Values values;
   Rot3 R;
   Point3 t1(0,0,-6);
   Pose3 x1(R,t1);
-  values.insert("x1", GeneralCamera(x1));
-  Point3 l1;  values.insert("l1", l1);
+  values.insert(Symbol("x1"), GeneralCamera(x1));
+  Point3 l1;  values.insert(Symbol("l1"), l1);
   EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -140,10 +140,9 @@ vector<GeneralCamera> genCameraVariableCalibration() {
 }
 
 shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
-  list<Symbol> keys;
-  for ( size_t i = 0 ; i < L.size() ; ++i ) keys.push_back(Symbol('l', i)) ;
-  for ( size_t i = 0 ; i < X.size() ; ++i ) keys.push_back(Symbol('x', i)) ;
-  shared_ptr<Ordering> ordering(new Ordering(keys));
+  shared_ptr<Ordering> ordering(new Ordering);
+  for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ;
+  for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ;
   return ordering ;
 }
 
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index 4646bb5d7..3bccccca3 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -46,6 +46,8 @@ static noiseModel::Gaussian::shared_ptr covariance(
 	)));
 //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
 
+const Key kx0 = Symbol("x0"), kx1 = Symbol("x1"), kx2 = Symbol("x2"), kx3 = Symbol("x3"), kx4 = Symbol("x4"), kx5 = Symbol("x5"), kl1 = Symbol("l1");
+
 /* ************************************************************************* */
 // Test constraint, small displacement
 Vector f1(const Pose2& pose1, const Pose2& pose2) {
@@ -140,7 +142,7 @@ TEST( Pose2SLAM, linearization )
 
 	Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
 	SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
-	lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering["x1"], A1, ordering["x2"], A2, b, probModel1)));
+	lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[kx1], A1, ordering[kx2], A2, b, probModel1)));
 
 	CHECK(assert_equal(lfg_expected, *lfg_linearized));
 }
@@ -160,7 +162,7 @@ TEST(Pose2Graph, optimize) {
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "x0","x1";
+  *ordering += kx0, kx1;
   typedef NonlinearOptimizer<pose2SLAM::Graph> Optimizer;
 
   NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
@@ -198,7 +200,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "x0","x1","x2";
+  *ordering += kx0,kx1,kx2;
 
   // optimize
   NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
@@ -241,7 +243,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "x0","x1","x2","x3","x4","x5";
+  *ordering += kx0,kx1,kx2,kx3,kx4,kx5;
 
   // optimize
   NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
@@ -400,14 +402,14 @@ TEST( Pose2Prior, error )
 
 	// Check error at x0, i.e. delta = zero !
 	VectorValues delta(VectorValues::Zero(x0.dims(ordering)));
-	delta[ordering["x1"]] = zero(3);
+	delta[ordering[kx1]] = zero(3);
 	Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
 	CHECK(assert_equal(error_at_zero,factor.whitenedError(x0)));
 	CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
 
 	// Check error after increasing p2
 	VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
-	addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
+	addition[ordering[kx1]] = Vector_(3, 0.1, 0.0, 0.0);
 	VectorValues plus = delta + addition;
 	pose2SLAM::Values x1 = x0.retract(plus, ordering);
 	Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
@@ -441,7 +443,7 @@ TEST( Pose2Prior, linearize )
 
 	// Test with numerical derivative
 	Matrix numericalH = numericalDerivative11(hprior, priorVal);
-	CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering["x1"]))));
+	CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[kx1]))));
 }
 
 /* ************************************************************************* */
@@ -466,15 +468,15 @@ TEST( Pose2Factor, error )
 
 	// Check error at x0, i.e. delta = zero !
 	VectorValues delta(x0.dims(ordering));
-	delta[ordering["x1"]] = zero(3);
-	delta[ordering["x2"]] = zero(3);
+	delta[ordering[kx1]] = zero(3);
+	delta[ordering[kx2]] = zero(3);
 	Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
 	CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0)));
 	CHECK(assert_equal(-error_at_zero, linear->error_vector(delta)));
 
 	// Check error after increasing p2
 	VectorValues plus = delta;
-	plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
+	plus[ordering[kx2]] = Vector_(3, 0.1, 0.0, 0.0);
 	pose2SLAM::Values x1 = x0.retract(plus, ordering);
 	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.whitenedError(x1)));
@@ -542,7 +544,7 @@ TEST( Pose2Factor, linearize )
 	// expected linear factor
 	Ordering ordering(*x0.orderingArbitrary());
 	SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
-	JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1);
+	JacobianFactor expected(ordering[kx1], expectedH1, ordering[kx2], expectedH2, expected_b, probModel1);
 
 	// Actual linearization
 	boost::shared_ptr<JacobianFactor> actual =
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index 9eb6b3c3c..c04bc2191 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -43,6 +43,8 @@ static Matrix covariance = eye(6);
 
 const double tol=1e-5;
 
+const Key kx0 = Symbol("x0"), kx1 = Symbol("x1"), kx2 = Symbol("x2"), kx3 = Symbol("x3"), kx4 = Symbol("x4"), kx5 = Symbol("x5");
+
 /* ************************************************************************* */
 // test optimization with 6 poses arranged in a hexagon and a loop closure
 TEST(Pose3Graph, optimizeCircle) {
@@ -76,7 +78,7 @@ TEST(Pose3Graph, optimizeCircle) {
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "x0","x1","x2","x3","x4","x5";
+  *ordering += kx0,kx1,kx2,kx3,kx4,kx5;
   NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
   pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index 00c41aaad..ab8621cac 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -40,6 +40,8 @@ static Cal3_S2 K(fov,w,h);
 static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 static shared_ptrK sK(new Cal3_S2(K));
 
+const Key kx1 = Symbol("x1"), kl1 = Symbol("l1");
+
 // make cameras
 
 /* ************************************************************************* */
@@ -62,12 +64,12 @@ TEST( ProjectionFactor, error )
 	DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
 
 	// Check linearize
-	Ordering ordering; ordering += "x1","l1";
+	Ordering ordering; ordering += kx1,kl1;
   Matrix Ax1 = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
 	Matrix Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.);
 	Vector b = Vector_(2,3.,0.);
 	SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
-	JacobianFactor expected(ordering["x1"], Ax1, ordering["l1"], Al1, b, probModel1);
+	JacobianFactor expected(ordering[kx1], Ax1, ordering[kl1], Al1, b, probModel1);
 	JacobianFactor::shared_ptr actual =
 	    boost::dynamic_pointer_cast<JacobianFactor>(factor->linearize(config, ordering));
 	CHECK(assert_equal(expected,*actual,1e-3));
@@ -85,8 +87,8 @@ TEST( ProjectionFactor, error )
   Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
   Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
-	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
-	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
+	delta[ordering[kx1]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
+	delta[ordering[kl1]] = Vector_(3, 1.,2.,3.);
 	Values actual_config = config.retract(delta, ordering);
 	CHECK(assert_equal(expected_config,actual_config,1e-9));
 }
diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp
index c75e0a148..afd759110 100644
--- a/gtsam/slam/tests/testSerializationSLAM.cpp
+++ b/gtsam/slam/tests/testSerializationSLAM.cpp
@@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 TEST (Serialization, smallExample_linear) {
   using namespace example;
 
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += Symbol("x1"),Symbol("x2"),Symbol("l1");
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
   EXPECT(equalsObj(ordering));
   EXPECT(equalsXML(ordering));
diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp
index fb53d4b65..c4779a1cf 100644
--- a/gtsam/slam/tests/testVSLAM.cpp
+++ b/gtsam/slam/tests/testVSLAM.cpp
@@ -101,7 +101,7 @@ TEST( Graph, optimizeLM)
 
   // Create an ordering of the variables
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "l1","l2","l3","l4","x1","x2";
+  *ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
@@ -138,7 +138,7 @@ TEST( Graph, optimizeLM2)
 
   // Create an ordering of the variables
   shared_ptr<Ordering> ordering(new Ordering);
-  *ordering += "l1","l2","l3","l4","x1","x2";
+  *ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
@@ -204,11 +204,11 @@ TEST( Values, update_with_large_delta) {
 	Ordering largeOrdering;
 	Values largeValues = init;
 	largeValues.insert(PoseKey(2), Pose3());
-	largeOrdering += "x1","l1","x2";
+	largeOrdering += PoseKey(1),PointKey(1),PoseKey(2);
 	VectorValues delta(largeValues.dims(largeOrdering));
-	delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
-	delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
-	delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
+	delta[largeOrdering[PoseKey(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
+	delta[largeOrdering[PointKey(1)]] = Vector_(3, 0.1, 0.1, 0.1);
+	delta[largeOrdering[PoseKey(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
 	Values actual = init.retract(delta, largeOrdering);
 
 	CHECK(assert_equal(expected,actual));
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index 40e5eafcc..5416e2142 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -67,8 +67,8 @@ namespace visualSLAM {
     }
 
     /// print out graph
-    void print(const std::string& s = "") const {
-      NonlinearFactorGraph::print(s);
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      NonlinearFactorGraph::print(s, keyFormatter);
     }
 
     /// check if two graphs are equal
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 4d3279250..b38e7e3fe 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -155,8 +155,8 @@ public:
   /* print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearMotionModel\n";
-    std::cout << "  TestKey1: " << (std::string) key1() << std::endl;
-    std::cout << "  TestKey2: " << (std::string) key2() << std::endl;
+    std::cout << "  TestKey1: " << DefaultKeyFormatter(key1()) << std::endl;
+    std::cout << "  TestKey2: " << DefaultKeyFormatter(key2()) << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
@@ -293,7 +293,7 @@ public:
   /* print */
   virtual void print(const std::string& s = "") const {
     std::cout << s << ": NonlinearMeasurementModel\n";
-    std::cout << "  TestKey: " << (std::string) key() << std::endl;
+    std::cout << "  TestKey: " << DefaultKeyFormatter(key()) << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp
index f6cf7d54b..f39af5c3d 100644
--- a/tests/testGaussianFactor.cpp
+++ b/tests/testGaussianFactor.cpp
@@ -26,7 +26,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx3 to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
@@ -44,19 +44,21 @@ static SharedDiagonal
 	sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
 	constraintModel = noiseModel::Constrained::All(2);
 
+const Key kx1 = Symbol("x1"), kx2 = Symbol("x2"), kl1 = Symbol("l1");
+
 /* ************************************************************************* */
 TEST( GaussianFactor, linearFactor )
 {
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx1,kx2,kl1;
 
   Matrix I = eye(2);
 	Vector b = Vector_(2, 2.0, -1.0);
-	JacobianFactor expected(ordering["x1"], -10*I,ordering["x2"], 10*I, b, noiseModel::Unit::Create(2));
+	JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2));
 
 	// create a small linear factor graph
 	FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
 
-	// get the factor "f2" from the factor graph
+	// get the factor kf2 from the factor graph
 	JacobianFactor::shared_ptr lf = fg[1];
 
 	// check if the two factors are the same
@@ -66,26 +68,26 @@ TEST( GaussianFactor, linearFactor )
 ///* ************************************************************************* */
 // SL-FIX TEST( GaussianFactor, keys )
 //{
-//	// get the factor "f2" from the small linear factor graph
-//  Ordering ordering; ordering += "x1","x2","l1";
+//	// get the factor kf2 from the small linear factor graph
+//  Ordering ordering; ordering += kx1,kx2,kl1;
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //	GaussianFactor::shared_ptr lf = fg[1];
 //	list<Symbol> expected;
-//	expected.push_back("x1");
-//	expected.push_back("x2");
+//	expected.push_back(kx1);
+//	expected.push_back(kx2);
 //	EXPECT(lf->keys() == expected);
 //}
 
 ///* ************************************************************************* */
 // SL-FIX TEST( GaussianFactor, dimensions )
 //{
-//  // get the factor "f2" from the small linear factor graph
-//  Ordering ordering; ordering += "x1","x2","l1";
+//  // get the factor kf2 from the small linear factor graph
+//  Ordering ordering; ordering += kx1,kx2,kl1;
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //
 //  // Check a single factor
 //  Dimensions expected;
-//  insert(expected)("x1", 2)("x2", 2);
+//  insert(expected)(kx1, 2)(kx2, 2);
 //  Dimensions actual = fg[1]->dimensions();
 //  EXPECT(expected==actual);
 //}
@@ -94,12 +96,12 @@ TEST( GaussianFactor, linearFactor )
 TEST( GaussianFactor, getDim )
 {
 	// get a factor
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx1,kx2,kl1;
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 	GaussianFactor::shared_ptr factor = fg[0];
 
 	// get the size of a variable
-	size_t actual = factor->getDim(factor->find(ordering["x1"]));
+	size_t actual = factor->getDim(factor->find(ordering[kx1]));
 
 	// verify
 	size_t expected = 2;
@@ -110,7 +112,7 @@ TEST( GaussianFactor, getDim )
 // SL-FIX TEST( GaussianFactor, combine )
 //{
 //	// create a small linear factor graph
-//  Ordering ordering; ordering += "x1","x2","l1";
+//  Ordering ordering; ordering += kx1,kx2,kl1;
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //
 //	// get two factors from it and insert the factors into a vector
@@ -155,9 +157,9 @@ TEST( GaussianFactor, getDim )
 //
 //	// use general constructor for making arbitrary factors
 //	vector<pair<Symbol, Matrix> > meas;
-//	meas.push_back(make_pair("x2", Ax2));
-//	meas.push_back(make_pair("l1", Al1));
-//	meas.push_back(make_pair("x1", Ax1));
+//	meas.push_back(make_pair(kx2, Ax2));
+//	meas.push_back(make_pair(kl1, Al1));
+//	meas.push_back(make_pair(kx1, Ax1));
 //	GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4)));
 //	EXPECT(assert_equal(expected,combined));
 //}
@@ -166,7 +168,7 @@ TEST( GaussianFactor, getDim )
 TEST( GaussianFactor, error )
 {
 	// create a small linear factor graph
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx1,kx2,kl1;
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 
 	// get the first factor from the factor graph
@@ -175,7 +177,7 @@ TEST( GaussianFactor, error )
 	// check the error of the first factor with noisy config
 	VectorValues cfg = createZeroDelta(ordering);
 
-	// calculate the error from the factor "f1"
+	// calculate the error from the factor kf1
 	// note the error is the same as in testNonlinearFactor
 	double actual = lf->error(cfg);
 	DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
@@ -185,7 +187,7 @@ TEST( GaussianFactor, error )
 // SL-FIX TEST( GaussianFactor, eliminate )
 //{
 //	// create a small linear factor graph
-//  Ordering ordering; ordering += "x1","x2","l1";
+//  Ordering ordering; ordering += kx1,kx2,kl1;
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //
 //	// get two factors from it and insert the factors into a vector
@@ -199,7 +201,7 @@ TEST( GaussianFactor, error )
 //	// eliminate the combined factor
 //	GaussianConditional::shared_ptr actualCG;
 //	GaussianFactor::shared_ptr actualLF;
-//	boost::tie(actualCG,actualLF) = combined.eliminate("x2");
+//	boost::tie(actualCG,actualLF) = combined.eliminate(kx2);
 //
 //	// create expected Conditional Gaussian
 //	Matrix I = eye(2)*sqrt(125.0);
@@ -208,14 +210,14 @@ TEST( GaussianFactor, error )
 //
 //	// Check the conditional Gaussian
 //	GaussianConditional
-//	expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1.0));
+//	expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0));
 //
 //	// the expected linear factor
 //	I = eye(2)/0.2236;
 //	Matrix Bl1 = I, Bx1 = -I;
 //	Vector b1 = I*Vector_(2,0.0,0.2);
 //
-//	GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,1.0));
+//	GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0));
 //
 //	// check if the result matches
 //	EXPECT(assert_equal(expectedCG,*actualCG,1e-3));
@@ -226,17 +228,17 @@ TEST( GaussianFactor, error )
 TEST( GaussianFactor, matrix )
 {
 	// create a small linear factor graph
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx1,kx2,kl1;
   FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
 
-	// get the factor "f2" from the factor graph
+	// get the factor kf2 from the factor graph
 	//GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version
 	Vector b2 = Vector_(2, 0.2, -0.1);
 	Matrix I = eye(2);
   // render with a given ordering
   Ordering ord;
-  ord += "x1","x2";
-	JacobianFactor::shared_ptr lf(new JacobianFactor(ord["x1"], -I, ord["x2"], I, b2, sigma0_1));
+  ord += kx1,kx2;
+	JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1));
 
 	// Test whitened version
 	Matrix A_act1; Vector b_act1;
@@ -274,17 +276,17 @@ TEST( GaussianFactor, matrix )
 TEST( GaussianFactor, matrix_aug )
 {
 	// create a small linear factor graph
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx1,kx2,kl1;
   FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
 
-	// get the factor "f2" from the factor graph
+	// get the factor kf2 from the factor graph
 	//GaussianFactor::shared_ptr lf = fg[1];
 	Vector b2 = Vector_(2, 0.2, -0.1);
 	Matrix I = eye(2);
   // render with a given ordering
   Ordering ord;
-  ord += "x1","x2";
-	JacobianFactor::shared_ptr lf(new JacobianFactor(ord["x1"], -I, ord["x2"], I, b2, sigma0_1));
+  ord += kx1,kx2;
+	JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1));
 
 
 	// Test unwhitened version
@@ -325,15 +327,15 @@ void print(const list<T>& i) {
 // SL-FIX TEST( GaussianFactor, sparse )
 //{
 //	// create a small linear factor graph
-//  Ordering ordering; ordering += "x1","x2","l1";
+//  Ordering ordering; ordering += kx1,kx2,kl1;
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //
-//	// get the factor "f2" from the factor graph
+//	// get the factor kf2 from the factor graph
 //	GaussianFactor::shared_ptr lf = fg[1];
 //
 //	// render with a given ordering
 //	Ordering ord;
-//	ord += "x1","x2";
+//	ord += kx1,kx2;
 //
 //	list<int> i,j;
 //	list<double> s;
@@ -355,15 +357,15 @@ void print(const list<T>& i) {
 // SL-FIX TEST( GaussianFactor, sparse2 )
 //{
 //	// create a small linear factor graph
-//  Ordering ordering; ordering += "x1","x2","l1";
+//  Ordering ordering; ordering += kx1,kx2,kl1;
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //
-//	// get the factor "f2" from the factor graph
+//	// get the factor kf2 from the factor graph
 //	GaussianFactor::shared_ptr lf = fg[1];
 //
 //	// render with a given ordering
 //	Ordering ord;
-//	ord += "x2","l1","x1";
+//	ord += kx2,kl1,kx1;
 //
 //	list<int> i,j;
 //	list<double> s;
@@ -385,7 +387,7 @@ void print(const list<T>& i) {
 TEST( GaussianFactor, size )
 {
 	// create a linear factor graph
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx1,kx2,kl1;
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 
 	// get some factors from the graph
diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp
index 73b1f2d63..bec246854 100644
--- a/tests/testGaussianFactorGraph.cpp
+++ b/tests/testGaussianFactorGraph.cpp
@@ -28,7 +28,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
@@ -44,10 +44,13 @@ using namespace example;
 
 double tol=1e-5;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, equals ) {
 
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx(1),kx(2),kl(1);
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
   GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering);
   EXPECT(fg.equals(fg2));
@@ -55,7 +58,7 @@ TEST( GaussianFactorGraph, equals ) {
 
 /* ************************************************************************* */
 //TEST( GaussianFactorGraph, error ) {
-//  Ordering ordering; ordering += "x1","x2","l1";
+//  Ordering ordering; ordering += kx(1),kx(2),kl(1);
 //  FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
 //  VectorValues cfg = createZeroDelta(ordering);
 //
@@ -73,10 +76,10 @@ TEST( GaussianFactorGraph, equals ) {
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
 //
-//  set<Symbol> separator = fg.find_separator("x2");
+//  set<Symbol> separator = fg.find_separator(kx(2));
 //  set<Symbol> expected;
-//  expected.insert("x1");
-//  expected.insert("l1");
+//  expected.insert(kx(1));
+//  expected.insert(kl(1));
 //
 //  EXPECT(separator.size()==expected.size());
 //  set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin();
@@ -91,7 +94,7 @@ TEST( GaussianFactorGraph, equals ) {
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
 //
 //  // combine all factors
-//  GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
+//  GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1));
 //
 //  // the expected linear factor
 //  Matrix Al1 = Matrix_(6,2,
@@ -131,11 +134,11 @@ TEST( GaussianFactorGraph, equals ) {
 //  b(5) =  1;
 //
 //  vector<pair<Symbol, Matrix> > meas;
-//  meas.push_back(make_pair("l1", Al1));
-//  meas.push_back(make_pair("x1", Ax1));
-//  meas.push_back(make_pair("x2", Ax2));
+//  meas.push_back(make_pair(kl(1), Al1));
+//  meas.push_back(make_pair(kx(1), Ax1));
+//  meas.push_back(make_pair(kx(2), Ax2));
 //  GaussianFactor expected(meas, b, ones(6));
-//  //GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
+//  //GaussianFactor expected(kl(1), Al1, kx(1), Ax1, kx(2), Ax2, b);
 //
 //  // check if the two factors are the same
 //  EXPECT(assert_equal(expected,*actual));
@@ -148,7 +151,7 @@ TEST( GaussianFactorGraph, equals ) {
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
 //
 //  // combine all factors
-//  GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2");
+//  GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(2));
 //
 //  // the expected linear factor
 //  Matrix Al1 = Matrix_(4,2,
@@ -183,9 +186,9 @@ TEST( GaussianFactorGraph, equals ) {
 //  b(3) =  1.5;
 //
 //  vector<pair<Symbol, Matrix> > meas;
-//  meas.push_back(make_pair("l1", Al1));
-//  meas.push_back(make_pair("x1", Ax1));
-//  meas.push_back(make_pair("x2", Ax2));
+//  meas.push_back(make_pair(kl(1), Al1));
+//  meas.push_back(make_pair(kx(1), Ax1));
+//  meas.push_back(make_pair(kx(2), Ax2));
 //  GaussianFactor expected(meas, b, ones(4));
 //
 //  // check if the two factors are the same
@@ -195,14 +198,14 @@ TEST( GaussianFactorGraph, equals ) {
 ///* ************************************************************************* */
 //TEST( GaussianFactorGraph, eliminateOne_x1 )
 //{
-//  Ordering ordering; ordering += "x1","l1","x2";
+//  Ordering ordering; ordering += kx(1),kl(1),kx(2);
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //  GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
 //
 //  // create expected Conditional Gaussian
 //  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
 //  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
-//  GaussianConditional expected(ordering["x1"],15*d,R11,ordering["l1"],S12,ordering["x2"],S13,sigma);
+//  GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
 //
 //  EXPECT(assert_equal(expected,*actual,tol));
 //}
@@ -210,7 +213,7 @@ TEST( GaussianFactorGraph, equals ) {
 ///* ************************************************************************* */
 //TEST( GaussianFactorGraph, eliminateOne_x2 )
 //{
-//   Ordering ordering; ordering += "x2","l1","x1";
+//   Ordering ordering; ordering += kx(2),kl(1),kx(1);
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //  GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
 //
@@ -218,7 +221,7 @@ TEST( GaussianFactorGraph, equals ) {
 //  double sig = 0.0894427;
 //  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
 //  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
-//  GaussianConditional expected(ordering["x2"],d,R11,ordering["l1"],S12,ordering["x1"],S13,sigma);
+//  GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma);
 //
 //  EXPECT(assert_equal(expected,*actual,tol));
 //}
@@ -226,7 +229,7 @@ TEST( GaussianFactorGraph, equals ) {
 ///* ************************************************************************* */
 //TEST( GaussianFactorGraph, eliminateOne_l1 )
 //{
-//  Ordering ordering; ordering += "l1","x1","x2";
+//  Ordering ordering; ordering += kl(1),kx(1),kx(2);
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //  GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
 //
@@ -234,7 +237,7 @@ TEST( GaussianFactorGraph, equals ) {
 //  double sig = sqrt(2)/10.;
 //  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
 //  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
-//  GaussianConditional expected(ordering["l1"],d,R11,ordering["x1"],S12,ordering["x2"],S13,sigma);
+//  GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
 //
 //  EXPECT(assert_equal(expected,*actual,tol));
 //}
@@ -243,12 +246,12 @@ TEST( GaussianFactorGraph, equals ) {
 //TEST( GaussianFactorGraph, eliminateOne_x1_fast )
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  GaussianConditional::shared_ptr actual = fg.eliminateOne("x1", false);
+//  GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(1), false);
 //
 //  // create expected Conditional Gaussian
 //  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
 //  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
-//  GaussianConditional expected("x1",15*d,R11,"l1",S12,"x2",S13,sigma);
+//  GaussianConditional expected(kx(1),15*d,R11,kl(1),S12,kx(2),S13,sigma);
 //
 //  EXPECT(assert_equal(expected,*actual,tol));
 //}
@@ -257,13 +260,13 @@ TEST( GaussianFactorGraph, equals ) {
 //TEST( GaussianFactorGraph, eliminateOne_x2_fast )
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  GaussianConditional::shared_ptr actual = fg.eliminateOne("x2", false);
+//  GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(2), false);
 //
 //  // create expected Conditional Gaussian
 //  double sig = 0.0894427;
 //  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
 //  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
-//  GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma);
+//  GaussianConditional expected(kx(2),d,R11,kl(1),S12,kx(1),S13,sigma);
 //
 //  EXPECT(assert_equal(expected,*actual,tol));
 //}
@@ -272,13 +275,13 @@ TEST( GaussianFactorGraph, equals ) {
 //TEST( GaussianFactorGraph, eliminateOne_l1_fast )
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  GaussianConditional::shared_ptr actual = fg.eliminateOne("l1", false);
+//  GaussianConditional::shared_ptr actual = fg.eliminateOne(kl(1), false);
 //
 //  // create expected Conditional Gaussian
 //  double sig = sqrt(2)/10.;
 //  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
 //  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
-//  GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma);
+//  GaussianConditional expected(kl(1),d,R11,kx(1),S12,kx(2),S13,sigma);
 //
 //  EXPECT(assert_equal(expected,*actual,tol));
 //}
@@ -290,18 +293,18 @@ TEST( GaussianFactorGraph, eliminateAll )
 	Matrix I = eye(2);
 
   Ordering ordering;
-  ordering += "x2","l1","x1";
+  ordering += kx(2),kl(1),kx(1);
 
 	Vector d1 = Vector_(2, -0.1,-0.1);
-	GaussianBayesNet expected = simpleGaussian(ordering["x1"],d1,0.1);
+	GaussianBayesNet expected = simpleGaussian(ordering[kx(1)],d1,0.1);
 
 	double sig1 = 0.149071;
 	Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
-	push_front(expected,ordering["l1"],d2, I/sig1,ordering["x1"], (-1)*I/sig1,sigma2);
+	push_front(expected,ordering[kl(1)],d2, I/sig1,ordering[kx(1)], (-1)*I/sig1,sigma2);
 
 	double sig2 = 0.0894427;
 	Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
-	push_front(expected,ordering["x2"],d3, I/sig2,ordering["l1"], (-0.2)*I/sig2, ordering["x1"], (-0.8)*I/sig2, sigma3);
+	push_front(expected,ordering[kx(2)],d3, I/sig2,ordering[kl(1)], (-0.2)*I/sig2, ordering[kx(1)], (-0.8)*I/sig2, sigma3);
 
 	// Check one ordering
 	GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
@@ -319,20 +322,20 @@ TEST( GaussianFactorGraph, eliminateAll )
 //	Matrix I = eye(2);
 //
 //	Vector d1 = Vector_(2, -0.1,-0.1);
-//	GaussianBayesNet expected = simpleGaussian("x1",d1,0.1);
+//	GaussianBayesNet expected = simpleGaussian(kx(1),d1,0.1);
 //
 //	double sig1 = 0.149071;
 //	Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
-//	push_front(expected,"l1",d2, I/sig1,"x1", (-1)*I/sig1,sigma2);
+//	push_front(expected,kl(1),d2, I/sig1,kx(1), (-1)*I/sig1,sigma2);
 //
 //	double sig2 = 0.0894427;
 //	Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
-//	push_front(expected,"x2",d3, I/sig2,"l1", (-0.2)*I/sig2, "x1", (-0.8)*I/sig2, sigma3);
+//	push_front(expected,kx(2),d3, I/sig2,kl(1), (-0.2)*I/sig2, kx(1), (-0.8)*I/sig2, sigma3);
 //
 //	// Check one ordering
 //	GaussianFactorGraph fg1 = createGaussianFactorGraph();
 //	Ordering ordering;
-//	ordering += "x2","l1","x1";
+//	ordering += kx(2),kl(1),kx(1);
 //	GaussianBayesNet actual = fg1.eliminate(ordering, false);
 //	EXPECT(assert_equal(expected,actual,tol));
 //}
@@ -340,16 +343,16 @@ TEST( GaussianFactorGraph, eliminateAll )
 ///* ************************************************************************* */
 //TEST( GaussianFactorGraph, add_priors )
 //{
-//  Ordering ordering; ordering += "l1","x1","x2";
+//  Ordering ordering; ordering += kl(1),kx(1),kx(2);
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 //  GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
 //  GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
 //  Matrix A = eye(2);
 //  Vector b = zero(2);
 //  SharedDiagonal sigma = sharedSigma(2,3.0);
-//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering["l1"],A,b,sigma)));
-//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering["x1"],A,b,sigma)));
-//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering["x2"],A,b,sigma)));
+//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kl(1)],A,b,sigma)));
+//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kx(1)],A,b,sigma)));
+//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kx(2)],A,b,sigma)));
 //  EXPECT(assert_equal(expected,actual));
 //}
 
@@ -357,7 +360,7 @@ TEST( GaussianFactorGraph, eliminateAll )
 TEST( GaussianFactorGraph, copying )
 {
   // Create a graph
-  Ordering ordering; ordering += "x2","l1","x1";
+  Ordering ordering; ordering += kx(2),kl(1),kx(1);
   GaussianFactorGraph actual = createGaussianFactorGraph(ordering);
 
   // Copy the graph !
@@ -378,7 +381,7 @@ TEST( GaussianFactorGraph, copying )
 //{
 //  // render with a given ordering
 //  Ordering ord;
-//  ord += "x2","l1","x1";
+//  ord += kx(2),kl(1),kx(1);
 //
 //  // Create a graph
 //  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
@@ -417,7 +420,7 @@ TEST( GaussianFactorGraph, copying )
 TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
 {
   Ordering ord;
-  ord += "x2","l1","x1";
+  ord += kx(2),kl(1),kx(1);
   GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 
   // render with a given ordering
@@ -437,20 +440,20 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, getOrdering)
 {
-  Ordering original; original += "l1","x1","x2";
+  Ordering original; original += kl(1),kx(1),kx(2);
   FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
   Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic)));
   Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
-  Ordering expected; expected += "l1","x2","x1";
+  Ordering expected; expected += kl(1),kx(2),kx(1);
   EXPECT(assert_equal(expected,actual));
 }
 
 // SL-FIX TEST( GaussianFactorGraph, getOrdering2)
 //{
 //  Ordering expected;
-//  expected += "l1","x1";
+//  expected += kl(1),kx(1);
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  set<Symbol> interested; interested += "l1","x1";
+//  set<Symbol> interested; interested += kl(1),kx(1);
 //  Ordering actual = fg.getOrdering(interested);
 //  EXPECT(assert_equal(expected,actual));
 //}
@@ -459,7 +462,7 @@ TEST( GaussianFactorGraph, getOrdering)
 TEST( GaussianFactorGraph, optimize_LDL )
 {
   // create an ordering
-  Ordering ord; ord += "x2","l1","x1";
+  Ordering ord; ord += kx(2),kl(1),kx(1);
 
   // create a graph
 	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
@@ -477,7 +480,7 @@ TEST( GaussianFactorGraph, optimize_LDL )
 TEST( GaussianFactorGraph, optimize_QR )
 {
   // create an ordering
-  Ordering ord; ord += "x2","l1","x1";
+  Ordering ord; ord += kx(2),kl(1),kx(1);
 
   // create a graph
 	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
@@ -495,7 +498,7 @@ TEST( GaussianFactorGraph, optimize_QR )
 // SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas )
 //{
 //  // create an ordering
-//  Ordering ord; ord += "x2","l1","x1";
+//  Ordering ord; ord += kx(2),kl(1),kx(1);
 //
 //	// create a graph
 //	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
@@ -513,7 +516,7 @@ TEST( GaussianFactorGraph, optimize_QR )
 TEST( GaussianFactorGraph, combine)
 {
   // create an ordering
-  Ordering ord; ord += "x2","l1","x1";
+  Ordering ord; ord += kx(2),kl(1),kx(1);
 
   // create a test graph
 	GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
@@ -535,7 +538,7 @@ TEST( GaussianFactorGraph, combine)
 TEST( GaussianFactorGraph, combine2)
 {
   // create an ordering
-  Ordering ord; ord += "x2","l1","x1";
+  Ordering ord; ord += kx(2),kl(1),kx(1);
 
 	// create a test graph
 	GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
@@ -568,13 +571,13 @@ void print(vector<int> v) {
 //	GaussianFactorGraph fg = createGaussianFactorGraph();
 //
 //	// ask for all factor indices connected to x1
-//	list<size_t> x1_factors = fg.factors("x1");
+//	list<size_t> x1_factors = fg.factors(kx(1));
 //	size_t x1_indices[] = { 0, 1, 2 };
 //	list<size_t> x1_expected(x1_indices, x1_indices + 3);
 //	EXPECT(x1_factors==x1_expected);
 //
 //	// ask for all factor indices connected to x2
-//	list<size_t> x2_factors = fg.factors("x2");
+//	list<size_t> x2_factors = fg.factors(kx(2));
 //	size_t x2_indices[] = { 1, 3 };
 //	list<size_t> x2_expected(x2_indices, x2_indices + 2);
 //	EXPECT(x2_factors==x2_expected);
@@ -592,7 +595,7 @@ void print(vector<int> v) {
 //  GaussianFactor::shared_ptr f2 = fg[2];
 //
 //  // call the function
-//  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
+//  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(kx(1));
 //
 //  // Check the factors
 //  EXPECT(f0==factors[0]);
@@ -617,7 +620,7 @@ TEST(GaussianFactorGraph, createSmoother)
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
 //  Dimensions expected;
-//  insert(expected)("l1", 2)("x1", 2)("x2", 2);
+//  insert(expected)(kl(1), 2)(kx(1), 2)(kx(2), 2);
 //  Dimensions actual = fg.dimensions();
 //  EXPECT(expected==actual);
 //}
@@ -627,7 +630,7 @@ TEST(GaussianFactorGraph, createSmoother)
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
 //  Ordering expected;
-//  expected += "l1","x1","x2";
+//  expected += kl(1),kx(1),kx(2);
 //  EXPECT(assert_equal(expected,fg.keys()));
 //}
 
@@ -635,16 +638,16 @@ TEST(GaussianFactorGraph, createSmoother)
 // SL-NEEDED? TEST( GaussianFactorGraph, involves )
 //{
 //  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  EXPECT(fg.involves("l1"));
-//  EXPECT(fg.involves("x1"));
-//  EXPECT(fg.involves("x2"));
-//  EXPECT(!fg.involves("x3"));
+//  EXPECT(fg.involves(kl(1)));
+//  EXPECT(fg.involves(kx(1)));
+//  EXPECT(fg.involves(kx(2)));
+//  EXPECT(!fg.involves(kx(3)));
 //}
 
 /* ************************************************************************* */
 double error(const VectorValues& x) {
   // create an ordering
-  Ordering ord; ord += "x2","l1","x1";
+  Ordering ord; ord += kx(2),kl(1),kx(1);
 
 	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 	return fg.error(x);
@@ -658,11 +661,11 @@ double error(const VectorValues& x) {
 //	// Construct expected gradient
 //	VectorValues expected;
 //
-//  // 2*f(x) = 100*(x1+c["x1"])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
+//  // 2*f(x) = 100*(x1+c[kx(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
 //	// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
-//  expected.insert("l1",Vector_(2,  5.0,-12.5));
-//  expected.insert("x1",Vector_(2, 30.0,  5.0));
-//  expected.insert("x2",Vector_(2,-25.0, 17.5));
+//  expected.insert(kl(1),Vector_(2,  5.0,-12.5));
+//  expected.insert(kx(1),Vector_(2, 30.0,  5.0));
+//  expected.insert(kx(2),Vector_(2,-25.0, 17.5));
 //
 //	// Check the gradient at delta=0
 //  VectorValues zero = createZeroDelta();
@@ -675,7 +678,7 @@ double error(const VectorValues& x) {
 //
 //	// Check the gradient at the solution (should be zero)
 //	Ordering ord;
-//  ord += "x2","l1","x1";
+//  ord += kx(2),kl(1),kx(1);
 //	GaussianFactorGraph fg2 = createGaussianFactorGraph();
 //  VectorValues solution = fg2.optimize(ord); // destructive
 //	VectorValues actual2 = fg.gradient(solution);
@@ -686,7 +689,7 @@ double error(const VectorValues& x) {
 TEST( GaussianFactorGraph, multiplication )
 {
   // create an ordering
-  Ordering ord; ord += "x2","l1","x1";
+  Ordering ord; ord += kx(2),kl(1),kx(1);
 
 	FactorGraph<JacobianFactor> A = createGaussianFactorGraph(ord);
   VectorValues x = createCorrectDelta(ord);
@@ -703,7 +706,7 @@ TEST( GaussianFactorGraph, multiplication )
 // SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication )
 //{
 //  // create an ordering
-//  Ordering ord; ord += "x2","l1","x1";
+//  Ordering ord; ord += kx(2),kl(1),kx(1);
 //
 //	GaussianFactorGraph A = createGaussianFactorGraph(ord);
 //  Errors e;
@@ -713,9 +716,9 @@ TEST( GaussianFactorGraph, multiplication )
 //  e += Vector_(2,-7.5,-5.0);
 //
 //  VectorValues expected = createZeroDelta(ord), actual = A ^ e;
-//  expected[ord["l1"]] = Vector_(2, -37.5,-50.0);
-//  expected[ord["x1"]] = Vector_(2,-150.0, 25.0);
-//  expected[ord["x2"]] = Vector_(2, 187.5, 25.0);
+//  expected[ord[kl(1)]] = Vector_(2, -37.5,-50.0);
+//  expected[ord[kx(1)]] = Vector_(2,-150.0, 25.0);
+//  expected[ord[kx(2)]] = Vector_(2, 187.5, 25.0);
 //	EXPECT(assert_equal(expected,actual));
 //}
 
@@ -723,7 +726,7 @@ TEST( GaussianFactorGraph, multiplication )
 // SL-NEEDED? TEST( GaussianFactorGraph, rhs )
 //{
 //  // create an ordering
-//  Ordering ord; ord += "x2","l1","x1";
+//  Ordering ord; ord += kx(2),kl(1),kx(1);
 //
 //	GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
 //	Errors expected = createZeroDelta(ord), actual = Ab.rhs();
@@ -739,21 +742,21 @@ TEST( GaussianFactorGraph, multiplication )
 TEST( GaussianFactorGraph, elimination )
 {
   Ordering ord;
-  ord += "x1", "x2";
+  ord += kx(1), kx(2);
 	// Create Gaussian Factor Graph
 	GaussianFactorGraph fg;
 	Matrix Ap = eye(1), An = eye(1) * -1;
 	Vector b = Vector_(1, 0.0);
   SharedDiagonal sigma = sharedSigma(1,2.0);
-	fg.add(ord["x1"], An, ord["x2"], Ap, b, sigma);
-	fg.add(ord["x1"], Ap, b, sigma);
-	fg.add(ord["x2"], Ap, b, sigma);
+	fg.add(ord[kx(1)], An, ord[kx(2)], Ap, b, sigma);
+	fg.add(ord[kx(1)], Ap, b, sigma);
+	fg.add(ord[kx(2)], Ap, b, sigma);
 
 	// Eliminate
 	GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
 
 	// Check sigma
-	EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord["x2"]]->get_sigmas()(0),1e-5);
+	EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[kx(2)]]->get_sigmas()(0),1e-5);
 
 	// Check matrix
 	Matrix R;Vector d;
@@ -808,7 +811,7 @@ TEST( GaussianFactorGraph, constrained_single )
 //
 //	// eliminate and solve
 //	Ordering ord;
-//	ord += "y", "x";
+//	ord += "yk, x";
 //	VectorValues actual = fg.optimize(ord);
 //
 //	// verify
@@ -839,7 +842,7 @@ TEST( GaussianFactorGraph, constrained_multi1 )
 //
 //	// eliminate and solve
 //	Ordering ord;
-//	ord += "z", "x", "y";
+//	ord += "zk, xk, y";
 //	VectorValues actual = fg.optimize(ord);
 //
 //	// verify
@@ -856,18 +859,18 @@ SharedDiagonal model = sharedSigma(2,1);
 //	GaussianFactorGraph g;
 //	Matrix I = eye(2);
 //	Vector b = Vector_(0, 0, 0);
-//	g.add("x1", I, "x2", I, b, model);
-//	g.add("x1", I, "x3", I, b, model);
-//	g.add("x1", I, "x4", I, b, model);
-//	g.add("x2", I, "x3", I, b, model);
-//	g.add("x2", I, "x4", I, b, model);
-//	g.add("x3", I, "x4", I, b, model);
+//	g.add(kx(1), I, kx(2), I, b, model);
+//	g.add(kx(1), I, kx(3), I, b, model);
+//	g.add(kx(1), I, kx(4), I, b, model);
+//	g.add(kx(2), I, kx(3), I, b, model);
+//	g.add(kx(2), I, kx(4), I, b, model);
+//	g.add(kx(3), I, kx(4), I, b, model);
 //
 //	map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
-//	EXPECT(tree["x1"].compare("x1")==0);
-//	EXPECT(tree["x2"].compare("x1")==0);
-//	EXPECT(tree["x3"].compare("x1")==0);
-//	EXPECT(tree["x4"].compare("x1")==0);
+//	EXPECT(tree[kx(1)].compare(kx(1))==0);
+//	EXPECT(tree[kx(2)].compare(kx(1))==0);
+//	EXPECT(tree[kx(3)].compare(kx(1))==0);
+//	EXPECT(tree[kx(4)].compare(kx(1))==0);
 //}
 
 ///* ************************************************************************* */
@@ -876,17 +879,17 @@ SharedDiagonal model = sharedSigma(2,1);
 //	GaussianFactorGraph g;
 //	Matrix I = eye(2);
 //	Vector b = Vector_(0, 0, 0);
-//	g.add("x1", I, "x2", I, b, model);
-//	g.add("x1", I, "x3", I, b, model);
-//	g.add("x1", I, "x4", I, b, model);
-//	g.add("x2", I, "x3", I, b, model);
-//	g.add("x2", I, "x4", I, b, model);
+//	g.add(kx(1), I, kx(2), I, b, model);
+//	g.add(kx(1), I, kx(3), I, b, model);
+//	g.add(kx(1), I, kx(4), I, b, model);
+//	g.add(kx(2), I, kx(3), I, b, model);
+//	g.add(kx(2), I, kx(4), I, b, model);
 //
 //	PredecessorMap<string> tree;
-//	tree["x1"] = "x1";
-//	tree["x2"] = "x1";
-//	tree["x3"] = "x1";
-//	tree["x4"] = "x1";
+//	tree[kx(1)] = kx(1);
+//	tree[kx(2)] = kx(1);
+//	tree[kx(3)] = kx(1);
+//	tree[kx(4)] = kx(1);
 //
 //	GaussianFactorGraph Ab1, Ab2;
 //  g.split<string, GaussianFactor>(tree, Ab1, Ab2);
@@ -897,17 +900,17 @@ SharedDiagonal model = sharedSigma(2,1);
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, replace)
 {
-  Ordering ord; ord += "x1","x2","x3","x4","x5","x6";
+  Ordering ord; ord += kx(1),kx(2),kx(3),kx(4),kx(5),kx(6);
 	SharedDiagonal noise(sharedSigma(3, 1.0));
 
 	GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
-	    ord["x1"], eye(3,3), ord["x2"], eye(3,3), zero(3), noise));
+	    ord[kx(1)], eye(3,3), ord[kx(2)], eye(3,3), zero(3), noise));
 	GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
-	    ord["x2"], eye(3,3), ord["x3"], eye(3,3), zero(3), noise));
+	    ord[kx(2)], eye(3,3), ord[kx(3)], eye(3,3), zero(3), noise));
 	GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
-	    ord["x3"], eye(3,3), ord["x4"], eye(3,3), zero(3), noise));
+	    ord[kx(3)], eye(3,3), ord[kx(4)], eye(3,3), zero(3), noise));
 	GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
-	    ord["x5"], eye(3,3), ord["x6"], eye(3,3), zero(3), noise));
+	    ord[kx(5)], eye(3,3), ord[kx(6)], eye(3,3), zero(3), noise));
 
 	GaussianFactorGraph actual;
 	actual.push_back(f1);
@@ -943,7 +946,7 @@ TEST(GaussianFactorGraph, replace)
 //
 //	// combine in a factor
 //	Matrix Ab; SharedDiagonal noise;
-//	Ordering order; order += "x2", "l1", "x1";
+//	Ordering order; order += kx(2), kl(1), kx(1);
 //	boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions);
 //
 //	// the expected augmented matrix
@@ -971,26 +974,26 @@ TEST(GaussianFactorGraph, replace)
 //	typedef GaussianFactorGraph::sharedFactor Factor;
 //	SharedDiagonal model(Vector_(1, 0.5));
 //	GaussianFactorGraph fg;
-//	Factor factor1(new JacobianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.),  model));
-//	Factor factor2(new JacobianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.),  model));
-//	Factor factor3(new JacobianFactor("x3", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.),  model));
+//	Factor factor1(new JacobianFactor(kx(1), Matrix_(1,1,1.), kx(2), Matrix_(1,1,1.), Vector_(1,1.),  model));
+//	Factor factor2(new JacobianFactor(kx(2), Matrix_(1,1,1.), kx(3), Matrix_(1,1,1.), Vector_(1,1.),  model));
+//	Factor factor3(new JacobianFactor(kx(3), Matrix_(1,1,1.), kx(3), Matrix_(1,1,1.), Vector_(1,1.),  model));
 //	fg.push_back(factor1);
 //	fg.push_back(factor2);
 //	fg.push_back(factor3);
 //
-//	Ordering frontals; frontals += "x2", "x1";
+//	Ordering frontals; frontals += kx(2), kx(1);
 //	GaussianBayesNet bn = fg.eliminateFrontals(frontals);
 //
 //	GaussianBayesNet bn_expected;
-//	GaussianBayesNet::sharedConditional conditional1(new GaussianConditional("x2", Vector_(1, 2.), Matrix_(1, 1, 2.),
-//			"x1", Matrix_(1, 1, 1.), "x3", Matrix_(1, 1, 1.), Vector_(1, 1.)));
-//	GaussianBayesNet::sharedConditional conditional2(new GaussianConditional("x1", Vector_(1, 0.), Matrix_(1, 1, -1.),
-//			"x3", Matrix_(1, 1, 1.), Vector_(1, 1.)));
+//	GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(kx(2), Vector_(1, 2.), Matrix_(1, 1, 2.),
+//			kx(1), Matrix_(1, 1, 1.), kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
+//	GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(kx(1), Vector_(1, 0.), Matrix_(1, 1, -1.),
+//			kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
 //	bn_expected.push_back(conditional1);
 //	bn_expected.push_back(conditional2);
 //	EXPECT(assert_equal(bn_expected, bn));
 //
-//	GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor("x3", Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
+//	GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(kx(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
 //	GaussianFactorGraph fg_expected;
 //	fg_expected.push_back(factor_expected);
 //	EXPECT(assert_equal(fg_expected, fg));
@@ -1006,8 +1009,8 @@ TEST(GaussianFactorGraph, createSmoother2)
   LONGS_EQUAL(5,fg2.size());
 
   // eliminate
-  vector<Index> x3var; x3var.push_back(ordering["x3"]);
-  vector<Index> x1var; x1var.push_back(ordering["x1"]);
+  vector<Index> x3var; x3var.push_back(ordering[kx(3)]);
+  vector<Index> x1var; x1var.push_back(ordering[kx(1)]);
   GaussianBayesNet p_x3 = *GaussianSequentialSolver(
       *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
   GaussianBayesNet p_x1 = *GaussianSequentialSolver(
@@ -1024,7 +1027,7 @@ TEST(GaussianFactorGraph, hasConstraints)
 	FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
 	EXPECT(hasConstraints(fgc2));
 
-	Ordering ordering; ordering += "x1", "x2", "l1";
+	Ordering ordering; ordering += kx(1), kx(2), kl(1);
 	FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering);
 	EXPECT(!hasConstraints(fg));
 }
diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp
index bd592fc68..40ad8b428 100644
--- a/tests/testGaussianISAM.cpp
+++ b/tests/testGaussianISAM.cpp
@@ -21,7 +21,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/geometry/Rot2.h>
@@ -37,6 +37,9 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 /* ************************************************************************* */
 // Some numbers that should be consistent among all smoother tests
 
@@ -49,7 +52,7 @@ const double tol = 1e-4;
 TEST_UNSAFE( ISAM, iSAM_smoother )
 {
   Ordering ordering;
-  for (int t = 1; t <= 7; t++) ordering += Symbol('x', t);
+  for (int t = 1; t <= 7; t++) ordering += kx(t);
 
   // Create smoother with 7 nodes
 	GaussianFactorGraph smoother = createSmoother(7, ordering).first;
@@ -82,7 +85,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
 //	GaussianFactorGraph smoother = createSmoother(7);
 //
 //	// Create initial tree from first 4 timestamps in reverse order !
-//	Ordering ord; ord += "x4","x3","x2","x1";
+//	Ordering ord; ord += kx(4),kx(3),kx(2),kx(1);
 //	GaussianFactorGraph factors1;
 //	for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
 //	GaussianISAM actual(*Inference::Eliminate(factors1));
@@ -129,7 +132,7 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
 	EXPECT(assert_equal(empty,actual1,tol));
 
 	// Check the conditional P(C2|Root)
-	GaussianISAM::sharedClique C2 = isamTree[ordering["x5"]];
+	GaussianISAM::sharedClique C2 = isamTree[ordering[kx(5)]];
 	GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
 	EXPECT(assert_equal(empty,actual2,tol));
 
@@ -137,8 +140,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
 	double sigma3 = 0.61808;
 	Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
 	GaussianBayesNet expected3;
-	push_front(expected3,ordering["x5"], zero(2), eye(2)/sigma3, ordering["x6"], A56/sigma3, ones(2));
-	GaussianISAM::sharedClique C3 = isamTree[ordering["x4"]];
+	push_front(expected3,ordering[kx(5)], zero(2), eye(2)/sigma3, ordering[kx(6)], A56/sigma3, ones(2));
+	GaussianISAM::sharedClique C3 = isamTree[ordering[kx(4)]];
 	GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
 	EXPECT(assert_equal(expected3,actual3,tol));
 
@@ -146,8 +149,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
 	double sigma4 = 0.661968;
 	Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
 	GaussianBayesNet expected4;
-	push_front(expected4, ordering["x4"], zero(2), eye(2)/sigma4, ordering["x6"], A46/sigma4, ones(2));
-	GaussianISAM::sharedClique C4 = isamTree[ordering["x3"]];
+	push_front(expected4, ordering[kx(4)], zero(2), eye(2)/sigma4, ordering[kx(6)], A46/sigma4, ones(2));
+	GaussianISAM::sharedClique C4 = isamTree[ordering[kx(3)]];
 	GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R);
 	EXPECT(assert_equal(expected4,actual4,tol));
 }
@@ -175,7 +178,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
 {
   // Create smoother with 7 nodes
   Ordering ordering;
-  ordering += "x1","x3","x5","x7","x2","x6","x4";
+  ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4);
   GaussianFactorGraph smoother = createSmoother(7, ordering).first;
 
   // Create the Bayes tree
@@ -192,48 +195,48 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
 	double tol=1e-5;
 
 	// Check marginal on x1
-	GaussianBayesNet expected1 = simpleGaussian(ordering["x1"], zero(2), sigmax1);
-	GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering["x1"]);
+	GaussianBayesNet expected1 = simpleGaussian(ordering[kx(1)], zero(2), sigmax1);
+	GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[kx(1)]);
 	Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
 	Matrix actualCovarianceX1;
-	actualCovarianceX1 = bayesTree.marginalCovariance(ordering["x1"]);
+	actualCovarianceX1 = bayesTree.marginalCovariance(ordering[kx(1)]);
 	EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
 	EXPECT(assert_equal(expected1,actual1,tol));
 
 	// Check marginal on x2
 	double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
-	GaussianBayesNet expected2 = simpleGaussian(ordering["x2"], zero(2), sigx2);
-	GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering["x2"]);
+	GaussianBayesNet expected2 = simpleGaussian(ordering[kx(2)], zero(2), sigx2);
+	GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[kx(2)]);
 	Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2);
 	Matrix actualCovarianceX2;
-	actualCovarianceX2 = bayesTree.marginalCovariance(ordering["x2"]);
+	actualCovarianceX2 = bayesTree.marginalCovariance(ordering[kx(2)]);
 	EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol));
 	EXPECT(assert_equal(expected2,actual2,tol));
 
 	// Check marginal on x3
-	GaussianBayesNet expected3 = simpleGaussian(ordering["x3"], zero(2), sigmax3);
-	GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering["x3"]);
+	GaussianBayesNet expected3 = simpleGaussian(ordering[kx(3)], zero(2), sigmax3);
+	GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[kx(3)]);
 	Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3);
 	Matrix actualCovarianceX3;
-	actualCovarianceX3 = bayesTree.marginalCovariance(ordering["x3"]);
+	actualCovarianceX3 = bayesTree.marginalCovariance(ordering[kx(3)]);
 	EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol));
 	EXPECT(assert_equal(expected3,actual3,tol));
 
 	// Check marginal on x4
-	GaussianBayesNet expected4 = simpleGaussian(ordering["x4"], zero(2), sigmax4);
-	GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering["x4"]);
+	GaussianBayesNet expected4 = simpleGaussian(ordering[kx(4)], zero(2), sigmax4);
+	GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[kx(4)]);
 	Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4);
 	Matrix actualCovarianceX4;
-	actualCovarianceX4 = bayesTree.marginalCovariance(ordering["x4"]);
+	actualCovarianceX4 = bayesTree.marginalCovariance(ordering[kx(4)]);
 	EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol));
 	EXPECT(assert_equal(expected4,actual4,tol));
 
 	// Check marginal on x7 (should be equal to x1)
-	GaussianBayesNet expected7 = simpleGaussian(ordering["x7"], zero(2), sigmax7);
-	GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering["x7"]);
+	GaussianBayesNet expected7 = simpleGaussian(ordering[kx(7)], zero(2), sigmax7);
+	GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[kx(7)]);
 	Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7);
 	Matrix actualCovarianceX7;
-	actualCovarianceX7 = bayesTree.marginalCovariance(ordering["x7"]);
+	actualCovarianceX7 = bayesTree.marginalCovariance(ordering[kx(7)]);
 	EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol));
 	EXPECT(assert_equal(expected7,actual7,tol));
 }
@@ -243,7 +246,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
 {
 	// Create smoother with 7 nodes
   Ordering ordering;
-  ordering += "x1","x3","x5","x7","x2","x6","x4";
+  ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4);
 	GaussianFactorGraph smoother = createSmoother(7, ordering).first;
 
 	// Create the Bayes tree
@@ -257,19 +260,19 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
 	EXPECT(assert_equal(empty,actual1,tol));
 
 	// Check the conditional P(C2|Root)
-	GaussianISAM::sharedClique C2 = isamTree[ordering["x3"]];
+	GaussianISAM::sharedClique C2 = isamTree[ordering[kx(3)]];
 	GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
 	EXPECT(assert_equal(empty,actual2,tol));
 
 	// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
 	/** TODO: Note for multifrontal conditional:
-	 * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering["x2"]]->conditional()
+	 * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[kx(2)]]->conditional()
 	 * We don't know yet how to take it out.
 	 */
-//	GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering["x2"]]->conditional();
+//	GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[kx(2)]]->conditional();
 //	p_x2_x4->print("Conditional p_x2_x4: ");
 //	GaussianBayesNet expected3(p_x2_x4);
-//	GaussianISAM::sharedClique C3 = isamTree[ordering["x1"]];
+//	GaussianISAM::sharedClique C3 = isamTree[ordering[kx(1)]];
 //	GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
 //	EXPECT(assert_equal(expected3,actual3,tol));
 }
@@ -279,7 +282,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
 //{
 //  // Create smoother with 7 nodes
 //  Ordering ordering;
-//  ordering += "x1","x3","x5","x7","x2","x6","x4";
+//  ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4);
 //  GaussianFactorGraph smoother = createSmoother(7, ordering).first;
 //
 //  // Create the Bayes tree
@@ -288,9 +291,9 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
 //
 //	// Check the clique marginal P(C3)
 //	double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
-//	GaussianBayesNet expected = simpleGaussian(ordering["x2"],zero(2),sigmax2_alt);
-//	push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2));
-//	GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]];
+//	GaussianBayesNet expected = simpleGaussian(ordering[kx(2)],zero(2),sigmax2_alt);
+//	push_front(expected,ordering[kx(1)], zero(2), eye(2)*sqrt(2), ordering[kx(2)], -eye(2)*sqrt(2)/2, ones(2));
+//	GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[kx(1)]];
 //	GaussianFactorGraph marginal = C3->marginal(R);
 //	GaussianVariableIndex varIndex(marginal);
 //	Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
@@ -308,7 +311,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint )
 {
 	// Create smoother with 7 nodes
 	Ordering ordering;
-	ordering += "x1","x3","x5","x7","x2","x6","x4";
+	ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4);
 	GaussianFactorGraph smoother = createSmoother(7, ordering).first;
 
 	// Create the Bayes tree, expected to look like:
@@ -327,41 +330,41 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint )
 	GaussianBayesNet expected1;
 	// Why does the sign get flipped on the prior?
 	GaussianConditional::shared_ptr
-		parent1(new GaussianConditional(ordering["x7"], zero(2), -1*I/sigmax7, ones(2)));
+		parent1(new GaussianConditional(ordering[kx(7)], zero(2), -1*I/sigmax7, ones(2)));
 	expected1.push_front(parent1);
-	push_front(expected1,ordering["x1"], zero(2), I/sigmax7, ordering["x7"], A/sigmax7, sigma);
-	GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x7"]);
+	push_front(expected1,ordering[kx(1)], zero(2), I/sigmax7, ordering[kx(7)], A/sigmax7, sigma);
+	GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(7)]);
 	EXPECT(assert_equal(expected1,actual1,tol));
 
 //	// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
 //	GaussianBayesNet expected2;
 //	GaussianConditional::shared_ptr
-//			parent2(new GaussianConditional(ordering["x1"], zero(2), -1*I/sigmax1, ones(2)));
+//			parent2(new GaussianConditional(ordering[kx(1)], zero(2), -1*I/sigmax1, ones(2)));
 //		expected2.push_front(parent2);
-//	push_front(expected2,ordering["x7"], zero(2), I/sigmax1, ordering["x1"], A/sigmax1, sigma);
-//	GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering["x7"],ordering["x1"]);
+//	push_front(expected2,ordering[kx(7)], zero(2), I/sigmax1, ordering[kx(1)], A/sigmax1, sigma);
+//	GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[kx(7)],ordering[kx(1)]);
 //	EXPECT(assert_equal(expected2,actual2,tol));
 
 	// Check the joint density P(x1,x4), i.e. with a root variable
 	GaussianBayesNet expected3;
 	GaussianConditional::shared_ptr
-			parent3(new GaussianConditional(ordering["x4"], zero(2), I/sigmax4, ones(2)));
+			parent3(new GaussianConditional(ordering[kx(4)], zero(2), I/sigmax4, ones(2)));
 		expected3.push_front(parent3);
 	double sig14 = 0.784465;
 	Matrix A14 = -0.0769231*I;
-	push_front(expected3,ordering["x1"], zero(2), I/sig14, ordering["x4"], A14/sig14, sigma);
-	GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x4"]);
+	push_front(expected3,ordering[kx(1)], zero(2), I/sig14, ordering[kx(4)], A14/sig14, sigma);
+	GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(4)]);
 	EXPECT(assert_equal(expected3,actual3,tol));
 
 //	// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
 //	GaussianBayesNet expected4;
 //	GaussianConditional::shared_ptr
-//			parent4(new GaussianConditional(ordering["x1"], zero(2), -1.0*I/sigmax1, ones(2)));
+//			parent4(new GaussianConditional(ordering[kx(1)], zero(2), -1.0*I/sigmax1, ones(2)));
 //		expected4.push_front(parent4);
 //	double sig41 = 0.668096;
 //	Matrix A41 = -0.055794*I;
-//	push_front(expected4,ordering["x4"], zero(2), I/sig41, ordering["x1"], A41/sig41, sigma);
-//	GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering["x4"],ordering["x1"]);
+//	push_front(expected4,ordering[kx(4)], zero(2), I/sig41, ordering[kx(1)], A41/sig41, sigma);
+//	GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[kx(4)],ordering[kx(1)]);
 //	EXPECT(assert_equal(expected4,actual4,tol));
 }
 
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index b0a955484..443bc41fe 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -788,7 +788,7 @@ TEST(ISAM2, constrained_ordering)
   planarSLAM::Graph fullgraph;
 
   // We will constrain x3 and x4 to the end
-  FastSet<Symbol> constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4));
+  FastSet<Key> constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4));
 
   // i keeps track of the time step
   size_t i = 0;
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index 19b0c676d..a00afae11 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -25,7 +25,7 @@
 #include <boost/assign/std/vector.hpp>
 using namespace boost::assign;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/debug.h>
@@ -43,6 +43,9 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 /* ************************************************************************* *
  Bayes tree for smoother with "nested dissection" ordering:
 	 C1		 x5 x6 x4
@@ -53,20 +56,20 @@ using namespace example;
 TEST( GaussianJunctionTree, constructor2 )
 {
 	// create a graph
-  Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
+  Ordering ordering; ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4);
   GaussianFactorGraph fg = createSmoother(7, ordering).first;
 
 	// create an ordering
 	GaussianJunctionTree actual(fg);
 
-	vector<Index> frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"];
-	vector<Index> frontal2; frontal2 += ordering["x3"], ordering["x2"];
-	vector<Index> frontal3; frontal3 += ordering["x1"];
-	vector<Index> frontal4; frontal4 += ordering["x7"];
+	vector<Index> frontal1; frontal1 += ordering[kx(5)], ordering[kx(6)], ordering[kx(4)];
+	vector<Index> frontal2; frontal2 += ordering[kx(3)], ordering[kx(2)];
+	vector<Index> frontal3; frontal3 += ordering[kx(1)];
+	vector<Index> frontal4; frontal4 += ordering[kx(7)];
 	vector<Index> sep1;
-	vector<Index> sep2; sep2 += ordering["x4"];
-	vector<Index> sep3; sep3 += ordering["x2"];
-	vector<Index> sep4; sep4 += ordering["x6"];
+	vector<Index> sep2; sep2 += ordering[kx(4)];
+	vector<Index> sep3; sep3 += ordering[kx(2)];
+	vector<Index> sep4; sep4 += ordering[kx(6)];
 	EXPECT(assert_equal(frontal1, actual.root()->frontal));
 	EXPECT(assert_equal(sep1,     actual.root()->separator));
 	LONGS_EQUAL(5,               actual.root()->size());
@@ -111,7 +114,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
 	// create a graph
 	example::Graph nlfg = createNonlinearFactorGraph();
 	Values noisy = createNoisyValues();
-  Ordering ordering; ordering += "x1","x2","l1";
+  Ordering ordering; ordering += kx(1),kx(2),kl(1);
 	GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering);
 
 	// optimize the graph
@@ -203,7 +206,7 @@ TEST(GaussianJunctionTree, simpleMarginal) {
   init.insert(pose2SLAM::PoseKey(1), Pose2(1.0, 0.0, 0.0));
 
   Ordering ordering;
-  ordering += "x1", "x0";
+  ordering += kx(1), kx(0);
 
   GaussianFactorGraph gfg = *fg.linearize(init, ordering);
 
diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp
index 2ac2ce691..9131d801d 100644
--- a/tests/testGraph.cpp
+++ b/tests/testGraph.cpp
@@ -34,26 +34,28 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
+Key kx(size_t i) { return Symbol('x',i); }
+
 /* ************************************************************************* */
 // x1 -> x2
 //		-> x3 -> x4
 //    -> x5
 TEST ( Ordering, predecessorMap2Keys ) {
-	PredecessorMap<Symbol> p_map;
-	p_map.insert("x1","x1");
-	p_map.insert("x2","x1");
-	p_map.insert("x3","x1");
-	p_map.insert("x4","x3");
-	p_map.insert("x5","x1");
+	PredecessorMap<Key> p_map;
+	p_map.insert(kx(1),kx(1));
+	p_map.insert(kx(2),kx(1));
+	p_map.insert(kx(3),kx(1));
+	p_map.insert(kx(4),kx(3));
+	p_map.insert(kx(5),kx(1));
 
-	list<Symbol> expected;
-	expected += "x4","x5","x3","x2","x1";//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
+	list<Key> expected;
+	expected += kx(4),kx(5),kx(3),kx(2),kx(1);//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
 
-	list<Symbol> actual = predecessorMap2Keys<Symbol>(p_map);
+	list<Key> actual = predecessorMap2Keys<Key>(p_map);
 	LONGS_EQUAL(expected.size(), actual.size());
 
-	list<Symbol>::const_iterator it1 = expected.begin();
-	list<Symbol>::const_iterator it2 = actual.begin();
+	list<Key>::const_iterator it1 = expected.begin();
+	list<Key>::const_iterator it2 = actual.begin();
 	for(; it1!=expected.end(); it1++, it2++)
 		CHECK(*it1 == *it2)
 }
@@ -62,18 +64,18 @@ TEST ( Ordering, predecessorMap2Keys ) {
 TEST( Graph, predecessorMap2Graph )
 {
 	typedef SGraph<string>::Vertex SVertex;
-	SGraph<string> graph;
+	SGraph<Key> graph;
 	SVertex root;
-	map<string, SVertex> key2vertex;
+	map<Key, SVertex> key2vertex;
 
-	PredecessorMap<string> p_map;
-	p_map.insert("x1", "x2");
-	p_map.insert("x2", "x2");
-	p_map.insert("x3", "x2");
-	tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<string>, SVertex, string>(p_map);
+	PredecessorMap<Key> p_map;
+	p_map.insert(kx(1), kx(2));
+	p_map.insert(kx(2), kx(2));
+	p_map.insert(kx(3), kx(2));
+	tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
 
 	LONGS_EQUAL(3, boost::num_vertices(graph));
-	CHECK(root == key2vertex["x2"]);
+	CHECK(root == key2vertex[kx(2)]);
 }
 
 /* ************************************************************************* */
@@ -87,7 +89,7 @@ TEST( Graph, composePoses )
 	graph.addOdometry(2,3, p23, cov);
 	graph.addOdometry(4,3, p43, cov);
 
-	PredecessorMap<Symbol> tree;
+	PredecessorMap<Key> tree;
 	tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2));
 	tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2));
 	tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2));
@@ -96,7 +98,7 @@ TEST( Graph, composePoses )
 	Pose2 rootPose = p2;
 
 	boost::shared_ptr<Values> actual = composePoses<pose2SLAM::Graph, pose2SLAM::Odometry,
-			Pose2, Symbol> (graph, tree, rootPose);
+			Pose2, Key> (graph, tree, rootPose);
 
 	Values expected;
 	expected.insert(pose2SLAM::PoseKey(1), p1);
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 5b4050997..a12857ed5 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -25,7 +25,7 @@
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 2
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like PoseKey(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
@@ -211,7 +211,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
 	// create expected
 	Ordering ord(*config.orderingArbitrary());
 	Vector b = Vector_(2, 0., -3.);
-	JacobianFactor expected(ord["x1"], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
+	JacobianFactor expected(ord[PoseKey(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
 	CHECK(assert_equal((const GaussianFactor&)expected, *actual));
 }
 
@@ -233,15 +233,15 @@ TEST( NonlinearFactor, linearize_constraint2 )
 	Ordering ord(*config.orderingArbitrary());
 	Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
 	Vector b = Vector_(2, -15., -3.);
-	JacobianFactor expected(ord["x1"], -1*A, ord["l1"], A, b, constraint);
+	JacobianFactor expected(ord[PoseKey(1)], -1*A, ord[PointKey(1)], A, b, constraint);
 	CHECK(assert_equal((const GaussianFactor&)expected, *actual));
 }
 
 /* ************************************************************************* */
 class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> {
 public:
-  typedef NoiseModeFactor4<LieVector, LieVector, LieVector, LieVector> Base;
-  TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {}
+  typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base;
+  TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4)) {}
 
   virtual Vector
     evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@@ -263,13 +263,13 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor4) {
   TestFactor4 tf;
   Values tv;
-  tv.insert("x1", LieVector(1, 1.0));
-  tv.insert("x2", LieVector(1, 2.0));
-  tv.insert("x3", LieVector(1, 3.0));
-  tv.insert("x4", LieVector(1, 4.0));
+  tv.insert(PoseKey(1), LieVector(1, 1.0));
+  tv.insert(PoseKey(2), LieVector(1, 2.0));
+  tv.insert(PoseKey(3), LieVector(1, 3.0));
+  tv.insert(PoseKey(4), LieVector(1, 4.0));
   EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
-  Ordering ordering; ordering += "x1", "x2", "x3", "x4";
+  Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
   LONGS_EQUAL(jf.keys()[0], 0);
   LONGS_EQUAL(jf.keys()[1], 1);
@@ -286,7 +286,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
 class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
   typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
-  TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5") {}
+  TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5)) {}
 
   virtual Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
@@ -310,14 +310,14 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor5) {
   TestFactor5 tf;
   Values tv;
-  tv.insert("x1", LieVector(1, 1.0));
-  tv.insert("x2", LieVector(1, 2.0));
-  tv.insert("x3", LieVector(1, 3.0));
-  tv.insert("x4", LieVector(1, 4.0));
-  tv.insert("x5", LieVector(1, 5.0));
+  tv.insert(PoseKey(1), LieVector(1, 1.0));
+  tv.insert(PoseKey(2), LieVector(1, 2.0));
+  tv.insert(PoseKey(3), LieVector(1, 3.0));
+  tv.insert(PoseKey(4), LieVector(1, 4.0));
+  tv.insert(PoseKey(5), LieVector(1, 5.0));
   EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
-  Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5";
+  Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
   LONGS_EQUAL(jf.keys()[0], 0);
   LONGS_EQUAL(jf.keys()[1], 1);
@@ -336,7 +336,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
 class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
 public:
   typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
-  TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5", "x6") {}
+  TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6)) {}
 
   virtual Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
@@ -362,15 +362,15 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor6) {
   TestFactor6 tf;
   Values tv;
-  tv.insert("x1", LieVector(1, 1.0));
-  tv.insert("x2", LieVector(1, 2.0));
-  tv.insert("x3", LieVector(1, 3.0));
-  tv.insert("x4", LieVector(1, 4.0));
-  tv.insert("x5", LieVector(1, 5.0));
-  tv.insert("x6", LieVector(1, 6.0));
+  tv.insert(PoseKey(1), LieVector(1, 1.0));
+  tv.insert(PoseKey(2), LieVector(1, 2.0));
+  tv.insert(PoseKey(3), LieVector(1, 3.0));
+  tv.insert(PoseKey(4), LieVector(1, 4.0));
+  tv.insert(PoseKey(5), LieVector(1, 5.0));
+  tv.insert(PoseKey(6), LieVector(1, 6.0));
   EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
-  Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5", "x6";
+  Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
   LONGS_EQUAL(jf.keys()[0], 0);
   LONGS_EQUAL(jf.keys()[1], 1);
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index da8d8a114..08d50afb6 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -39,6 +39,9 @@ using namespace boost::assign;
 using namespace gtsam;
 using namespace example;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 /* ************************************************************************* */
 TEST( Graph, equals )
 {
@@ -67,16 +70,16 @@ TEST( Graph, keys )
 	set<Key> actual = fg.keys();
 	LONGS_EQUAL(3, actual.size());
 	set<Key>::const_iterator it = actual.begin();
-	CHECK(assert_equal(Symbol('l', 1), *(it++)));
-	CHECK(assert_equal(Symbol('x', 1), *(it++)));
-	CHECK(assert_equal(Symbol('x', 2), *(it++)));
+	LONGS_EQUAL(kl(1), *(it++));
+	LONGS_EQUAL(kx(1), *(it++));
+	LONGS_EQUAL(kx(2), *(it++));
 }
 
 /* ************************************************************************* */
 TEST( Graph, GET_ORDERING)
 {
 //  Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1
-  Ordering expected; expected += "l1","x2","x1"; // For starting with l1,x1,x2
+  Ordering expected; expected += kl(1), kx(2), kx(1); // For starting with l1,x1,x2
   Graph nlfg = createNonlinearFactorGraph();
   SymbolicFactorGraph::shared_ptr symbolic;
   Ordering::shared_ptr ordering;
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index f7a09f95f..41db76e1e 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -26,7 +26,7 @@ using namespace boost::assign;
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Matrix.h>
@@ -44,6 +44,9 @@ using namespace gtsam;
 
 const double tol = 1e-5;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 typedef NonlinearOptimizer<example::Graph> Optimizer;
 
 /* ************************************************************************* */
@@ -55,20 +58,20 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
 
 	// Expected values structure is the difference between the noisy config
 	// and the ground-truth config. One step only because it's linear !
-  Ordering ord1; ord1 += "x2","l1","x1";
+  Ordering ord1; ord1 += kx(2),kl(1),kx(1);
 	VectorValues expected(initial->dims(ord1));
 	Vector dl1(2);
 	dl1(0) = -0.1;
 	dl1(1) = 0.1;
-	expected[ord1["l1"]] = dl1;
+	expected[ord1[kl(1)]] = dl1;
 	Vector dx1(2);
 	dx1(0) = -0.1;
 	dx1(1) = -0.1;
-	expected[ord1["x1"]] = dx1;
+	expected[ord1[kx(1)]] = dx1;
 	Vector dx2(2);
 	dx2(0) = 0.1;
 	dx2(1) = -0.2;
-	expected[ord1["x2"]] = dx2;
+	expected[ord1[kx(2)]] = dx2;
 
 	// Check one ordering
 	Optimizer optimizer1(fg, initial, Optimizer::shared_ordering(new Ordering(ord1)));
@@ -78,7 +81,7 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
 
 // SL-FIX	// Check another
 //	shared_ptr<Ordering> ord2(new Ordering());
-//	*ord2 += "x1","x2","l1";
+//	*ord2 += kx(1),kx(2),kl(1);
 //	solver = Optimizer::shared_solver(new Optimizer::solver(ord2));
 //	Optimizer optimizer2(fg, initial, solver);
 //
@@ -87,7 +90,7 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
 //
 //	// And yet another...
 //	shared_ptr<Ordering> ord3(new Ordering());
-//	*ord3 += "l1","x1","x2";
+//	*ord3 += kl(1),kx(1),kx(2);
 //	solver = Optimizer::shared_solver(new Optimizer::solver(ord3));
 //	Optimizer optimizer3(fg, initial, solver);
 //
@@ -96,7 +99,7 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
 //
 //	// More...
 //	shared_ptr<Ordering> ord4(new Ordering());
-//	*ord4 += "x1","x2", "l1";
+//	*ord4 += kx(1),kx(2), kl(1);
 //	solver = Optimizer::shared_solver(new Optimizer::solver(ord4));
 //	Optimizer optimizer4(fg, initial, solver);
 //
@@ -118,7 +121,7 @@ TEST( NonlinearOptimizer, iterateLM )
 
 	// ordering
 	shared_ptr<Ordering> ord(new Ordering());
-	ord->push_back("x1");
+	ord->push_back(kx(1));
 
 	// create initial optimization state, with lambda=0
 	Optimizer optimizer(fg, config, ord, NonlinearOptimizationParameters::newLambda(0.));
@@ -161,7 +164,7 @@ TEST( NonlinearOptimizer, optimize )
 
 	// optimize parameters
 	shared_ptr<Ordering> ord(new Ordering());
-	ord->push_back("x1");
+	ord->push_back(kx(1));
 
 	// initial optimization state is the same in both cases tested
 	boost::shared_ptr<NonlinearOptimizationParameters> params = boost::make_shared<NonlinearOptimizationParameters>();
@@ -299,7 +302,7 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) {
 
   // optimize parameters
   shared_ptr<Ordering> ord(new Ordering());
-  ord->push_back("x1");
+  ord->push_back(kx(1));
 
   // initial optimization state is the same in both cases tested
   boost::shared_ptr<NonlinearOptimizationParameters> params = boost::make_shared<NonlinearOptimizationParameters>();
@@ -367,7 +370,7 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) {
 //
 //	// Check one ordering
 //	shared_ptr<Ordering> ord1(new Ordering());
-//	*ord1 += "x2","l1","x1";
+//	*ord1 += kx(2),kl(1),kx(1);
 //	solver = Optimizer::shared_solver(new Optimizer::solver(ord1));
 //	Optimizer optimizer1(fg, initial, solver);
 //
diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp
index efa794a25..adddfab34 100644
--- a/tests/testSymbolicBayesNet.cpp
+++ b/tests/testSymbolicBayesNet.cpp
@@ -21,7 +21,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/base/Testable.h>
@@ -34,6 +34,9 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 //Symbol _B_('B', 0), _L_('L', 0);
 //IndexConditional::shared_ptr
 //	B(new IndexConditional(_B_)),
@@ -42,12 +45,12 @@ using namespace example;
 /* ************************************************************************* */
 TEST( SymbolicBayesNet, constructor )
 {
-  Ordering o; o += "x2","l1","x1";
+  Ordering o; o += kx(2),kl(1),kx(1);
 	// Create manually
 	IndexConditional::shared_ptr
-		x2(new IndexConditional(o["x2"],o["l1"], o["x1"])),
-		l1(new IndexConditional(o["l1"],o["x1"])),
-		x1(new IndexConditional(o["x1"]));
+		x2(new IndexConditional(o[kx(2)],o[kl(1)], o[kx(1)])),
+		l1(new IndexConditional(o[kl(1)],o[kx(1)])),
+		x1(new IndexConditional(o[kx(1)]));
 	BayesNet<IndexConditional> expected;
 	expected.push_back(x2);
 	expected.push_back(l1);
diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp
index 4295a5263..596a715cb 100644
--- a/tests/testSymbolicFactorGraph.cpp
+++ b/tests/testSymbolicFactorGraph.cpp
@@ -20,7 +20,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
+// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/slam/smallExample.h>
@@ -33,16 +33,19 @@ using namespace std;
 using namespace gtsam;
 using namespace example;
 
+Key kx(size_t i) { return Symbol('x',i); }
+Key kl(size_t i) { return Symbol('l',i); }
+
 /* ************************************************************************* */
 TEST( SymbolicFactorGraph, symbolicFactorGraph )
 {
-  Ordering o; o += "x1","l1","x2";
+  Ordering o; o += kx(1),kl(1),kx(2);
 	// construct expected symbolic graph
 	SymbolicFactorGraph expected;
-	expected.push_factor(o["x1"]);
-	expected.push_factor(o["x1"],o["x2"]);
-	expected.push_factor(o["x1"],o["l1"]);
-	expected.push_factor(o["x2"],o["l1"]);
+	expected.push_factor(o[kx(1)]);
+	expected.push_factor(o[kx(1)],o[kx(2)]);
+	expected.push_factor(o[kx(1)],o[kl(1)]);
+	expected.push_factor(o[kx(2)],o[kl(1)]);
 
 	// construct it from the factor graph
 	GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
@@ -59,7 +62,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
 //	SymbolicFactorGraph actual(factorGraph);
 //  SymbolicFactor::shared_ptr f1 = actual[0];
 //  SymbolicFactor::shared_ptr f3 = actual[2];
-//	actual.findAndRemoveFactors("x2");
+//	actual.findAndRemoveFactors(kx(2));
 //
 //	// construct expected graph after find_factors_and_remove
 //	SymbolicFactorGraph expected;
@@ -79,13 +82,13 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
 //	SymbolicFactorGraph fg(factorGraph);
 //
 //	// ask for all factor indices connected to x1
-//	list<size_t> x1_factors = fg.factors("x1");
+//	list<size_t> x1_factors = fg.factors(kx(1));
 //	int x1_indices[] = { 0, 1, 2 };
 //	list<size_t> x1_expected(x1_indices, x1_indices + 3);
 //	CHECK(x1_factors==x1_expected);
 //
 //	// ask for all factor indices connected to x2
-//	list<size_t> x2_factors = fg.factors("x2");
+//	list<size_t> x2_factors = fg.factors(kx(2));
 //	int x2_indices[] = { 1, 3 };
 //	list<size_t> x2_expected(x2_indices, x2_indices + 2);
 //	CHECK(x2_factors==x2_expected);
@@ -99,26 +102,26 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
 //	SymbolicFactorGraph fg(factorGraph);
 //
 //  // combine all factors connected to x1
-//  SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
+//  SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1));
 //
 //  // check result
-//  SymbolicFactor expected("l1","x1","x2");
+//  SymbolicFactor expected(kl(1),kx(1),kx(2));
 //  CHECK(assert_equal(expected,*actual));
 //}
 
 ///* ************************************************************************* */
 //TEST( SymbolicFactorGraph, eliminateOne )
 //{
-//  Ordering o; o += "x1","l1","x2";
+//  Ordering o; o += kx(1),kl(1),kx(2);
 //	// create a test graph
 //	GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
 //	SymbolicFactorGraph fg(factorGraph);
 //
 //	// eliminate
-//	IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o["x1"]+1);
+//	IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[kx(1)]+1);
 //
 //  // create expected symbolic IndexConditional
-//  IndexConditional expected(o["x1"],o["l1"],o["x2"]);
+//  IndexConditional expected(o[kx(1)],o[kl(1)],o[kx(2)]);
 //
 //  CHECK(assert_equal(expected,*actual));
 //}
@@ -126,12 +129,12 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
 /* ************************************************************************* */
 TEST( SymbolicFactorGraph, eliminate )
 {
-  Ordering o; o += "x2","l1","x1";
+  Ordering o; o += kx(2),kl(1),kx(1);
 
   // create expected Chordal bayes Net
-  IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
-  IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
-  IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
+  IndexConditional::shared_ptr x2(new IndexConditional(o[kx(2)], o[kl(1)], o[kx(1)]));
+  IndexConditional::shared_ptr l1(new IndexConditional(o[kl(1)], o[kx(1)]));
+  IndexConditional::shared_ptr x1(new IndexConditional(o[kx(1)]));
 
   SymbolicBayesNet expected;
   expected.push_back(x2);

From 2213391b4ce3e7b65547d0254acfc9c262c3d6e3 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 21 Feb 2012 22:18:36 +0000
Subject: [PATCH 78/88] Added missing const that was causing method hiding

---
 gtsam/slam/simulated3D.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h
index a386736a1..7b4dfce37 100644
--- a/gtsam/slam/simulated3D.h
+++ b/gtsam/slam/simulated3D.h
@@ -86,7 +86,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
 	 * @return Vector error between prior value and x (Dimension: 3)
 	 */
 	Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
-			boost::none) {
+			boost::none) const {
 		return (prior(x, H) - measured_).vector();
 	}
 };
@@ -118,7 +118,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
 	 * @return vector error between measurement and prediction (Dimension: 3)
 	 */
 	Vector evaluateError(const Point3& x1, const Point3& x2,
-	    boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
+	    boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
 		return (mea(x1, x2, H1, H2) - measured_).vector();
 	}
 };

From cebd4631d292c5b4e8edc165c1daa805b1909f1f Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 21 Feb 2012 22:18:37 +0000
Subject: [PATCH 79/88] Added missing argument that was causing method hiding

---
 tests/testExtendedKalmanFilter.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index b38e7e3fe..54761ddc9 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -153,10 +153,10 @@ public:
   }
 
   /* print */
-  virtual void print(const std::string& s = "") const {
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << s << ": NonlinearMotionModel\n";
-    std::cout << "  TestKey1: " << DefaultKeyFormatter(key1()) << std::endl;
-    std::cout << "  TestKey2: " << DefaultKeyFormatter(key2()) << std::endl;
+    std::cout << "  TestKey1: " << keyFormatter(key1()) << std::endl;
+    std::cout << "  TestKey2: " << keyFormatter(key2()) << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
@@ -291,9 +291,9 @@ public:
   }
 
   /* print */
-  virtual void print(const std::string& s = "") const {
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << s << ": NonlinearMeasurementModel\n";
-    std::cout << "  TestKey: " << DefaultKeyFormatter(key()) << std::endl;
+    std::cout << "  TestKey: " << keyFormatter(key()) << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */

From 29d787a0adf0f5fb346d24cd6d5bd1d2ff2581d0 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 21 Feb 2012 22:18:39 +0000
Subject: [PATCH 80/88] Added Values filtering by key test and Value type

---
 gtsam/nonlinear/Values.h             | 118 +++++++++++++++++++++++++--
 gtsam/nonlinear/tests/testValues.cpp |  42 ++++++++++
 2 files changed, 153 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 659779858..08bf9edca 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -30,6 +30,7 @@
 #include <boost/pool/pool_alloc.hpp>
 #include <boost/ptr_container/ptr_map.hpp>
 #include <boost/iterator/transform_iterator.hpp>
+#include <boost/iterator/filter_iterator.hpp>
 #include <boost/function.hpp>
 #include <boost/bind.hpp>
 #include <boost/ptr_container/serialize_ptr_map.hpp>
@@ -81,13 +82,6 @@ namespace gtsam {
     /// A shared_ptr to this class
     typedef boost::shared_ptr<Values> shared_ptr;
 
-    /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
-    struct ConstKeyValuePair {
-      const Key first;
-      const Value& second;
-      ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {}
-    };
-
     /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
     struct KeyValuePair {
       const Key first;
@@ -95,6 +89,14 @@ namespace gtsam {
       KeyValuePair(Key key, Value& value) : first(key), second(value) {}
     };
 
+    /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
+    struct ConstKeyValuePair {
+      const Key first;
+      const Value& second;
+      ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {}
+      ConstKeyValuePair(const KeyValuePair& key_value) : first(key_value.first), second(key_value.second) {}
+    };
+
     /// Mutable forward iterator, with value type KeyValuePair
     typedef boost::transform_iterator<
         boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
@@ -111,6 +113,28 @@ namespace gtsam {
     typedef boost::transform_iterator<
         boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
 
+    /// Mutable filtered iterator
+    typedef boost::filter_iterator<boost::function<bool(const KeyValuePair&)>, iterator> filter_iterator;
+
+    /// Const filtered iterator
+    typedef boost::filter_iterator<boost::function<bool(const ConstKeyValuePair&)>, const_iterator> const_filter_iterator;
+
+    /// Mutable type-filtered iterator
+    template<class ValueType>
+    struct type_filter_iterator {
+      typedef boost::transform_iterator<
+          std::pair<const Key, ValueType&>(*)(KeyValuePair),
+          boost::filter_iterator<bool(*)(const KeyValuePair&), iterator> > type;
+    };
+
+    /// Const type-filtered iterator
+    template<class ValueType>
+    struct const_type_filter_iterator {
+      typedef boost::transform_iterator<
+          std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair),
+          boost::filter_iterator<bool(*)(const ConstKeyValuePair&), const_iterator> > type;
+    };
+
     /** Default constructor creates an empty Values class */
     Values() {}
 
@@ -191,6 +215,86 @@ namespace gtsam {
     reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
     reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
 
+    /** Iterator over a subset of the Key-Value pairs, for which the given
+     * \c filter function returns true when evaluated on the Key. */
+    const_filter_iterator beginFilterByKey(const boost::function<bool(Key)>& filter) const {
+      return boost::make_filter_iterator(
+          boost::function<bool(const ConstKeyValuePair&)>(boost::bind(filter, boost::bind(&ConstKeyValuePair::first, _1))),
+          begin(), end()); }
+
+    /** Iterator over a subset of the Key-Value pairs, for which the given
+     * \c filter function returns true when evaluated on the Key. */
+    const_filter_iterator endFilterByKey(const boost::function<bool(Key)>& filter) const {
+      return boost::make_filter_iterator(
+          boost::function<bool(const ConstKeyValuePair&)>(boost::bind(filter, boost::bind(&ConstKeyValuePair::first, _1))),
+          end(), end()); }
+
+    /** Iterator over a subset of the Key-Value pairs, for which the given
+     * \c filter function returns true when evaluated on the Key. */
+    filter_iterator beginFilterByKey(const boost::function<bool(Key)>& filter) {
+      return boost::make_filter_iterator(
+          boost::function<bool(const KeyValuePair&)>(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))),
+          begin(), end()); }
+
+    /** Iterator over a subset of the Key-Value pairs, for which the given
+     * \c filter function returns true when evaluated on the Key. */
+    filter_iterator endFilterByKey(const boost::function<bool(Key)>& filter) {
+      return boost::make_filter_iterator(
+          boost::function<bool(const KeyValuePair&)>(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))),
+          end(), end()); }
+
+  private:
+    template<class ValueType, class KeyValue>
+    static bool _typeFilterHelper(const KeyValue& key_value) {
+      return typeid(ValueType) == typeid(key_value.second);
+    }
+
+    template<class ValueType>
+    static std::pair<const Key, const ValueType&> _typeCastHelperConst(ConstKeyValuePair key_value) {
+      return std::make_pair<const Key, const ValueType&>(key_value.first, static_cast<const ValueType&>(key_value.second)); }
+
+    template<class ValueType>
+    static std::pair<const Key, ValueType&> _typeCastHelper(KeyValuePair key_value) {
+      return std::make_pair<const Key, ValueType&>(key_value.first, static_cast<ValueType&>(key_value.second)); }
+
+  public:
+
+    /** Iterator over a subset of the Key-Value pairs, for which the Value type
+     * matches the template parameter \c ValueType.
+     */
+    template<class ValueType>
+    typename const_type_filter_iterator<ValueType>::type beginFilterByType() const {
+      return boost::make_transform_iterator(
+          boost::filter_iterator<bool(*)(ConstKeyValuePair), const_iterator>(&_typeFilterHelper<ValueType,ConstKeyValuePair>, begin(), end()),
+          &_typeCastHelperConst<ValueType>); }
+
+    /** Iterator over a subset of the Key-Value pairs, for which the Value type
+     * matches the template parameter \c ValueType.
+     */
+    template<class ValueType>
+    typename const_type_filter_iterator<ValueType>::type endFilterByType() const {
+      return boost::make_transform_iterator(
+          boost::filter_iterator<bool(*)(ConstKeyValuePair), const_iterator>(&_typeFilterHelper<ValueType,ConstKeyValuePair>, end(), end()),
+          &_typeCastHelperConst<ValueType>); }
+
+    /** Iterator over a subset of the Key-Value pairs, for which the Value type
+     * matches the template parameter \c ValueType.
+     */
+    template<class ValueType>
+    typename type_filter_iterator<ValueType>::type beginFilterByType() {
+      return boost::make_transform_iterator(
+          boost::make_filter_iterator(&_typeFilterHelper<ValueType,KeyValuePair>, begin(), end()),
+          &_typeCastHelper<ValueType>); }
+
+    /** Iterator over a subset of the Key-Value pairs, for which the Value type
+     * matches the template parameter \c ValueType.
+     */
+    template<class ValueType>
+    typename type_filter_iterator<ValueType>::type endFilterByType() {
+      return boost::make_transform_iterator(
+          boost::make_filter_iterator(&_typeFilterHelper<ValueType,KeyValuePair>, end(), end()),
+          &_typeCastHelper<ValueType>); }
+
     /// @name Manifold Operations
     /// @{
 
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 1ea70e213..1e9752127 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -26,6 +26,7 @@ using namespace boost::assign;
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/LieVector.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
@@ -259,6 +260,47 @@ TEST(Values, update)
 	CHECK(assert_equal(expected,config0));
 }
 
+/* ************************************************************************* */
+TEST(Values, filter) {
+  Values values;
+  values.insert(0, Pose2());
+  values.insert(1, Pose3());
+  values.insert(2, Pose2());
+  values.insert(3, Pose3());
+
+  // Filter by key
+  int i = 0;
+  for(Values::filter_iterator it = values.beginFilterByKey(boost::bind(std::greater_equal<Key>(), _1, 2));
+      it != values.endFilterByKey(boost::bind(std::greater_equal<Key>(), _1, 2)); ++it, ++i) {
+    if(i == 0) {
+      LONGS_EQUAL(2, it->first);
+      EXPECT(typeid(Pose2) == typeid(it->second));
+    } else if(i == 1) {
+      LONGS_EQUAL(3, it->first);
+      EXPECT(typeid(Pose3) == typeid(it->second));
+    } else {
+      EXPECT(false);
+    }
+  }
+  LONGS_EQUAL(2, i);
+
+  // Filter by type
+  i = 0;
+  for(Values::type_filter_iterator<Pose3>::type it = values.beginFilterByType<Pose3>();
+      it != values.endFilterByType<Pose3>(); ++it, ++i) {
+    if(i == 0) {
+      LONGS_EQUAL(1, it->first);
+      EXPECT(assert_equal(Pose3(), it->second));
+    } else if(i == 1) {
+      LONGS_EQUAL(3, it->first);
+      EXPECT(assert_equal(Pose3(), it->second));
+    } else {
+      EXPECT(false);
+    }
+  }
+  LONGS_EQUAL(2, i);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From b9dd42d7afd230efcc6cc6883d5d6160f8fefb93 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 21 Feb 2012 22:18:40 +0000
Subject: [PATCH 81/88] Project file

---
 .cproject | 179 +++++++++++++++++++++++++++---------------------------
 1 file changed, 88 insertions(+), 91 deletions(-)

diff --git a/.cproject b/.cproject
index c47c94ecf..f2f172b9e 100644
--- a/.cproject
+++ b/.cproject
@@ -310,6 +310,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -336,7 +344,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -344,7 +351,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -392,7 +398,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -400,7 +405,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -408,7 +412,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -424,20 +427,11 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -518,15 +512,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -534,14 +520,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -582,7 +560,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -590,6 +576,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -622,14 +616,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -638,6 +624,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -888,7 +882,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1344,6 +1337,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1383,6 +1377,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1390,6 +1385,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1653,6 +1649,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1674,14 +1671,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1778,54 +1767,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
 				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSpirit.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testWrap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j1</buildArguments>
@@ -2193,6 +2142,54 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSpirit.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testWrap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>

From 565185da028736e4417b7fa43f885886e9ddc3b7 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 22 Feb 2012 15:58:06 +0000
Subject: [PATCH 82/88] Added cmake targets, expanded on the Values filtering
 test

---
 .cproject                            | 2191 +++++++++++++++-----------
 gtsam/nonlinear/tests/testValues.cpp |   19 +-
 2 files changed, 1251 insertions(+), 959 deletions(-)

diff --git a/.cproject b/.cproject
index f2f172b9e..e75e9b8e8 100644
--- a/.cproject
+++ b/.cproject
@@ -39,6 +39,7 @@
 									<listOptionValue builtIn="false" value="/usr/include/c++/4.6.1"/>
 									<listOptionValue builtIn="false" value="/usr/include/c++/4.6"/>
 									<listOptionValue builtIn="false" value="/usr/include/c++/4.6/backward"/>
+									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}&quot;"/>
 								</option>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
 							</tool>
@@ -286,7 +287,7 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 		<buildTargets>
-			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -294,15 +295,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -310,10 +303,162 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testTestableAssertions.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testBlockMatrices.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCholesky.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testLieConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>vSFMexample.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -344,6 +489,7 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -351,6 +497,7 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -398,6 +545,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -405,6 +553,7 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -412,6 +561,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -427,99 +577,12 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -560,15 +623,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -576,7 +631,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -616,15 +679,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>vSFMexample.run</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -632,10 +703,65 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMGraph</buildTarget>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -792,23 +918,79 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildTarget>testRot3.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>testRot2.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCal3_S2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSimpleCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testHomography2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -816,15 +998,502 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianConditional.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVectorValues.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testHessianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testKalmanFilter.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianDensity.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="cmake" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>..</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5 VERBOSE=1</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j1</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base.testMatrix.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>base.testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.base" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check.base</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.base" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.base</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>base</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base.testVector.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>base.testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="base.timeMatrix.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>base.timeMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.geometry" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.geometry</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.geometry" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.geometry</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>geometry</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testPoint2.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>geometry.testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testPose2.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>geometry.testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testPose3.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>geometry.testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="geometry.testHomography2.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j1 VERBOSE=1</buildArguments>
+				<buildTarget>geometry.testHomography2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.inference" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.inference</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.linear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.linear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.nonlinear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.nonlinear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.slam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2 VERBOSE=1</buildArguments>
+				<buildTarget>check.slam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="gtsam-static" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>gtsam-static</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="gtsam-shared" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>gtsam-shared</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.tests" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check.tests</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests.testSerialization.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests.testSerialization.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.inference" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.inference</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.linear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.linear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.nonlinear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.nonlinear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.slam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.slam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timing.tests" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timing.tests</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="inference" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>inference</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="linear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>linear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="nonlinear" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>nonlinear</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="slam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>slam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="examples" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>examples</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.wrap" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check.wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap_gtsam" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>wrap_gtsam</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check install j5" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check install" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap.testWrap.run" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>wrap.testWrap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>testGaussianFactor.run</buildTarget>
@@ -832,59 +1501,68 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeGaussianFactor.run</buildTarget>
+				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeVectorConfig.run</buildTarget>
+				<buildTarget>install</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorBTree.run</buildTarget>
+				<buildTarget>tests/testSpirit.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorMap.run</buildTarget>
+				<buildTarget>tests/testWrap.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNoiseModel.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildTarget>testErrors.run</buildTarget>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@@ -927,7 +1605,39 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testRot3M.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -935,7 +1645,79 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timePose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -943,63 +1725,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
+				<buildTarget>install</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -1007,7 +1741,30 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>..</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>SimpleRotation.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMGraph</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>testGaussianFactor.run</buildTarget>
@@ -1015,6 +1772,160 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianISAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testIterative.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearEquality.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearOptimizer.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSubgraphPreconditioner.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testTupleConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testInference.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testSymbolicBayesNet.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianJunctionTree" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianJunctionTree</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSerialization.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSerialization.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1111,23 +2022,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -1135,7 +2030,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1143,7 +2038,87 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeVectorConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testErrors.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -1151,7 +2126,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1159,7 +2134,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1167,23 +2158,31 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testStereoCamera.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testRot3M.run</buildTarget>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
+				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1191,70 +2190,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timePose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeStereoCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1337,7 +2272,6 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1377,7 +2311,6 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1385,7 +2318,6 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1399,166 +2331,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testTestableAssertions.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testBlockMatrices.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCholesky.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVectorValues.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNoiseModel.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testHessianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testKalmanFilter.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianDensity.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1639,119 +2411,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
+				<buildTarget>install</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>tests/testGaussianISAM2</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testLieConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCal3_S2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSimpleCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testHomography2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1759,430 +2427,39 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j1</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
+				<buildTarget>dist</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5 VERBOSE=1</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>base</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base.testMatrix.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>base.testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base.testVector.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>base.testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="base.timeMatrix.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>base.timeMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.base" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check.base</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.geometry" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.geometry</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.inference" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.inference</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.linear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.linear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.nonlinear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.nonlinear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.slam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2 VERBOSE=1</buildArguments>
-				<buildTarget>check.slam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.tests" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check.tests</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.wrap" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check.wrap</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check install j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="cmake" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cmake</buildCommand>
 				<buildArguments>..</buildArguments>
+				<buildTarget/>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="examples" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>examples</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>geometry</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testHomography2.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j1 VERBOSE=1</buildArguments>
-				<buildTarget>geometry.testHomography2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testPoint2.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>geometry.testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testPose2.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>geometry.testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="geometry.testPose3.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>geometry.testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="gtsam-shared" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>gtsam-shared</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="gtsam-static" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>gtsam-static</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="inference" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>inference</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="linear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>linear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="nonlinear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>nonlinear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="slam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>slam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests.testSerialization.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>tests.testSerialization.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.base" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.base</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.geometry" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.geometry</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.inference" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.inference</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.linear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.linear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.nonlinear" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.nonlinear</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.slam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.slam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timing.tests" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>timing.tests</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap_gtsam" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap_gtsam</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap.testWrap.run" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap.testWrap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSpirit.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testWrap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>all</buildTarget>
@@ -2190,6 +2467,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 1e9752127..fac527dec 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -262,11 +262,16 @@ TEST(Values, update)
 
 /* ************************************************************************* */
 TEST(Values, filter) {
+	Pose2 pose0(1.0, 2.0, 0.3);
+	Pose3 pose1(Pose2(0.1, 0.2, 0.3));
+	Pose2 pose2(4.0, 5.0, 0.6);
+	Pose3 pose3(Pose2(0.3, 0.7, 0.9));
+
   Values values;
-  values.insert(0, Pose2());
-  values.insert(1, Pose3());
-  values.insert(2, Pose2());
-  values.insert(3, Pose3());
+  values.insert(0, pose0);
+  values.insert(1, pose1);
+  values.insert(2, pose2);
+  values.insert(3, pose3);
 
   // Filter by key
   int i = 0;
@@ -275,9 +280,11 @@ TEST(Values, filter) {
     if(i == 0) {
       LONGS_EQUAL(2, it->first);
       EXPECT(typeid(Pose2) == typeid(it->second));
+      EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(it->second)));
     } else if(i == 1) {
       LONGS_EQUAL(3, it->first);
       EXPECT(typeid(Pose3) == typeid(it->second));
+      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(it->second)));
     } else {
       EXPECT(false);
     }
@@ -290,10 +297,10 @@ TEST(Values, filter) {
       it != values.endFilterByType<Pose3>(); ++it, ++i) {
     if(i == 0) {
       LONGS_EQUAL(1, it->first);
-      EXPECT(assert_equal(Pose3(), it->second));
+      EXPECT(assert_equal(pose1, it->second));
     } else if(i == 1) {
       LONGS_EQUAL(3, it->first);
-      EXPECT(assert_equal(Pose3(), it->second));
+      EXPECT(assert_equal(pose3, it->second));
     } else {
       EXPECT(false);
     }

From 1d0aaacbd69267315c3a30b7df5ffe43ae8e7a5d Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 22 Feb 2012 23:38:09 +0000
Subject: [PATCH 83/88] Removed use of GTSAM_MAGIC_KEY for constructing Symbols
 from strings

---
 examples/vSLAMexample/vISAMexample.cpp        |  3 --
 examples/vSLAMexample/vSFMexample.cpp         |  3 --
 gtsam/inference/tests/testFactorGraph.cpp     |  3 --
 gtsam/inference/tests/testISAM.cpp            |  3 --
 gtsam/nonlinear/Symbol.h                      | 34 -------------------
 gtsam/nonlinear/tests/testValues.cpp          |  4 +--
 gtsam/slam/smallExample.cpp                   | 26 +++++++-------
 gtsam/slam/tests/testGeneralSFMFactor.cpp     | 11 +++---
 .../testGeneralSFMFactor_Cal3Bundler.cpp      | 11 +++---
 gtsam/slam/tests/testPose2SLAM.cpp            |  5 +--
 gtsam/slam/tests/testPose3SLAM.cpp            |  5 +--
 gtsam/slam/tests/testProjectionFactor.cpp     |  5 +--
 gtsam/slam/tests/testSerializationSLAM.cpp    |  4 +--
 gtsam/slam/tests/testVSLAM.cpp                |  3 --
 tests/testBoundingConstraint.cpp              | 12 +++----
 tests/testExtendedKalmanFilter.cpp            |  4 +--
 tests/testGaussianBayesNet.cpp                |  3 --
 tests/testGaussianFactor.cpp                  |  5 +--
 tests/testGaussianFactorGraph.cpp             |  3 --
 tests/testGaussianISAM.cpp                    |  3 --
 tests/testGaussianISAM2.cpp                   |  2 --
 tests/testGaussianJunctionTree.cpp            |  3 --
 tests/testGraph.cpp                           |  2 --
 tests/testInference.cpp                       |  3 --
 tests/testNonlinearFactor.cpp                 |  3 --
 tests/testNonlinearFactorGraph.cpp            |  3 --
 tests/testNonlinearOptimizer.cpp              |  3 --
 tests/testSymbolicBayesNet.cpp                |  3 --
 tests/testSymbolicFactorGraph.cpp             |  3 --
 tests/testTupleValues.cpp                     |  3 --
 tests/timeGaussianFactorGraph.cpp             |  3 --
 wrap/matlab.h                                 |  2 --
 32 files changed, 33 insertions(+), 150 deletions(-)

diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index ae26a9c47..71d5516a2 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -20,9 +20,6 @@
 #include <boost/foreach.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/slam/visualSLAM.h>
diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp
index 468620f29..c7e0df05c 100644
--- a/examples/vSLAMexample/vSFMexample.cpp
+++ b/examples/vSLAMexample/vSFMexample.cpp
@@ -19,9 +19,6 @@
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/slam/visualSLAM.h>
diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp
index 641b1f4f9..8669329bb 100644
--- a/gtsam/inference/tests/testFactorGraph.cpp
+++ b/gtsam/inference/tests/testFactorGraph.cpp
@@ -25,9 +25,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/inference/SymbolicFactorGraph.h>
 
 using namespace std;
diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/inference/tests/testISAM.cpp
index 1c6a2897c..acb849b8b 100644
--- a/gtsam/inference/tests/testISAM.cpp
+++ b/gtsam/inference/tests/testISAM.cpp
@@ -21,9 +21,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/IndexConditional.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h
index 854e18203..79d6a049b 100644
--- a/gtsam/nonlinear/Symbol.h
+++ b/gtsam/nonlinear/Symbol.h
@@ -23,14 +23,9 @@
 #include <boost/mpl/char.hpp>
 #include <boost/format.hpp>
 #include <boost/serialization/nvp.hpp>
-#ifdef GTSAM_MAGIC_KEY
-#include <boost/lexical_cast.hpp>
-#endif
 
 #include <gtsam/nonlinear/Key.h>
 
-#define ALPHA '\224'
-
 namespace gtsam {
 
 /**
@@ -60,35 +55,6 @@ public:
     c_(c), j_(j) {
   }
 
-  /** "Magic" key casting constructor from string */
-#ifdef GTSAM_MAGIC_KEY
-  Symbol(const std::string& str) {
-    if(str.length() < 1)
-    throw std::invalid_argument("Cannot parse string key '" + str + "'");
-    else {
-      const char *c_str = str.c_str();
-      c_ = c_str[0];
-      if(str.length() > 1)
-      j_ = boost::lexical_cast<size_t>(c_str+1);
-      else
-      j_ = 0;
-    }
-  }
-
-  Symbol(const char *c_str) {
-    std::string str(c_str);
-    if(str.length() < 1)
-    throw std::invalid_argument("Cannot parse string key '" + str + "'");
-    else {
-      c_ = c_str[0];
-      if(str.length() > 1)
-      j_ = boost::lexical_cast<size_t>(c_str+1);
-      else
-      j_ = 0;
-    }
-  }
-#endif
-
   /** Constructor that decodes an integer Key */
   Symbol(Key key) {
     const size_t keyBytes = sizeof(Key);
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index fac527dec..55854b339 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -20,8 +20,6 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/LieVector.h>
@@ -33,7 +31,7 @@ using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
-Key key1(Symbol("v1")), key2(Symbol("v2")), key3(Symbol("v3")), key4(Symbol("v4"));
+Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4));
 
 /* ************************************************************************* */
 TEST( Values, equals1 )
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index f48d363b5..170f3ef03 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -24,9 +24,6 @@
 
 using namespace std;
 
-// Magically casts strings like Symbol("x3") to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Matrix.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/slam/smallExample.h>
@@ -52,6 +49,9 @@ namespace example {
 	static const Index _l1_=0, _x1_=1, _x2_=2;
 	static const Index _x_=0, _y_=1, _z_=2;
 
+	Key kx(size_t i) { return Symbol('x',i); }
+	Key kl(size_t i) { return Symbol('l',i); }
+
 	/* ************************************************************************* */
 	boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
 		// Create
@@ -121,18 +121,18 @@ namespace example {
 	/* ************************************************************************* */
 	VectorValues createCorrectDelta(const Ordering& ordering) {
 		VectorValues c(vector<size_t>(3,2));
-		c[ordering[Symbol(Symbol("l1"))]] = Vector_(2, -0.1, 0.1);
-		c[ordering[Symbol("x1")]] = Vector_(2, -0.1, -0.1);
-		c[ordering[Symbol("x2")]] = Vector_(2, 0.1, -0.2);
+		c[ordering[kl(1)]] = Vector_(2, -0.1, 0.1);
+		c[ordering[kx(1)]] = Vector_(2, -0.1, -0.1);
+		c[ordering[kx(2)]] = Vector_(2, 0.1, -0.2);
 		return c;
 	}
 
 	/* ************************************************************************* */
 	VectorValues createZeroDelta(const Ordering& ordering) {
 		VectorValues c(vector<size_t>(3,2));
-		c[ordering[Symbol(Symbol("l1"))]] = zero(2);
-		c[ordering[Symbol("x1")]] = zero(2);
-		c[ordering[Symbol("x2")]] = zero(2);
+		c[ordering[kl(1)]] = zero(2);
+		c[ordering[kx(1)]] = zero(2);
+		c[ordering[kx(2)]] = zero(2);
 		return c;
 	}
 
@@ -144,16 +144,16 @@ namespace example {
 		SharedDiagonal unit2 = noiseModel::Unit::Create(2);
 
 		// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
-		fg.add(ordering[Symbol("x1")], 10*eye(2), -1.0*ones(2), unit2);
+		fg.add(ordering[kx(1)], 10*eye(2), -1.0*ones(2), unit2);
 
 		// odometry between x1 and x2: x2-x1=[0.2;-0.1]
-		fg.add(ordering[Symbol("x1")], -10*eye(2),ordering[Symbol("x2")], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
+		fg.add(ordering[kx(1)], -10*eye(2),ordering[kx(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
 
     // measurement between x1 and l1: l1-x1=[0.0;0.2]
-		fg.add(ordering[Symbol("x1")], -5*eye(2), ordering[Symbol("l1")], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
+		fg.add(ordering[kx(1)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
 
 		// measurement between x2 and l1: l1-x2=[-0.2;0.3]
-		fg.add(ordering[Symbol("x2")], -5*eye(2), ordering[Symbol("l1")], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
+		fg.add(ordering[kx(2)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
 
 		return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
 	}
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 7833bce18..7a8dc4825 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -12,9 +12,6 @@
 #include <CppUnitLite/TestHarness.h>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
@@ -74,7 +71,7 @@ TEST( GeneralSFMFactor, equals )
 {
 	// Create two identical factors and make sure they're equal
 	Vector z = Vector_(2,323.,240.);
-  const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
+  const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
 	const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 	boost::shared_ptr<Projection>
 	  factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@@ -89,14 +86,14 @@ TEST( GeneralSFMFactor, equals )
 TEST( GeneralSFMFactor, error ) {
 	Point2 z(3.,0.);
 	const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
-	boost::shared_ptr<Projection>	factor(new Projection(z, sigma, Symbol("x1"), Symbol("l1")));
+	boost::shared_ptr<Projection>	factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1)));
 	// For the following configuration, the factor predicts 320,240
 	Values values;
 	Rot3 R;
 	Point3 t1(0,0,-6);
 	Pose3 x1(R,t1);
-	values.insert(Symbol("x1"), GeneralCamera(x1));
-	Point3 l1;  values.insert(Symbol("l1"), l1);
+	values.insert(Symbol('x',1), GeneralCamera(x1));
+	Point3 l1;  values.insert(Symbol('l',1), l1);
 	EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index 3f6d698d5..f762505a3 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -12,9 +12,6 @@
 #include <CppUnitLite/TestHarness.h>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
@@ -75,7 +72,7 @@ TEST( GeneralSFMFactor, equals )
 {
   // Create two identical factors and make sure they're equal
   Vector z = Vector_(2,323.,240.);
-  const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
+  const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
   const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
   boost::shared_ptr<Projection>
     factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@@ -91,14 +88,14 @@ TEST( GeneralSFMFactor, error ) {
   Point2 z(3.,0.);
   const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
   boost::shared_ptr<Projection>
-  factor(new Projection(z, sigma, Symbol("x1"), Symbol("l1")));
+  factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1)));
   // For the following configuration, the factor predicts 320,240
   Values values;
   Rot3 R;
   Point3 t1(0,0,-6);
   Pose3 x1(R,t1);
-  values.insert(Symbol("x1"), GeneralCamera(x1));
-  Point3 l1;  values.insert(Symbol("l1"), l1);
+  values.insert(Symbol('x',1), GeneralCamera(x1));
+  Point3 l1;  values.insert(Symbol('l',1), l1);
   EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index 3bccccca3..81ce20334 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -14,9 +14,6 @@
  *  @author Frank Dellaert, Viorela Ila
  **/
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/inference/FactorGraph.h>
@@ -46,7 +43,7 @@ static noiseModel::Gaussian::shared_ptr covariance(
 	)));
 //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
 
-const Key kx0 = Symbol("x0"), kx1 = Symbol("x1"), kx2 = Symbol("x2"), kx3 = Symbol("x3"), kx4 = Symbol("x4"), kx5 = Symbol("x5"), kl1 = Symbol("l1");
+const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1);
 
 /* ************************************************************************* */
 // Test constraint, small displacement
diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp
index c04bc2191..2edf6c4f0 100644
--- a/gtsam/slam/tests/testPose3SLAM.cpp
+++ b/gtsam/slam/tests/testPose3SLAM.cpp
@@ -28,9 +28,6 @@ using namespace boost::assign;
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 6
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/slam/pose3SLAM.h>
 #include <gtsam/slam/PartialPriorFactor.h>
 
@@ -43,7 +40,7 @@ static Matrix covariance = eye(6);
 
 const double tol=1e-5;
 
-const Key kx0 = Symbol("x0"), kx1 = Symbol("x1"), kx2 = Symbol("x2"), kx3 = Symbol("x3"), kx4 = Symbol("x4"), kx5 = Symbol("x5");
+const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5);
 
 /* ************************************************************************* */
 // test optimization with 6 poses arranged in a hexagon and a loop closure
diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp
index ab8621cac..8b4ecbf21 100644
--- a/gtsam/slam/tests/testProjectionFactor.cpp
+++ b/gtsam/slam/tests/testProjectionFactor.cpp
@@ -18,9 +18,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/slam/visualSLAM.h>
 
 using namespace std;
@@ -40,7 +37,7 @@ static Cal3_S2 K(fov,w,h);
 static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 static shared_ptrK sK(new Cal3_S2(K));
 
-const Key kx1 = Symbol("x1"), kl1 = Symbol("l1");
+const Key kx1 = Symbol('x',1), kl1 = Symbol('l',1);
 
 // make cameras
 
diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp
index afd759110..9cfb8bb72 100644
--- a/gtsam/slam/tests/testSerializationSLAM.cpp
+++ b/gtsam/slam/tests/testSerializationSLAM.cpp
@@ -16,8 +16,6 @@
  * @date Feb 7, 2012
  */
 
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/linear/GaussianISAM.h>
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
 #include <gtsam/slam/smallExample.h>
@@ -51,7 +49,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 TEST (Serialization, smallExample_linear) {
   using namespace example;
 
-  Ordering ordering; ordering += Symbol("x1"),Symbol("x2"),Symbol("l1");
+  Ordering ordering; ordering += Symbol('x',1),Symbol('x',2),Symbol('l',1);
   GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
   EXPECT(equalsObj(ordering));
   EXPECT(equalsXML(ordering));
diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp
index c4779a1cf..0188daa96 100644
--- a/gtsam/slam/tests/testVSLAM.cpp
+++ b/gtsam/slam/tests/testVSLAM.cpp
@@ -22,9 +22,6 @@
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/slam/visualSLAM.h>
diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp
index 4fddf1ba0..09baec6e1 100644
--- a/tests/testBoundingConstraint.cpp
+++ b/tests/testBoundingConstraint.cpp
@@ -17,8 +17,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/slam/simulated2DConstraints.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
@@ -153,7 +151,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
 	Point2 start_pt(0.0, 1.0);
 
 	shared_graph graph(new Graph());
-	Symbol x1("x1");
+	Symbol x1('x',1);
 	graph->add(iq2D::PoseXInequality(x1, 1.0, true));
 	graph->add(iq2D::PoseYInequality(x1, 2.0, true));
 	graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
@@ -175,7 +173,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 	Point2 start_pt(2.0, 3.0);
 
 	shared_graph graph(new Graph());
-	Symbol x1("x1");
+	Symbol x1('x',1);
 	graph->add(iq2D::PoseXInequality(x1, 1.0, false));
 	graph->add(iq2D::PoseYInequality(x1, 2.0, false));
 	graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
@@ -191,7 +189,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 
 /* ************************************************************************* */
 TEST( testBoundingConstraint, MaxDistance_basics) {
-	Symbol key1("x1"), key2("x2");
+	Symbol key1('x',1), key2('x',2);
 	Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
 	iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
 	EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
@@ -233,7 +231,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
 TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
 
 	Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
-  Symbol x1("x1"), x2("x2");
+  Symbol x1('x',1), x2('x',2);
 
 	Graph graph;
 	graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
@@ -256,7 +254,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, avoid_demo) {
 
-  Symbol x1("x1"), x2("x2"), x3("x3"), l1("l1");
+  Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
 	double radius = 1.0;
 	Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
 	Point2 odo(2.0, 0.0);
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index 54761ddc9..927499050 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -16,8 +16,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
@@ -37,7 +35,7 @@ TEST( ExtendedKalmanFilter, linear ) {
   ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
 
   // Create the TestKeys for our example
-  Symbol x0("x0"), x1("x1"), x2("x2"), x3("x3");
+  Symbol x0('x',0), x1('x',1), x2('x',2), x3('x',3);
 
   // Create the controls and measurement properties for our example
   double dt = 1.0;
diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp
index dd8f49988..f0cec7530 100644
--- a/tests/testGaussianBayesNet.cpp
+++ b/tests/testGaussianBayesNet.cpp
@@ -25,9 +25,6 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/linear/GaussianBayesNet.h>
diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp
index f39af5c3d..2cfb7a076 100644
--- a/tests/testGaussianFactor.cpp
+++ b/tests/testGaussianFactor.cpp
@@ -26,9 +26,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like kx3 to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/linear/GaussianConditional.h>
@@ -44,7 +41,7 @@ static SharedDiagonal
 	sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
 	constraintModel = noiseModel::Constrained::All(2);
 
-const Key kx1 = Symbol("x1"), kx2 = Symbol("x2"), kl1 = Symbol("l1");
+const Key kx1 = Symbol('x',1), kx2 = Symbol('x',2), kl1 = Symbol('l',1);
 
 /* ************************************************************************* */
 TEST( GaussianFactor, linearFactor )
diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp
index bec246854..671d09cc7 100644
--- a/tests/testGaussianFactorGraph.cpp
+++ b/tests/testGaussianFactorGraph.cpp
@@ -28,9 +28,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp
index 40ad8b428..b02d68583 100644
--- a/tests/testGaussianISAM.cpp
+++ b/tests/testGaussianISAM.cpp
@@ -21,9 +21,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/linear/GaussianBayesNet.h>
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 443bc41fe..f41cff097 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -11,8 +11,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/debug.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/nonlinear/Ordering.h>
diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp
index a00afae11..b01aa2ba4 100644
--- a/tests/testGaussianJunctionTree.cpp
+++ b/tests/testGaussianJunctionTree.cpp
@@ -25,9 +25,6 @@
 #include <boost/assign/std/vector.hpp>
 using namespace boost::assign;
 
-// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/debug.h>
 #include <gtsam/base/cholesky.h>
 #include <gtsam/inference/BayesTree-inl.h>
diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp
index 9131d801d..f56efae82 100644
--- a/tests/testGraph.cpp
+++ b/tests/testGraph.cpp
@@ -23,8 +23,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-#define GTSAM_MAGIC_KEY
-
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 3
 
diff --git a/tests/testInference.cpp b/tests/testInference.cpp
index d1ef745e6..df8843415 100644
--- a/tests/testInference.cpp
+++ b/tests/testInference.cpp
@@ -17,9 +17,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/slam/planarSLAM.h>
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index a12857ed5..d32a50fe5 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -25,9 +25,6 @@
 // TODO: DANGEROUS, create shared pointers
 #define GTSAM_MAGIC_GAUSSIAN 2
 
-// Magically casts strings like PoseKey(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/LieVector.h>
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 08d50afb6..92e569038 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -27,9 +27,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/slam/smallExample.h>
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 41db76e1e..e397ea443 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -26,9 +26,6 @@ using namespace boost::assign;
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
-// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Matrix.h>
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/slam/pose2SLAM.h>
diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp
index adddfab34..200530e6d 100644
--- a/tests/testSymbolicBayesNet.cpp
+++ b/tests/testSymbolicBayesNet.cpp
@@ -21,9 +21,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp
index 596a715cb..f8fa777d1 100644
--- a/tests/testSymbolicFactorGraph.cpp
+++ b/tests/testSymbolicFactorGraph.cpp
@@ -20,9 +20,6 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/inference/BayesNet-inl.h>
diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp
index 06232c64e..d98afd5f6 100644
--- a/tests/testTupleValues.cpp
+++ b/tests/testTupleValues.cpp
@@ -20,9 +20,6 @@
 
 #include <gtsam/base/TestableAssertions.h>
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose3.h>
diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp
index 3af046971..3c9d83b3d 100644
--- a/tests/timeGaussianFactorGraph.cpp
+++ b/tests/timeGaussianFactorGraph.cpp
@@ -15,9 +15,6 @@
  * @author  Frank Dellaert
  */
 
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
 #include <time.h>
 #include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for operator += in Ordering
diff --git a/wrap/matlab.h b/wrap/matlab.h
index 2c256a55d..631abb659 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -40,8 +40,6 @@ using namespace std;
 using namespace boost; // not usual, but for conciseness of generated code
 
 // start GTSAM Specifics /////////////////////////////////////////////////
-// to make keys be constructed from strings:
-#define GTSAM_MAGIC_KEY
 // to enable Matrix and Vector constructor for SharedGaussian:
 #define GTSAM_MAGIC_GAUSSIAN
 // end GTSAM Specifics /////////////////////////////////////////////////

From 736a3ae1a90e2dcba2bbd6be46bd0a30ee846899 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 23 Feb 2012 19:15:26 +0000
Subject: [PATCH 84/88] Removed unnecessary linking cmake option

---
 CMakeLists.txt       | 4 +---
 tests/CMakeLists.txt | 6 +++---
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0e7e2ad8d..81454a9c7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,9 +39,7 @@ option(GTSAM_BUILD_TESTS    "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_TIMING   "Enable/Disable building of timing scripts" ON)
 option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples"       ON)
 option(GTSAM_BUILD_WRAP     "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
-option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable using convenience librares for tests" ON)
-option(GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS    
-    "Enable/Disable linking tests against the convenince libraries for faster debugging" ON)
+option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable using convenience librares for tests and examples" ON)
 
 # Avoid building non-installed exes and unit tests when installing
 # FIXME: breaks generation and install of matlab toolbox
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 05c51f4d5..296013062 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,4 +1,4 @@
-if (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
+if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
     set(convenience_libs
        slam
        nonlinear
@@ -7,10 +7,10 @@ if (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
        geometry
        base
        ccolamd)
-else (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
+else (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
     set(convenience_libs
        gtsam-static)
-endif (GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS)
+endif (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
 
 # exclude certain files
 # note the source dir on each 

From f7577762f3d0ab4d8538610ae37129fd35da09bf Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Fri, 24 Feb 2012 15:14:06 +0000
Subject: [PATCH 85/88] Added quaternion cmake flag from trunk

---
 CMakeLists.txt | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 81454a9c7..1880fedef 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,7 +39,15 @@ option(GTSAM_BUILD_TESTS    "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_TIMING   "Enable/Disable building of timing scripts" ON)
 option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples"       ON)
 option(GTSAM_BUILD_WRAP     "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
-option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable using convenience librares for tests and examples" ON)
+option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
+option(GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS    
+    "Enable/Disable linking tests against the convenience libraries for faster debugging" ON)
+
+# Add the Quaternion Build Flag if requested
+if (GTSAM_USE_QUATERNIONS)
+	set(CMAKE_C_FLAGS            "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
+	set(CMAKE_CXX_FLAGS          "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
+endif(GTSAM_USE_QUATERNIONS)
 
 # Avoid building non-installed exes and unit tests when installing
 # FIXME: breaks generation and install of matlab toolbox

From 8517f852fd8cfe9957a046c15b4616b98d206622 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Fri, 24 Feb 2012 15:47:23 +0000
Subject: [PATCH 86/88] Removed unnecessary files, cmake flags

---
 CMakeLists.txt                    |   2 -
 examples/vSLAMexample/Makefile.am |  39 --
 tests/CMakeLists.txt              |   1 -
 tests/testTupleValues.cpp         | 624 ------------------------------
 4 files changed, 666 deletions(-)
 delete mode 100644 examples/vSLAMexample/Makefile.am
 delete mode 100644 tests/testTupleValues.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1880fedef..f58596520 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,8 +40,6 @@ option(GTSAM_BUILD_TIMING   "Enable/Disable building of timing scripts" ON)
 option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples"       ON)
 option(GTSAM_BUILD_WRAP     "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
 option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
-option(GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS    
-    "Enable/Disable linking tests against the convenience libraries for faster debugging" ON)
 
 # Add the Quaternion Build Flag if requested
 if (GTSAM_USE_QUATERNIONS)
diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am
deleted file mode 100644
index 625c57564..000000000
--- a/examples/vSLAMexample/Makefile.am
+++ /dev/null
@@ -1,39 +0,0 @@
-#----------------------------------------------------------------------------------------------------
-# GTSAM Examples
-#----------------------------------------------------------------------------------------------------
-
-# use nostdinc to turn off -I. and -I.., we do not need them because 
-# header files are qualified so they can be included in external projects.
-AUTOMAKE_OPTIONS = nostdinc
-
-headers =
-sources = 
-check_PROGRAMS =
-
-# Examples
-noinst_PROGRAMS = vSFMexample
-vSFMexample_SOURCES = vSFMexample.cpp vSLAMutils.cpp
-
-noinst_PROGRAMS += vISAMexample
-vISAMexample_SOURCES = vISAMexample.cpp vSLAMutils.cpp
-
-headers += Feature2D.h vSLAMutils.h
-
-noinst_HEADERS = $(headers)
-
-#----------------------------------------------------------------------------------------------------
-# rules to build local programs
-#----------------------------------------------------------------------------------------------------
-AM_CPPFLAGS = $(BOOST_CPPFLAGS)  -I$(top_srcdir)
-AM_LDFLAGS = $(BOOST_LDFLAGS)
-LDADD = ../../gtsam/libgtsam.la
-AM_DEFAULT_SOURCE_EXT = .cpp
-
-# rule to run an executable
-%.run: % $(LDADD)
-	./%
-
-# rule to run executable with valgrind
-%.valgrind: % $(LDADD)
-	valgrind ./$^
-#----------------------------------------------------------------------------------------------------
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 296013062..8a28c8dad 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -15,7 +15,6 @@ endif (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
 # exclude certain files
 # note the source dir on each 
 set (tests_exclude
-    "${CMAKE_CURRENT_SOURCE_DIR}/testTupleValues.cpp"
     "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp"
 )
 
diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp
deleted file mode 100644
index d98afd5f6..000000000
--- a/tests/testTupleValues.cpp
+++ /dev/null
@@ -1,624 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file testTupleValues.cpp
- * @author Richard Roberts
- * @author Alex Cunningham
- */
-
-#include <CppUnitLite/TestHarness.h>
-#include <stdexcept>
-
-#include <gtsam/base/TestableAssertions.h>
-
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Point3.h>
-
-#include <gtsam/base/Vector.h>
-#include <gtsam/nonlinear/Symbol.h>
-#include <gtsam/nonlinear/TupleValues.h>
-
-using namespace gtsam;
-using namespace std;
-
-static const double tol = 1e-5;
-
-typedef TypedSymbol<Pose2, 'x'> PoseKey;
-typedef TypedSymbol<Point2, 'l'> PointKey;
-typedef Values<PoseKey> PoseValues;
-typedef Values<PointKey> PointValues;
-typedef TupleValues2<PoseValues, PointValues> TestValues;
-
-/* ************************************************************************* */
-TEST( TupleValues, constructors )
-{
-	Pose2 x1(1,2,3), x2(6,7,8);
-	Point2 l1(4,5), l2(9,10);
-
-	TestValues::Values1 cfg1;
-	cfg1.insert(PoseKey(1), x1);
-	cfg1.insert(PoseKey(2), x2);
-	TestValues::Values2 cfg2;
-	cfg2.insert(PointKey(1), l1);
-	cfg2.insert(PointKey(2), l2);
-
-	TestValues actual(cfg1, cfg2), expected;
-	expected.insert(PoseKey(1), x1);
-	expected.insert(PoseKey(2), x2);
-	expected.insert(PointKey(1), l1);
-	expected.insert(PointKey(2), l2);
-
-	CHECK(assert_equal(expected, actual));
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, insert_equals1 )
-{
-	Pose2 x1(1,2,3), x2(6,7,8);
-	Point2 l1(4,5), l2(9,10);
-
-  TestValues expected;
-  expected.insert(PoseKey(1), x1);
-  expected.insert(PoseKey(2), x2);
-  expected.insert(PointKey(1), l1);
-  expected.insert(PointKey(2), l2);
-
-  TestValues actual;
-  actual.insert(PoseKey(1), x1);
-  actual.insert(PoseKey(2), x2);
-  actual.insert(PointKey(1), l1);
-  actual.insert(PointKey(2), l2);
-
-  CHECK(assert_equal(expected,actual));
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, insert_equals2 )
-{
-  Pose2 x1(1,2,3), x2(6,7,8);
-  Point2 l1(4,5), l2(9,10);
-
-  TestValues values1;
-  values1.insert(PoseKey(1), x1);
-  values1.insert(PoseKey(2), x2);
-  values1.insert(PointKey(1), l1);
-  values1.insert(PointKey(2), l2);
-
-  TestValues values2;
-  values2.insert(PoseKey(1), x1);
-  values2.insert(PoseKey(2), x2);
-  values2.insert(PointKey(1), l1);
-
-  CHECK(!values1.equals(values2));
-
-  values2.insert(2, Point2(9,11));
-
-  CHECK(!values1.equals(values2));
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, insert_duplicate )
-{
-  Pose2 x1(1,2,3), x2(6,7,8);
-  Point2 l1(4,5), l2(9,10);
-
-  TestValues values1;
-  values1.insert(1, x1); // 3
-  values1.insert(2, x2); // 6
-  values1.insert(1, l1); // 8
-  values1.insert(2, l2); // 10
-  CHECK_EXCEPTION(values1.insert(2, l1), KeyAlreadyExists<PointKey>); // still 10 !!!!
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, size_dim )
-{
-  Pose2 x1(1,2,3), x2(6,7,8);
-  Point2 l1(4,5), l2(9,10);
-
-  TestValues values1;
-  values1.insert(PoseKey(1), x1);
-  values1.insert(PoseKey(2), x2);
-  values1.insert(PointKey(1), l1);
-  values1.insert(PointKey(2), l2);
-
-  EXPECT(values1.size() == 4);
-  EXPECT(values1.dim() == 10);
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, at)
-{
-  Pose2 x1(1,2,3), x2(6,7,8);
-  Point2 l1(4,5), l2(9,10);
-
-  TestValues values1;
-  values1.insert(PoseKey(1), x1);
-  values1.insert(PoseKey(2), x2);
-  values1.insert(PointKey(1), l1);
-  values1.insert(PointKey(2), l2);
-
-  EXPECT(assert_equal(x1, values1[PoseKey(1)]));
-  EXPECT(assert_equal(x2, values1[PoseKey(2)]));
-  EXPECT(assert_equal(l1, values1[PointKey(1)]));
-  EXPECT(assert_equal(l2, values1[PointKey(2)]));
-
-  CHECK_EXCEPTION(values1[PoseKey(3)], KeyDoesNotExist<PoseKey>);
-  CHECK_EXCEPTION(values1[PointKey(3)], KeyDoesNotExist<PointKey>);
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, zero_expmap_logmap)
-{
-  Pose2 x1(1,2,3), x2(6,7,8);
-  Point2 l1(4,5), l2(9,10);
-
-  TestValues values1;
-  values1.insert(PoseKey(1), x1);
-  values1.insert(PoseKey(2), x2);
-  values1.insert(PointKey(1), l1);
-  values1.insert(PointKey(2), l2);
-
-  Ordering o; o += "x1", "x2", "l1", "l2";
-  VectorValues expected_zero(values1.dims(o));
-  expected_zero[o["x1"]] = zero(3);
-  expected_zero[o["x2"]] = zero(3);
-  expected_zero[o["l1"]] = zero(2);
-  expected_zero[o["l2"]] = zero(2);
-
-  CHECK(assert_equal(expected_zero, values1.zero(o)));
-
-  VectorValues delta(values1.dims(o));
-  delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2);
-  delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5);
-  delta[o["l1"]] = Vector_(2, 1.0, 1.1);
-  delta[o["l2"]] = Vector_(2, 1.3, 1.4);
-
-  TestValues expected;
-  expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
-  expected.insert(PoseKey(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
-  expected.insert(PointKey(1), Point2(5.0, 6.1));
-  expected.insert(PointKey(2), Point2(10.3, 11.4));
-
-  TestValues actual = values1.retract(delta, o);
-  CHECK(assert_equal(expected, actual));
-
-  // Check log
-  VectorValues expected_log = delta;
-  VectorValues actual_log = values1.localCoordinates(actual, o);
-  CHECK(assert_equal(expected_log, actual_log));
-}
-
-/* ************************************************************************* */
-
-// some key types
-typedef TypedSymbol<LieVector, 'L'> LamKey;
-typedef TypedSymbol<Pose3, 'a'> Pose3Key;
-typedef TypedSymbol<Point3, 'b'> Point3Key;
-typedef TypedSymbol<Point3, 'c'> Point3Key2;
-
-// some values types
-typedef Values<PoseKey> PoseValues;
-typedef Values<PointKey> PointValues;
-typedef Values<LamKey> LamValues;
-typedef Values<Pose3Key> Pose3Values;
-typedef Values<Point3Key> Point3Values;
-typedef Values<Point3Key2> Point3Values2;
-
-// some TupleValues types
-typedef TupleValues<PoseValues, TupleValuesEnd<PointValues> > ValuesA;
-typedef TupleValues<PoseValues, TupleValues<PointValues, TupleValuesEnd<LamValues> > > ValuesB;
-
-typedef TupleValues1<PoseValues> TuplePoseValues;
-typedef TupleValues1<PointValues> TuplePointValues;
-typedef TupleValues2<PoseValues, PointValues> SimpleValues;
-
-/* ************************************************************************* */
-TEST(TupleValues, slicing) {
-	PointKey l1(1), l2(2);
-	Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
-	PoseKey x1(1), x2(2);
-	Pose2 x1_val(1.0, 2.0, 0.3), x2_val(3.0, 4.0, 0.4);
-
-	PoseValues liePoseValues;
-	liePoseValues.insert(x1, x1_val);
-	liePoseValues.insert(x2, x2_val);
-
-	PointValues liePointValues;
-	liePointValues.insert(l1, l1_val);
-	liePointValues.insert(l2, l2_val);
-
-	// construct TupleValues1 from the base values
-	TuplePoseValues tupPoseValues1(liePoseValues);
-	EXPECT(assert_equal(liePoseValues, tupPoseValues1.first(), tol));
-
-	TuplePointValues tupPointValues1(liePointValues);
-	EXPECT(assert_equal(liePointValues, tupPointValues1.first(), tol));
-
-//	// construct a TupleValues2 from a TupleValues1
-//	SimpleValues pairValues1(tupPoseValues1);
-//	EXPECT(assert_equal(liePoseValues, pairValues1.first(), tol));
-//	EXPECT(pairValues1.second().empty());
-//
-//	SimpleValues pairValues2(tupPointValues1);
-//	EXPECT(assert_equal(liePointValues, pairValues2.second(), tol));
-//	EXPECT(pairValues1.first().empty());
-
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, basic_functions) {
-	// create some tuple values
-	ValuesA valuesA;
-	ValuesB valuesB;
-
-	PoseKey x1(1);
-	PointKey l1(1);
-	LamKey L1(1);
-	Pose2 pose1(1.0, 2.0, 0.3);
-	Point2 point1(2.0, 3.0);
-	LieVector lam1 = LieVector(2.3);
-
-	// Insert
-	valuesA.insert(x1, pose1);
-	valuesA.insert(l1, point1);
-
-	valuesB.insert(x1, pose1);
-	valuesB.insert(l1, point1);
-	valuesB.insert(L1, lam1);
-
-	// bracket operator
-	EXPECT(assert_equal(valuesA[x1], pose1));
-	EXPECT(assert_equal(valuesA[l1], point1));
-	EXPECT(assert_equal(valuesB[x1], pose1));
-	EXPECT(assert_equal(valuesB[l1], point1));
-	EXPECT(assert_equal(valuesB[L1], lam1));
-
-	// exists
-	EXPECT(valuesA.exists(x1));
-	EXPECT(valuesA.exists(l1));
-	EXPECT(valuesB.exists(x1));
-	EXPECT(valuesB.exists(l1));
-	EXPECT(valuesB.exists(L1));
-
-	// at
-	EXPECT(assert_equal(valuesA.at(x1), pose1));
-	EXPECT(assert_equal(valuesA.at(l1), point1));
-	EXPECT(assert_equal(valuesB.at(x1), pose1));
-	EXPECT(assert_equal(valuesB.at(l1), point1));
-	EXPECT(assert_equal(valuesB.at(L1), lam1));
-
-	// size
-	EXPECT(valuesA.size() == 2);
-	EXPECT(valuesB.size() == 3);
-
-	// dim
-	EXPECT(valuesA.dim() == 5);
-	EXPECT(valuesB.dim() == 6);
-
-	// erase
-	valuesA.erase(x1);
-	CHECK(!valuesA.exists(x1));
-	CHECK(valuesA.size() == 1);
-	valuesA.erase(l1);
-	CHECK(!valuesA.exists(l1));
-	CHECK(valuesA.size() == 0);
-	valuesB.erase(L1);
-	CHECK(!valuesB.exists(L1));
-	CHECK(valuesB.size() == 2);
-
-	// clear
-	valuesA.clear();
-	EXPECT(valuesA.size() == 0);
-	valuesB.clear();
-	EXPECT(valuesB.size() == 0);
-
-	// empty
-	EXPECT(valuesA.empty());
-	EXPECT(valuesB.empty());
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, insert_values) {
-	ValuesB values1, values2, expected;
-
-	PoseKey x1(1), x2(2);
-	PointKey l1(1), l2(2);
-	LamKey L1(1), L2(2);
-	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
-	Point2 point1(2.0, 3.0), point2(5.0, 6.0);
-	LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5);
-
-	values1.insert(x1, pose1);
-	values1.insert(l1, point1);
-	values1.insert(L1, lam1);
-
-	values2.insert(x2, pose2);
-	values2.insert(l2, point2);
-	values2.insert(L2, lam2);
-
-	values1.insert(values2);
-
-	expected.insert(x1, pose1);
-	expected.insert(l1, point1);
-	expected.insert(L1, lam1);
-	expected.insert(x2, pose2);
-	expected.insert(l2, point2);
-	expected.insert(L2, lam2);
-
-	CHECK(assert_equal(expected, values1));
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, update_element )
-{
-	TupleValues2<PoseValues, PointValues> cfg;
-	Pose2 x1(2.0, 1.0, 2.0), x2(3.0, 4.0, 5.0);
-	Point2 l1(1.0, 2.0), l2(3.0, 4.0);
-	PoseKey xk(1);
-	PointKey lk(1);
-
-	cfg.insert(xk, x1);
-	CHECK(cfg.size() == 1);
-	CHECK(assert_equal(x1, cfg.at(xk)));
-
-	cfg.update(xk, x2);
-	CHECK(cfg.size() == 1);
-	CHECK(assert_equal(x2, cfg.at(xk)));
-
-	cfg.insert(lk, l1);
-	CHECK(cfg.size() == 2);
-	CHECK(assert_equal(l1, cfg.at(lk)));
-
-	cfg.update(lk, l2);
-	CHECK(cfg.size() == 2);
-	CHECK(assert_equal(l2, cfg.at(lk)));
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, equals )
-{
-	Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7);
-	PoseKey x1k(1), x2k(2);
-	Point2 l1(4,5), l2(9,10);
-	PointKey l1k(1), l2k(2);
-
-	ValuesA values1, values2, values3, values4, values5;
-
-	values1.insert(x1k, x1);
-	values1.insert(x2k, x2);
-	values1.insert(l1k, l1);
-	values1.insert(l2k, l2);
-
-	values2.insert(x1k, x1);
-	values2.insert(x2k, x2);
-	values2.insert(l1k, l1);
-	values2.insert(l2k, l2);
-
-	values3.insert(x2k, x2);
-	values3.insert(l1k, l1);
-
-	values4.insert(x1k, x1);
-	values4.insert(x2k, x2_alt);
-	values4.insert(l1k, l1);
-	values4.insert(l2k, l2);
-
-	ValuesA values6(values1);
-
-	EXPECT(assert_equal(values1,values2));
-	EXPECT(assert_equal(values1,values1));
-	EXPECT(assert_inequal(values1,values3));
-	EXPECT(assert_inequal(values1,values4));
-	EXPECT(assert_inequal(values1,values5));
-	EXPECT(assert_equal(values1, values6));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, expmap)
-{
-	Pose2 x1(1,2,3), x2(6,7,8);
-	PoseKey x1k(1), x2k(2);
-	Point2 l1(4,5), l2(9,10);
-	PointKey l1k(1), l2k(2);
-
-	Ordering o; o += "x1", "x2", "l1", "l2";
-
-	ValuesA values1;
-	values1.insert(x1k, x1);
-	values1.insert(x2k, x2);
-	values1.insert(l1k, l1);
-	values1.insert(l2k, l2);
-
-	VectorValues delta(values1.dims(o));
-	delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2);
-	delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5);
-	delta[o["l1"]] = Vector_(2, 1.0, 1.1);
-	delta[o["l2"]] = Vector_(2, 1.3, 1.4);
-
-	ValuesA expected;
-	expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
-	expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
-	expected.insert(l1k, Point2(5.0, 6.1));
-	expected.insert(l2k, Point2(10.3, 11.4));
-
-	CHECK(assert_equal(expected, values1.retract(delta, o)));
-	CHECK(assert_equal(delta, values1.localCoordinates(expected, o)));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, expmap_typedefs)
-{
-	Pose2 x1(1,2,3), x2(6,7,8);
-	PoseKey x1k(1), x2k(2);
-	Point2 l1(4,5), l2(9,10);
-	PointKey l1k(1), l2k(2);
-
-  Ordering o; o += "x1", "x2", "l1", "l2";
-
-	TupleValues2<PoseValues, PointValues> values1, expected, actual;
-	values1.insert(x1k, x1);
-	values1.insert(x2k, x2);
-	values1.insert(l1k, l1);
-	values1.insert(l2k, l2);
-
-  VectorValues delta(values1.dims(o));
-  delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2);
-  delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5);
-  delta[o["l1"]] = Vector_(2, 1.0, 1.1);
-  delta[o["l2"]] = Vector_(2, 1.3, 1.4);
-
-	expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
-	expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
-	expected.insert(l1k, Point2(5.0, 6.1));
-	expected.insert(l2k, Point2(10.3, 11.4));
-
-	CHECK(assert_equal(expected, TupleValues2<PoseValues, PointValues>(values1.retract(delta, o))));
-	//CHECK(assert_equal(delta, values1.localCoordinates(expected)));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, typedefs)
-{
-	TupleValues2<PoseValues, PointValues> values1;
-	TupleValues3<PoseValues, PointValues, LamValues> values2;
-	TupleValues4<PoseValues, PointValues, LamValues, Point3Values> values3;
-	TupleValues5<PoseValues, PointValues, LamValues, Point3Values, Pose3Values> values4;
-	TupleValues6<PoseValues, PointValues, LamValues, Point3Values, Pose3Values, Point3Values2> values5;
-}
-
-/* ************************************************************************* */
-TEST( TupleValues, pairvalues_style )
-{
-	PoseKey x1(1);
-	PointKey l1(1);
-	LamKey L1(1);
-	Pose2 pose1(1.0, 2.0, 0.3);
-	Point2 point1(2.0, 3.0);
-	LieVector lam1 = LieVector(2.3);
-
-	PoseValues values1; values1.insert(x1, pose1);
-	PointValues values2; values2.insert(l1, point1);
-	LamValues values3; values3.insert(L1, lam1);
-
-	// Constructor
-	TupleValues3<PoseValues, PointValues, LamValues> values(values1, values2, values3);
-
-	// access
-	CHECK(assert_equal(values1, values.first()));
-	CHECK(assert_equal(values2, values.second()));
-	CHECK(assert_equal(values3, values.third()));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, insert_values_typedef) {
-
-	TupleValues4<PoseValues, PointValues, LamValues, Point3Values> values1, values2, expected;
-
-	PoseKey x1(1), x2(2);
-	PointKey l1(1), l2(2);
-	LamKey L1(1), L2(2);
-	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
-	Point2 point1(2.0, 3.0), point2(5.0, 6.0);
-	LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5);
-
-	values1.insert(x1, pose1);
-	values1.insert(l1, point1);
-	values1.insert(L1, lam1);
-
-	values2.insert(x2, pose2);
-	values2.insert(l2, point2);
-	values2.insert(L2, lam2);
-
-	values1.insert(values2);
-
-	expected.insert(x1, pose1);
-	expected.insert(l1, point1);
-	expected.insert(L1, lam1);
-	expected.insert(x2, pose2);
-	expected.insert(l2, point2);
-	expected.insert(L2, lam2);
-
-	CHECK(assert_equal(expected, values1));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, partial_insert) {
-	TupleValues3<PoseValues, PointValues, LamValues> init, expected;
-
-	PoseKey x1(1), x2(2);
-	PointKey l1(1), l2(2);
-	LamKey L1(1), L2(2);
-	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
-	Point2 point1(2.0, 3.0), point2(5.0, 6.0);
-	LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5);
-
-	init.insert(x1, pose1);
-	init.insert(l1, point1);
-	init.insert(L1, lam1);
-
-	PoseValues cfg1;
-	cfg1.insert(x2, pose2);
-
-	init.insertSub(cfg1);
-
-	expected.insert(x1, pose1);
-	expected.insert(l1, point1);
-	expected.insert(L1, lam1);
-	expected.insert(x2, pose2);
-
-	CHECK(assert_equal(expected, init));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, update) {
-	TupleValues3<PoseValues, PointValues, LamValues> init, superset, expected;
-
-	PoseKey x1(1), x2(2);
-	PointKey l1(1), l2(2);
-	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
-	Point2 point1(2.0, 3.0), point2(5.0, 6.0);
-
-	init.insert(x1, pose1);
-	init.insert(l1, point1);
-
-
-	Pose2 pose1_(1.0, 2.0, 0.4);
-	Point2 point1_(2.0, 4.0);
-	superset.insert(x1, pose1_);
-	superset.insert(l1, point1_);
-	superset.insert(x2, pose2);
-	superset.insert(l2, point2);
-	init.update(superset);
-
-	expected.insert(x1, pose1_);
-	expected.insert(l1, point1_);
-
-	CHECK(assert_equal(expected, init));
-}
-
-/* ************************************************************************* */
-TEST(TupleValues, arbitrary_ordering ) {
-	TupleValues1<PoseValues> values;
-	PoseKey x1(1), x2(2);
-	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
-	values.insert(x1, pose1);
-	values.insert(x2, pose2);
-	Ordering::shared_ptr actual = values.orderingArbitrary();
-	Ordering expected; expected += x1, x2;
-	EXPECT(assert_equal(expected, *actual));
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */

From 84f5d87b52ff810729a79c428eb0c816b71c98b6 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 24 Feb 2012 16:45:48 +0000
Subject: [PATCH 87/88] Reworked Values filtering to use boost range, so much
 less code and simpler syntax

---
 .cproject                            | 2208 +++++++++++++-------------
 gtsam/nonlinear/Values.h             |  259 +--
 gtsam/nonlinear/tests/testValues.cpp |   29 +-
 3 files changed, 1273 insertions(+), 1223 deletions(-)

diff --git a/.cproject b/.cproject
index e75e9b8e8..f54554f76 100644
--- a/.cproject
+++ b/.cproject
@@ -287,7 +287,7 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 		<buildTargets>
-			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -295,7 +295,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -303,162 +311,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testTestableAssertions.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testMatrix.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testBlockMatrices.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCholesky.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testLieConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>vSFMexample.run</buildTarget>
+				<buildTarget>testGaussianFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -489,7 +345,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -497,7 +352,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -545,7 +399,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -553,7 +406,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -561,7 +413,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -577,12 +428,291 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianISAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testIterative.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearEquality.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearOptimizer.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSubgraphPreconditioner.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testTupleConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeGaussianFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testInference.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testSymbolicBayesNet.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianJunctionTree" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianJunctionTree</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSerialization.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSerialization.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -623,7 +753,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -631,22 +761,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testSPQRUtil.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSPQRUtil.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -679,69 +793,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>tests/testGaussianISAM2</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -766,6 +817,30 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>vSFMexample.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMGraph</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -918,87 +993,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot3.run</buildTarget>
+				<buildTarget>SimpleRotation.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot2.run</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCal3_S2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSimpleCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testHomography2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1006,15 +1017,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1022,39 +1025,78 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVectorValues.run</buildTarget>
+				<buildTarget>testGaussianConditional.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testNoiseModel.run</buildTarget>
+				<buildTarget>testGaussianFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactor.run</buildTarget>
+				<buildTarget>timeGaussianFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testHessianFactor.run</buildTarget>
+				<buildTarget>timeVectorConfig.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVectorMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNoiseModel.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>testErrors.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1062,23 +1104,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianJunctionTree.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
@@ -1086,7 +1112,39 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianFactor.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianConditional.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeSLAMlike.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeSLAMlike.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1094,18 +1152,170 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testKalmanFilter.run</buildTarget>
+				<buildTarget>testBTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianDensity.run</buildTarget>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSPQRUtil.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testClusterTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testClusterTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testJunctionTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testJunctionTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testEliminationTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testEliminationTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testSymbolicFactor.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVariableSlots.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVariableSlots.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testConditional.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testConditional.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testSymbolicFactorGraph.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSymbolicFactorGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testNonlinearConstraint.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testNonlinearConstraint.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testLieConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testConstraintOptimizer.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testConstraintOptimizer.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1126,6 +1336,278 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testRot3M.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timePose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/timeStereoCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPlanarSLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPlanarSLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2Config.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2Factor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2Prior.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2Prior.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2SLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3Config.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3SLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testSimulated2DOriented.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMConfig.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMFactor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVSLAMGraph.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testVSLAMGraph.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3Factor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testSimulated2D.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>testSimulated3D.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianISAM2" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="cmake" path="build_cmake" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cmake</buildCommand>
 				<buildArguments>..</buildArguments>
@@ -1493,15 +1975,39 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
+				<buildTarget>tests/testDSFVector.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testTestableAssertions.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testTestableAssertions.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -1509,31 +2015,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
+				<buildTarget>tests/testNumericalDerivative.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSpirit.run</buildTarget>
+				<buildTarget>tests/testBlockMatrices.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testWrap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1541,47 +2039,31 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildTarget>tests/testCholesky.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildTarget>tests/testVectorValues.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>tests/testNoiseModel.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianJunctionTree.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianFactor.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>tests/testGaussianFactor.run</buildTarget>
@@ -1589,7 +2071,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testGaussianConditional.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testHessianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>tests/testGaussianConditional.run</buildTarget>
@@ -1597,127 +2095,23 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/timeSLAMlike.run" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeSLAMlike.run</buildTarget>
+				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
+				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testStereoCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testRot3M.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timePose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeStereoCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -1725,608 +2119,18 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
+				<buildTarget>tests/testKalmanFilter.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>cmake</buildCommand>
-				<buildArguments>..</buildArguments>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>SimpleRotation.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMGraph</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianISAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testIterative.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearEquality.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearOptimizer.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSubgraphPreconditioner.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testTupleConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeGaussianFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testInference.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSymbolicBayesNet.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianJunctionTree" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianJunctionTree</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSerialization.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSerialization.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testClusterTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testClusterTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testJunctionTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testJunctionTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testEliminationTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testEliminationTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSymbolicFactor.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testVariableSlots.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testVariableSlots.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testConditional.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testSymbolicFactorGraph.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testSymbolicFactorGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNonlinearConstraint.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNonlinearConstraint.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testLieConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testConstraintOptimizer.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testConstraintOptimizer.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianConditional.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeVectorConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVectorMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testNoiseModel.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testBayesNetPreconditioner.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>testErrors.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPlanarSLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPlanarSLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2Config.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2Factor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2Prior.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2Prior.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2SLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3Config.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3SLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSimulated2DOriented.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMConfig.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMConfig.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMFactor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testVSLAMGraph.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testVSLAMGraph.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3Factor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSimulated2D.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>testSimulated3D.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests/testGaussianISAM2" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<buildTarget>tests/testGaussianDensity.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -2411,6 +2215,38 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>tests/testGaussianISAM2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testLieConfig.run" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testLieConfig.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2454,20 +2290,99 @@
 			<target name="cmake" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cmake</buildCommand>
 				<buildArguments>..</buildArguments>
-				<buildTarget/>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="nonlinear.testValues.run" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildArguments/>
+				<buildTarget>nonlinear.testValues.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testRot2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCal3_S2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSimpleCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testHomography2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>clean</buildTarget>
@@ -2475,6 +2390,101 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPoint2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>..</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSpirit.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testWrap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 08bf9edca..745498c29 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -34,6 +34,10 @@
 #include <boost/function.hpp>
 #include <boost/bind.hpp>
 #include <boost/ptr_container/serialize_ptr_map.hpp>
+#include <boost/range/iterator_range.hpp>
+#include <boost/range/adaptor/filtered.hpp>
+#include <boost/range/adaptor/transformed.hpp>
+#include <boost/lambda/lambda.hpp>
 
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
@@ -82,20 +86,11 @@ namespace gtsam {
     /// A shared_ptr to this class
     typedef boost::shared_ptr<Values> shared_ptr;
 
-    /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
-    struct KeyValuePair {
-      const Key first;
-      Value& second;
-      KeyValuePair(Key key, Value& value) : first(key), second(value) {}
-    };
+    /// A key-value pair, which you get by dereferencing iterators
+    typedef std::pair<const Key, Value&> KeyValuePair;
 
-    /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
-    struct ConstKeyValuePair {
-      const Key first;
-      const Value& second;
-      ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {}
-      ConstKeyValuePair(const KeyValuePair& key_value) : first(key_value.first), second(key_value.second) {}
-    };
+    /// A key-value pair, which you get by dereferencing iterators
+    typedef std::pair<const Key, const Value&> ConstKeyValuePair;
 
     /// Mutable forward iterator, with value type KeyValuePair
     typedef boost::transform_iterator<
@@ -113,26 +108,40 @@ namespace gtsam {
     typedef boost::transform_iterator<
         boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
 
-    /// Mutable filtered iterator
-    typedef boost::filter_iterator<boost::function<bool(const KeyValuePair&)>, iterator> filter_iterator;
-
-    /// Const filtered iterator
-    typedef boost::filter_iterator<boost::function<bool(const ConstKeyValuePair&)>, const_iterator> const_filter_iterator;
-
-    /// Mutable type-filtered iterator
     template<class ValueType>
-    struct type_filter_iterator {
-      typedef boost::transform_iterator<
-          std::pair<const Key, ValueType&>(*)(KeyValuePair),
-          boost::filter_iterator<bool(*)(const KeyValuePair&), iterator> > type;
+    class Filtered : public boost::transformed_range<
+    std::pair<const Key, ValueType&>(*)(KeyValuePair key_value),
+    const boost::filtered_range<
+    boost::function<bool(const ConstKeyValuePair&)>,
+    const boost::iterator_range<iterator> > > {
+    private:
+      typedef boost::transformed_range<
+          std::pair<const Key, ValueType&>(*)(KeyValuePair key_value),
+          const boost::filtered_range<
+          boost::function<bool(const ConstKeyValuePair&)>,
+          const boost::iterator_range<iterator> > > Base;
+
+      Filtered(const Base& base) : Base(base) {}
+
+      friend class Values;
     };
 
-    /// Const type-filtered iterator
     template<class ValueType>
-    struct const_type_filter_iterator {
-      typedef boost::transform_iterator<
-          std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair),
-          boost::filter_iterator<bool(*)(const ConstKeyValuePair&), const_iterator> > type;
+    class ConstFiltered : public boost::transformed_range<
+    std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair key_value),
+    const boost::filtered_range<
+    boost::function<bool(const ConstKeyValuePair&)>,
+    const boost::iterator_range<const_iterator> > > {
+    private:
+      typedef boost::transformed_range<
+          std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair key_value),
+          const boost::filtered_range<
+          boost::function<bool(const ConstKeyValuePair&)>,
+          const boost::iterator_range<const_iterator> > > Base;
+
+      ConstFiltered(const Base& base) : Base(base) {}
+
+      friend class Values;
     };
 
     /** Default constructor creates an empty Values class */
@@ -215,86 +224,6 @@ namespace gtsam {
     reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
     reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
 
-    /** Iterator over a subset of the Key-Value pairs, for which the given
-     * \c filter function returns true when evaluated on the Key. */
-    const_filter_iterator beginFilterByKey(const boost::function<bool(Key)>& filter) const {
-      return boost::make_filter_iterator(
-          boost::function<bool(const ConstKeyValuePair&)>(boost::bind(filter, boost::bind(&ConstKeyValuePair::first, _1))),
-          begin(), end()); }
-
-    /** Iterator over a subset of the Key-Value pairs, for which the given
-     * \c filter function returns true when evaluated on the Key. */
-    const_filter_iterator endFilterByKey(const boost::function<bool(Key)>& filter) const {
-      return boost::make_filter_iterator(
-          boost::function<bool(const ConstKeyValuePair&)>(boost::bind(filter, boost::bind(&ConstKeyValuePair::first, _1))),
-          end(), end()); }
-
-    /** Iterator over a subset of the Key-Value pairs, for which the given
-     * \c filter function returns true when evaluated on the Key. */
-    filter_iterator beginFilterByKey(const boost::function<bool(Key)>& filter) {
-      return boost::make_filter_iterator(
-          boost::function<bool(const KeyValuePair&)>(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))),
-          begin(), end()); }
-
-    /** Iterator over a subset of the Key-Value pairs, for which the given
-     * \c filter function returns true when evaluated on the Key. */
-    filter_iterator endFilterByKey(const boost::function<bool(Key)>& filter) {
-      return boost::make_filter_iterator(
-          boost::function<bool(const KeyValuePair&)>(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))),
-          end(), end()); }
-
-  private:
-    template<class ValueType, class KeyValue>
-    static bool _typeFilterHelper(const KeyValue& key_value) {
-      return typeid(ValueType) == typeid(key_value.second);
-    }
-
-    template<class ValueType>
-    static std::pair<const Key, const ValueType&> _typeCastHelperConst(ConstKeyValuePair key_value) {
-      return std::make_pair<const Key, const ValueType&>(key_value.first, static_cast<const ValueType&>(key_value.second)); }
-
-    template<class ValueType>
-    static std::pair<const Key, ValueType&> _typeCastHelper(KeyValuePair key_value) {
-      return std::make_pair<const Key, ValueType&>(key_value.first, static_cast<ValueType&>(key_value.second)); }
-
-  public:
-
-    /** Iterator over a subset of the Key-Value pairs, for which the Value type
-     * matches the template parameter \c ValueType.
-     */
-    template<class ValueType>
-    typename const_type_filter_iterator<ValueType>::type beginFilterByType() const {
-      return boost::make_transform_iterator(
-          boost::filter_iterator<bool(*)(ConstKeyValuePair), const_iterator>(&_typeFilterHelper<ValueType,ConstKeyValuePair>, begin(), end()),
-          &_typeCastHelperConst<ValueType>); }
-
-    /** Iterator over a subset of the Key-Value pairs, for which the Value type
-     * matches the template parameter \c ValueType.
-     */
-    template<class ValueType>
-    typename const_type_filter_iterator<ValueType>::type endFilterByType() const {
-      return boost::make_transform_iterator(
-          boost::filter_iterator<bool(*)(ConstKeyValuePair), const_iterator>(&_typeFilterHelper<ValueType,ConstKeyValuePair>, end(), end()),
-          &_typeCastHelperConst<ValueType>); }
-
-    /** Iterator over a subset of the Key-Value pairs, for which the Value type
-     * matches the template parameter \c ValueType.
-     */
-    template<class ValueType>
-    typename type_filter_iterator<ValueType>::type beginFilterByType() {
-      return boost::make_transform_iterator(
-          boost::make_filter_iterator(&_typeFilterHelper<ValueType,KeyValuePair>, begin(), end()),
-          &_typeCastHelper<ValueType>); }
-
-    /** Iterator over a subset of the Key-Value pairs, for which the Value type
-     * matches the template parameter \c ValueType.
-     */
-    template<class ValueType>
-    typename type_filter_iterator<ValueType>::type endFilterByType() {
-      return boost::make_transform_iterator(
-          boost::make_filter_iterator(&_typeFilterHelper<ValueType,KeyValuePair>, end(), end()),
-          &_typeCastHelper<ValueType>); }
-
     /// @name Manifold Operations
     /// @{
 
@@ -350,8 +279,118 @@ namespace gtsam {
      */
     Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
 
+    /**
+     * Return a filtered view of this Values class, without copying any data.
+     * When iterating over the filtered view, only the key-value pairs
+     * with a key causing \c filterFcn to return \c true are visited.  Because
+     * the object Filtered<Value> returned from filter() is only a
+     * <emph>view</emph> the original Values object must not be deallocated or
+     * go out of scope as long as the view is needed.
+     * @param filterFcn The function that determines which key-value pairs are
+     * included in the filtered view, for which this function returns \c true
+     * on their keys.
+     * @return A filtered view of the original Values class, which references
+     * the original Values class.
+     */
+    Filtered<Value>
+    filter(const boost::function<bool(Key)>& filterFcn) {
+      return filter<Value>(filterFcn);
+    }
+
+    /**
+     * Return a filtered view of this Values class, without copying any data.
+     * In this templated version, only key-value pairs whose value matches the
+     * template argument \c ValueType and whose key causes the function argument
+     * \c filterFcn to return true are visited when iterating over the filtered
+     * view.  Because the object Filtered<Value> returned from filter() is only
+     * a <emph>view</emph> the original Values object must not be deallocated or
+     * go out of scope as long as the view is needed.
+     * @tparam ValueType The type that the value in a key-value pair must match
+     * to be included in the filtered view.  Currently, base classes are not
+     * resolved so the type must match exactly, except if ValueType = Value, in
+     * which case no type filtering is done.
+     * @param filterFcn The function that determines which key-value pairs are
+     * included in the filtered view, for which this function returns \c true
+     * on their keys (default:  always return true so that filter() only
+     * filters by type, matching \c ValueType).
+     * @return A filtered view of the original Values class, which references
+     * the original Values class.
+     */
+    template<class ValueType>
+    Filtered<ValueType>
+    filter(const boost::function<bool(Key)>& filterFcn = (boost::lambda::_1, true)) {
+      return boost::adaptors::transform(
+          boost::adaptors::filter(
+              boost::make_iterator_range(begin(), end()),
+              boost::function<bool(const ConstKeyValuePair&)>(
+                  boost::bind(&filterHelper<ValueType>, filterFcn, _1))),
+          &castHelper<ValueType, KeyValuePair>);
+    }
+
+    /**
+     * Return a filtered view of this Values class, without copying any data.
+     * When iterating over the filtered view, only the key-value pairs
+     * with a key causing \c filterFcn to return \c true are visited.  Because
+     * the object Filtered<Value> returned from filter() is only a
+     * <emph>view</emph> the original Values object must not be deallocated or
+     * go out of scope as long as the view is needed.
+     * @param filterFcn The function that determines which key-value pairs are
+     * included in the filtered view, for which this function returns \c true
+     * on their keys.
+     * @return A filtered view of the original Values class, which references
+     * the original Values class.
+     */
+    ConstFiltered<Value>
+    filter(const boost::function<bool(Key)>& filterFcn) const {
+      return filter<Value>(filterFcn);
+    }
+
+    /**
+     * Return a filtered view of this Values class, without copying any data.
+     * In this templated version, only key-value pairs whose value matches the
+     * template argument \c ValueType and whose key causes the function argument
+     * \c filterFcn to return true are visited when iterating over the filtered
+     * view.  Because the object Filtered<Value> returned from filter() is only
+     * a <emph>view</emph> the original Values object must not be deallocated or
+     * go out of scope as long as the view is needed.
+     * @tparam ValueType The type that the value in a key-value pair must match
+     * to be included in the filtered view.  Currently, base classes are not
+     * resolved so the type must match exactly, except if ValueType = Value, in
+     * which case no type filtering is done.
+     * @param filterFcn The function that determines which key-value pairs are
+     * included in the filtered view, for which this function returns \c true
+     * on their keys.
+     * @return A filtered view of the original Values class, which references
+     * the original Values class.
+     */
+    template<class ValueType>
+    ConstFiltered<ValueType>
+    filter(const boost::function<bool(Key)>& filterFcn = (boost::lambda::_1, true)) const {
+      return boost::adaptors::transform(
+          boost::adaptors::filter(
+              boost::make_iterator_range(begin(), end()),
+              boost::function<bool(const ConstKeyValuePair&)>(
+                  boost::bind(&filterHelper<ValueType>, filterFcn, _1))),
+          &castHelper<const ValueType, ConstKeyValuePair>);
+    }
+
   private:
-  	/** Serialization function */
+    // Filters based on ValueType (if not Value) and also based on the user-
+    // supplied \c filter function.
+    template<class ValueType>
+    static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
+      // Filter and check the type
+      return filter(key_value.first) && (typeid(ValueType) == typeid(key_value.second) || typeid(ValueType) == typeid(Value));
+    }
+
+    // Cast to the derived ValueType
+    template<class ValueType, class KeyValuePairType>
+    static std::pair<const Key, ValueType&> castHelper(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      return std::pair<const Key, ValueType&>(key_value.first, static_cast<ValueType&>(key_value.second));
+    }
+
+    /** Serialization function */
   	friend class boost::serialization::access;
   	template<class ARCHIVE>
   	void serialize(ARCHIVE & ar, const unsigned int version) {
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 55854b339..bdf3e1191 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -273,35 +273,36 @@ TEST(Values, filter) {
 
   // Filter by key
   int i = 0;
-  for(Values::filter_iterator it = values.beginFilterByKey(boost::bind(std::greater_equal<Key>(), _1, 2));
-      it != values.endFilterByKey(boost::bind(std::greater_equal<Key>(), _1, 2)); ++it, ++i) {
+  Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
+  BOOST_FOREACH(const Values::Filtered<Value>::value_type& key_value, filtered) {
     if(i == 0) {
-      LONGS_EQUAL(2, it->first);
-      EXPECT(typeid(Pose2) == typeid(it->second));
-      EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(it->second)));
+      LONGS_EQUAL(2, key_value.first);
+      EXPECT(typeid(Pose2) == typeid(key_value.second));
+      EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.second)));
     } else if(i == 1) {
-      LONGS_EQUAL(3, it->first);
-      EXPECT(typeid(Pose3) == typeid(it->second));
-      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(it->second)));
+      LONGS_EQUAL(3, key_value.first);
+      EXPECT(typeid(Pose3) == typeid(key_value.second));
+      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.second)));
     } else {
       EXPECT(false);
     }
+    ++ i;
   }
   LONGS_EQUAL(2, i);
 
   // Filter by type
   i = 0;
-  for(Values::type_filter_iterator<Pose3>::type it = values.beginFilterByType<Pose3>();
-      it != values.endFilterByType<Pose3>(); ++it, ++i) {
+  BOOST_FOREACH(const Values::Filtered<Pose3>::value_type& key_value, values.filter<Pose3>()) {
     if(i == 0) {
-      LONGS_EQUAL(1, it->first);
-      EXPECT(assert_equal(pose1, it->second));
+      LONGS_EQUAL(1, key_value.first);
+      EXPECT(assert_equal(pose1, key_value.second));
     } else if(i == 1) {
-      LONGS_EQUAL(3, it->first);
-      EXPECT(assert_equal(pose3, it->second));
+      LONGS_EQUAL(3, key_value.first);
+      EXPECT(assert_equal(pose3, key_value.second));
     } else {
       EXPECT(false);
     }
+    ++ i;
   }
   LONGS_EQUAL(2, i);
 }

From 9121df78839ad9211d056b650f303caffd0871f7 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 24 Feb 2012 16:45:50 +0000
Subject: [PATCH 88/88] Removed unused files

---
 gtsam/inference/Doxyfile                 | 1417 ----------------------
 gtsam/inference/tests/timeSymbolMaps.cpp |  190 ---
 2 files changed, 1607 deletions(-)
 delete mode 100644 gtsam/inference/Doxyfile
 delete mode 100644 gtsam/inference/tests/timeSymbolMaps.cpp

diff --git a/gtsam/inference/Doxyfile b/gtsam/inference/Doxyfile
deleted file mode 100644
index 97b4f769d..000000000
--- a/gtsam/inference/Doxyfile
+++ /dev/null
@@ -1,1417 +0,0 @@
-# Doxyfile 1.5.6
-
-# This file describes the settings to be used by the documentation system
-# doxygen (www.doxygen.org) for a project
-#
-# All text after a hash (#) is considered a comment and will be ignored
-# The format is:
-#       TAG = value [value, ...]
-# For lists items can also be appended using:
-#       TAG += value [value, ...]
-# Values that contain spaces should be placed between quotes (" ")
-
-#---------------------------------------------------------------------------
-# Project related configuration options
-#---------------------------------------------------------------------------
-
-# This tag specifies the encoding used for all characters in the config file 
-# that follow. The default is UTF-8 which is also the encoding used for all 
-# text before the first occurrence of this tag. Doxygen uses libiconv (or the 
-# iconv built into libc) for the transcoding. See 
-# http://www.gnu.org/software/libiconv for the list of possible encodings.
-
-DOXYFILE_ENCODING = UTF-8
-
-# The PROJECT_NAME tag is a single word (or a sequence of words surrounded 
-# by quotes) that should identify the project.
-
-PROJECT_NAME = gtsam
-
-# The PROJECT_NUMBER tag can be used to enter a project or revision number. 
-# This could be handy for archiving the generated documentation or 
-# if some version control system is used.
-
-PROJECT_NUMBER = 
-
-# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) 
-# base path where the generated documentation will be put. 
-# If a relative path is entered, it will be relative to the location 
-# where doxygen was started. If left blank the current directory will be used.
-
-OUTPUT_DIRECTORY = 
-
-# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 
-# 4096 sub-directories (in 2 levels) under the output directory of each output 
-# format and will distribute the generated files over these directories. 
-# Enabling this option can be useful when feeding doxygen a huge amount of 
-# source files, where putting all generated files in the same directory would 
-# otherwise cause performance problems for the file system.
-
-CREATE_SUBDIRS = NO
-
-# The OUTPUT_LANGUAGE tag is used to specify the language in which all 
-# documentation generated by doxygen is written. Doxygen will use this 
-# information to generate all constant output in the proper language. 
-# The default language is English, other supported languages are: 
-# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, 
-# Croatian, Czech, Danish, Dutch, Farsi, Finnish, French, German, Greek, 
-# Hungarian, Italian, Japanese, Japanese-en (Japanese with English messages), 
-# Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, Polish, 
-# Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, Swedish, 
-# and Ukrainian.
-
-OUTPUT_LANGUAGE = English
-
-# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will 
-# include brief member descriptions after the members that are listed in 
-# the file and class documentation (similar to JavaDoc). 
-# Set to NO to disable this.
-
-BRIEF_MEMBER_DESC = YES
-
-# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend 
-# the brief description of a member or function before the detailed description. 
-# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the 
-# brief descriptions will be completely suppressed.
-
-REPEAT_BRIEF = YES
-
-# This tag implements a quasi-intelligent brief description abbreviator 
-# that is used to form the text in various listings. Each string 
-# in this list, if found as the leading text of the brief description, will be 
-# stripped from the text and the result after processing the whole list, is 
-# used as the annotated text. Otherwise, the brief description is used as-is. 
-# If left blank, the following values are used ("$name" is automatically 
-# replaced with the name of the entity): "The $name class" "The $name widget" 
-# "The $name file" "is" "provides" "specifies" "contains" 
-# "represents" "a" "an" "the"
-
-ABBREVIATE_BRIEF = 
-
-# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then 
-# Doxygen will generate a detailed section even if there is only a brief 
-# description.
-
-ALWAYS_DETAILED_SEC = NO
-
-# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all 
-# inherited members of a class in the documentation of that class as if those 
-# members were ordinary class members. Constructors, destructors and assignment 
-# operators of the base classes will not be shown.
-
-INLINE_INHERITED_MEMB = NO
-
-# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full 
-# path before files name in the file list and in the header files. If set 
-# to NO the shortest path that makes the file name unique will be used.
-
-FULL_PATH_NAMES = YES
-
-# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag 
-# can be used to strip a user-defined part of the path. Stripping is 
-# only done if one of the specified strings matches the left-hand part of 
-# the path. The tag can be used to show relative paths in the file list. 
-# If left blank the directory from which doxygen is run is used as the 
-# path to strip.
-
-STRIP_FROM_PATH = 
-
-# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of 
-# the path mentioned in the documentation of a class, which tells 
-# the reader which header file to include in order to use a class. 
-# If left blank only the name of the header file containing the class 
-# definition is used. Otherwise one should specify the include paths that 
-# are normally passed to the compiler using the -I flag.
-
-STRIP_FROM_INC_PATH = 
-
-# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter 
-# (but less readable) file names. This can be useful is your file systems 
-# doesn't support long names like on DOS, Mac, or CD-ROM.
-
-SHORT_NAMES = NO
-
-# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen 
-# will interpret the first line (until the first dot) of a JavaDoc-style 
-# comment as the brief description. If set to NO, the JavaDoc 
-# comments will behave just like regular Qt-style comments 
-# (thus requiring an explicit @brief command for a brief description.)
-
-JAVADOC_AUTOBRIEF = NO
-
-# If the QT_AUTOBRIEF tag is set to YES then Doxygen will 
-# interpret the first line (until the first dot) of a Qt-style 
-# comment as the brief description. If set to NO, the comments 
-# will behave just like regular Qt-style comments (thus requiring 
-# an explicit \brief command for a brief description.)
-
-QT_AUTOBRIEF = NO
-
-# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen 
-# treat a multi-line C++ special comment block (i.e. a block of //! or /// 
-# comments) as a brief description. This used to be the default behaviour. 
-# The new default is to treat a multi-line C++ comment block as a detailed 
-# description. Set this tag to YES if you prefer the old behaviour instead.
-
-MULTILINE_CPP_IS_BRIEF = NO
-
-# If the DETAILS_AT_TOP tag is set to YES then Doxygen 
-# will output the detailed description near the top, like JavaDoc.
-# If set to NO, the detailed description appears after the member 
-# documentation.
-
-DETAILS_AT_TOP = NO
-
-# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented 
-# member inherits the documentation from any documented member that it 
-# re-implements.
-
-INHERIT_DOCS = YES
-
-# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce 
-# a new page for each member. If set to NO, the documentation of a member will 
-# be part of the file/class/namespace that contains it.
-
-SEPARATE_MEMBER_PAGES = NO
-
-# The TAB_SIZE tag can be used to set the number of spaces in a tab. 
-# Doxygen uses this value to replace tabs by spaces in code fragments.
-
-TAB_SIZE = 8
-
-# This tag can be used to specify a number of aliases that acts 
-# as commands in the documentation. An alias has the form "name=value". 
-# For example adding "sideeffect=\par Side Effects:\n" will allow you to 
-# put the command \sideeffect (or @sideeffect) in the documentation, which 
-# will result in a user-defined paragraph with heading "Side Effects:". 
-# You can put \n's in the value part of an alias to insert newlines.
-
-ALIASES = 
-
-# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C 
-# sources only. Doxygen will then generate output that is more tailored for C. 
-# For instance, some of the names that are used will be different. The list 
-# of all members will be omitted, etc.
-
-OPTIMIZE_OUTPUT_FOR_C = NO
-
-# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java 
-# sources only. Doxygen will then generate output that is more tailored for 
-# Java. For instance, namespaces will be presented as packages, qualified 
-# scopes will look different, etc.
-
-OPTIMIZE_OUTPUT_JAVA = NO
-
-# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran 
-# sources only. Doxygen will then generate output that is more tailored for 
-# Fortran.
-
-OPTIMIZE_FOR_FORTRAN = NO
-
-# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL 
-# sources. Doxygen will then generate output that is tailored for 
-# VHDL.
-
-OPTIMIZE_OUTPUT_VHDL = NO
-
-# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want 
-# to include (a tag file for) the STL sources as input, then you should 
-# set this tag to YES in order to let doxygen match functions declarations and 
-# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. 
-# func(std::string) {}). This also make the inheritance and collaboration 
-# diagrams that involve STL classes more complete and accurate.
-
-BUILTIN_STL_SUPPORT = NO
-
-# If you use Microsoft's C++/CLI language, you should set this option to YES to
-# enable parsing support.
-
-CPP_CLI_SUPPORT = NO
-
-# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. 
-# Doxygen will parse them like normal C++ but will assume all classes use public 
-# instead of private inheritance when no explicit protection keyword is present.
-
-SIP_SUPPORT = NO
-
-# For Microsoft's IDL there are propget and propput attributes to indicate getter 
-# and setter methods for a property. Setting this option to YES (the default) 
-# will make doxygen to replace the get and set methods by a property in the 
-# documentation. This will only work if the methods are indeed getting or 
-# setting a simple type. If this is not the case, or you want to show the 
-# methods anyway, you should set this option to NO.
-
-IDL_PROPERTY_SUPPORT = YES
-
-# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC 
-# tag is set to YES, then doxygen will reuse the documentation of the first 
-# member in the group (if any) for the other members of the group. By default 
-# all members of a group must be documented explicitly.
-
-DISTRIBUTE_GROUP_DOC = NO
-
-# Set the SUBGROUPING tag to YES (the default) to allow class member groups of 
-# the same type (for instance a group of public functions) to be put as a 
-# subgroup of that type (e.g. under the Public Functions section). Set it to 
-# NO to prevent subgrouping. Alternatively, this can be done per class using 
-# the \nosubgrouping command.
-
-SUBGROUPING = YES
-
-# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum 
-# is documented as struct, union, or enum with the name of the typedef. So 
-# typedef struct TypeS {} TypeT, will appear in the documentation as a struct 
-# with name TypeT. When disabled the typedef will appear as a member of a file, 
-# namespace, or class. And the struct will be named TypeS. This can typically 
-# be useful for C code in case the coding convention dictates that all compound 
-# types are typedef'ed and only the typedef is referenced, never the tag name.
-
-TYPEDEF_HIDES_STRUCT = NO
-
-#---------------------------------------------------------------------------
-# Build related configuration options
-#---------------------------------------------------------------------------
-
-# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in 
-# documentation are documented, even if no documentation was available. 
-# Private class members and static file members will be hidden unless 
-# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
-
-EXTRACT_ALL = YES
-
-# If the EXTRACT_PRIVATE tag is set to YES all private members of a class 
-# will be included in the documentation.
-
-EXTRACT_PRIVATE = YES
-
-# If the EXTRACT_STATIC tag is set to YES all static members of a file 
-# will be included in the documentation.
-
-EXTRACT_STATIC = YES
-
-# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) 
-# defined locally in source files will be included in the documentation. 
-# If set to NO only classes defined in header files are included.
-
-EXTRACT_LOCAL_CLASSES = YES
-
-# This flag is only useful for Objective-C code. When set to YES local 
-# methods, which are defined in the implementation section but not in 
-# the interface are included in the documentation. 
-# If set to NO (the default) only methods in the interface are included.
-
-EXTRACT_LOCAL_METHODS = NO
-
-# If this flag is set to YES, the members of anonymous namespaces will be 
-# extracted and appear in the documentation as a namespace called 
-# 'anonymous_namespace{file}', where file will be replaced with the base 
-# name of the file that contains the anonymous namespace. By default 
-# anonymous namespace are hidden.
-
-EXTRACT_ANON_NSPACES = NO
-
-# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all 
-# undocumented members of documented classes, files or namespaces. 
-# If set to NO (the default) these members will be included in the 
-# various overviews, but no documentation section is generated. 
-# This option has no effect if EXTRACT_ALL is enabled.
-
-HIDE_UNDOC_MEMBERS = NO
-
-# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all 
-# undocumented classes that are normally visible in the class hierarchy. 
-# If set to NO (the default) these classes will be included in the various 
-# overviews. This option has no effect if EXTRACT_ALL is enabled.
-
-HIDE_UNDOC_CLASSES = NO
-
-# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all 
-# friend (class|struct|union) declarations. 
-# If set to NO (the default) these declarations will be included in the 
-# documentation.
-
-HIDE_FRIEND_COMPOUNDS = NO
-
-# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any 
-# documentation blocks found inside the body of a function. 
-# If set to NO (the default) these blocks will be appended to the 
-# function's detailed documentation block.
-
-HIDE_IN_BODY_DOCS = NO
-
-# The INTERNAL_DOCS tag determines if documentation 
-# that is typed after a \internal command is included. If the tag is set 
-# to NO (the default) then the documentation will be excluded. 
-# Set it to YES to include the internal documentation.
-
-INTERNAL_DOCS = NO
-
-# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate 
-# file names in lower-case letters. If set to YES upper-case letters are also 
-# allowed. This is useful if you have classes or files whose names only differ 
-# in case and if your file system supports case sensitive file names. Windows 
-# and Mac users are advised to set this option to NO.
-
-CASE_SENSE_NAMES = YES
-
-# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen 
-# will show members with their full class and namespace scopes in the 
-# documentation. If set to YES the scope will be hidden.
-
-HIDE_SCOPE_NAMES = NO
-
-# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen 
-# will put a list of the files that are included by a file in the documentation 
-# of that file.
-
-SHOW_INCLUDE_FILES = YES
-
-# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] 
-# is inserted in the documentation for inline members.
-
-INLINE_INFO = YES
-
-# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen 
-# will sort the (detailed) documentation of file and class members 
-# alphabetically by member name. If set to NO the members will appear in 
-# declaration order.
-
-SORT_MEMBER_DOCS = YES
-
-# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the 
-# brief documentation of file, namespace and class members alphabetically 
-# by member name. If set to NO (the default) the members will appear in 
-# declaration order.
-
-SORT_BRIEF_DOCS = NO
-
-# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the 
-# hierarchy of group names into alphabetical order. If set to NO (the default) 
-# the group names will appear in their defined order.
-
-SORT_GROUP_NAMES = NO
-
-# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be 
-# sorted by fully-qualified names, including namespaces. If set to 
-# NO (the default), the class list will be sorted only by class name, 
-# not including the namespace part. 
-# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
-# Note: This option applies only to the class list, not to the 
-# alphabetical list.
-
-SORT_BY_SCOPE_NAME = NO
-
-# The GENERATE_TODOLIST tag can be used to enable (YES) or 
-# disable (NO) the todo list. This list is created by putting \todo 
-# commands in the documentation.
-
-GENERATE_TODOLIST = YES
-
-# The GENERATE_TESTLIST tag can be used to enable (YES) or 
-# disable (NO) the test list. This list is created by putting \test 
-# commands in the documentation.
-
-GENERATE_TESTLIST = YES
-
-# The GENERATE_BUGLIST tag can be used to enable (YES) or 
-# disable (NO) the bug list. This list is created by putting \bug 
-# commands in the documentation.
-
-GENERATE_BUGLIST = YES
-
-# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or 
-# disable (NO) the deprecated list. This list is created by putting 
-# \deprecated commands in the documentation.
-
-GENERATE_DEPRECATEDLIST = YES
-
-# The ENABLED_SECTIONS tag can be used to enable conditional 
-# documentation sections, marked by \if sectionname ... \endif.
-
-ENABLED_SECTIONS = 
-
-# The MAX_INITIALIZER_LINES tag determines the maximum number of lines 
-# the initial value of a variable or define consists of for it to appear in 
-# the documentation. If the initializer consists of more lines than specified 
-# here it will be hidden. Use a value of 0 to hide initializers completely. 
-# The appearance of the initializer of individual variables and defines in the 
-# documentation can be controlled using \showinitializer or \hideinitializer 
-# command in the documentation regardless of this setting.
-
-MAX_INITIALIZER_LINES = 30
-
-# Set the SHOW_USED_FILES tag to NO to disable the list of files generated 
-# at the bottom of the documentation of classes and structs. If set to YES the 
-# list will mention the files that were used to generate the documentation.
-
-SHOW_USED_FILES = YES
-
-# If the sources in your project are distributed over multiple directories 
-# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy 
-# in the documentation. The default is NO.
-
-SHOW_DIRECTORIES = NO
-
-# Set the SHOW_FILES tag to NO to disable the generation of the Files page.
-# This will remove the Files entry from the Quick Index and from the 
-# Folder Tree View (if specified). The default is YES.
-
-SHOW_FILES = YES
-
-# Set the SHOW_NAMESPACES tag to NO to disable the generation of the 
-# Namespaces page.  This will remove the Namespaces entry from the Quick Index
-# and from the Folder Tree View (if specified). The default is YES.
-
-SHOW_NAMESPACES = YES
-
-# The FILE_VERSION_FILTER tag can be used to specify a program or script that 
-# doxygen should invoke to get the current version for each file (typically from 
-# the version control system). Doxygen will invoke the program by executing (via 
-# popen()) the command <command> <input-file>, where <command> is the value of 
-# the FILE_VERSION_FILTER tag, and <input-file> is the name of an input file 
-# provided by doxygen. Whatever the program writes to standard output 
-# is used as the file version. See the manual for examples.
-
-FILE_VERSION_FILTER = 
-
-#---------------------------------------------------------------------------
-# configuration options related to warning and progress messages
-#---------------------------------------------------------------------------
-
-# The QUIET tag can be used to turn on/off the messages that are generated 
-# by doxygen. Possible values are YES and NO. If left blank NO is used.
-
-QUIET = NO
-
-# The WARNINGS tag can be used to turn on/off the warning messages that are 
-# generated by doxygen. Possible values are YES and NO. If left blank 
-# NO is used.
-
-WARNINGS = YES
-
-# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings 
-# for undocumented members. If EXTRACT_ALL is set to YES then this flag will 
-# automatically be disabled.
-
-WARN_IF_UNDOCUMENTED = YES
-
-# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for 
-# potential errors in the documentation, such as not documenting some 
-# parameters in a documented function, or documenting parameters that 
-# don't exist or using markup commands wrongly.
-
-WARN_IF_DOC_ERROR = YES
-
-# This WARN_NO_PARAMDOC option can be abled to get warnings for 
-# functions that are documented, but have no documentation for their parameters 
-# or return value. If set to NO (the default) doxygen will only warn about 
-# wrong or incomplete parameter documentation, but not about the absence of 
-# documentation.
-
-WARN_NO_PARAMDOC = NO
-
-# The WARN_FORMAT tag determines the format of the warning messages that 
-# doxygen can produce. The string should contain the $file, $line, and $text 
-# tags, which will be replaced by the file and line number from which the 
-# warning originated and the warning text. Optionally the format may contain 
-# $version, which will be replaced by the version of the file (if it could 
-# be obtained via FILE_VERSION_FILTER)
-
-WARN_FORMAT = "$file:$line: $text"
-
-# The WARN_LOGFILE tag can be used to specify a file to which warning 
-# and error messages should be written. If left blank the output is written 
-# to stderr.
-
-WARN_LOGFILE = 
-
-#---------------------------------------------------------------------------
-# configuration options related to the input files
-#---------------------------------------------------------------------------
-
-# The INPUT tag can be used to specify the files and/or directories that contain 
-# documented source files. You may enter file names like "myfile.cpp" or 
-# directories like "/usr/src/myproject". Separate the files or directories 
-# with spaces.
-
-INPUT = 
-
-# This tag can be used to specify the character encoding of the source files 
-# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is 
-# also the default input encoding. Doxygen uses libiconv (or the iconv built 
-# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for 
-# the list of possible encodings.
-
-INPUT_ENCODING = UTF-8
-
-# If the value of the INPUT tag contains directories, you can use the 
-# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
-# and *.h) to filter out the source-files in the directories. If left 
-# blank the following patterns are tested: 
-# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx 
-# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90
-
-FILE_PATTERNS = 
-
-# The RECURSIVE tag can be used to turn specify whether or not subdirectories 
-# should be searched for input files as well. Possible values are YES and NO. 
-# If left blank NO is used.
-
-RECURSIVE = NO
-
-# The EXCLUDE tag can be used to specify files and/or directories that should 
-# excluded from the INPUT source files. This way you can easily exclude a 
-# subdirectory from a directory tree whose root is specified with the INPUT tag.
-
-EXCLUDE = 
-
-# The EXCLUDE_SYMLINKS tag can be used select whether or not files or 
-# directories that are symbolic links (a Unix filesystem feature) are excluded 
-# from the input.
-
-EXCLUDE_SYMLINKS = NO
-
-# If the value of the INPUT tag contains directories, you can use the 
-# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude 
-# certain files from those directories. Note that the wildcards are matched 
-# against the file with absolute path, so to exclude all test directories 
-# for example use the pattern */test/*
-
-EXCLUDE_PATTERNS = 
-
-# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names 
-# (namespaces, classes, functions, etc.) that should be excluded from the 
-# output. The symbol name can be a fully qualified name, a word, or if the 
-# wildcard * is used, a substring. Examples: ANamespace, AClass, 
-# AClass::ANamespace, ANamespace::*Test
-
-EXCLUDE_SYMBOLS = 
-
-# The EXAMPLE_PATH tag can be used to specify one or more files or 
-# directories that contain example code fragments that are included (see 
-# the \include command).
-
-EXAMPLE_PATH = 
-
-# If the value of the EXAMPLE_PATH tag contains directories, you can use the 
-# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
-# and *.h) to filter out the source-files in the directories. If left 
-# blank all files are included.
-
-EXAMPLE_PATTERNS = 
-
-# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be 
-# searched for input files to be used with the \include or \dontinclude 
-# commands irrespective of the value of the RECURSIVE tag. 
-# Possible values are YES and NO. If left blank NO is used.
-
-EXAMPLE_RECURSIVE = NO
-
-# The IMAGE_PATH tag can be used to specify one or more files or 
-# directories that contain image that are included in the documentation (see 
-# the \image command).
-
-IMAGE_PATH = 
-
-# The INPUT_FILTER tag can be used to specify a program that doxygen should 
-# invoke to filter for each input file. Doxygen will invoke the filter program 
-# by executing (via popen()) the command <filter> <input-file>, where <filter> 
-# is the value of the INPUT_FILTER tag, and <input-file> is the name of an 
-# input file. Doxygen will then use the output that the filter program writes 
-# to standard output.  If FILTER_PATTERNS is specified, this tag will be 
-# ignored.
-
-INPUT_FILTER = 
-
-# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern 
-# basis.  Doxygen will compare the file name with each pattern and apply the 
-# filter if there is a match.  The filters are a list of the form: 
-# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further 
-# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER 
-# is applied to all files.
-
-FILTER_PATTERNS = 
-
-# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using 
-# INPUT_FILTER) will be used to filter the input files when producing source 
-# files to browse (i.e. when SOURCE_BROWSER is set to YES).
-
-FILTER_SOURCE_FILES = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to source browsing
-#---------------------------------------------------------------------------
-
-# If the SOURCE_BROWSER tag is set to YES then a list of source files will 
-# be generated. Documented entities will be cross-referenced with these sources. 
-# Note: To get rid of all source code in the generated output, make sure also 
-# VERBATIM_HEADERS is set to NO.
-
-SOURCE_BROWSER = NO
-
-# Setting the INLINE_SOURCES tag to YES will include the body 
-# of functions and classes directly in the documentation.
-
-INLINE_SOURCES = YES
-
-# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct 
-# doxygen to hide any special comment blocks from generated source code 
-# fragments. Normal C and C++ comments will always remain visible.
-
-STRIP_CODE_COMMENTS = YES
-
-# If the REFERENCED_BY_RELATION tag is set to YES 
-# then for each documented function all documented 
-# functions referencing it will be listed.
-
-REFERENCED_BY_RELATION = NO
-
-# If the REFERENCES_RELATION tag is set to YES 
-# then for each documented function all documented entities 
-# called/used by that function will be listed.
-
-REFERENCES_RELATION = NO
-
-# If the REFERENCES_LINK_SOURCE tag is set to YES (the default)
-# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from
-# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will
-# link to the source code.  Otherwise they will link to the documentstion.
-
-REFERENCES_LINK_SOURCE = YES
-
-# If the USE_HTAGS tag is set to YES then the references to source code 
-# will point to the HTML generated by the htags(1) tool instead of doxygen 
-# built-in source browser. The htags tool is part of GNU's global source 
-# tagging system (see http://www.gnu.org/software/global/global.html). You 
-# will need version 4.8.6 or higher.
-
-USE_HTAGS = NO
-
-# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen 
-# will generate a verbatim copy of the header file for each class for 
-# which an include is specified. Set to NO to disable this.
-
-VERBATIM_HEADERS = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the alphabetical class index
-#---------------------------------------------------------------------------
-
-# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index 
-# of all compounds will be generated. Enable this if the project 
-# contains a lot of classes, structs, unions or interfaces.
-
-ALPHABETICAL_INDEX = NO
-
-# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then 
-# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns 
-# in which this list will be split (can be a number in the range [1..20])
-
-COLS_IN_ALPHA_INDEX = 5
-
-# In case all classes in a project start with a common prefix, all 
-# classes will be put under the same header in the alphabetical index. 
-# The IGNORE_PREFIX tag can be used to specify one or more prefixes that 
-# should be ignored while generating the index headers.
-
-IGNORE_PREFIX = 
-
-#---------------------------------------------------------------------------
-# configuration options related to the HTML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_HTML tag is set to YES (the default) Doxygen will 
-# generate HTML output.
-
-GENERATE_HTML = YES
-
-# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `html' will be used as the default path.
-
-HTML_OUTPUT = html
-
-# The HTML_FILE_EXTENSION tag can be used to specify the file extension for 
-# each generated HTML page (for example: .htm,.php,.asp). If it is left blank 
-# doxygen will generate files with .html extension.
-
-HTML_FILE_EXTENSION = .html
-
-# The HTML_HEADER tag can be used to specify a personal HTML header for 
-# each generated HTML page. If it is left blank doxygen will generate a 
-# standard header.
-
-HTML_HEADER = 
-
-# The HTML_FOOTER tag can be used to specify a personal HTML footer for 
-# each generated HTML page. If it is left blank doxygen will generate a 
-# standard footer.
-
-HTML_FOOTER = 
-
-# The HTML_STYLESHEET tag can be used to specify a user-defined cascading 
-# style sheet that is used by each HTML page. It can be used to 
-# fine-tune the look of the HTML output. If the tag is left blank doxygen 
-# will generate a default style sheet. Note that doxygen will try to copy 
-# the style sheet file to the HTML output directory, so don't put your own 
-# stylesheet in the HTML output directory as well, or it will be erased!
-
-HTML_STYLESHEET = 
-
-# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, 
-# files or namespaces will be aligned in HTML using tables. If set to 
-# NO a bullet list will be used.
-
-HTML_ALIGN_MEMBERS = YES
-
-# If the GENERATE_HTMLHELP tag is set to YES, additional index files 
-# will be generated that can be used as input for tools like the 
-# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) 
-# of the generated HTML documentation.
-
-GENERATE_HTMLHELP = NO
-
-# If the GENERATE_DOCSET tag is set to YES, additional index files 
-# will be generated that can be used as input for Apple's Xcode 3 
-# integrated development environment, introduced with OSX 10.5 (Leopard). 
-# To create a documentation set, doxygen will generate a Makefile in the 
-# HTML output directory. Running make will produce the docset in that 
-# directory and running "make install" will install the docset in 
-# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find 
-# it at startup.
-
-GENERATE_DOCSET = NO
-
-# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the 
-# feed. A documentation feed provides an umbrella under which multiple 
-# documentation sets from a single provider (such as a company or product suite) 
-# can be grouped.
-
-DOCSET_FEEDNAME = "Doxygen generated docs"
-
-# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that 
-# should uniquely identify the documentation set bundle. This should be a 
-# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen 
-# will append .docset to the name.
-
-DOCSET_BUNDLE_ID = org.doxygen.Project
-
-# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML 
-# documentation will contain sections that can be hidden and shown after the 
-# page has loaded. For this to work a browser that supports 
-# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox 
-# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari).
-
-HTML_DYNAMIC_SECTIONS = NO
-
-# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can 
-# be used to specify the file name of the resulting .chm file. You 
-# can add a path in front of the file if the result should not be 
-# written to the html output directory.
-
-CHM_FILE = 
-
-# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can 
-# be used to specify the location (absolute path including file name) of 
-# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run 
-# the HTML help compiler on the generated index.hhp.
-
-HHC_LOCATION = 
-
-# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag 
-# controls if a separate .chi index file is generated (YES) or that 
-# it should be included in the master .chm file (NO).
-
-GENERATE_CHI = NO
-
-# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING
-# is used to encode HtmlHelp index (hhk), content (hhc) and project file
-# content.
-
-CHM_INDEX_ENCODING = 
-
-# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag 
-# controls whether a binary table of contents is generated (YES) or a 
-# normal table of contents (NO) in the .chm file.
-
-BINARY_TOC = NO
-
-# The TOC_EXPAND flag can be set to YES to add extra items for group members 
-# to the contents of the HTML help documentation and to the tree view.
-
-TOC_EXPAND = NO
-
-# The DISABLE_INDEX tag can be used to turn on/off the condensed index at 
-# top of each HTML page. The value NO (the default) enables the index and 
-# the value YES disables it.
-
-DISABLE_INDEX = NO
-
-# This tag can be used to set the number of enum values (range [1..20]) 
-# that doxygen will group on one line in the generated HTML documentation.
-
-ENUM_VALUES_PER_LINE = 4
-
-# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
-# structure should be generated to display hierarchical information.
-# If the tag value is set to FRAME, a side panel will be generated
-# containing a tree-like index structure (just like the one that 
-# is generated for HTML Help). For this to work a browser that supports 
-# JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, 
-# Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are 
-# probably better off using the HTML help feature. Other possible values 
-# for this tag are: HIERARCHIES, which will generate the Groups, Directories,
-# and Class Hiererachy pages using a tree view instead of an ordered list;
-# ALL, which combines the behavior of FRAME and HIERARCHIES; and NONE, which
-# disables this behavior completely. For backwards compatibility with previous
-# releases of Doxygen, the values YES and NO are equivalent to FRAME and NONE
-# respectively.
-
-GENERATE_TREEVIEW = YES
-
-# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be 
-# used to set the initial width (in pixels) of the frame in which the tree 
-# is shown.
-
-TREEVIEW_WIDTH = 250
-
-# Use this tag to change the font size of Latex formulas included 
-# as images in the HTML documentation. The default is 10. Note that 
-# when you change the font size after a successful doxygen run you need 
-# to manually remove any form_*.png images from the HTML output directory 
-# to force them to be regenerated.
-
-FORMULA_FONTSIZE = 10
-
-#---------------------------------------------------------------------------
-# configuration options related to the LaTeX output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will 
-# generate Latex output.
-
-GENERATE_LATEX = NO
-
-# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `latex' will be used as the default path.
-
-LATEX_OUTPUT = latex
-
-# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be 
-# invoked. If left blank `latex' will be used as the default command name.
-
-LATEX_CMD_NAME = latex
-
-# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to 
-# generate index for LaTeX. If left blank `makeindex' will be used as the 
-# default command name.
-
-MAKEINDEX_CMD_NAME = makeindex
-
-# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact 
-# LaTeX documents. This may be useful for small projects and may help to 
-# save some trees in general.
-
-COMPACT_LATEX = NO
-
-# The PAPER_TYPE tag can be used to set the paper type that is used 
-# by the printer. Possible values are: a4, a4wide, letter, legal and 
-# executive. If left blank a4wide will be used.
-
-PAPER_TYPE = a4wide
-
-# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX 
-# packages that should be included in the LaTeX output.
-
-EXTRA_PACKAGES = 
-
-# The LATEX_HEADER tag can be used to specify a personal LaTeX header for 
-# the generated latex document. The header should contain everything until 
-# the first chapter. If it is left blank doxygen will generate a 
-# standard header. Notice: only use this tag if you know what you are doing!
-
-LATEX_HEADER = 
-
-# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated 
-# is prepared for conversion to pdf (using ps2pdf). The pdf file will 
-# contain links (just like the HTML output) instead of page references 
-# This makes the output suitable for online browsing using a pdf viewer.
-
-PDF_HYPERLINKS = YES
-
-# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of 
-# plain latex in the generated Makefile. Set this option to YES to get a 
-# higher quality PDF documentation.
-
-USE_PDFLATEX = YES
-
-# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. 
-# command to the generated LaTeX files. This will instruct LaTeX to keep 
-# running if errors occur, instead of asking the user for help. 
-# This option is also used when generating formulas in HTML.
-
-LATEX_BATCHMODE = NO
-
-# If LATEX_HIDE_INDICES is set to YES then doxygen will not 
-# include the index chapters (such as File Index, Compound Index, etc.) 
-# in the output.
-
-LATEX_HIDE_INDICES = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the RTF output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output 
-# The RTF output is optimized for Word 97 and may not look very pretty with 
-# other RTF readers or editors.
-
-GENERATE_RTF = NO
-
-# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `rtf' will be used as the default path.
-
-RTF_OUTPUT = rtf
-
-# If the COMPACT_RTF tag is set to YES Doxygen generates more compact 
-# RTF documents. This may be useful for small projects and may help to 
-# save some trees in general.
-
-COMPACT_RTF = NO
-
-# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated 
-# will contain hyperlink fields. The RTF file will 
-# contain links (just like the HTML output) instead of page references. 
-# This makes the output suitable for online browsing using WORD or other 
-# programs which support those fields. 
-# Note: wordpad (write) and others do not support links.
-
-RTF_HYPERLINKS = NO
-
-# Load stylesheet definitions from file. Syntax is similar to doxygen's 
-# config file, i.e. a series of assignments. You only have to provide 
-# replacements, missing definitions are set to their default value.
-
-RTF_STYLESHEET_FILE = 
-
-# Set optional variables used in the generation of an rtf document. 
-# Syntax is similar to doxygen's config file.
-
-RTF_EXTENSIONS_FILE = 
-
-#---------------------------------------------------------------------------
-# configuration options related to the man page output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_MAN tag is set to YES (the default) Doxygen will 
-# generate man pages
-
-GENERATE_MAN = NO
-
-# The MAN_OUTPUT tag is used to specify where the man pages will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `man' will be used as the default path.
-
-MAN_OUTPUT = man
-
-# The MAN_EXTENSION tag determines the extension that is added to 
-# the generated man pages (default is the subroutine's section .3)
-
-MAN_EXTENSION = .3
-
-# If the MAN_LINKS tag is set to YES and Doxygen generates man output, 
-# then it will generate one additional man file for each entity 
-# documented in the real man page(s). These additional files 
-# only source the real man page, but without them the man command 
-# would be unable to find the correct page. The default is NO.
-
-MAN_LINKS = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the XML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_XML tag is set to YES Doxygen will 
-# generate an XML file that captures the structure of 
-# the code including all documentation.
-
-GENERATE_XML = NO
-
-# The XML_OUTPUT tag is used to specify where the XML pages will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `xml' will be used as the default path.
-
-XML_OUTPUT = xml
-
-# The XML_SCHEMA tag can be used to specify an XML schema, 
-# which can be used by a validating XML parser to check the 
-# syntax of the XML files.
-
-XML_SCHEMA = 
-
-# The XML_DTD tag can be used to specify an XML DTD, 
-# which can be used by a validating XML parser to check the 
-# syntax of the XML files.
-
-XML_DTD = 
-
-# If the XML_PROGRAMLISTING tag is set to YES Doxygen will 
-# dump the program listings (including syntax highlighting 
-# and cross-referencing information) to the XML output. Note that 
-# enabling this will significantly increase the size of the XML output.
-
-XML_PROGRAMLISTING = YES
-
-#---------------------------------------------------------------------------
-# configuration options for the AutoGen Definitions output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will 
-# generate an AutoGen Definitions (see autogen.sf.net) file 
-# that captures the structure of the code including all 
-# documentation. Note that this feature is still experimental 
-# and incomplete at the moment.
-
-GENERATE_AUTOGEN_DEF = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the Perl module output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_PERLMOD tag is set to YES Doxygen will 
-# generate a Perl module file that captures the structure of 
-# the code including all documentation. Note that this 
-# feature is still experimental and incomplete at the 
-# moment.
-
-GENERATE_PERLMOD = NO
-
-# If the PERLMOD_LATEX tag is set to YES Doxygen will generate 
-# the necessary Makefile rules, Perl scripts and LaTeX code to be able 
-# to generate PDF and DVI output from the Perl module output.
-
-PERLMOD_LATEX = NO
-
-# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be 
-# nicely formatted so it can be parsed by a human reader.  This is useful 
-# if you want to understand what is going on.  On the other hand, if this 
-# tag is set to NO the size of the Perl module output will be much smaller 
-# and Perl will parse it just the same.
-
-PERLMOD_PRETTY = YES
-
-# The names of the make variables in the generated doxyrules.make file 
-# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. 
-# This is useful so different doxyrules.make files included by the same 
-# Makefile don't overwrite each other's variables.
-
-PERLMOD_MAKEVAR_PREFIX = 
-
-#---------------------------------------------------------------------------
-# Configuration options related to the preprocessor   
-#---------------------------------------------------------------------------
-
-# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will 
-# evaluate all C-preprocessor directives found in the sources and include 
-# files.
-
-ENABLE_PREPROCESSING = YES
-
-# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro 
-# names in the source code. If set to NO (the default) only conditional 
-# compilation will be performed. Macro expansion can be done in a controlled 
-# way by setting EXPAND_ONLY_PREDEF to YES.
-
-MACRO_EXPANSION = NO
-
-# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES 
-# then the macro expansion is limited to the macros specified with the 
-# PREDEFINED and EXPAND_AS_DEFINED tags.
-
-EXPAND_ONLY_PREDEF = NO
-
-# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files 
-# in the INCLUDE_PATH (see below) will be search if a #include is found.
-
-SEARCH_INCLUDES = YES
-
-# The INCLUDE_PATH tag can be used to specify one or more directories that 
-# contain include files that are not input files but should be processed by 
-# the preprocessor.
-
-INCLUDE_PATH = 
-
-# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard 
-# patterns (like *.h and *.hpp) to filter out the header-files in the 
-# directories. If left blank, the patterns specified with FILE_PATTERNS will 
-# be used.
-
-INCLUDE_FILE_PATTERNS = 
-
-# The PREDEFINED tag can be used to specify one or more macro names that 
-# are defined before the preprocessor is started (similar to the -D option of 
-# gcc). The argument of the tag is a list of macros of the form: name 
-# or name=definition (no spaces). If the definition and the = are 
-# omitted =1 is assumed. To prevent a macro definition from being 
-# undefined via #undef or recursively expanded use the := operator 
-# instead of the = operator.
-
-PREDEFINED = 
-
-# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then 
-# this tag can be used to specify a list of macro names that should be expanded. 
-# The macro definition that is found in the sources will be used. 
-# Use the PREDEFINED tag if you want to use a different macro definition.
-
-EXPAND_AS_DEFINED = 
-
-# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then 
-# doxygen's preprocessor will remove all function-like macros that are alone 
-# on a line, have an all uppercase name, and do not end with a semicolon. Such 
-# function macros are typically used for boiler-plate code, and will confuse 
-# the parser if not removed.
-
-SKIP_FUNCTION_MACROS = YES
-
-#---------------------------------------------------------------------------
-# Configuration::additions related to external references   
-#---------------------------------------------------------------------------
-
-# The TAGFILES option can be used to specify one or more tagfiles. 
-# Optionally an initial location of the external documentation 
-# can be added for each tagfile. The format of a tag file without 
-# this location is as follows: 
-#   TAGFILES = file1 file2 ... 
-# Adding location for the tag files is done as follows: 
-#   TAGFILES = file1=loc1 "file2 = loc2" ... 
-# where "loc1" and "loc2" can be relative or absolute paths or 
-# URLs. If a location is present for each tag, the installdox tool 
-# does not have to be run to correct the links.
-# Note that each tag file must have a unique name
-# (where the name does NOT include the path)
-# If a tag file is not located in the directory in which doxygen 
-# is run, you must also specify the path to the tagfile here.
-
-TAGFILES = 
-
-# When a file name is specified after GENERATE_TAGFILE, doxygen will create 
-# a tag file that is based on the input files it reads.
-
-GENERATE_TAGFILE = 
-
-# If the ALLEXTERNALS tag is set to YES all external classes will be listed 
-# in the class index. If set to NO only the inherited external classes 
-# will be listed.
-
-ALLEXTERNALS = NO
-
-# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed 
-# in the modules index. If set to NO, only the current project's groups will 
-# be listed.
-
-EXTERNAL_GROUPS = YES
-
-# The PERL_PATH should be the absolute path and name of the perl script 
-# interpreter (i.e. the result of `which perl').
-
-PERL_PATH = /usr/bin/perl
-
-#---------------------------------------------------------------------------
-# Configuration options related to the dot tool   
-#---------------------------------------------------------------------------
-
-# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will 
-# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base 
-# or super classes. Setting the tag to NO turns the diagrams off. Note that 
-# this option is superseded by the HAVE_DOT option below. This is only a 
-# fallback. It is recommended to install and use dot, since it yields more 
-# powerful graphs.
-
-CLASS_DIAGRAMS = YES
-
-# You can define message sequence charts within doxygen comments using the \msc 
-# command. Doxygen will then run the mscgen tool (see 
-# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the 
-# documentation. The MSCGEN_PATH tag allows you to specify the directory where 
-# the mscgen tool resides. If left empty the tool is assumed to be found in the 
-# default search path.
-
-MSCGEN_PATH = 
-
-# If set to YES, the inheritance and collaboration graphs will hide 
-# inheritance and usage relations if the target is undocumented 
-# or is not a class.
-
-HIDE_UNDOC_RELATIONS = YES
-
-# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is 
-# available from the path. This tool is part of Graphviz, a graph visualization 
-# toolkit from AT&T and Lucent Bell Labs. The other options in this section 
-# have no effect if this option is set to NO (the default)
-
-HAVE_DOT = YES
-
-# By default doxygen will write a font called FreeSans.ttf to the output 
-# directory and reference it in all dot files that doxygen generates. This 
-# font does not include all possible unicode characters however, so when you need 
-# these (or just want a differently looking font) you can specify the font name 
-# using DOT_FONTNAME. You need need to make sure dot is able to find the font, 
-# which can be done by putting it in a standard location or by setting the 
-# DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory 
-# containing the font.
-
-DOT_FONTNAME = FreeSans
-
-# By default doxygen will tell dot to use the output directory to look for the 
-# FreeSans.ttf font (which doxygen will put there itself). If you specify a 
-# different font using DOT_FONTNAME you can set the path where dot 
-# can find it using this tag.
-
-DOT_FONTPATH = 
-
-# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen 
-# will generate a graph for each documented class showing the direct and 
-# indirect inheritance relations. Setting this tag to YES will force the 
-# the CLASS_DIAGRAMS tag to NO.
-
-CLASS_GRAPH = YES
-
-# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen 
-# will generate a graph for each documented class showing the direct and 
-# indirect implementation dependencies (inheritance, containment, and 
-# class references variables) of the class with other documented classes.
-
-COLLABORATION_GRAPH = YES
-
-# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen 
-# will generate a graph for groups, showing the direct groups dependencies
-
-GROUP_GRAPHS = YES
-
-# If the UML_LOOK tag is set to YES doxygen will generate inheritance and 
-# collaboration diagrams in a style similar to the OMG's Unified Modeling 
-# Language.
-
-UML_LOOK = NO
-
-# If set to YES, the inheritance and collaboration graphs will show the 
-# relations between templates and their instances.
-
-TEMPLATE_RELATIONS = NO
-
-# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT 
-# tags are set to YES then doxygen will generate a graph for each documented 
-# file showing the direct and indirect include dependencies of the file with 
-# other documented files.
-
-INCLUDE_GRAPH = YES
-
-# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and 
-# HAVE_DOT tags are set to YES then doxygen will generate a graph for each 
-# documented header file showing the documented files that directly or 
-# indirectly include this file.
-
-INCLUDED_BY_GRAPH = YES
-
-# If the CALL_GRAPH and HAVE_DOT options are set to YES then 
-# doxygen will generate a call dependency graph for every global function 
-# or class method. Note that enabling this option will significantly increase 
-# the time of a run. So in most cases it will be better to enable call graphs 
-# for selected functions only using the \callgraph command.
-
-CALL_GRAPH = YES
-
-# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then 
-# doxygen will generate a caller dependency graph for every global function 
-# or class method. Note that enabling this option will significantly increase 
-# the time of a run. So in most cases it will be better to enable caller 
-# graphs for selected functions only using the \callergraph command.
-
-CALLER_GRAPH = NO
-
-# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen 
-# will graphical hierarchy of all classes instead of a textual one.
-
-GRAPHICAL_HIERARCHY = YES
-
-# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES 
-# then doxygen will show the dependencies a directory has on other directories 
-# in a graphical way. The dependency relations are determined by the #include
-# relations between the files in the directories.
-
-DIRECTORY_GRAPH = YES
-
-# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images 
-# generated by dot. Possible values are png, jpg, or gif
-# If left blank png will be used.
-
-DOT_IMAGE_FORMAT = png
-
-# The tag DOT_PATH can be used to specify the path where the dot tool can be 
-# found. If left blank, it is assumed the dot tool can be found in the path.
-
-DOT_PATH = 
-
-# The DOTFILE_DIRS tag can be used to specify one or more directories that 
-# contain dot files that are included in the documentation (see the 
-# \dotfile command).
-
-DOTFILE_DIRS = 
-
-# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of 
-# nodes that will be shown in the graph. If the number of nodes in a graph 
-# becomes larger than this value, doxygen will truncate the graph, which is 
-# visualized by representing a node as a red box. Note that doxygen if the 
-# number of direct children of the root node in a graph is already larger than 
-# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note 
-# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
-
-DOT_GRAPH_MAX_NODES = 50
-
-# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the 
-# graphs generated by dot. A depth value of 3 means that only nodes reachable 
-# from the root by following a path via at most 3 edges will be shown. Nodes 
-# that lay further from the root node will be omitted. Note that setting this 
-# option to 1 or 2 may greatly reduce the computation time needed for large 
-# code bases. Also note that the size of a graph can be further restricted by 
-# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
-
-MAX_DOT_GRAPH_DEPTH = 0
-
-# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent 
-# background. This is enabled by default, which results in a transparent 
-# background. Warning: Depending on the platform used, enabling this option 
-# may lead to badly anti-aliased labels on the edges of a graph (i.e. they 
-# become hard to read).
-
-DOT_TRANSPARENT = YES
-
-# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output 
-# files in one run (i.e. multiple -o and -T options on the command line). This 
-# makes dot run faster, but since only newer versions of dot (>1.8.10) 
-# support this, this feature is disabled by default.
-
-DOT_MULTI_TARGETS = NO
-
-# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will 
-# generate a legend page explaining the meaning of the various boxes and 
-# arrows in the dot generated graphs.
-
-GENERATE_LEGEND = YES
-
-# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will 
-# remove the intermediate dot files that are used to generate 
-# the various graphs.
-
-DOT_CLEANUP = YES
-
-#---------------------------------------------------------------------------
-# Configuration::additions related to the search engine   
-#---------------------------------------------------------------------------
-
-# The SEARCHENGINE tag specifies whether or not a search engine should be 
-# used. If set to NO the values of all tags below this one will be ignored.
-
-SEARCHENGINE = NO
diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp
deleted file mode 100644
index 1912f34d3..000000000
--- a/gtsam/inference/tests/timeSymbolMaps.cpp
+++ /dev/null
@@ -1,190 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file timeSymbolMaps.cpp
- * @date Jan 20, 2010
- * @author richard
- */
-
-#include <boost/unordered_map.hpp>
-#include <string>
-#include <boost/timer.hpp>
-#include <boost/lexical_cast.hpp>
-#include <vector>
-#include <map>
-
-#include <gtsam/nonlinear/Symbol.h>
-
-using namespace std;
-using namespace boost;
-using namespace gtsam;
-
-template<class T>
-class SymbolMapExp {
-private:
-	typedef map<unsigned char, vector<T> > Map;
-	typedef vector<T> Vec;
-
-	Map values_;
-
-public:
-	typedef pair<Key, T> value_type;
-
-	SymbolMapExp() {}
-
-	T& at(Key key) {
-		typename Map::iterator it = values_.find(key.chr());
-		if(it != values_.end())
-			return it->second.at(key.index());
-		else
-			throw invalid_argument("Key " + (string)key + " not present");
-	}
-
-	void set(Key key, const T& value) {
-		Vec& vec(values_[key.chr()]);
-		//vec.reserve(10000);
-		if(key.index() >= vec.size()) {
-			vec.reserve(key.index()+1);
-			vec.resize(key.index());
-			vec.push_back(value);
-		} else
-			vec[key.index()] = value;
-	}
-};
-
-template<class T>
-class SymbolMapBinary : public std::map<Key, T> {
-private:
-	typedef std::map<Key, T> Base;
-public:
-	SymbolMapBinary() : std::map<Key, T>() {}
-
-	T& at(Key key) {
-		typename Base::iterator it = Base::find(key);
-		if (it == Base::end())
-			throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
-		return it->second;
-	}
-};
-
-struct SymbolHash : public std::unary_function<Key, std::size_t> {
-	std::size_t operator()(Key const& x) const {
-		std::size_t seed = 0;
-		boost::hash_combine(seed, x.chr());
-		boost::hash_combine(seed, x.index());
-		return ((size_t(x.chr()) << 24) & x.index());
-	}
-};
-
-template<class T>
-class SymbolMapHash : public boost::unordered_map<Key, T, SymbolHash> {
-public:
-	SymbolMapHash() : boost::unordered_map<Key, T, SymbolHash>(60000) {}
-};
-
-struct Value {
-	double v;
-	Value() : v(0.0) {}
-	Value(double vi) : v(vi) {}
-	operator string() { return lexical_cast<string>(v); }
-	bool operator!=(const Value& vc) { return v != vc.v; }
-};
-
-#define ELEMS 3000
-#define TIMEAT 300
-
-int main(int argc, char *argv[]) {
-	timer tmr;
-
-	// pre-allocate
-	cout << "Generating test data ..." << endl;
-	vector<pair<Key, Value> > values;
-	for(size_t i=0; i<ELEMS; i++) {
-		values.push_back(make_pair(Symbol('a',i), (double)i));
-		values.push_back(make_pair(Symbol('b',i), (double)i));
-		values.push_back(make_pair(Symbol('c',i), (double)i));
-	}
-
-	// time binary map
-	cout << "Timing binary map ..." << endl;
-	{
-		SymbolMapBinary<Value> binary;
-		for(size_t i=0; i<ELEMS*3; ) {
-			size_t stop = i + TIMEAT;
-			tmr.restart();
-			for( ; i<stop; i++)
-				binary.insert(values[i]);
-			double time = tmr.elapsed();
-			cout << i << " values, avg " << (time/(double)TIMEAT)*1e6 << " mu-s per insert" << endl;
-
-			tmr.restart();
-			for(size_t j=0; j<i; j++)
-				if(values[j].second != binary[values[j].first]) {
-					cout << "Wrong value!  At key " << (string)values[j].first <<
-							" expecting " << (string)values[j].second <<
-							" got " << (string)binary[values[j].first] << endl;
-				}
-			time = tmr.elapsed();
-			cout << i << " values, avg " << (time)*1e3 << " ms per lookup" << endl;
-		}
-	}
-
-	// time hash map
-	cout << "Timing hash map ..." << endl;
-	{
-		SymbolMapHash<Value> hash;
-		for(size_t i=0; i<ELEMS*3; ) {
-			size_t stop = i + TIMEAT;
-			tmr.restart();
-			for( ; i<stop; i++)
-				hash.insert(values[i]);
-			double time = tmr.elapsed();
-			cout << i << " values, avg " << (time/(double)TIMEAT)*1e6 << " mu-s per insert" << endl;
-
-			tmr.restart();
-			for(size_t j=0; j<i; j++)
-				if(values[j].second != hash[values[j].first]) {
-					cout << "Wrong value!  At key " << (string)values[j].first <<
-							" expecting " << (string)values[j].second <<
-							" got " << (string)hash[values[j].first] << endl;
-				}
-			time = tmr.elapsed();
-			cout << i << " values, avg " << (time/(double)i)*1e6 << " mu-s per lookup" << endl;
-		}
-	}
-
-	// time experimental map
-	cout << "Timing experimental map ..." << endl;
-	{
-		SymbolMapExp<Value> experimental;
-		for(size_t i=0; i<ELEMS*3; ) {
-			size_t stop = i + TIMEAT;
-			tmr.restart();
-			for( ; i<stop; i++)
-				experimental.set(values[i].first, values[i].second);
-			double time = tmr.elapsed();
-			cout << i << " values, avg " << (time/(double)TIMEAT)*1e6 << " mu-s per insert" << endl;
-
-			tmr.restart();
-			for(size_t j=0; j<i; j++)
-				if(values[j].second != experimental.at(values[j].first)) {
-					cout << "Wrong value!  At key " << (string)values[j].first <<
-							" expecting " << (string)values[j].second <<
-							" got " << (string)experimental.at(values[j].first) << endl;
-				}
-			time = tmr.elapsed();
-			cout << i << " values, avg " << (time/(double)i)*1e6 << " mu-s per lookup" << endl;
-		}
-	}
-
-	return 0;
-}