diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp
new file mode 100644
index 000000000..03d11d795
--- /dev/null
+++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp
@@ -0,0 +1,93 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char *argv[]) {
+
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("pose3example.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  bool is3D = true;
+  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+
+  // Add prior on the first key
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+  Key firstKey = 0;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
+
+  std::cout << "Initializing Pose3" << std::endl;
+  Values initialization = InitializePose3::initialize(graphWithPrior);
+  std::cout << "done!" << std::endl;
+
+  if (argc < 3) {
+    initialization.print("initialization");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    writeG2o(*graph, initialization, outputFile);
+    std::cout << "done! " << std::endl;
+
+    // This should be deleted: only for debug
+    //    if (argc == 4){
+    //      const string inputFileRewritten = argv[3];
+    //      std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
+    //      // Additional: rewrite input with simplified keys 0,1,...
+    //      Values simpleInitial;
+    //      BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    //        Key key = key_value.key - firstKey;
+    //        simpleInitial.insert(key, initial->at(key_value.key));
+    //      }
+    //      NonlinearFactorGraph simpleGraph;
+    //      BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
+    //        boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    //            boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    //        if (pose3Between){
+    //          Key key1 = pose3Between->key1() - firstKey;
+    //          Key key2 = pose3Between->key2() - firstKey;
+    //          NonlinearFactor::shared_ptr simpleFactor(
+    //              new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
+    //          simpleGraph.add(simpleFactor);
+    //        }
+    //      }
+    //      writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
+    //    }
+  }
+  return 0;
+}
diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp
similarity index 65%
rename from examples/Pose3SLAMExample_initializePose3.cpp
rename to examples/Pose3SLAMExample_initializePose3Gradient.cpp
index bd46bb917..eac964075 100644
--- a/examples/Pose3SLAMExample_initializePose3.cpp
+++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp
@@ -53,7 +53,8 @@ int main(const int argc, const char *argv[]) {
   }
 
   std::cout << "Initializing Pose3" << std::endl;
-  Values initialization = InitializePose3::initialize(graphWithPrior);
+  bool useGradient = true;
+  Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
@@ -63,31 +64,6 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Writing results to file: " << outputFile  << std::endl;
     writeG2o(*graph, initialization, outputFile);
     std::cout << "done! " << std::endl;
-
-    // This should be deleted: only for debug
-    if (argc == 4){
-      const string inputFileRewritten = argv[3];
-      std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
-      // Additional: rewrite input with simplified keys 0,1,...
-      Values simpleInitial;
-      BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
-        Key key = key_value.key - firstKey;
-        simpleInitial.insert(key, initial->at(key_value.key));
-      }
-      NonlinearFactorGraph simpleGraph;
-      BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
-        boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
-            boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        if (pose3Between){
-          Key key1 = pose3Between->key1() - firstKey;
-          Key key2 = pose3Between->key2() - firstKey;
-          NonlinearFactor::shared_ptr simpleFactor(
-              new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
-          simpleGraph.add(simpleFactor);
-        }
-      }
-      writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
-    }
   }
   return 0;
 }
diff --git a/examples/Pose3SLAMExample_rotOnly.cpp b/examples/Pose3SLAMExample_rotOnlyGN.cpp
similarity index 100%
rename from examples/Pose3SLAMExample_rotOnly.cpp
rename to examples/Pose3SLAMExample_rotOnlyGN.cpp
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index af3e24da0..8b3f54644 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -366,16 +366,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
   else
     orientations = computeOrientationsChordal(pose3Graph);
 
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
-    Key key = key_value.key;
-    if (key != keyAnchor) {
-      const Point3& pos = givenGuess.at<Pose3>(key).translation();
-      const Rot3& rot = orientations.at<Rot3>(key);
-      Pose3 initializedPoses = Pose3(rot, pos);
-      initialValues.insert(key, initializedPoses);
-    }
-  }
-  return initialValues;
+  // Compute the full poses (1 GN iteration on full poses)
+  return computePoses(pose3Graph, orientations);
+
+  //  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
+  //    Key key = key_value.key;
+  //    if (key != keyAnchor) {
+  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
+  //      const Rot3& rot = orientations.at<Rot3>(key);
+  //      Pose3 initializedPoses = Pose3(rot, pos);
+  //      initialValues.insert(key, initializedPoses);
+  //    }
+  //  }
+  //  return initialValues;
 }
 
 } // end of namespace lago