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); }
+/* ************************************************************************* */