diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp
index 74adf8a6a..87b17fbce 100644
--- a/cpp/testVSLAMGraph.cpp
+++ b/cpp/testVSLAMGraph.cpp
@@ -18,56 +18,52 @@ using namespace gtsam;
 typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> Optimizer;
 
 /* ************************************************************************* */
-Point3 landmark1_local(-1.0,-1.0, 0.0);
-Point3 landmark2_local(-1.0, 1.0, 0.0);
-Point3 landmark3_local( 1.0, 1.0, 0.0);
-Point3 landmark4_local( 1.0,-1.0, 0.0);
+Point3 landmark1(-1.0,-1.0, 0.0);
+Point3 landmark2(-1.0, 1.0, 0.0);
+Point3 landmark3( 1.0, 1.0, 0.0);
+Point3 landmark4( 1.0,-1.0, 0.0);
 
-Pose3 camera1_local(Matrix_(3,3,
+Pose3 camera1(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
 		       ),
 	      Point3(0,0,6.25));
 
-Pose3 camera2_local(Matrix_(3,3,
+Pose3 camera2(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
 		       ),
 	      Point3(0,0,5.00));
 
-Point3 landmark1() { return landmark1_local;}
-Point3 landmark2() { return landmark2_local;}
-Point3 landmark3() { return landmark3_local;}
-Point3 landmark4() { return landmark4_local;}
-
-Pose3 camera1() { return camera1_local;}
-Pose3 camera2() { return camera2_local;}
-
-
 /* ************************************************************************* */
 VSLAMGraph testGraph() {
   Point2 z11(-100, 100);
-	Point2 z12(-100, -100);
-	Point2 z13(100, -100);
-	Point2 z14(100, 100);
+	Point2 z12(-100,-100);
+	Point2 z13( 100,-100);
+	Point2 z14( 100, 100);
 	Point2 z21(-125, 125);
-	Point2 z22(-125, -125);
-	Point2 z23(125, -125);
-	Point2 z24(125, 125);
+	Point2 z22(-125,-125);
+	Point2 z23( 125,-125);
+	Point2 z24( 125, 125);
 
   double sigma = 1;
   Cal3_S2 K(625, 625, 0, 0, 0);
   VSLAMGraph g;
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11.vector(), sigma, 1, 1, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12.vector(), sigma, 1, 2, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13.vector(), sigma, 1, 3, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14.vector(), sigma, 1, 4, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21.vector(), sigma, 2, 1, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22.vector(), sigma, 2, 2, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23.vector(), sigma, 2, 3, K)));
-  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24.vector(), sigma, 2, 4, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11, sigma, 1, 1, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12, sigma, 1, 2, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13, sigma, 1, 3, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14, sigma, 1, 4, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21, sigma, 2, 1, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22, sigma, 2, 2, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23, sigma, 2, 3, K)));
+  g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24, sigma, 2, 4, K)));
+
+	// add 3 landmark constraints
+  g.addLandmarkConstraint(1, landmark1);
+  g.addLandmarkConstraint(2, landmark2);
+  g.addLandmarkConstraint(3, landmark3);
 
   return g;
 }
@@ -78,14 +74,14 @@ TEST( VSLAMGraph, optimizeLM)
   // build a graph
   VSLAMGraph graph = testGraph();
 
-  // Create a configuration corresponding to the ground truth
-  VSLAMConfig groundTruth;
-  groundTruth.addCameraPose(1, camera1());
-  groundTruth.addCameraPose(2, camera2());
-  groundTruth.addLandmarkPoint(1, landmark1());
-  groundTruth.addLandmarkPoint(2, landmark2());
-  groundTruth.addLandmarkPoint(3, landmark3());
-  groundTruth.addLandmarkPoint(4, landmark4());
+  // Create an initial configuration corresponding to the ground truth
+  boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
+  initialEstimate->addCameraPose(1, camera1);
+  initialEstimate->addCameraPose(2, camera2);
+  initialEstimate->addLandmarkPoint(1, landmark1);
+  initialEstimate->addLandmarkPoint(2, landmark2);
+  initialEstimate->addLandmarkPoint(3, landmark3);
+  initialEstimate->addLandmarkPoint(4, landmark4);
 
   // Create an ordering of the variables
   list<string> keys;
@@ -97,14 +93,9 @@ TEST( VSLAMGraph, optimizeLM)
   keys.push_back("x2");
   Ordering ordering(keys);
 
-  // optimize it
-  double relativeErrorTreshold=1e-6, absoluteErrorTreshold=1.0;
-  int verbosity=0;
-
-  Optimizer::shared_config initialConfig(new VSLAMConfig(groundTruth));
-  Optimizer optimizer(graph, ordering, initialConfig, 1e-5);
-
+  // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
+  Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
   DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
 
   // Iterate once, and the config should not have changed because we started
@@ -112,10 +103,8 @@ TEST( VSLAMGraph, optimizeLM)
   Optimizer afterOneIteration = optimizer.iterate();
   DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
 
-  //afterOneIteration.config()->print();
-
   // check if correct
-  CHECK(groundTruth.equals(*(afterOneIteration.config())) );
+  CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
 }
 
 /* ************************************************************************* */