diff --git a/.cproject b/.cproject index 21ac9d913..a0cec2816 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -784,18 +782,18 @@ true true - + make -j5 - testGaussianFactorGraph.run + testGaussianFactorGraphUnordered.run true true true - + make -j5 - testGaussianBayesNet.run + testGaussianBayesNetUnordered.run true true true @@ -2622,6 +2620,14 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + make -j2 diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/GPSFactor.h new file mode 100644 index 000000000..7d720cd15 --- /dev/null +++ b/gtsam/slam/GPSFactor.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GPSFactor.h + * @author Luca Carlone + **/ +#pragma once + +#include + +#include +#include + +namespace gtsam { + + /** + * A class to model GPS measurements, including a bias term which models + * common-mode errors and that can be partially corrected if other sensors are used + * @addtogroup SLAM + */ + class GPSFactor: public NoiseModelFactor2 { + + private: + + typedef GPSFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** The measurement */ + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + GPSFactor() {} + + /** Constructor */ + GPSFactor(Key posekey, Key biaskey, const Point3 measured, + const SharedNoiseModel& model) : + Base(model, posekey, biaskey), measured_(measured) { + } + + virtual ~GPSFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GPSFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + measured_.print(" measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose, const Point3& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + + if (H1 || H2){ + H1->resize(3,6); // jacobian wrt pose + (*H1) << Matrix3::Zero(), pose.rotation().matrix(); + H2->resize(3,3); // jacobian wrt bias + (*H2) << Matrix3::Identity(); + } + return pose.translation().vector() + bias.vector() - measured_.vector(); + } + + /** return the measured */ + const Point3 measured() const { + return measured_; + } + + /** number of variables attached to this factor */ + size_t size() const { + return 2; + } + + 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_); + } + }; // \class GPSFactor + +} /// namespace gtsam diff --git a/gtsam/slam/tests/testGPSFactor.cpp b/gtsam/slam/tests/testGPSFactor.cpp new file mode 100644 index 000000000..f501183b3 --- /dev/null +++ b/gtsam/slam/tests/testGPSFactor.cpp @@ -0,0 +1,85 @@ +/** + * @file testGPSFactor.cpp + * @brief + * @author Luca Carlone + * @date July 30, 2014 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace gtsam::noiseModel; +// Convenience for named keys + +using symbol_shorthand::X; +using symbol_shorthand::B; + +TEST(GPSFactor, errorNoiseless) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(GPSFactor, errorNoisy) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(1.0,2.0,3.0); + Point3 measured = t - noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(GPSFactor, jacobian) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + + Matrix actualH1, actualH2; + factor.evaluateError(pose,bias, actualH1, actualH2); + + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &GPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &GPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */