From 5f56d70000e4192cad99334c77484465f712bac2 Mon Sep 17 00:00:00 2001
From: cbeall3 <cbeall3@gatech.edu>
Date: Tue, 3 Jun 2014 17:11:01 -0400
Subject: [PATCH] comment out things that wouldn't work with conversion to
 Stereo

---
 gtsam/slam/SmartStereoProjectionFactor.h | 44 ++++++++++++++----------
 1 file changed, 25 insertions(+), 19 deletions(-)

diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h
index f93fb468b..9bfe72eae 100644
--- a/gtsam/slam/SmartStereoProjectionFactor.h
+++ b/gtsam/slam/SmartStereoProjectionFactor.h
@@ -23,6 +23,7 @@
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
 
@@ -110,7 +111,10 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// shorthand for a pinhole camera
+  // TODO: Switch to stereoCamera:
   typedef PinholeCamera<CALIBRATION> Camera;
+//  typedef StereoCamera Camera;
+
   typedef std::vector<Camera> Cameras;
 
   /**
@@ -243,8 +247,10 @@ public:
       // We triangulate the 3D position of the landmark
       try {
         // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
-        point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
-            rankTolerance_, enableEPI_);
+
+        //TODO: Chris will replace this with something else for stereo
+//        point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
+//            rankTolerance_, enableEPI_);
         degenerate_ = false;
         cheiralityException_ = false;
 
@@ -639,23 +645,23 @@ public:
     }
 
     if (this->degenerate_) {
-      // return 0.0; // TODO: this maybe should be zero?
-      std::cout
-          << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
-          << std::endl;
-      size_t i = 0;
-      double overallError = 0;
-      BOOST_FOREACH(const Camera& camera, cameras) {
-        const Point2& zi = this->measured_.at(i);
-        if (i == 0) // first pose
-          this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
-        Point2 reprojectionError(
-            camera.projectPointAtInfinity(this->point_) - zi);
-        overallError += 0.5
-            * this->noise_.at(i)->distance(reprojectionError.vector());
-        i += 1;
-      }
-      return overallError;
+       return 0.0; // TODO: this maybe should be zero?
+//      std::cout
+//          << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
+//          << std::endl;
+//      size_t i = 0;
+//      double overallError = 0;
+//      BOOST_FOREACH(const Camera& camera, cameras) {
+//        const Point2& zi = this->measured_.at(i);
+//        if (i == 0) // first pose
+//          this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
+//        Point2 reprojectionError(
+//            camera.projectPointAtInfinity(this->point_) - zi);
+//        overallError += 0.5
+//            * this->noise_.at(i)->distance(reprojectionError.vector());
+//        i += 1;
+//      }
+//      return overallError;
     } else {
       // Just use version in base class
       return Base::totalReprojectionError(cameras, point_);