From 744269343f2284bb54d13a5b64e07d227fee2817 Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Sat, 23 Jan 2010 04:46:00 +0000
Subject: [PATCH] add print

---
 cpp/Pose2SLAMOptimizer.cpp | 11 +++++++++++
 cpp/Pose2SLAMOptimizer.h   |  5 +++++
 cpp/gtsam.h                |  1 +
 3 files changed, 17 insertions(+)

diff --git a/cpp/Pose2SLAMOptimizer.cpp b/cpp/Pose2SLAMOptimizer.cpp
index c5b97feb9..d779d50b6 100644
--- a/cpp/Pose2SLAMOptimizer.cpp
+++ b/cpp/Pose2SLAMOptimizer.cpp
@@ -33,6 +33,17 @@ namespace gtsam {
 
 		// initialize non-linear solver
 		solver_.initialize(*graph_, *theta_);
+
+		linearize();
+	}
+
+	/* ************************************************************************* */
+	void Pose2SLAMOptimizer::print(const string& str) const {
+		GTSAM_PRINT(*graph_);
+		GTSAM_PRINT(*theta_);
+		//TODO
+		//GTSAM_PRINT(solver_);
+		GTSAM_PRINT(*system_);
 	}
 
 	/* ************************************************************************* */
diff --git a/cpp/Pose2SLAMOptimizer.h b/cpp/Pose2SLAMOptimizer.h
index 08e5460ff..c7fc9b7b8 100644
--- a/cpp/Pose2SLAMOptimizer.h
+++ b/cpp/Pose2SLAMOptimizer.h
@@ -44,6 +44,11 @@ namespace gtsam {
 		Pose2SLAMOptimizer(const std::string& dataset = "",
 				const std::string& path = "");
 
+		/**
+		 * print the object
+		 */
+		void print(const std::string& str = "") const;
+
 		/**
 		 * Virtual destructor
 		 */
diff --git a/cpp/gtsam.h b/cpp/gtsam.h
index c8cf91dea..85350a2bc 100644
--- a/cpp/gtsam.h
+++ b/cpp/gtsam.h
@@ -1,5 +1,6 @@
 class Pose2SLAMOptimizer {
 	Pose2SLAMOptimizer(string dataset_name);
+	void print(string s) const;
 	void update(Vector x) const;
 	Vector optimize() const;
 	double error() const;