From b5aa7fb7f0d1879fae4303d85915530ea773ace1 Mon Sep 17 00:00:00 2001
From: cbeall3 <cbeall3@gatech.edu>
Date: Mon, 27 Jul 2015 23:25:05 -0400
Subject: [PATCH] change interface to match PinholePose.

---
 gtsam/geometry/StereoCamera.h                |  4 ++--
 gtsam/slam/tests/testTriangulationFactor.cpp | 25 ++++++++++++++++++++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index f09626c9d..ac32be7ae 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -66,8 +66,8 @@ public:
   StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
 
   /// Return shared pointer to calibration
-  const Cal3_S2Stereo::shared_ptr calibration() const {
-    return K_;
+  const Cal3_S2Stereo& calibration() const {
+    return *K_;
   }
 
   /// @}
diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp
index 2c3a64495..bfd63ab56 100644
--- a/gtsam/slam/tests/testTriangulationFactor.cpp
+++ b/gtsam/slam/tests/testTriangulationFactor.cpp
@@ -18,6 +18,7 @@
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
@@ -63,6 +64,30 @@ TEST( triangulation, TriangulationFactor ) {
   CHECK(assert_equal(HExpected, HActual, 1e-3));
 }
 
+//******************************************************************************
+TEST( triangulation, TriangulationFactorStereo ) {
+  // Create the factor with a measurement that is 3 pixels off in x
+  Key pointKey(1);
+  SharedNoiseModel model;
+  Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5));
+  StereoCamera camera2(pose1, stereoCal);
+
+  StereoPoint2 z2 = camera2.project(landmark);
+
+  typedef TriangulationFactor<StereoCamera> Factor;
+  Factor factor(camera2, z2, model, pointKey);
+
+  // Use the factor to calculate the Jacobians
+  Matrix HActual;
+  factor.evaluateError(landmark, HActual);
+
+  Matrix HExpected = numericalDerivative11<Vector,Point3>(
+      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
+
+  // Verify the Jacobians are correct
+  CHECK(assert_equal(HExpected, HActual, 1e-3));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;