diff --git a/cpp/Pose2SLAMOptimizer.h b/cpp/Pose2SLAMOptimizer.h
index 342c37ad3..542016a75 100644
--- a/cpp/Pose2SLAMOptimizer.h
+++ b/cpp/Pose2SLAMOptimizer.h
@@ -77,11 +77,12 @@ namespace gtsam {
 		}
 
 		/**
-		 * linearize around current theta: done at construction and at update
+		 * Optimize to get a
 		 */
-		void optimize() {
+		Vector optimize() {
 			VectorConfig X = solver_.optimize(*system_);
-			update(X);
+			Vector x; // TODO convert to Vector
+			return x;
 		}
 
 		/**