added BearingRangeFactor which captures both BearingFactor and RangeFactor

cpp/BearingRangeFactor.h

@@ -0,0 +1,58 @@
+/*
+ * BearingRangeFactor.h
+ *
+ *   Created on: Apr 1, 2010
+ *       Author: Kai Ni
+ *  Description: a single factor contains both the bearing and the range to prevent handle to pair bearing and range factors
+ */
+
+#pragma once
+
+#include "BearingFactor.h"
+#include "RangeFactor.h"
+
+namespace gtsam {
+
+	/**
+	 * Binary factor for a bearing measurement
+	 */
+	template<class Config, class PoseKey, class PointKey>
+	class BearingRangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
+	PointKey, Point2> {
+	private:
+
+		// the bearing factor
+		BearingFactor<Config, PoseKey, PointKey> bearing_;
+
+		// the range factor
+		RangeFactor<Config, PoseKey, PointKey> range_;
+
+		typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
+
+	public:
+
+		BearingRangeFactor(); /* Default constructor */
+		BearingRangeFactor(const PoseKey& i, const PointKey& j, const std::pair<Rot2, double>& z,
+				const SharedGaussian& model) :
+					Base(model, i, j), bearing_(i, j, z.first, model), range_(i, j, z.second, model) {
+		}
+
+		/** 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 {
+			boost::optional<Matrix&> H11 = H1, H21 = H1;
+			boost::optional<Matrix&> H12 = H2, H22 = H2;
+			Vector e1 = bearing_.evaluateError(pose, point, H11, H12);
+			Vector e2 =   range_.evaluateError(pose, point, H21, H21);
+			if (H1) *H1 = stack(2, &(*H11), &(*H21));
+			if (H2) *H2 = stack(2, &(*H12), &(*H22));
+			return concatVectors(2, &e1, &e2);
+		}
+
+		/** return the measured */
+		inline const std::pair<Rot2, double> measured() const {
+			return concatVectors(2, bearing_.measured(), range_.measured());
+		}
+	}; // BearingRangeFactor
+
+} // namespace gtsam

cpp/Makefile.am

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

cpp/planarSLAM.h

@@ -6,8 +6,7 @@
 
 #pragma once
 
-#include "BearingFactor.h"
-#include "RangeFactor.h"
+#include "BearingRangeFactor.h"
 #include "TupleConfig.h"
 #include "NonlinearEquality.h"
 #include "PriorFactor.h"
@@ -34,6 +33,7 @@ namespace gtsam {
 		typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
 		typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
 		typedef RangeFactor<Config, PoseKey, PointKey> Range;
+		typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
 
 		// Graph
 		struct Graph: public NonlinearFactorGraph<Config> {

cpp/testPlanarSLAM.cpp

@@ -7,6 +7,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include "planarSLAM.h"
+#include "BearingRangeFactor.h"
 
 using namespace std;
 using namespace gtsam;
@@ -17,6 +18,7 @@ static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
 
 SharedGaussian
 	sigma(noiseModel::Isotropic::Sigma(1,0.1)),
+	sigma2(noiseModel::Isotropic::Sigma(2,0.1)),
 	I3(noiseModel::Unit::Create(3));
 
 /* ************************************************************************* */
@@ -53,6 +55,24 @@ TEST( planarSLAM, RangeFactor )
 	CHECK(assert_equal(Vector_(1,0.22),actual));
 }
 
+/* ************************************************************************* */
+TEST( planarSLAM, BearingRangeFactor )
+{
+	// Create factor
+	Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
+	double b(sqrt(2) - 0.22); // h(x) - z = 0.22
+	planarSLAM::BearingRange factor(2, 3, make_pair(r,b), sigma2);
+
+	// create config
+	planarSLAM::Config c;
+	c.insert(2, x2);
+	c.insert(3, l3);
+
+	// Check error
+	Vector actual = factor.unwhitenedError(c);
+	CHECK(assert_equal(Vector_(2,-0.1, 0.22),actual));
+}
+
 /* ************************************************************************* */
 TEST( planarSLAM, constructor )
 {