diff --git a/.cproject b/.cproject index 280ac750d..3cddd9134 100644 --- a/.cproject +++ b/.cproject @@ -365,38 +365,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - make -j5 @@ -493,6 +461,38 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + make -j2 @@ -567,6 +567,7 @@ make + tests/testBayesTree.run true false @@ -574,6 +575,7 @@ make + testBinaryBayesNet.run true false @@ -621,6 +623,7 @@ make + testSymbolicBayesNet.run true false @@ -628,6 +631,7 @@ make + tests/testSymbolicFactor.run true false @@ -635,6 +639,7 @@ make + testSymbolicFactorGraph.run true false @@ -650,19 +655,12 @@ make + tests/testBayesTree true false true - - make - -j2 - testVSLAMGraph - true - true - true - make -j2 @@ -745,7 +743,6 @@ make - testSimulated2DOriented.run true false @@ -785,7 +782,6 @@ make - testSimulated2D.run true false @@ -793,7 +789,6 @@ make - testSimulated3D.run true false @@ -807,6 +802,14 @@ true true + + make + -j2 + testVSLAMGraph + true + true + true + make -j2 @@ -846,6 +849,21 @@ false true + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + true + make -j5 @@ -984,6 +1002,7 @@ make + testGraph.run true false @@ -991,6 +1010,7 @@ make + testJunctionTree.run true false @@ -998,6 +1018,7 @@ make + testSymbolicBayesNetB.run true false @@ -1059,22 +1080,6 @@ true true - - make - -j2 - check - true - true - true - - - make - - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -1443,14 +1448,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1531,66 +1528,10 @@ true true - + make -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run + testGaussianFactor.run true true true @@ -1691,6 +1632,86 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1755,22 +1776,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -2212,6 +2217,7 @@ cpack + -G DEB true false @@ -2219,6 +2225,7 @@ cpack + -G RPM true false @@ -2226,6 +2233,7 @@ cpack + -G TGZ true false @@ -2233,6 +2241,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2398,6 +2407,14 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + make -j4 @@ -2406,6 +2423,30 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testSPQRUtil.run + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -2446,26 +2487,18 @@ true true - + make -j2 - check + tests/testPose2.run true true true - + make -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean + tests/testPose3.run true true true @@ -2560,27 +2593,12 @@ make + testErrors.run true false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h new file mode 100644 index 000000000..e5d7655fe --- /dev/null +++ b/gtsam/navigation/GPSFactor.h @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * 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 Frank Dellaert + * @brief Header file for GPS factor + * @date January 22, 2014 + **/ +#pragma once + +#include +#include + +namespace gtsam { + +/// Geodetic coordinates +class Geodetic { +private: + double latitude_, longitude_, altitude_; + Geodetic(double latitude, double longitude, double altitude); +public: + Point3 toNED(const Geodetic& originNED) const { + return Point3(); + } +}; + + +/** + * Prior on position in a local North-East-Down (NED) navigation frame + * @addtogroup Navigation + */ +class GPSFactor: public NoiseModelFactor1 { + +private: + + typedef NoiseModelFactor1 Base; + + Point3 nT_; ///< Position measurement in NED + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactor This; + + /** default constructor - only use for serialization */ + GPSFactor() { + } + + virtual ~GPSFactor() { + } + + /** + * Constructor from a measurement already in local NED + * @param key of the Pose3 variable that will be constrained + * @param gpsInNED measurement already in NED coordinates + * @param model Gaussian noise model + */ + GPSFactor(Key key, const Point3& gpsInNED, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsInNED) { + } + + /** + * Constructor that converts geodetic to local NED + * @param key of the Pose3 variable that will be constrained + * @param gps measurement + * @param originNED the origin of the NED frame + * @param model Gaussian noise model + */ + GPSFactor(Key key, const Geodetic& gps, const Geodetic& originNED, + const SharedNoiseModel& model) : + Base(model, key), nT_(gps.toNED(originNED)) { + } + + /// @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))); + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n"; + nT_.print(" prior mean: "); + 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->nT_.equals(e->nT_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& p, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + *H << zeros(3, 3) << eye(3); // TODO make static + } + // manifold equivalent of h(x)-z -> log(z,h(x)) + return nT_.localCoordinates(p.translation()); + } + + const Point3 & measurementInNED() const { + return nT_; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(nT_); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp new file mode 100644 index 000000000..ea8e54aef --- /dev/null +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGPSFactor.cpp + * @brief Unit test for GPSFactor + * @author Frank Dellaert + * @date January 22, 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// ************************************************************************* +TEST( GPSFactor, Constructors ) { + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + Point3 gpsInNED; + GPSFactor::Geodetic gps, originNED; + GPSFactor factor1(key, gpsInNED, model); + GPSFactor factor2(key, gps, originNED, model); +} + +// ************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// *************************************************************************