diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 37b33204b..4ae9b1d36 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -239,26 +239,25 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
 //  return computePoses(pose2Graph, orientationsLago);
 //}
 //
-///* ************************************************************************* */
-//Values initialize(const NonlinearFactorGraph& graph,
-//    const Values& initialGuess) {
-//  Values initialGuessLago;
-//
-//  // get the orientation estimates from LAGO
-//  VectorValues orientations = initializeOrientations(graph);
-//
-//  // for all nodes in the tree
-//  BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
-//    Key key = it.first;
-//    if (key != keyAnchor) {
-//      const Pose2& pose = initialGuess.at<Pose2>(key);
-//      const Vector& orientation = it.second;
-//      Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
-//      initialGuessLago.insert(key, poseLago);
-//    }
-//  }
-//  return initialGuessLago;
-//}
+/* ************************************************************************* */
+Values initialize(const NonlinearFactorGraph& graph,
+    const Values& givenGuess) {
+  Values initialValues;
+
+  // get the orientation estimates from LAGO
+  Values orientations = initializeOrientations(graph);
+
+  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
 } // end of namespace gtsam
diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h
index 33efe3f85..04a10659c 100644
--- a/gtsam/slam/InitializePose3.h
+++ b/gtsam/slam/InitializePose3.h
@@ -38,6 +38,8 @@ GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph);
 
 GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
 
+GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess);
+
 ///** Linear factor graph with regularized orientation measurements */
 //GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
 //    const std::vector<size_t>& spanningTreeIds,
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index 6800cc66c..77cfe8e12 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -89,6 +89,20 @@ TEST( InitializePose3, orientations ) {
   EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
 }
 
+/* *************************************************************************** */
+TEST( InitializePose3, posesWithGivenGuess ) {
+  Values givenPoses;
+  givenPoses.insert(x0,simple::pose0);
+  givenPoses.insert(x1,simple::pose1);
+  givenPoses.insert(x2,simple::pose2);
+  givenPoses.insert(x3,simple::pose3);
+
+  Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(givenPoses, initial, 1e-6));
+}
+
 
 /* ************************************************************************* */
 int main() {