From 55a153ebc67b8a0b3287dad6ec64e01ff3731e37 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 12 Jul 2012 02:11:32 +0000
Subject: [PATCH] Wrapped Cal3DS2, RangeFactor, BearingFactor, and
 GenericProjectionFactor

---
 gtsam.h                       | 67 +++++++++++++++++++++++++++++++++--
 gtsam/slam/BearingFactor.h    |  4 +--
 gtsam/slam/ProjectionFactor.h |  2 +-
 wrap/Module.cpp               |  2 +-
 4 files changed, 69 insertions(+), 6 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index abe7817d3..00f5a9030 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -398,17 +398,18 @@ virtual class Pose3 : gtsam::Value {
 	Matrix matrix() const;
 	gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
 	double range(const gtsam::Point3& point);
-	double range(const gtsam::Pose3& pose); // FIXME: shadows other range
+	double range(const gtsam::Pose3& pose);
 };
 
 virtual class Cal3_S2 : gtsam::Value {
   // Standard Constructors
   Cal3_S2();
   Cal3_S2(double fx, double fy, double s, double u0, double v0);
+	Cal3_S2(Vector v);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::Cal3_S2& pose, double tol) const;
+  bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
 
   // Manifold
   static size_t Dim();
@@ -432,6 +433,37 @@ virtual class Cal3_S2 : gtsam::Value {
   Matrix matrix_inverse() const;
 };
 
+virtual class Cal3DS2 : gtsam::Value {
+	// Standard Constructors
+	Cal3DS2();
+	Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
+	Cal3DS2(Vector v);
+
+	// Testable
+	void print(string s) const;
+	bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
+
+	// Manifold
+	static size_t Dim();
+	size_t dim() const;
+	gtsam::Cal3DS2 retract(Vector v) const;
+	Vector localCoordinates(const gtsam::Cal3DS2& c) const;
+
+	// Action on Point2
+	gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+	// TODO: D2d functions that start with an uppercase letter
+
+	// Standard Interface
+	double fx() const;
+	double fy() const;
+	double skew() const;
+	double px() const;
+	double py() const;
+	Vector vector() const;
+	Vector k() const;
+	//Matrix K() const; //FIXME: Uppercase
+};
+
 class Cal3_S2Stereo {
   // Standard Constructors
   Cal3_S2Stereo();
@@ -1092,11 +1124,42 @@ virtual class PriorFactor : gtsam::NonlinearFactor {
 	PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
 };
 
+
 template<T = {gtsam::LieVector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
 virtual class BetweenFactor : gtsam::NonlinearFactor {
 	BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
 };
 
+
+template<POSE, POINT>
+virtual class RangeFactor : gtsam::NonlinearFactor {
+	RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCamera;
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCamera;
+
+template<POSE, POINT, ROT>
+virtual class BearingFactor : gtsam::NonlinearFactor {
+	BearingFactor(size_t key1, size_t key2, const ROT& measured, const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
+
+
+#include <ProjectionFactor.h>
+template<POSE, LANDMARK, CALIBRATION>
+virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
+	GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
+		size_t poseKey, size_t pointKey, const CALIBRATION* k);
+	gtsam::Point2 measured() const;
+	CALIBRATION* calibration() const;
+};
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
+typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
+
 }///\namespace gtsam
 
 //*************************************************************************
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index c42c2798d..62e46d53e 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -25,12 +25,12 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class POSE, class POINT>
+	template<class POSE, class POINT, class ROT = typename POSE::Rotation>
 	class BearingFactor: public NoiseModelFactor2<POSE, POINT> {
 	private:
 
 		typedef POSE Pose;
-		typedef typename Pose::Rotation Rot;
+		typedef ROT Rot;
 		typedef POINT Point;
 
 		typedef BearingFactor<POSE, POINT> This;
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index bce7bcf84..048756435 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -62,7 +62,7 @@ namespace gtsam {
 		 * @param K shared pointer to the constant calibration
 		 */
 		GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-				const Key poseKey, Key pointKey, const shared_ptrK& K) :
+				Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K) :
 				  Base(model, poseKey, pointKey), measured_(measured), K_(K) {
 		}
 
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 3cc5af18e..d1e502bac 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -56,7 +56,7 @@ void handle_possible_template(vector<Class>& classes, const Class& cls, const st
 	if(instantiations.empty()) {
 		classes.push_back(cls);
 	} else {
-		vector<Class>& classInstantiations = cls.expandTemplate(templateArgument, instantiations);
+		vector<Class> classInstantiations = cls.expandTemplate(templateArgument, instantiations);
 		BOOST_FOREACH(const Class& c, classInstantiations) {
 			classes.push_back(c);
 		}