bearing functions and derivatives

cpp/BearingFactor.h

@@ -0,0 +1,76 @@
+/**
+ *  @file  BearingFactor.H
+ *  @authors Frank Dellaert
+ **/
+
+#pragma once
+
+#include "Rot2.h"
+#include "Pose2.h"
+#include "Point2.h"
+
+namespace gtsam {
+
+	/**
+	 * Calculate relative bearing to a landmark in local coordinate frame
+	 * @param point 2D location of landmark
+	 * @param H optional reference for Jacobian
+	 * @return 2D rotation \in SO(2)
+	 */
+	Rot2 relativeBearing(const Point2& d) {
+		double n = d.norm();
+		return Rot2(d.x() / n, d.y() / n);
+	}
+
+	/**
+	 * Calculate relative bearing and optional derivative
+	 */
+	Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
+		double x = d.x(), y = d.y();
+		double d2 = x * x + y * y;
+		double n = sqrt(d2);
+		if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
+		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
+	 * @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;
+	}
+
+	/** old style derivative */
+	inline Matrix Dbearing1(const Pose2& pose, const Point2& point) {
+		Matrix H; bearing(pose, point, H, boost::none); return H;
+	}
+
+	/** old style derivative */
+	inline Matrix Dbearing2(const Pose2& pose, const Point2& point) {
+		Matrix H; bearing(pose, point, boost::none, H); return H;
+	}
+
+} // namespace gtsam

cpp/Makefile.am

@@ -208,6 +208,13 @@ testPose2Config_LDADD   = libgtsam.la
 testPose2Graph_SOURCES  = $(example) testPose2Graph.cpp
 testPose2Graph_LDADD    = libgtsam.la
 
+# 2D Bearing and Range
+headers += BearingFactor.h  
+sources += 
+check_PROGRAMS += testBearingFactor
+testBearingFactor_SOURCES = $(example) testBearingFactor.cpp
+testBearingFactor_LDADD   = libgtsam.la
+
 # 3D Pose constraints
 headers += Pose3Factor.h  
 sources += Pose3Config.cpp Pose3Graph.cpp 

cpp/testBearingFactor.cpp

@@ -0,0 +1,84 @@
+/**
+ *  @file  testBearingFactor.cpp
+ *  @brief Unit tests for BearingFactor Class
+ *  @authors Frank Dellaert, Viorela Ila
+ **/
+
+#include <boost/bind.hpp>
+#include <CppUnitLite/TestHarness.h>
+#include "numericalDerivative.h"
+#include "BearingFactor.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( BearingFactor, relativeBearing )
+{
+	Matrix expectedH, actualH;
+
+	// establish relativeBearing is indeed zero
+	Point2 l1(1, 0);
+	CHECK(assert_equal(Rot2(),relativeBearing(l1)));
+
+	// 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)));
+
+	// Check numerical derivative
+	expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);
+	actualH = DrelativeBearing(l2);
+	CHECK(assert_equal(expectedH,actualH));
+}
+
+/* ************************************************************************* */
+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)));
+
+	// 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)));
+
+	// Check numerical derivatives, optional style
+	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));
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */