Moved RelativeElevationFactor from MastSLAM

gtsam_unstable/slam/CMakeLists.txt

@@ -4,7 +4,7 @@ install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
 
 # Components to link tests in this subfolder against
 set(slam_local_libs 
-   #slam_unstable # No sources currently, so no convenience lib
+   slam_unstable 
    slam
    nonlinear
    linear

gtsam_unstable/slam/RelativeElevationFactor.cpp

@@ -0,0 +1,53 @@
+/**
+ * @file RelativeElevationFactor.cpp
+ *
+ * @date Aug 17, 2012
+ * @author Alex Cunningham
+ */
+
+#include <gtsam_unstable/slam/RelativeElevationFactor.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+RelativeElevationFactor::RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
+		const SharedNoiseModel& model)
+: Base(model, poseKey, pointKey), measured_(measured)
+{
+}
+
+/* ************************************************************************* */
+Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& point,
+		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	double hx = pose.z() - point.z();
+	if (H1) {
+		*H1 = zeros(1, 6);
+		// Use bottom row of rotation matrix for derivative of translation
+		(*H1)(0, 3) = pose.rotation().r1().z();
+		(*H1)(0, 4) = pose.rotation().r2().z();
+		(*H1)(0, 5) = pose.rotation().r3().z();
+	}
+
+	if (H2) {
+		*H2 = zeros(1, 3);
+		(*H2)(0, 2) = -1.0;
+	}
+	return Vector_(1, hx - measured_);
+}
+
+/* ************************************************************************* */
+bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const {
+	const This *e = dynamic_cast<const This*> (&expected);
+	return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol;
+}
+
+/* ************************************************************************* */
+void RelativeElevationFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+	std::cout << s << "RelativeElevationFactor, relative elevation = " << measured_ << std::endl;
+	Base::print("", keyFormatter);
+}
+/* ************************************************************************* */
+
+} // \namespace gtsam
+
+

gtsam_unstable/slam/RelativeElevationFactor.h

@@ -0,0 +1,77 @@
+/**
+ * @file RelativeElevationFactor.h
+ *
+ * @brief Factor representing a known relative altitude in global frame
+ * 
+ * @date Aug 17, 2012
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor for a relative elevation.  Note that this
+ * factor takes into account only elevation, and corrects for orientation.
+ * Unlike a range factor, the relative elevation is signed, and only affects
+ * the Z coordinate.  Measurement function h(pose, pt) = h.z() - pt.z()
+ *
+ * Dimension: 1
+ *
+ * TODO: enable use of a Pose3 for the target as well
+ */
+class RelativeElevationFactor: public NoiseModelFactor2<Pose3, Point3> {
+private:
+
+	double measured_; /** measurement */
+
+	typedef RelativeElevationFactor This;
+	typedef NoiseModelFactor2<Pose3, Point3> Base;
+
+public:
+
+	RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */
+
+	RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
+			const SharedNoiseModel& model);
+
+	virtual ~RelativeElevationFactor() {}
+
+	/// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+	  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+	      gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+
+	/** h(x)-z */
+	Vector evaluateError(const Pose3& pose, const Point3& point,
+			boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const;
+
+	/** return the measured */
+	inline double measured() const { return measured_; }
+
+	/** equals specialized to this factor */
+	virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
+
+	/** print contents */
+	void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+private:
+
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class ARCHIVE>
+	void serialize(ARCHIVE & ar, const unsigned int version) {
+		ar & boost::serialization::make_nvp("NoiseModelFactor2",
+				boost::serialization::base_object<Base>(*this));
+		ar & BOOST_SERIALIZATION_NVP(measured_);
+	}
+}; // RelativeElevationFactor
+
+
+} // \namespace gtsam
+
+

gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp

@@ -0,0 +1,138 @@
+/**
+ * @file testRelativeElevationFactor.cpp
+ *
+ * @date Aug 17, 2012
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam_unstable/slam/RelativeElevationFactor.h>
+
+#include <gtsam/base/numericalDerivative.h>
+
+using namespace gtsam;
+
+SharedNoiseModel model1 = noiseModel::Unit::Create(1);
+
+const double tol = 1e-5;
+
+const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
+const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
+const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
+const Point3 point1(3.0, 4.0, 2.0);
+const gtsam::Key poseKey = 1, pointKey = 2;
+
+/* ************************************************************************* */
+LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
+	return LieVector(factor.evaluateError(x, p));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, level_zero_error ) {
+	// Zero error
+	double measured = 2.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, level_positive ) {
+	// Positive meas
+	double measured = 0.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose1, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, level_negative ) {
+	// Negative meas
+	double measured = -1.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose1, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, rotated_zero_error ) {
+	// Zero error
+	double measured = 2.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, rotated_positive ) {
+	// Positive meas
+	double measured = 0.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose2, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, rotated_negative1 ) {
+	// Negative meas
+	double measured = -1.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose2, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+TEST( testRelativeElevationFactor, rotated_negative2 ) {
+	// Negative meas
+	double measured = -1.0;
+	RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
+	Matrix actH1, actH2;
+	EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose3, point1, actH1, actH2)));
+	Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
+	Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
+			boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
+	EXPECT(assert_equal(expH1, actH1, tol));
+	EXPECT(assert_equal(expH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */