diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 8b564c093..2c7d95bbe 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -600,10 +600,7 @@ VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
 template<class CONDITIONAL, class VALUES, class GRAPH>
 VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
   VectorValues delta = *allocateVectorValues(isam);
-  for(Index j=0; j<isam.getDelta().size(); ++j)
-    delta[j] = isam.getDelta()[j];
-  delta.print("delta: ");
-  assert_equal(isam.getDelta().container(), delta);
+  optimize2(isam.root(), delta);
   return delta;
 }
 
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 265d01860..179deed62 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -194,7 +194,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(ISAM2, slamlike_solution)
+TEST(ISAM2, slamlike_solution_gaussnewton)
 {
 
 //  SETDEBUG("ISAM2 update", true);
@@ -230,7 +230,7 @@ TEST_UNSAFE(ISAM2, slamlike_solution)
     isam.update(newfactors, init);
   }
 
-  EXPECT(isam_check(fullgraph, fullinit, isam));
+  CHECK(isam_check(fullgraph, fullinit, isam));
 
   // Add odometry from time 0 to time 5
   for( ; i<5; ++i) {
@@ -295,7 +295,7 @@ TEST_UNSAFE(ISAM2, slamlike_solution)
   }
 
   // Compare solutions
-  EXPECT(isam_check(fullgraph, fullinit, isam));
+  CHECK(isam_check(fullgraph, fullinit, isam));
 
   // Check gradient at each node
   typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
@@ -327,7 +327,140 @@ TEST_UNSAFE(ISAM2, slamlike_solution)
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(ISAM2, clone) {
+TEST(ISAM2, slamlike_solution_dogleg)
+{
+
+//  SETDEBUG("ISAM2 update", true);
+//  SETDEBUG("ISAM2 update verbose", true);
+//  SETDEBUG("ISAM2 recalculate", true);
+
+  // Pose and landmark key types from planarSLAM
+  typedef planarSLAM::PoseKey PoseKey;
+  typedef planarSLAM::PointKey PointKey;
+
+  // Set up parameters
+  SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
+  SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
+
+  // These variables will be reused and accumulate factors and values
+  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
+  planarSLAM::Values fullinit;
+  planarSLAM::Graph fullgraph;
+
+  // i keeps track of the time step
+  size_t i = 0;
+
+  // Add a prior at time 0 and update isam
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+    fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Add odometry from time 0 to time 5
+  for( ; i<5; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 5 to 6 and landmark measurement at time 5
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+    fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Add odometry from time 6 to time 10
+  for( ; i<10; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 10 to 11 and landmark measurement at time 10
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Compare solutions
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Check gradient at each node
+  typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
+  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
+    // Compute expected gradient
+    FactorGraph<JacobianFactor> jfg;
+    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
+    VectorValues expectedGradient(*allocateVectorValues(isam));
+    gradientAtZero(jfg, expectedGradient);
+    // Compare with actual gradients
+    int variablePosition = 0;
+    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
+      const int dim = clique->conditional()->dim(jit);
+      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
+      EXPECT(assert_equal(expectedGradient[*jit], actual));
+      variablePosition += dim;
+    }
+    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
+  }
+
+  // Check gradient
+  VectorValues expectedGradient(*allocateVectorValues(isam));
+  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
+  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
+  VectorValues actualGradient(*allocateVectorValues(isam));
+  gradientAtZero(isam, actualGradient);
+  EXPECT(assert_equal(expectedGradient2, expectedGradient));
+  EXPECT(assert_equal(expectedGradient, actualGradient));
+}
+
+/* ************************************************************************* */
+TEST(ISAM2, clone) {
 
   // Pose and landmark key types from planarSLAM
   typedef planarSLAM::PoseKey PoseKey;