From 39658269d4df3a28e6ac92c79a1d9d8e75911a6c Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Fri, 7 Oct 2011 14:46:43 +0000
Subject: [PATCH] Generalized bearing and bearingrange factors to allow for
 different variable types

---
 gtsam/slam/BearingFactor.h      | 12 +++++++-----
 gtsam/slam/BearingRangeFactor.h | 25 ++++++++++++++-----------
 gtsam/slam/planarSLAM.h         |  7 +++++--
 3 files changed, 26 insertions(+), 18 deletions(-)

diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index 1860343c8..768ca419b 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -16,7 +16,6 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -28,18 +27,21 @@ namespace gtsam {
 	class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
 	private:
 
-		Rot2 z_; /** measurement */
+		typedef typename POSEKEY::Value Pose;
+		typedef typename POSEKEY::Value::Rotation Rot;
+		typedef typename POINTKEY::Value Point;
 
 		typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This;
 		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
 
+		Rot z_; /** measurement */
 	public:
 
 		/** default constructor for serialization/testing only */
 		BearingFactor() {}
 
 		/** primary constructor */
-		BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& z,
+		BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z,
 				const SharedNoiseModel& model) :
 					Base(model, i, j), z_(z) {
 		}
@@ -47,10 +49,10 @@ namespace gtsam {
 		virtual ~BearingFactor() {}
 
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
-		Vector evaluateError(const Pose2& pose, const Point2& point,
+		Vector evaluateError(const Pose& pose, const Point& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 			Rot2 hx = pose.bearing(point, H1, H2);
-			return Rot2::Logmap(z_.between(hx));
+			return Rot::Logmap(z_.between(hx));
 		}
 
 		/** return the measured */
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 070f6cf73..9d5267777 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -19,8 +19,7 @@
 
 #pragma once
 
-#include <gtsam/slam/BearingFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
 
@@ -31,25 +30,29 @@ namespace gtsam {
 	class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
 	private:
 
-		// the measurement
-		Rot2 bearing_;
-		double range_;
+		typedef typename POSEKEY::Value Pose;
+		typedef typename POSEKEY::Value::Rotation Rot;
+		typedef typename POINTKEY::Value Point;
 
 		typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This;
 		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
 
+		// the measurement
+		Rot bearing_;
+		double range_;
+
 	public:
 
 		BearingRangeFactor() {} /* Default constructor */
-		BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& bearing, const double range,
+		BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range,
 				const SharedNoiseModel& model) :
 					Base(model, i, j), bearing_(bearing), range_(range) {
 		}
 
 		virtual ~BearingRangeFactor() {}
 
-		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
-		Vector evaluateError(const Pose2& pose, const Point2& point,
+		/** h(x)-z -> between(z,h(x)) for Rot manifold */
+		Vector evaluateError(const Pose& pose, const Point& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 			Matrix H11, H21, H12, H22;
 			boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>();
@@ -57,8 +60,8 @@ namespace gtsam {
 			boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
 			boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
 
-			Rot2 y1 = pose.bearing(point, H11_, H12_);
-			Vector e1 = Rot2::Logmap(bearing_.between(y1));
+			Rot y1 = pose.bearing(point, H11_, H12_);
+			Vector e1 = Rot::Logmap(bearing_.between(y1));
 
 			double y2 = pose.range(point, H21_, H22_);
 			Vector e2 = Vector_(1, y2 - range_);
@@ -69,7 +72,7 @@ namespace gtsam {
 		}
 
 		/** return the measured */
-		inline const std::pair<Rot2, double> measured() const {
+		inline const std::pair<Rot, double> measured() const {
 			return std::make_pair(bearing_, range_);
 		}
 
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index 14440b5a2..989bdb327 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -13,15 +13,18 @@
  *  @file  planarSLAM.h
  *  @brief: bearing/range measurements in 2D plane
  *  @authors Frank Dellaert
- **/
+ */
 
 #pragma once
 
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/slam/BearingFactor.h>
+#include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>