BearingFactor works

cpp/BearingFactor.h

@@ -8,6 +8,7 @@
 #include "Rot2.h"
 #include "Pose2.h"
 #include "Point2.h"
+#include "NonlinearFactor.h"
 
 namespace gtsam {
 
@@ -33,11 +34,6 @@ namespace gtsam {
 		return Rot2(x / n, y / n);
 	}
 
-	/** old style derivative */
-	inline Matrix DrelativeBearing(const Point2& d) {
-		Matrix H; relativeBearing(d, H); return H;
-	}
-
 	/**
 	 * Calculate bearing to a landmark
 	 * @param pose 2D pose of robot
@@ -54,7 +50,7 @@ namespace gtsam {
 	 */
 	Rot2 bearing(const Pose2& pose, const Point2& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-		if (!H1 && !H2) return bearing(pose,point);
+		if (!H1 && !H2) return bearing(pose, point);
 		Point2 d = transform_to(pose, point);
 		Matrix D_result_d;
 		Rot2 result = relativeBearing(d, D_result_d);
@@ -63,14 +59,33 @@ namespace gtsam {
 		return result;
 	}
 
-	/** old style derivative */
-	inline Matrix Dbearing1(const Pose2& pose, const Point2& point) {
-		Matrix H; bearing(pose, point, H, boost::none); return H;
-	}
+	/**
+	 * 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> {
+	private:
 
-	/** old style derivative */
-	inline Matrix Dbearing2(const Pose2& pose, const Point2& point) {
-		Matrix H; bearing(pose, point, boost::none, H); return H;
-	}
+		Rot2 z_; /** measurement */
+
+		typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
+
+	public:
+
+		BearingFactor(); /* Default constructor */
+		BearingFactor(const Rot2& z, double sigma, const PoseKey& i,
+				const PointKey& j) :
+			Base(sigma, 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));
+		}
+	}; // BearingFactor
 
 } // namespace gtsam

cpp/testBearingFactor.cpp

@@ -6,33 +6,43 @@
 
 #include <boost/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
+#include "Key.h"
 #include "numericalDerivative.h"
 #include "BearingFactor.h"
+#include "TupleConfig.h"
 
 using namespace std;
 using namespace gtsam;
 
+// typedefs
+typedef Symbol<Pose2, 'x'> PoseKey;
+typedef Symbol<Point2, 'l'> PointKey;
+typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
+typedef BearingFactor<Config, PoseKey, PointKey> MyFactor;
+
+// some shared test values
+Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
+Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
+
 /* ************************************************************************* */
 TEST( BearingFactor, relativeBearing )
 {
 	Matrix expectedH, actualH;
 
 	// establish relativeBearing is indeed zero
-	Point2 l1(1, 0);
-	CHECK(assert_equal(Rot2(),relativeBearing(l1)));
+	Rot2 actual1 = relativeBearing(l1, actualH);
+	CHECK(assert_equal(Rot2(),actual1));
 
 	// Check numerical derivative
 	expectedH = numericalDerivative11(relativeBearing, l1, 1e-5);
-	actualH = DrelativeBearing(l1);
 	CHECK(assert_equal(expectedH,actualH));
 
 	// establish relativeBearing is indeed 45 degrees
-	Point2 l2(1, 1);
-	CHECK(assert_equal(Rot2(M_PI_4),relativeBearing(l2)));
+	Rot2 actual2 = relativeBearing(l2, actualH);
+	CHECK(assert_equal(Rot2(M_PI_4),actual2));
 
 	// Check numerical derivative
 	expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);
-	actualH = DrelativeBearing(l2);
 	CHECK(assert_equal(expectedH,actualH));
 }
 
@@ -42,40 +52,50 @@ TEST( BearingFactor, bearing )
 	Matrix expectedH1, actualH1, expectedH2, actualH2;
 
 	// establish bearing is indeed zero
-	Pose2 x1;
-	Point2 l1(1, 0);
 	CHECK(assert_equal(Rot2(),bearing(x1,l1)));
 
 	// establish bearing is indeed 45 degrees
-	Point2 l2(1, 1);
 	CHECK(assert_equal(Rot2(M_PI_4),bearing(x1,l2)));
 
 	// establish bearing is indeed 45 degrees even if shifted
-	Pose2 x2(1, 1, 0);
-	Point2 l3(2, 2);
-	CHECK(assert_equal(Rot2(M_PI_4),bearing(x2,l3)));
+	Rot2 actual23 = bearing(x2, l3, actualH1, actualH2);
+	CHECK(assert_equal(Rot2(M_PI_4),actual23));
 
 	// Check numerical derivatives
 	expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5);
-	actualH1 = Dbearing1(x2, l3);
 	CHECK(assert_equal(expectedH1,actualH1));
 	expectedH2 = numericalDerivative22(bearing, x2, l3, 1e-5);
-	actualH2 = Dbearing1(x2, l3);
 	CHECK(assert_equal(expectedH1,actualH1));
 
 	// establish bearing is indeed 45 degrees even if rotated
-	Pose2 x3(1, 1, M_PI_4);
-	Point2 l4(1, 3);
-	CHECK(assert_equal(Rot2(M_PI_4),bearing(x3,l4)));
+	Rot2 actual34 = bearing(x3, l4, actualH1, actualH2);
+	CHECK(assert_equal(Rot2(M_PI_4),actual34));
 
-	// Check numerical derivatives, optional style
+	// Check numerical derivatives
 	expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5);
 	expectedH2 = numericalDerivative22(bearing, x3, l4, 1e-5);
-	bearing(x3, l4, actualH1, actualH2);
 	CHECK(assert_equal(expectedH1,actualH1));
 	CHECK(assert_equal(expectedH1,actualH1));
 }
 
+/* ************************************************************************* */
+TEST( BearingFactor, evaluateError )
+{
+	// Create factor
+	Rot2 z(M_PI_4+0.1); // h(x) - z = -0.1
+	double sigma = 0.1;
+	MyFactor factor(z, sigma, 2, 3);
+
+	// create config
+	Config c;
+	c.insert(2, x2);
+	c.insert(3, l3);
+
+	// Check error
+	Vector actual = factor.error_vector(c);
+	CHECK(assert_equal(Vector_(1,-0.1),actual));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;