found there are two implementations of bearing and range in gtsam. removed the redundant one.

cpp/BearingFactor.h

@@ -13,37 +13,11 @@
 namespace gtsam {
 
 	/**
-	 * Calculate bearing to a landmark
+	 * Binary factor for a bearing measurement
-	 * @param pose 2D pose of robot
-	 * @param point 2D location of landmark
-	 * @return 2D rotation \in SO(2)
-	 */
-	Rot2 bearing(const Pose2& pose, const Point2& point) {
-		Point2 d = transform_to(pose, point);
-		return relativeBearing(d);
-	}
-
-	/**
-	 * Calculate bearing and optional derivative(s)
-	 */
-	Rot2 bearing(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-		if (!H1 && !H2) return bearing(pose, point);
-		Point2 d = transform_to(pose, point);
-		Matrix D_result_d;
-		Rot2 result = relativeBearing(d, D_result_d);
-		if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
-		if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
-		return result;
-	}
-
-	/**
-	 * Non-linear factor for a constraint derived from a 2D measurement,
-	 * i.e. the main building block for visual SLAM.
 	 */
 	template<class Config, class PoseKey, class PointKey>
 	class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
-			PointKey, Point2> {
+	PointKey, Point2> {
 	private:
 
 		Rot2 z_; /** measurement */
@@ -53,9 +27,9 @@ namespace gtsam {
 	public:
 
 		BearingFactor(); /* Default constructor */
-		BearingFactor(const Rot2& z, double sigma, const PoseKey& i,
+		BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
-				const PointKey& j) :
+				const SharedGaussian& model) :
-			Base(sigma, i, j), z_(z) {
+					Base(model, i, j), z_(z) {
 		}
 
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
@@ -64,6 +38,11 @@ namespace gtsam {
 			Rot2 hx = bearing(pose, point, H1, H2);
 			return logmap(between(z_, hx));
 		}
+
+		/** return the measured */
+		inline const Rot2 measured() const {
+			return z_;
+		}
 	}; // BearingFactor
 
 } // namespace gtsam

cpp/Makefile.am

@@ -212,7 +212,7 @@ testPose2SLAM_SOURCES  = testPose2SLAM.cpp
 testPose2SLAM_LDADD    = libgtsam.la
 
 # 2D SLAM using Bearing and Range
-headers +=   
+headers += BearingFactor.h RangeFactor.h
 sources += planarSLAM.cpp
 check_PROGRAMS += testPlanarSLAM
 testPlanarSLAM_SOURCES = testPlanarSLAM.cpp

cpp/RangeFactor.h

@@ -12,37 +12,11 @@
 namespace gtsam {
 
 	/**
-	 * Calculate range to a landmark
+	 * Binary factor for a range measurement
-	 * @param pose 2D pose of robot
-	 * @param point 2D location of landmark
-	 * @return range (double)
-	 */
-	double range(const Pose2& pose, const Point2& point) {
-		Point2 d = transform_to(pose, point);
-		return d.norm();
-	}
-
-	/**
-	 * Calculate range and optional derivative(s)
-	 */
-	double range(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-		if (!H1 && !H2) return range(pose, point);
-		Point2 d = transform_to(pose, point);
-		double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
-		Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
-		if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
-		if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
-		return n;
-	}
-
-	/**
-	 * Non-linear factor for a constraint derived from a 2D measurement,
-	 * i.e. the main building block for visual SLAM.
 	 */
 	template<class Config, class PoseKey, class PointKey>
 	class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
-			Point2> {
+	Point2> {
 	private:
 
 		double z_; /** measurement */
@@ -53,8 +27,9 @@ namespace gtsam {
 
 		RangeFactor(); /* Default constructor */
 
-		RangeFactor(double z, double sigma, const PoseKey& i, const PointKey& j) :
+		RangeFactor(const PoseKey& i, const PointKey& j, double z,
-			Base(sigma, i, j), z_(z) {
+				const SharedGaussian& model) :
+					Base(model, i, j), z_(z) {
 		}
 
 		/** h(x)-z */
@@ -63,6 +38,11 @@ namespace gtsam {
 			double hx = gtsam::range(pose, point, H1, H2);
 			return Vector_(1, hx - z_);
 		}
+
+		/** return the measured */
+		inline const double measured() const {
+			return z_;
+		}
 	}; // RangeFactor
 
 } // namespace gtsam

cpp/planarSLAM.h

@@ -6,10 +6,8 @@
 
 #pragma once
 
-#include "Key.h"
+#include "BearingFactor.h"
-#include "Pose2.h"
+#include "RangeFactor.h"
-#include "Point2.h"
-#include "NonlinearFactor.h"
 #include "TupleConfig.h"
 #include "NonlinearEquality.h"
 #include "PriorFactor.h"
@@ -20,73 +18,6 @@
 // We use gtsam namespace for generally useful factors
 namespace gtsam {
 
-	/**
-	 * Binary factor for a bearing measurement
-	 */
-	template<class Config, class PoseKey, class PointKey>
-	class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
-			PointKey, Point2> {
-	private:
-
-		Rot2 z_; /** measurement */
-
-		typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
-
-	public:
-
-		BearingFactor(); /* Default constructor */
-		BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
-				const SharedGaussian& model) :
-			Base(model, i, j), z_(z) {
-		}
-
-		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
-		Vector evaluateError(const Pose2& pose, const Point2& point,
-				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-			Rot2 hx = bearing(pose, point, H1, H2);
-			return logmap(between(z_, hx));
-		}
-
-		/** return the measured */
-		inline const Rot2 measured() const {
-			return z_;
-		}
-	}; // BearingFactor
-
-	/**
-	 * Binary factor for a range measurement
-	 */
-	template<class Config, class PoseKey, class PointKey>
-	class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
-			Point2> {
-	private:
-
-		double z_; /** measurement */
-
-		typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
-
-	public:
-
-		RangeFactor(); /* Default constructor */
-
-		RangeFactor(const PoseKey& i, const PointKey& j, double z,
-				const SharedGaussian& model) :
-			Base(model, i, j), z_(z) {
-		}
-
-		/** h(x)-z */
-		Vector evaluateError(const Pose2& pose, const Point2& point,
-				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-			double hx = gtsam::range(pose, point, H1, H2);
-			return Vector_(1, hx - z_);
-		}
-
-		/** return the measured */
-		inline const double measured() const {
-			return z_;
-		}
-	}; // RangeFactor
-
 	// Use planarSLAM namespace for specific SLAM instance
 	namespace planarSLAM {