From e55aafd9bc187d701f59920253c93c2d543ce73a Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Fri, 11 Mar 2011 06:38:50 +0000
Subject: [PATCH] StereoFactor improvements

---
 gtsam/geometry/StereoCamera.h | 15 +++++++++++++++
 gtsam/slam/StereoFactor.h     |  6 ++++--
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index 3a206176e..c800992cd 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -68,6 +68,21 @@ namespace gtsam {
 			return StereoCamera(pose().expmap(d),K(),baseline());
 		}
 
+		Vector logmap(const StereoCamera &camera) const {
+				const Vector v1(leftCamPose_.logmap(camera.leftCamPose_));
+				return v1;
+		}
+
+		bool equals (const StereoCamera &camera, double tol = 1e-9) const {
+			return leftCamPose_.equals(camera.leftCamPose_, tol) &&
+				   K_.equals(camera.K_, tol);
+		}
+
+		void print(const std::string& s = "") const {
+			leftCamPose_.print(s + ".camera.") ;
+			K_.print(s + ".calibration.") ;
+		}
+
 	private:
 		/** utility functions */
 		Matrix Dproject_to_stereo_camera1(const Point3& P) const;
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 4cce7af37..6006a45f8 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -43,6 +43,8 @@ public:
 	// shorthand for a smart pointer to a factor
 	typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
 
+	typedef typename KEY1::Value CamPose;
+
 	/**
 	 * Default constructor
 	 */
@@ -61,7 +63,7 @@ public:
 		Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
 	}
 
-	virtual ~GenericStereoFactor() {}
+	~GenericStereoFactor() {}
 
 	/**
 	 * print
@@ -90,7 +92,7 @@ public:
 	}
 
 	/** h(x)-z */
-	Vector evaluateError(const Pose3& pose, const Point3& point,
+	Vector evaluateError(const CamPose& pose, const Point3& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 		StereoCamera stereoCam(pose, *K_, baseline_);
 		return (stereoCam.project(point, H1, H2) - z_).vector();