From 3e143c1f247e4bc6cb090038a82847d57d8a8041 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Thu, 17 Mar 2011 04:36:27 +0000
Subject: [PATCH] made Between Factor more flexible to work with measurement
 that's not of its own Key::Value type. default behavior unchanged

---
 gtsam/slam/BetweenFactor.h | 23 ++++++++++-------------
 gtsam/slam/StereoFactor.h  |  2 +-
 2 files changed, 11 insertions(+), 14 deletions(-)

diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index efb9e4e3f..c4db0cc48 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -26,20 +26,16 @@ namespace gtsam {
 
 	/**
 	 * A class for a measurement predicted by "between(config[key1],config[key2])"
-	 * T is the Lie group type, Values where the T's are gotten from
-	 *
-	 * FIXME: This should only need one key type, as we can't have different types
+	 * KEY1::Value is the Lie Group type
+	 * T is the measurement type, by default the same
 	 */
-	template<class VALUES, class KEY1, class KEY2 = KEY1>
-	class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
-
-	public:
-		typedef typename KEY1::Value T;
+	template<class VALUES, class KEY1, class T = typename KEY1::Value>
+	class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> {
 
 	private:
 
-		typedef BetweenFactor<VALUES, KEY1, KEY2> This;
-		typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
+		typedef BetweenFactor<VALUES, KEY1, T> This;
+		typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base;
 
 		T measured_; /** The measurement */
 
@@ -52,7 +48,7 @@ namespace gtsam {
 		BetweenFactor() {}
 
 		/** Constructor */
-		BetweenFactor(const KEY1& key1, const KEY2& key2, const T& measured,
+		BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured,
 				const SharedGaussian& model) :
 			Base(model, key1, key2), measured_(measured) {
 		}
@@ -76,8 +72,9 @@ namespace gtsam {
 		/** implement functions needed to derive from Factor */
 
 		/** vector of errors */
-		Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
-				boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+		Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2,
+				boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+						boost::none) const {
 			T hx = p1.between(p2, H1, H2); // h(x)
 			// manifold equivalent of h(x)-z -> log(z,h(x))
 			return measured_.logmap(hx);
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 8a21617f9..d7c120a7e 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -86,7 +86,7 @@ public:
 	}
 
 	/** h(x)-z */
-	Vector evaluateError(const CamPose& pose, const Point3& point,
+	Vector evaluateError(const Pose3& pose, const Point3& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 		StereoCamera stereoCam(pose, *K_);
 		return (stereoCam.project(point, H1, H2) - z_).vector();