From 4fb9f3b39ee0217c8abdf43fececff90eed96af5 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 20 Aug 2012 14:25:04 +0000 Subject: [PATCH] Moved RelativeElevationFactor from MastSLAM --- gtsam_unstable/slam/CMakeLists.txt | 2 +- .../slam/RelativeElevationFactor.cpp | 53 +++++++ gtsam_unstable/slam/RelativeElevationFactor.h | 77 ++++++++++ .../tests/testRelativeElevationFactor.cpp | 138 ++++++++++++++++++ 4 files changed, 269 insertions(+), 1 deletion(-) create mode 100644 gtsam_unstable/slam/RelativeElevationFactor.cpp create mode 100644 gtsam_unstable/slam/RelativeElevationFactor.h create mode 100644 gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt index 88dd60edd..33cb45df9 100644 --- a/gtsam_unstable/slam/CMakeLists.txt +++ b/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 diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp new file mode 100644 index 000000000..2d0ca7548 --- /dev/null +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -0,0 +1,53 @@ +/** + * @file RelativeElevationFactor.cpp + * + * @date Aug 17, 2012 + * @author Alex Cunningham + */ + +#include + +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 H1, boost::optional 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 (&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 + + diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h new file mode 100644 index 000000000..a0186a9f2 --- /dev/null +++ b/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 +#include + +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 { +private: + + double measured_; /** measurement */ + + typedef RelativeElevationFactor This; + typedef NoiseModelFactor2 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::shared_ptr(new This(*this))); } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1 = boost::none, boost::optional 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 + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; // RelativeElevationFactor + + +} // \namespace gtsam + + diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp new file mode 100644 index 000000000..321464c48 --- /dev/null +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -0,0 +1,138 @@ +/** + * @file testRelativeElevationFactor.cpp + * + * @date Aug 17, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +#include + +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( + boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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( + boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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( + boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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( + boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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( + boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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( + boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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( + boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + 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); } +/* ************************************************************************* */