From a2974341488152d5afcf88edebf9884f814bed66 Mon Sep 17 00:00:00 2001
From: Stephen Camp <stephen.v.camp@gmail.com>
Date: Tue, 22 Jul 2014 09:08:38 -0400
Subject: [PATCH] Modified SmartStereoProjectionFactorExample to write
 optimized poses to file

---
 .../examples/SmartStereoProjectionFactorExample.cpp  | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
index 9847ef5ed..f4329e665 100644
--- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
+++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
@@ -48,9 +48,12 @@ int main(int argc, char** argv){
 
   typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor;
 
+  bool output_poses = true;
+  string poseOutput("../../../examples/data/optimized_poses.txt");
   Values initial_estimate;
   NonlinearFactorGraph graph;
   const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  ofstream pose3Out;
 
   bool add_initial_noise = true;
 
@@ -126,5 +129,14 @@ int main(int argc, char** argv){
   Values pose_values = result.filter<Pose3>();
   pose_values.print("Final camera poses:\n");
 
+  if(output_poses){
+    pose3Out.open(poseOutput.c_str(),ios::out);
+    for(int i = 1; i<=pose_values.size(); i++){
+      pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
+        " ", " ")) << endl;
+    }
+    cout << "Writing output" << endl;
+  }
+
   return 0;
 }