diff --git a/.cproject b/.cproject index 3cddd9134..5640a5f52 100644 --- a/.cproject +++ b/.cproject @@ -365,6 +365,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 -j5 @@ -461,38 +493,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 -j2 @@ -567,7 +567,6 @@ make - tests/testBayesTree.run true false @@ -575,7 +574,6 @@ make - testBinaryBayesNet.run true false @@ -623,7 +621,6 @@ make - testSymbolicBayesNet.run true false @@ -631,7 +628,6 @@ make - tests/testSymbolicFactor.run true false @@ -639,7 +635,6 @@ make - testSymbolicFactorGraph.run true false @@ -655,12 +650,19 @@ make - tests/testBayesTree true false true + + make + -j2 + testVSLAMGraph + true + true + true + make -j2 @@ -743,6 +745,7 @@ make + testSimulated2DOriented.run true false @@ -782,6 +785,7 @@ make + testSimulated2D.run true false @@ -789,6 +793,7 @@ make + testSimulated3D.run true false @@ -802,14 +807,6 @@ true true - - make - -j2 - testVSLAMGraph - true - true - true - make -j2 @@ -849,21 +846,6 @@ false true - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j5 @@ -1002,7 +984,6 @@ make - testGraph.run true false @@ -1010,7 +991,6 @@ make - testJunctionTree.run true false @@ -1018,7 +998,6 @@ make - testSymbolicBayesNetB.run true false @@ -1080,6 +1059,22 @@ true true + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -1448,6 +1443,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1528,10 +1531,66 @@ true true - + make -j2 - testGaussianFactor.run + 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 @@ -1632,86 +1691,6 @@ 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 @@ -1776,6 +1755,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -2217,7 +2212,6 @@ cpack - -G DEB true false @@ -2225,7 +2219,6 @@ cpack - -G RPM true false @@ -2233,7 +2226,6 @@ cpack - -G TGZ true false @@ -2241,7 +2233,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2415,6 +2406,14 @@ true true + + make + -j5 + testAttitudeFactor.run + true + true + true + make -j4 @@ -2423,30 +2422,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -2487,18 +2462,26 @@ true true - + make -j2 - tests/testPose2.run + check true true true - + make -j2 - tests/testPose3.run + tests/testSPQRUtil.run + true + true + true + + + make + -j2 + clean true true true @@ -2593,12 +2576,27 @@ 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/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp new file mode 100644 index 000000000..ddfedd1b9 --- /dev/null +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * 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 AttitudeFactor.cpp + * @author Frank Dellaert + * @brief Implementation file for Attitude factor + * @date January 28, 2014 + **/ + +#include "AttitudeFactor.h" + +using namespace std; + +namespace gtsam { + +//*************************************************************************** +void AttitudeFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n"; + z_.print(" measured direction: "); + ref_.print(" reference direction: "); + this->noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol) + && this->ref_.equals(e->ref_, tol); +} + +//*************************************************************************** +Vector AttitudeFactor::evaluateError(const Pose3& p, + boost::optional H) const { + const Rot3& R = p.rotation(); + if (H) { + Matrix D_q_R, D_e_q; + Sphere2 q = R.rotate(z_, D_q_R); + Vector e = ref_.error(q, D_e_q); + H->resize(2, 6); + H->block < 2, 3 > (0, 0) = D_e_q * D_q_R; + H->block < 2, 3 > (0, 3) << Matrix::Zero(2, 3); + return e; + } else { + Sphere2 q = R * z_; + return ref_.error(q); + } +} + +//*************************************************************************** + +}/// namespace gtsam diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h new file mode 100644 index 000000000..6f97f539a --- /dev/null +++ b/gtsam/navigation/AttitudeFactor.h @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + + * 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 AttitudeFactor.h + * @author Frank Dellaert + * @brief Header file for Attitude factor + * @date January 28, 2014 + **/ +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Prior on the attitude of a Pose3. + * Example: + * - measurement is direction of gravity in body frame bF + * - reference is direction of gravity in navigation frame nG + * This factor will give zero error if nRb * bF == nG + * @addtogroup Navigation + */ +class AttitudeFactor: public NoiseModelFactor1 { + +private: + + typedef NoiseModelFactor1 Base; + + Sphere2 z_, ref_; ///< Position measurement in + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef AttitudeFactor This; + + /** default constructor - only use for serialization */ + AttitudeFactor() { + } + + virtual ~AttitudeFactor() { + } + + /** + * @brief Constructor + * @param key of the Pose3 variable that will be constrained + * @param z measured direction in body frame + * @param ref reference direction in navigation frame + * @param model Gaussian noise model + */ + AttitudeFactor(Key key, const Sphere2& z, const Sphere2& ref, + const SharedNoiseModel& model) : + Base(model, key), z_(z), ref_(ref) { + } + + /// @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; + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& p, + boost::optional H = boost::none) const; + +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(z_); + ar & BOOST_SERIALIZATION_NVP(ref_); + } +}; + +} /// namespace gtsam + diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 9fea29ade..cb52f25fd 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -23,7 +23,7 @@ namespace gtsam { /** - * Prior on position in a cartesian frame. + * Prior on position in a Cartesian frame. * Possibilities include: * ENU: East-North-Up navigation frame at some local origin * NED: North-East-Down navigation frame at some local origin diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp new file mode 100644 index 000000000..d079e0058 --- /dev/null +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 testAttitudeFactor.cpp + * @brief Unit test for AttitudeFactor + * @author Frank Dellaert + * @date January 22, 2014 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// ************************************************************************* +TEST( AttitudeFactor, Constructor ) { + + // Example: pitch and roll of aircraft in an ENU Cartesian frame. + // If pitch and roll are zero for an aerospace frame, + // that means Z is pointing down, i.e., direction of Z = (0,0,-1) + Sphere2 bZ(0, 0, 1); // body Z axis + Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + AttitudeFactor factor(key, bZ, nDown, model); + + // Create a linearization point at the zero-error point + Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); + EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none), T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +// ************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// ************************************************************************* diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 7acb0b82b..bade854af 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -17,15 +17,16 @@ */ #include -#include #include #include + #include + #include using namespace std; -using namespace GeographicLib; using namespace gtsam; +using namespace GeographicLib; // ************************************************************************* TEST( GPSFactor, Constructors ) { @@ -50,8 +51,9 @@ TEST( GPSFactor, Constructors ) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); GPSFactor factor(key, Point3(E, N, U), model); - // Create a linearization point at the zero-error point - Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + // Create a linearization point at zero error + Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); + EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -62,7 +64,7 @@ TEST( GPSFactor, Constructors ) { factor.evaluateError(T, actualH); // Verify we get the expected error - CHECK(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); } //***************************************************************************