From 41de41deec98fab862eafa47066f5d7833c26670 Mon Sep 17 00:00:00 2001
From: cbeall3 <cbeall3@gatech.edu>
Date: Fri, 30 May 2014 19:37:03 -0400
Subject: [PATCH] minor cleanup

---
 .../examples/SmartProjectionFactorExample.cpp | 38 +++++++------------
 1 file changed, 13 insertions(+), 25 deletions(-)

diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
index a9c60afba..31b45cbc9 100644
--- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
+++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
@@ -19,10 +19,11 @@
 
 
 /**
- * A smart projection factor example based on stereo data
- *  - robot starts at origin
+ * A smart projection factor example based on stereo data, throwing away the
+ * measurement from the right camera
+ *  -robot starts at origin
  *  -moves forward, taking periodic stereo measurements
- *  -takes stereo readings of many landmarks
+ *  -makes monocular observations of many landmarks
  */
 
 #include <gtsam/geometry/Pose3.h>
@@ -32,7 +33,6 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/slam/StereoFactor.h>
 #include <gtsam/slam/dataset.h>
 
 #include <gtsam_unstable/slam/SmartProjectionPoseFactor.h>
@@ -58,18 +58,16 @@ int main(int argc, char** argv){
   
   //read camera calibration info from file
   // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
-  double fx, fy, s, u0, v0, b;
-  ifstream calibration_file(calibration_loc);
   cout << "Reading calibration info" << endl;
-  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
+  ifstream calibration_file(calibration_loc.c_str());
 
-  //create stereo camera calibration object
-//  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
-  
+  double fx, fy, s, u0, v0, b;
+  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
   const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0));
 
-  ifstream pose_file(pose_loc);
   cout << "Reading camera poses" << endl;
+  ifstream pose_file(pose_loc.c_str());
+
   int pose_id;
   MatrixRowMajor m(4,4);
   //read camera pose parameters and use to make initial estimates of camera poses
@@ -86,17 +84,15 @@ int main(int argc, char** argv){
   // pixel coordinates uL, uR, v (same for left/right images due to rectification)
   // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
   double uL, uR, v, X, Y, Z;
-  ifstream factor_file(factor_loc);
+  ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
-  //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
+
+  //read stereo measurements and construct smart factors
 
   SmartFactor::shared_ptr factor(new SmartFactor());
-  size_t current_l = 3;   // TODO: Fix me!
+  size_t current_l = 3;   // hardcoded landmark ID from first measurement
 
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
-//    graph.push_back(
-//        GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
-//            Symbol('x', x), Symbol('l', l), K));
 
     if(current_l != l) {
       graph.push_back(factor);
@@ -104,14 +100,6 @@ int main(int argc, char** argv){
       current_l = l;
     }
     factor->add(Point2(uL,v), Symbol('x',x), model, K);
-
-    //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
-//    if (!initial_estimate.exists(Symbol('l', l))) {
-//      Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
-//      //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
-//      Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
-//      initial_estimate.insert(Symbol('l', l), worldPoint);
-//    }
   }
 
   Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));