diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 44772e3e2..982ac449b 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -43,6 +43,9 @@ public:
   /** Constructor from size */
   FastVector(size_t size) : Base(size) {}
 
+  /** Constructor from size and initial values */
+  FastVector(size_t size, const VALUE& initial) : Base(size, initial) {}
+
   /** Constructor from a range, passes through to base class */
   template<typename INPUTITERATOR>
   FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 6ed06842e..ebe59d135 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -22,10 +22,14 @@
 #include <vector>
 #include <stdexcept>
 #include <deque>
+#include <boost/function.hpp>
+#include <boost/shared_ptr.hpp>
 
 #include <gtsam/base/types.h>
+#include <gtsam/base/FastVector.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/BayesNet.h>
+#include <gtsam/linear/VectorValues.h>
 
 namespace gtsam {
 
@@ -351,4 +355,31 @@ namespace gtsam {
 
 	}; // BayesTree
 
+
+  /* ************************************************************************* */
+  template<class CONDITIONAL>
+  void _BayesTree_dim_adder(
+      std::vector<size_t>& dims,
+      const typename BayesTree<CONDITIONAL>::sharedClique& clique) {
+
+    if(clique) {
+      // Add dims from this clique
+      for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
+        dims[*it] = (*clique)->dim(it);
+
+      // Traverse children
+      BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, clique->children()) {
+        _BayesTree_dim_adder<CONDITIONAL>(dims, child);
+      }
+    }
+  }
+
+	/* ************************************************************************* */
+	template<class CONDITIONAL>
+	boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL>& bt) {
+	  std::vector<size_t> dimensions(bt.nodes().size(), 0);
+	  _BayesTree_dim_adder<CONDITIONAL>(dimensions, bt.root());
+	  return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
+	}
+
 } /// namespace gtsam
diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index 173f67c2e..533f36c05 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -24,6 +24,7 @@
 
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/graph-inl.h>
+#include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/base/DSF.h>
 
 #include <boost/foreach.hpp>
@@ -124,5 +125,30 @@ namespace gtsam {
 		return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd));
 	}
 
+  /* ************************************************************************* */
+	template<class FACTOR, class CONDITIONAL>
+	void _FactorGraph_BayesTree_adder(
+	    vector<typename FactorGraph<FACTOR>::sharedFactor>& factors,
+	    const typename BayesTree<CONDITIONAL>::sharedClique& clique) {
+
+	  if(clique) {
+	    // Add factor from this clique
+	    factors.push_back((*clique)->toFactor());
+
+	    // Traverse children
+	    BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, clique->children()) {
+	      _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL>(factors, child);
+	    }
+	  }
+	}
+
+  /* ************************************************************************* */
+  template<class FACTOR>
+  template<class CONDITIONAL>
+  FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL>& bayesTree) {
+    factors_.reserve(bayesTree.size());
+    _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL>(factors_, bayesTree.root());
+  }
+
 	/* ************************************************************************* */
 } // namespace gtsam
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index bea514cf1..91a879392 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -32,6 +32,9 @@
 
 namespace gtsam {
 
+// Forward declarations
+template<class CONDITIONAL> class BayesTree;
+
 	/**
 	 * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
 	 * In this class, however, only factor nodes are kept around.
@@ -74,6 +77,10 @@ namespace gtsam {
 		template<class CONDITIONAL>
 		FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
 
+    /** convert from Bayes net */
+		template<class CONDITIONAL>
+    FactorGraph(const BayesTree<CONDITIONAL>& bayesTree);
+
 		/** convert from a derived type */
 		template<class DERIVEDFACTOR>
 		FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) { factors_.insert(end(), factors.begin(), factors.end()); }
@@ -205,7 +212,7 @@ namespace gtsam {
 	template<class FACTORGRAPH>
 	FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
 
-	/**
+	/*
 	 * These functions are defined here because they are templated on an
 	 * additional parameter.  Putting them in the -inl.h file would mean these
 	 * would have to be explicitly instantiated for any possible derived factor
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 334c18676..5eace0094 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -25,8 +25,12 @@
 #include <gtsam/base/FastVector.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/inference/VariableSlots.h>
+#include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/inference/FactorGraph-inl.h>
+#include <gtsam/inference/VariableSlots.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/cholesky.h>
 
 using namespace std;
 using namespace gtsam;
@@ -36,9 +40,10 @@ namespace gtsam {
 	INSTANTIATE_FACTOR_GRAPH(GaussianFactor);
 
 	/* ************************************************************************* */
-	GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
-		FactorGraph<GaussianFactor> (CBN) {
-	}
+	GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {}
+
+	/* ************************************************************************* */
+	GaussianFactorGraph::GaussianFactorGraph(const BayesTree<GaussianConditional>& GBT) : Base(GBT) {}
 
 	/* ************************************************************************* */
 	GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 631d3872d..77a1ed68c 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -72,6 +72,11 @@ namespace gtsam {
      */
     GaussianFactorGraph(const GaussianBayesNet& CBN);
 
+    /**
+     * Constructor that receives a BayesTree and returns a GaussianFactorGraph
+     */
+    GaussianFactorGraph(const BayesTree<GaussianConditional>& GBT);
+
     /** Constructor from a factor graph of GaussianFactor or a derived type */
     template<class DERIVEDFACTOR>
     GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) {
diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp
index 703f97c2d..a9aa2479c 100644
--- a/gtsam/linear/GaussianISAM.cpp
+++ b/gtsam/linear/GaussianISAM.cpp
@@ -51,32 +51,44 @@ Matrix GaussianISAM::marginalCovariance(Index j) const {
 }
 
 /* ************************************************************************* */
-void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result) {
+void optimize(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result) {
 	// parents are assumed to already be solved and available in result
 	// RHS for current conditional should already be in place in result
   clique->conditional()->solveInPlace(result);
 
-	BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_)
+	BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_)
 		optimize(child, result);
 }
 
 /* ************************************************************************* */
-void GaussianISAM::treeRHS(const GaussianISAM::sharedClique& clique, VectorValues& result) {
+void treeRHS(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result) {
   clique->conditional()->rhs(result);
-	BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_)
+	BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_)
 		treeRHS(child, result);
 }
 
 /* ************************************************************************* */
-VectorValues GaussianISAM::rhs(const GaussianISAM& bayesTree) {
-	VectorValues result(bayesTree.dims_); // allocate
+VectorValues rhs(const BayesTree<GaussianConditional>& bayesTree, boost::optional<const GaussianISAM::Dims&> dims) {
+	VectorValues result;
+	if(dims)
+	  result = VectorValues(*dims);
+	else
+	  result = *allocateVectorValues(bayesTree); // allocate
 	treeRHS(bayesTree.root(), result);    // recursively fill
 	return result;
 }
 
 /* ************************************************************************* */
-VectorValues optimize(const GaussianISAM& bayesTree) {
-	VectorValues result = GaussianISAM::rhs(bayesTree);
+VectorValues optimize(const GaussianISAM& isam) {
+  VectorValues result = rhs(isam, isam.dims_);
+  // starting from the root, call optimize on each conditional
+  optimize(isam.root(), result);
+  return result;
+}
+
+/* ************************************************************************* */
+VectorValues optimize(const BayesTree<GaussianConditional>& bayesTree) {
+	VectorValues result = rhs(bayesTree);
 	// starting from the root, call optimize on each conditional
 	optimize(bayesTree.root(), result);
 	return result;
diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h
index 22923dd4e..a06c50657 100644
--- a/gtsam/linear/GaussianISAM.h
+++ b/gtsam/linear/GaussianISAM.h
@@ -19,6 +19,8 @@
 
 #pragma once
 
+#include <boost/optional.hpp>
+
 #include <gtsam/inference/ISAM.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
@@ -32,6 +34,8 @@ class GaussianISAM : public ISAM<GaussianConditional> {
 
 public:
 
+  typedef std::deque<size_t, boost::fast_pool_allocator<size_t> > Dims;
+
   /** Create an empty Bayes Tree */
   GaussianISAM() : Super() {}
 
@@ -84,20 +88,21 @@ public:
 	/** return the conditional P(S|Root) on the separator given the root */
 	static BayesNet<GaussianConditional> shortcut(sharedClique clique, sharedClique root);
 
-	/** load a VectorValues with the RHS of the system for backsubstitution */
-	static VectorValues rhs(const GaussianISAM& bayesTree);
-
-protected:
-
-	/** recursively load RHS for system */
-	static void treeRHS(const GaussianISAM::sharedClique& clique, VectorValues& result);
-
 }; // \class GaussianISAM
 
+/** load a VectorValues with the RHS of the system for backsubstitution */
+VectorValues rhs(const BayesTree<GaussianConditional>& bayesTree, boost::optional<const GaussianISAM::Dims&> dims = boost::none);
+
+/** recursively load RHS for system */
+void treeRHS(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result);
+
 // recursively optimize this conditional and all subtrees
-void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result);
+void optimize(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result);
 
 // optimize the BayesTree, starting from the root
-VectorValues optimize(const GaussianISAM& bayesTree);
+VectorValues optimize(const GaussianISAM& isam);
+
+// optimize the BayesTree, starting from the root
+VectorValues optimize(const BayesTree<GaussianConditional>& bayesTree);
 
 } // \namespace gtsam
diff --git a/gtsam/nonlinear/DoglegOptimizer-inl.h b/gtsam/nonlinear/DoglegOptimizer-inl.h
new file mode 100644
index 000000000..432844101
--- /dev/null
+++ b/gtsam/nonlinear/DoglegOptimizer-inl.h
@@ -0,0 +1,7 @@
+/**
+ * @file    DoglegOptimizer-inl.h
+ * @brief   Nonlinear factor graph optimizer using Powell's Dogleg algorithm
+ * @author  Richard Roberts
+ */
+
+#pragma once
diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h
new file mode 100644
index 000000000..b4ea90f3d
--- /dev/null
+++ b/gtsam/nonlinear/DoglegOptimizer.h
@@ -0,0 +1,67 @@
+/**
+ * @file    DoglegOptimizer.h
+ * @brief   Nonlinear factor graph optimizer using Powell's Dogleg algorithm
+ * @author  Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/**
+ * A class to perform nonlinear optimization using Powell's dogleg algorithm.
+ * This class is functional, meaning every method is \c const, and returns a new
+ * copy of the class.
+ *
+ * \tparam VALUES The LieValues or TupleValues type to hold the values to be
+ * estimated.
+ *
+ * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration,
+ * currently either GaussianSequentialSolver or GaussianMultifrontalSolver.
+ * The latter is typically faster, especially for non-trivial problems.
+ */
+template<class VALUES, class GAUSSIAN_SOLVER>
+class DoglegOptimizer {
+
+protected:
+
+  typedef DoglegOptimizer<VALUES, GAUSSIAN_SOLVER> This; ///< Typedef to this class
+
+  const sharedGraph graph_;
+  const sharedValues values_;
+  const double error_;
+
+public:
+
+  typedef VALUES ValuesType; ///< Typedef of the VALUES template parameter
+  typedef GAUSSIAN_SOLVER SolverType; ///< Typedef of the GAUSSIAN_SOLVER template parameter
+  typedef NonlinearFactorGraph<VALUES> GraphType; ///< A nonlinear factor graph templated on ValuesType
+
+  typedef boost::shared_ptr<const GraphType> sharedGraph; ///< A shared_ptr to GraphType
+  typedef boost::shared_ptr<const ValuesType> sharedValues; ///< A shared_ptr to ValuesType
+
+
+  /**
+   * Construct a DoglegOptimizer from the factor graph to optimize and the
+   * initial estimate of the variable values, using the default variable
+   * ordering method, currently COLAMD.
+   * @param graph  The factor graph to optimize
+   * @param initialization  An initial estimate of the variable values
+   */
+  DoglegOptimizer(sharedGraph graph, sharedValues initialization);
+
+  /**
+   * Construct a DoglegOptimizer from the factor graph to optimize and the
+   * initial estimate of the variable values, using the default variable
+   * ordering method, currently COLAMD. (this non-shared-pointer version
+   * incurs a performance hit for copying, see DoglegOptimizer(sharedGraph, sharedValues)).
+   * @param graph  The factor graph to optimize
+   * @param initialization  An initial estimate of the variable values
+   */
+  DoglegOptimizer(const GraphType& graph, const ValuesType& initialization);
+
+};
+
+}
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
new file mode 100644
index 000000000..f49205b58
--- /dev/null
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
@@ -0,0 +1,73 @@
+/**
+ * @file    DoglegOptimizerImpl.h
+ * @brief   Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
+
+namespace gtsam {
+/* ************************************************************************* */
+VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
+    double Delta, const VectorValues& x_u, const VectorValues& x_n) {
+
+  // Get magnitude of each update and find out which segment Delta falls in
+  assert(Delta >= 0.0);
+  double DeltaSq = Delta*Delta;
+  double x_u_norm_sq = x_u.vector().squaredNorm();
+  double x_n_norm_sq = x_n.vector().squaredNorm();
+  cout << "Steepest descent magnitude " << sqrt(x_u_norm_sq) << ", Newton's method magnitude " << sqrt(x_n_norm_sq) << endl;
+  if(DeltaSq < x_u_norm_sq) {
+    // Trust region is smaller than steepest descent update
+    VectorValues x_d = VectorValues::SameStructure(x_u);
+    x_d.vector() = x_u.vector() * sqrt(DeltaSq / x_u_norm_sq);
+    cout << "In steepest descent region with fraction " << sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
+    return x_d;
+  } else if(DeltaSq < x_n_norm_sq) {
+    // Trust region boundary is between steepest descent point and Newton's method point
+    return ComputeBlend(Delta, x_u, x_n);
+  } else {
+    assert(DeltaSq >= x_n_norm_sq);
+    cout << "In pure Newton's method region" << endl;
+    // Trust region is larger than Newton's method point
+    return x_n;
+  }
+}
+
+/* ************************************************************************* */
+VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues& x_u, const VectorValues& x_n) {
+
+  // See doc/trustregion.lyx or doc/trustregion.pdf
+
+  // Compute inner products
+  const double un = dot(x_u, x_n);
+  const double uu = dot(x_u, x_u);
+  const double nn = dot(x_n, x_n);
+
+  // Compute quadratic formula terms
+  const double a = uu - 2.*un + nn;
+  const double b = 2. * (un - uu);
+  const double c = uu - Delta*Delta;
+  double sqrt_b_m4ac = sqrt(b*b - 4*a*c);
+
+  // Compute blending parameter
+  double tau1 = (-b + sqrt_b_m4ac) / (2.*a);
+  double tau2 = (-b - sqrt_b_m4ac) / (2.*a);
+
+  double tau;
+  if(0.0 <= tau1 && tau1 <= 1.0) {
+    assert(!(0.0 <= tau2 && tau2 <= 1.0));
+    tau = tau1;
+  } else {
+    assert(0.0 <= tau2 && tau2 <= 1.0);
+    tau = tau2;
+  }
+
+  // Compute blended point
+  cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
+  VectorValues blend = VectorValues::SameStructure(x_u);
+  blend.vector() = (1. - tau) * x_u.vector() + tau * x_n.vector();
+  return blend;
+}
+
+}
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
new file mode 100644
index 000000000..7b9e2deca
--- /dev/null
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -0,0 +1,251 @@
+/**
+ * @file    DoglegOptimizerImpl.h
+ * @brief   Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
+ * @author  Richard Roberts
+ */
+#pragma once
+
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
+#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
+#include <gtsam/nonlinear/Ordering.h>
+
+namespace gtsam {
+
+/** This class contains the implementation of the Dogleg algorithm.  It is used
+ * by DoglegOptimizer and can be used to easily put together custom versions of
+ * Dogleg.  Each function is well-documented and unit-tested.  The notation
+ * here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this
+ * file for further explanation of the computations performed by this class.
+ *
+ * \tparam VALUES The LieValues or TupleValues type to hold the values to be
+ * estimated.
+ *
+ * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration,
+ * currently either GaussianSequentialSolver or GaussianMultifrontalSolver.
+ * The latter is typically faster, especially for non-trivial problems.
+ */
+struct DoglegOptimizerImpl {
+
+  struct IterationResult {
+    double Delta;
+    VectorValues dx_d;
+    double f_error;
+  };
+
+  enum TrustRegionAdaptationMode {
+    SEARCH_EACH_ITERATION,
+    ONE_STEP_PER_ITERATION
+  };
+
+  /**
+   * Compute the update point for one iteration of the Dogleg algorithm, given
+   * an initial trust region radius \f$ \Delta \f$.  The trust region radius is
+   * adapted based on the error of a NonlinearFactorGraph \f$ f(x) \f$, and
+   * depending on the update mode \c mode.
+   *
+   * The update is computed using a quadratic approximation \f$ M(\delta x) \f$
+   * of an original nonlinear error function (a NonlinearFactorGraph) \f$ f(x) \f$.
+   * The quadratic approximation is represented as a GaussianBayesNet \bayesNet, which is
+   * obtained by eliminating a GaussianFactorGraph resulting from linearizing
+   * the nonlinear factor graph \f$ f(x) \f$.  Thus, \f$ M(\delta x) \f$ is
+   * \f[
+   * M(\delta x) = (R \delta x - d)^T (R \delta x - d)
+   * \f]
+   * where \f$ R \f$ and \f$ d \f$ together are a Bayes' net or Bayes' tree.
+   * \f$ R \f$ is upper-triangular and \f$ d \f$ is a vector, in GTSAM represented
+   * as a BayesNet<GaussianConditional> (GaussianBayesNet) or
+   * BayesTree<GaussianConditional>, containing GaussianConditional s.
+   *
+   * @tparam M The type of the Bayes' net or tree, currently
+   * either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>.
+   * @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>.
+   * @tparam VALUES The LieValues or TupleValues to pass to F::error() to evaluate
+   * the error function.
+   * @param initialDelta The initial trust region radius.
+   * @param Rd The Bayes' net or tree as described above.
+   * @param f The original nonlinear factor graph with which to evaluate the
+   * accuracy of \f$ M(\delta x) \f$ to adjust \f$ \Delta \f$.
+   * @param x0 The linearization point about which \bayesNet was created
+   * @param ordering The variable ordering used to create \bayesNet
+   * @param f_error The result of <tt>f.error(x0)</tt>.
+   * @return A DoglegIterationResult containing the new \c Delta, the linear
+   * update \c dx_d, and the resulting nonlinear error \c f_error.
+   */
+  template<class M, class F, class VALUES>
+  static IterationResult Iterate(
+      double Delta, TrustRegionAdaptationMode mode, const M& Rd,
+      const F& f, const VALUES& x0, const Ordering& ordering, double f_error);
+
+  /**
+   * Compute the dogleg point given a trust region radius \f$ \Delta \f$.  The
+   * dogleg point is the intersection between the dogleg path and the trust
+   * region boundary, see doc/trustregion.pdf for more details.
+   *
+   * The update is computed using a quadratic approximation \f$ M(\delta x) \f$
+   * of an original nonlinear error function (a NonlinearFactorGraph) \f$ f(x) \f$.
+   * The quadratic approximation is represented as a GaussianBayesNet \bayesNet, which is
+   * obtained by eliminating a GaussianFactorGraph resulting from linearizing
+   * the nonlinear factor graph \f$ f(x) \f$.  Thus, \f$ M(\delta x) \f$ is
+   * \f[
+   * M(\delta x) = (R \delta x - d)^T (R \delta x - d)
+   * \f]
+   * where \f$ R \f$ and \f$ d \f$ together are a Bayes' net.  \f$ R \f$ is
+   * upper-triangular and \f$ d \f$ is a vector, in GTSAM represented as a
+   * GaussianBayesNet, containing GaussianConditional s.
+   *
+   * @param Delta The trust region radius
+   * @param bayesNet The Bayes' net \f$ (R,d) \f$ as described above.
+   * @return The dogleg point \f$ \delta x_d \f$
+   */
+  static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& x_u, const VectorValues& x_n);
+
+  /** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of
+   * the function
+   * \f[
+   * M(\delta x) = (R \delta x - d)^T (R \delta x - d)
+   * \f]
+   * where \f$ R \f$ is an upper-triangular matrix and \f$ d \f$ is a vector.
+   * Together \f$ (R,d) \f$ are either a Bayes' net or a Bayes' tree.
+   *
+   * The same quadratic error function written as a Taylor expansion of the original
+   * non-linear error function is
+   * \f[
+   * M(\delta x) = f(x_0) + g(x_0) + \frac{1}{2} \delta x^T G(x_0) \delta x,
+   * \f]
+   * @tparam M The type of the Bayes' net or tree, currently
+   * either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>.
+   * @param Rd The Bayes' net or tree \f$ (R,d) \f$ as described above, currently
+   * this must be of type BayesNet<GaussianConditional> (or GaussianBayesNet) or
+   * BayesTree<GaussianConditional>.
+   * @return The minimizer \f$ \delta x_u \f$ along the gradient descent direction.
+   */
+  template<class M>
+  static VectorValues ComputeSteepestDescentPoint(const M& Rd);
+
+  /** Compute the point on the line between the steepest descent point and the
+   * Newton's method point intersecting the trust region boundary.
+   * Mathematically, computes \f$ \tau \f$ such that \f$ 0<\tau<1 \f$ and
+   * \f$ \| (1-\tau)\delta x_u + \tau\delta x_n \| = \Delta \f$, where \f$ \Delta \f$
+   * is the trust region radius.
+   * @param Delta Trust region radius
+   * @param xu Steepest descent minimizer
+   * @param xn Newton's method minimizer
+   */
+  static VectorValues ComputeBlend(double Delta, const VectorValues& x_u, const VectorValues& x_n);
+};
+
+
+/* ************************************************************************* */
+template<class M, class F, class VALUES>
+typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
+    double Delta, TrustRegionAdaptationMode mode, const M& Rd,
+    const F& f, const VALUES& x0, const Ordering& ordering, double f_error) {
+
+  // Compute steepest descent and Newton's method points
+  VectorValues dx_u = ComputeSteepestDescentPoint(Rd);
+  VectorValues dx_n = optimize(Rd);
+  const GaussianFactorGraph jfg(Rd);
+  const double M_error = jfg.error(VectorValues::Zero(dx_u));
+
+  // Result to return
+  IterationResult result;
+
+  bool stay = true;
+  while(stay) {
+    // Compute dog leg point
+    result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n);
+
+    cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl;
+
+    // Compute expmapped solution
+    const VALUES x_d(x0.expmap(result.dx_d, ordering));
+
+    // Compute decrease in f
+    result.f_error = f.error(x_d);
+
+    // Compute decrease in M
+    const double new_M_error = jfg.error(result.dx_d);
+
+    cout << "f error: " << f_error << " -> " << result.f_error << endl;
+    cout << "M error: " << M_error << " -> " << new_M_error << endl;
+
+    // Compute gain ratio.  Here we take advantage of the invariant that the
+    // Bayes' net error at zero is equal to the nonlinear error
+    const double rho = fabs(M_error - new_M_error) < 1e-30 ?
+        0.5 :
+        (f_error - result.f_error) / (M_error - new_M_error);
+
+    cout << "rho = " << rho << endl;
+
+    if(rho >= 0.75) {
+      // M agrees very well with f, so try to increase lambda
+      const double dx_d_norm = result.dx_d.vector().norm();
+      const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
+
+
+      if(mode == ONE_STEP_PER_ITERATION)
+        stay = false;   // If not searching, just return with the new Delta
+      else if(mode == SEARCH_EACH_ITERATION) {
+        if(newDelta == Delta)
+          stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region
+        else
+          stay = true;  // Searching and increased Delta, so try again to increase Delta
+      } else {
+        assert(false); }
+
+      Delta = newDelta; // Update Delta from new Delta
+
+    } else if(0.75 > rho && rho >= 0.25) {
+      // M agrees so-so with f, keep the same Delta
+      stay = false;
+
+    } else if(0.25 > rho && rho >= 0.0) {
+      // M does not agree well with f, decrease Delta until it does
+      Delta *= 0.5;
+      if(mode == ONE_STEP_PER_ITERATION)
+        stay = false;   // If not searching, just return with the new smaller delta
+      else if(mode == SEARCH_EACH_ITERATION)
+        stay = true;
+      else {
+        assert(false); }
+    }
+
+    else {
+      // f actually increased, so keep decreasing Delta until f does not decrease
+      assert(0.0 > rho);
+      Delta *= 0.5;
+      if(Delta > 1e-5)
+        stay = true;
+      else
+        stay = false;
+    }
+  }
+
+  // dx_d and f_error have already been filled in during the loop
+  result.Delta = Delta;
+  return result;
+}
+
+/* ************************************************************************* */
+template<class M>
+VectorValues DoglegOptimizerImpl::ComputeSteepestDescentPoint(const M& Rd) {
+
+  // Compute gradient
+  // Convert to JacobianFactor's to use existing gradient function
+  FactorGraph<JacobianFactor> jfg(Rd);
+  VectorValues grad = gradient(jfg, VectorValues::Zero(*allocateVectorValues(Rd)));
+  double gradientSqNorm = grad.dot(grad);
+
+  // Compute R * g
+  Errors Rg = jfg * grad;
+
+  // Compute minimizing step size
+  double step = -gradientSqNorm / dot(Rg, Rg);
+
+  // Compute steepest descent point
+  scal(step, grad);
+  return grad;
+}
+
+}
diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am
index 184bfc81b..c187fd6f8 100644
--- a/gtsam/nonlinear/Makefile.am
+++ b/gtsam/nonlinear/Makefile.am
@@ -17,14 +17,15 @@ check_PROGRAMS =
 
 # Lie Groups
 headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h 
-check_PROGRAMS += tests/testLieValues
+check_PROGRAMS += tests/testLieValues tests/testKey tests/testOrdering
 
 # Nonlinear nonlinear
 headers += Key.h 
 headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h
 headers += NonlinearFactor.h
-sources += NonlinearOptimizer.cpp Ordering.cpp
+sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp
+headers += DoglegOptimizer.h DoglegOptimizer-inl.h
 
 # Nonlinear iSAM(2)
 headers += NonlinearISAM.h NonlinearISAM-inl.h
diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h
index c2380c672..a248f5606 100644
--- a/gtsam/nonlinear/NonlinearOptimizer-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h
@@ -24,6 +24,7 @@
 #include <boost/tuple/tuple.hpp>
 #include <gtsam/base/cholesky.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
 #define INSTANTIATE_NONLINEAR_OPTIMIZER(G,C) \
   template class NonlinearOptimizer<G,C>;
@@ -95,6 +96,7 @@ namespace gtsam {
 		if (verbosity >= Parameters::VALUES) newValues->print("newValues");
 
 		NonlinearOptimizer newOptimizer = newValues_(newValues);
+		++ newOptimizer.iterations_;
 
 		if (verbosity >= Parameters::ERROR)	cout << "error: " << newOptimizer.error_ << endl;
 		return newOptimizer;
@@ -176,6 +178,7 @@ namespace gtsam {
 		  // Try solving
 		  try {
 		    VectorValues delta = *solver->optimize();
+		    if (verbosity >= Parameters::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
 		    if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
 
 		    // update values
@@ -277,6 +280,7 @@ namespace gtsam {
 			error_ = next.error_;
 			values_ = next.values_;
 			parameters_ = next.parameters_;
+			iterations_ = next.iterations_;
 
 			// check convergence
 			// TODO: move convergence checks here and incorporate in verbosity levels
@@ -292,4 +296,62 @@ namespace gtsam {
 			iterations_++;
 		}
 	}
+
+  /* ************************************************************************* */
+  template<class G, class T, class L, class S, class W>
+  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
+
+    cout << "values: " << values_.get() << endl;
+    S solver(*graph_->linearize(*values_, *ordering_));
+    DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
+        parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
+        *graph_, *values_, *ordering_, error_);
+    shared_values newValues(new T(values_->expmap(result.dx_d, *ordering_)));
+    cout << "newValues: " << newValues.get() << endl;
+    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() {
+    static W writer(error_);
+
+    // check if we're already close enough
+    if (error_ < parameters_->sumError_) {
+      if ( parameters_->verbosity_ >= Parameters::ERROR )
+        cout << "Exiting, as sumError = " << error_ << " < " << parameters_->sumError_ << endl;
+      return *this;
+    }
+
+    // for the case that maxIterations_ = 0
+    iterations_ = 1;
+    if (iterations_ >= parameters_->maxIterations_)
+      return *this;
+
+    // Iterative loop that runs Dog Leg
+    while (true) {
+      double previous_error = error_;
+      // do one iteration of LM
+      NonlinearOptimizer next = iterateDogLeg();
+      writer.write(next.error_);
+      error_ = next.error_;
+      values_ = next.values_;
+      parameters_ = next.parameters_;
+
+      // check convergence
+      // TODO: move convergence checks here and incorporate in verbosity levels
+      // TODO: build into iterations somehow as an instance variable
+      bool converged = gtsam::check_convergence(*parameters_, previous_error, error_);
+
+      if(iterations_ >= parameters_->maxIterations_ || converged == true) {
+        if (parameters_->verbosity_ >= Parameters::VALUES) values_->print("final values");
+        if (parameters_->verbosity_ >= Parameters::ERROR) cout << "final error: " << error_ << endl;
+        if (parameters_->verbosity_ >= Parameters::LAMBDA) cout << "final Delta (called lambda) = " << lambda() << endl;
+        return *this;
+      }
+      iterations_++;
+    }
+
+  }
+
 }
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index 6ad3ee3c7..db92fc58a 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -170,7 +170,7 @@ public:
 	NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
 		graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
 		ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
-		iterations_(0), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
+		iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
 
 	// access to member variables
 
@@ -268,15 +268,23 @@ public:
 	///
 	NonlinearOptimizer levenbergMarquardt();
 
-	// static interfaces to LM and GN optimization techniques
+	/**
+	 * One iteration of the dog leg algorithm
+	 */
+	NonlinearOptimizer iterateDogLeg();
 
-	///
-	///Static interface to LM optimization using default ordering and thresholds
-	///@param graph 	   Nonlinear factor graph to optimize
-	///@param values       Initial values
-	///@param verbosity    Integer specifying how much output to provide
-	///@return 			   an optimized values structure
-	///
+	/**
+	 * Optimize using the Dog Leg algorithm
+	 */
+	NonlinearOptimizer dogLeg();
+
+	// static interfaces to LM, Dog leg, and GN optimization techniques
+
+  ///Static interface to Dog leg optimization using default ordering
+  ///@param graph      Nonlinear factor graph to optimize
+  ///@param values       Initial values
+  ///@param parameters Optimization parameters
+  ///@return         an optimized values structure
 	static shared_values optimizeLM(shared_graph graph,
 			shared_values values,
 			shared_parameters parameters = boost::make_shared<Parameters>()) {
@@ -293,8 +301,7 @@ public:
 
 	static shared_values optimizeLM(shared_graph graph,
 			shared_values values,
-			Parameters::verbosityLevel verbosity)
-	{
+			Parameters::verbosityLevel verbosity)	{
 		return optimizeLM(graph, values, Parameters::newVerbosity(verbosity));
 	}
 
@@ -317,6 +324,57 @@ public:
 				verbosity);
 	}
 
+  ///Static interface to Dog leg optimization using default ordering
+  ///@param graph      Nonlinear factor graph to optimize
+  ///@param values       Initial values
+  ///@param parameters Optimization parameters
+  ///@return         an optimized values structure
+  static shared_values optimizeDogLeg(shared_graph graph,
+      shared_values values,
+      shared_parameters parameters = boost::make_shared<Parameters>()) {
+
+    // Use a variable ordering from COLAMD
+    Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
+    // initial optimization state is the same in both cases tested
+    //GS solver(*graph->linearize(*values, *ordering));
+
+    NonlinearOptimizer optimizer(graph, values, ordering, parameters);
+    NonlinearOptimizer result = optimizer.dogLeg();
+    return result.values();
+  }
+
+  ///
+  ///Static interface to Dog leg optimization using default ordering and thresholds
+  ///@param graph      Nonlinear factor graph to optimize
+  ///@param values       Initial values
+  ///@param verbosity    Integer specifying how much output to provide
+  ///@return         an optimized values structure
+  ///
+  static shared_values optimizeDogLeg(shared_graph graph,
+      shared_values values,
+      Parameters::verbosityLevel verbosity) {
+    return optimizeDogLeg(graph, values, Parameters::newVerbosity(verbosity)->newLambda_(1.0));
+  }
+
+  /**
+   * Static interface to Dogleg optimization (no shared_ptr arguments) - see above
+   */
+  static shared_values optimizeDogLeg(const G& graph,
+      const T& values,
+      const Parameters parameters = Parameters()) {
+    return optimizeDogLeg(boost::make_shared<const G>(graph),
+        boost::make_shared<const T>(values),
+        boost::make_shared<Parameters>(parameters));
+  }
+
+  static shared_values optimizeDogLeg(const G& graph,
+      const T& values,
+      Parameters::verbosityLevel verbosity) {
+    return optimizeDogLeg(boost::make_shared<const G>(graph),
+        boost::make_shared<const T>(values),
+        verbosity);
+  }
+
 	///
 	///Static interface to GN optimization using default ordering and thresholds
 	///@param graph 	   Nonlinear factor graph to optimize
diff --git a/tests/Makefile.am b/tests/Makefile.am
index 8e922aa63..c88aa93a2 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -15,6 +15,7 @@ check_PROGRAMS += testInference
 check_PROGRAMS += testGaussianJunctionTree 
 check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
 check_PROGRAMS += testNonlinearOptimizer
+# check_PROGRAMS += testDoglegOptimizer # Uses debugging prints so commented out in SVN
 check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
 check_PROGRAMS += testTupleValues
 check_PROGRAMS += testNonlinearISAM
diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp
new file mode 100644
index 000000000..a7f2ab001
--- /dev/null
+++ b/tests/testDoglegOptimizer.cpp
@@ -0,0 +1,416 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    DoglegOptimizer.h
+ * @brief   Unit tests for DoglegOptimizer
+ * @author  Richard Roberts
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/FactorGraph-inl.h>
+#include <gtsam/inference/BayesTree-inl.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
+#include <gtsam/slam/pose2SLAM.h>
+#include <gtsam/slam/smallExample.h>
+
+#include <boost/bind.hpp>
+#include <boost/assign/list_of.hpp> // for 'list_of()'
+#include <functional>
+
+using namespace std;
+using namespace gtsam;
+using boost::shared_ptr;
+
+/* ************************************************************************* */
+double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
+
+  // Convert Vector to VectorValues
+  VectorValues vv = *allocateVectorValues(gbn);
+  vv.vector() = values;
+
+  // Convert to factor graph
+  GaussianFactorGraph gfg(gbn);
+  return gfg.error(vv);
+}
+
+/* ************************************************************************* */
+double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector& values) {
+
+  // Convert Vector to VectorValues
+  VectorValues vv = *allocateVectorValues(gbt);
+  vv.vector() = values;
+
+  // Convert to factor graph
+  GaussianFactorGraph gfg(gbt);
+  return gfg.error(vv);
+}
+
+/* ************************************************************************* */
+TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
+
+  // Create an arbitrary Bayes Net
+  GaussianBayesNet gbn;
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
+      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
+      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
+      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
+      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
+      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
+      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
+
+  // Compute the Hessian numerically
+  Matrix hessian = numericalHessian(
+      boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
+      LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).vector()));
+
+  // Compute the gradient numerically
+  VectorValues gradientValues = *allocateVectorValues(gbn);
+  Vector gradient = numericalGradient(
+      boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
+      LieVector(VectorValues::Zero(gradientValues).vector()));
+  gradientValues.vector() = gradient;
+
+  // Compute the gradient using dense matrices
+  Matrix augmentedHessian = GaussianFactorGraph(gbn).denseHessian();
+  LONGS_EQUAL(11, augmentedHessian.cols());
+  VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
+  denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
+  EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
+
+  // Compute the steepest descent point
+  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
+  VectorValues expected = gradientValues;  scal(step, expected);
+
+  // Compute the steepest descent point with the dogleg function
+  VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn);
+
+  // Check that points agree
+  EXPECT(assert_equal(expected, actual, 1e-5));
+
+  // Check that point causes a decrease in error
+  double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn)));
+  double newError = GaussianFactorGraph(gbn).error(actual);
+  EXPECT(newError < origError);
+}
+
+/* ************************************************************************* */
+TEST(DoglegOptimizer, BT_BN_equivalency) {
+
+  // Create an arbitrary Bayes Tree
+  BayesTree<GaussianConditional> bt;
+  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
+      GaussianConditional::shared_ptr(new GaussianConditional(
+          boost::assign::pair_list_of
+          (2, Matrix_(6,2,
+              31.0,32.0,
+              0.0,34.0,
+              0.0,0.0,
+              0.0,0.0,
+              0.0,0.0,
+              0.0,0.0))
+          (3, Matrix_(6,2,
+              35.0,36.0,
+              37.0,38.0,
+              41.0,42.0,
+              0.0,44.0,
+              0.0,0.0,
+              0.0,0.0))
+          (4, Matrix_(6,2,
+              0.0,0.0,
+              0.0,0.0,
+              45.0,46.0,
+              47.0,48.0,
+              51.0,52.0,
+              0.0,54.0)),
+          3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6))))));
+  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
+      GaussianConditional::shared_ptr(new GaussianConditional(
+          boost::assign::pair_list_of
+          (0, Matrix_(4,2,
+              3.0,4.0,
+              0.0,6.0,
+              0.0,0.0,
+              0.0,0.0))
+          (1, Matrix_(4,2,
+              0.0,0.0,
+              0.0,0.0,
+              17.0,18.0,
+              0.0,20.0))
+          (2, Matrix_(4,2,
+              0.0,0.0,
+              0.0,0.0,
+              21.0,22.0,
+              23.0,24.0))
+          (3, Matrix_(4,2,
+              7.0,8.0,
+              9.0,10.0,
+              0.0,0.0,
+              0.0,0.0))
+          (4, Matrix_(4,2,
+              11.0,12.0,
+              13.0,14.0,
+              25.0,26.0,
+              27.0,28.0)),
+          2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4))))));
+
+  // Create an arbitrary Bayes Net
+  GaussianBayesNet gbn;
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
+      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
+      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
+      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
+      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
+      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
+      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
+
+  GaussianFactorGraph expected(gbn);
+  GaussianFactorGraph actual(bt);
+
+  EXPECT(assert_equal(expected.denseJacobian(), actual.denseJacobian()));
+}
+
+/* ************************************************************************* */
+TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
+
+  // Create an arbitrary Bayes Tree
+  BayesTree<GaussianConditional> bt;
+  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
+      GaussianConditional::shared_ptr(new GaussianConditional(
+          boost::assign::pair_list_of
+          (2, Matrix_(6,2,
+              31.0,32.0,
+              0.0,34.0,
+              0.0,0.0,
+              0.0,0.0,
+              0.0,0.0,
+              0.0,0.0))
+          (3, Matrix_(6,2,
+              35.0,36.0,
+              37.0,38.0,
+              41.0,42.0,
+              0.0,44.0,
+              0.0,0.0,
+              0.0,0.0))
+          (4, Matrix_(6,2,
+              0.0,0.0,
+              0.0,0.0,
+              45.0,46.0,
+              47.0,48.0,
+              51.0,52.0,
+              0.0,54.0)),
+          3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6))))));
+  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
+      GaussianConditional::shared_ptr(new GaussianConditional(
+          boost::assign::pair_list_of
+          (0, Matrix_(4,2,
+              3.0,4.0,
+              0.0,6.0,
+              0.0,0.0,
+              0.0,0.0))
+          (1, Matrix_(4,2,
+              0.0,0.0,
+              0.0,0.0,
+              17.0,18.0,
+              0.0,20.0))
+          (2, Matrix_(4,2,
+              0.0,0.0,
+              0.0,0.0,
+              21.0,22.0,
+              23.0,24.0))
+          (3, Matrix_(4,2,
+              7.0,8.0,
+              9.0,10.0,
+              0.0,0.0,
+              0.0,0.0))
+          (4, Matrix_(4,2,
+              11.0,12.0,
+              13.0,14.0,
+              25.0,26.0,
+              27.0,28.0)),
+          2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4))))));
+
+  // Compute the Hessian numerically
+  Matrix hessian = numericalHessian(
+      boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
+      LieVector(VectorValues::Zero(*allocateVectorValues(bt)).vector()));
+
+  // Compute the gradient numerically
+  VectorValues gradientValues = *allocateVectorValues(bt);
+  Vector gradient = numericalGradient(
+      boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
+      LieVector(VectorValues::Zero(gradientValues).vector()));
+  gradientValues.vector() = gradient;
+
+  // Compute the gradient using dense matrices
+  Matrix augmentedHessian = GaussianFactorGraph(bt).denseHessian();
+  LONGS_EQUAL(11, augmentedHessian.cols());
+  VectorValues denseMatrixGradient = *allocateVectorValues(bt);
+  denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
+  EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
+
+  // Compute the steepest descent point
+  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
+  VectorValues expected = gradientValues;  scal(step, expected);
+
+  // Known steepest descent point from Bayes' net version
+  VectorValues expectedFromBN(5,2);
+  expectedFromBN[0] = Vector_(2, 0.000129034, 0.000688183);
+  expectedFromBN[1] = Vector_(2, 0.0109679, 0.0253767);
+  expectedFromBN[2] = Vector_(2, 0.0680441, 0.114496);
+  expectedFromBN[3] = Vector_(2, 0.16125, 0.241294);
+  expectedFromBN[4] = Vector_(2, 0.300134, 0.423233);
+
+  // Compute the steepest descent point with the dogleg function
+  VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(bt);
+
+  // Check that points agree
+  EXPECT(assert_equal(expected, actual, 1e-5));
+  EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
+
+  // Check that point causes a decrease in error
+  double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(*allocateVectorValues(bt)));
+  double newError = GaussianFactorGraph(bt).error(actual);
+  EXPECT(newError < origError);
+}
+
+/* ************************************************************************* */
+TEST(DoglegOptimizer, ComputeBlend) {
+  // Create an arbitrary Bayes Net
+  GaussianBayesNet gbn;
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
+      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
+      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
+      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
+      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
+      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
+      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
+
+  // Compute steepest descent point
+  VectorValues xu = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn);
+
+  // Compute Newton's method point
+  VectorValues xn = optimize(gbn);
+
+  // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
+  EXPECT(xu.vector().norm() < xn.vector().norm());
+
+  // Compute blend
+  double Delta = 1.5;
+  VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
+  DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
+}
+
+/* ************************************************************************* */
+TEST(DoglegOptimizer, ComputeDoglegPoint) {
+  // Create an arbitrary Bayes Net
+  GaussianBayesNet gbn;
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
+      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
+      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
+      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
+      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
+      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
+      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
+  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
+      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
+
+  // Compute dogleg point for different deltas
+
+  double Delta1 = 0.5;  // Less than steepest descent
+  VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);
+
+  double Delta2 = 1.5;  // Between steepest descent and Newton's method
+  VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
+  EXPECT(assert_equal(expected2, actual2));
+
+  double Delta3 = 5.0;  // Larger than Newton's method point
+  VectorValues expected3 = optimize(gbn);
+  VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  EXPECT(assert_equal(expected3, actual3));
+}
+
+/* ************************************************************************* */
+TEST(DoglegOptimizer, Iterate) {
+  // really non-linear factor graph
+  shared_ptr<example::Graph> fg(new example::Graph(
+      example::createReallyNonlinearFactorGraph()));
+
+  // config far from minimum
+  Point2 x0(3,0);
+  boost::shared_ptr<simulated2D::Values> config(new example::Values);
+  config->insert(simulated2D::PoseKey(1), x0);
+
+  // ordering
+  shared_ptr<Ordering> ord(new Ordering());
+  ord->push_back(simulated2D::PoseKey(1));
+
+  double Delta = 1.0;
+  for(size_t it=0; it<10; ++it) {
+    GaussianSequentialSolver solver(*fg->linearize(*config, *ord));
+    GaussianBayesNet gbn = *solver.eliminate();
+    // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
+    double nonlinearError = fg->error(*config);
+    double linearError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn)));
+    DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
+//    cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
+    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->expmap(result.dx_d, *ord));
+    (*config) = newConfig;
+    DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in
+  }
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */