From 6c41184d19a753b32fd3694f0d4b2a942d93473e Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Mon, 15 Nov 2010 16:43:23 +0000
Subject: [PATCH] made templated and cleaned up some more

---
 gtsam/slam/StereoFactor.h | 136 ++++++++++++++++++++------------------
 1 file changed, 70 insertions(+), 66 deletions(-)

diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 819530f13..79c2d93a4 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    StereoFactor.h
+ * @file    GenericStereoFactor.h
  * @brief   A non-linear factor for stereo measurements
  * @author  Alireza Fathi
  * @author  Chris Beall
@@ -26,86 +26,90 @@ namespace gtsam {
 using namespace gtsam;
 using namespace gtsam::visualSLAM;
 
-class StereoFactor: public NonlinearFactor2<Values, PoseKey, PointKey>, Testable<StereoFactor> {
+template<class VALUES=Values, class KEY1=PoseKey, class KEY2=PointKey>
+class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
 private:
 
-  // Keep a copy of measurement and calibration for I/O
-  StereoPoint2 z_;
-  boost::shared_ptr<Cal3_S2> K_;
-  double baseline_;
+	// Keep a copy of measurement and calibration for I/O
+	StereoPoint2 z_;
+	boost::shared_ptr<Cal3_S2> K_;
+	double baseline_;
 
 public:
 
-  // shorthand for base class type
-  typedef NonlinearFactor2<Values, PoseKey, PointKey> Base;
+	// shorthand for base class type
+	typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
 
-  // shorthand for a smart pointer to a factor
-  typedef boost::shared_ptr<StereoFactor> shared_ptr;
+	// shorthand for a smart pointer to a factor
+	typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
 
-  /**
-   * Default constructor
-   */
-  StereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
+	/**
+	 * Default constructor
+	 */
+	GenericStereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
 
-  /**
-   * Constructor
-   * @param z is the 2 dimensional location of point in image (the measurement)
-   * @param sigma is the standard deviation
-   * @param cameraFrameNumber is basically the frame number
-   * @param landmarkNumber is the index of the landmark
-   * @param K the constant calibration
-   */
-  StereoFactor(const StereoPoint2& z, const SharedGaussian& model, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K, double baseline) :
-  	Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {}
+	/**
+	 * Constructor
+	 * @param z is the 2 dimensional location of point in image (the measurement)
+	 * @param sigma is the standard deviation
+	 * @param cameraFrameNumber is basically the frame number
+	 * @param landmarkNumber is the index of the landmark
+	 * @param K the constant calibration
+	 */
+	GenericStereoFactor(const StereoPoint2& z, const SharedGaussian& model, KEY1 j_pose,
+			KEY2 j_landmark, const shared_ptrK& K, double baseline) :
+		Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
+	}
 
-  /**
-   * print
-   * @param s optional string naming the factor
-   */
-  void print(const std::string& s) const {
-    printf("%s %s %s\n", s.c_str(), ((string) key1_).c_str(),
-    ((string) key2_).c_str());
-    z_.print(s+".z");
-  }
+	/**
+	 * print
+	 * @param s optional string naming the factor
+	 */
+	void print(const std::string& s) const {
+		Base::print(s);
+		z_.print(s + ".z");
+	}
 
-  /**
-   * equals
-   */
-  bool equals(const StereoFactor& f, double tol) const {
-     const StereoFactor* p = dynamic_cast<const StereoFactor*>(&f);
-     if (p == NULL) goto fail;
-     //if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
-     if (!z_.equals(p->z_,tol)) goto fail;
-     return true;
+	/**
+	 * equals
+	 */
+	bool equals(const GenericStereoFactor& f, double tol) const {
+		const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
+		if (p == NULL)
+			goto fail;
+		//if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
+		if (!z_.equals(p->z_, tol))
+			goto fail;
+		return true;
 
-    fail:
-     print("actual");
-     p->print("expected");
-     return false;
-   }
+		fail: print("actual");
+		p->print("expected");
+		return false;
+	}
 
-  /** h(x)-z */
-  Vector evaluateError(const Pose3& pose, const Point3& point,
-      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	/** h(x)-z */
+	Vector evaluateError(const Pose3& 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();
+	}
 
-  	const Cal3_S2& K = *K_;
-    StereoCamera stereoCam(pose, K, baseline_);
-
-    return (stereoCam.project(point,H1,H2) - z_).vector();
-  }
-
-  StereoPoint2 z(){return z_;}
+	StereoPoint2 z() {
+		return z_;
+	}
 
 private:
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
-    //ar & BOOST_SERIALIZATION_NVP(key1_);
-    //ar & BOOST_SERIALIZATION_NVP(key2_);
-    ar & BOOST_SERIALIZATION_NVP(z_);
-    ar & BOOST_SERIALIZATION_NVP(K_);
-  }
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class Archive>
+	void serialize(Archive & ar, const unsigned int version) {
+		ar & BOOST_SERIALIZATION_NVP(z_);
+		ar & BOOST_SERIALIZATION_NVP(K_);
+		ar & BOOST_SERIALIZATION_NVP(baseline_);
+	}
 };
 
+// Typedef for general use
+typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
+
 }