diff --git a/.cproject b/.cproject index 5640a5f52..3f6979819 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 @@ -2414,6 +2423,14 @@ true true + + make + -j5 + testMagFactor.run + true + true + true + make -j4 @@ -2422,6 +2439,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 @@ -2462,26 +2503,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 @@ -2576,27 +2609,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/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index ac8124139..507fc5135 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -105,17 +105,15 @@ public: Matrix skew() const; /// Return unit-norm Point3 - Point3 point3(boost::optional H = boost::none) const { + const Point3& point3(boost::optional H = boost::none) const { if (H) *H = basis(); return p_; } - /// Return unit-norm Vector - Vector unitVector(boost::optional H = boost::none) const { - if (H) - *H = basis(); - return (p_.vector ()); + /// Return scaled direction as Point3 + friend Point3 operator*(double s, const Sphere2& d) { + return s*d.p_; } /// Signed, vector-valued error between two directions diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h new file mode 100644 index 000000000..6c47523a6 --- /dev/null +++ b/gtsam/navigation/MagFactor.h @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------------- + + * 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 MagFactor.h + * @brief Factors involving magnetometers + * @author Frank Dellaert + * @date January 29, 2014 + */ + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Factor to estimate rotation given magnetometer reading + * This version uses model measured bM = scale * bRn * direction + bias + * and assumes scale, direction, and the bias are given. + * Rotation is around negative Z axis, i.e. positive is yaw to right! + */ +class MagFactor: public NoiseModelFactor1 { + + const Point3 measured_; ///< The measured magnetometer values + const Point3 nM_; ///< Local magnetic field (mag output units) + const Point3 bias_; ///< bias + +public: + + /** Constructor */ + MagFactor(Key key, const Point3& measured, double scale, + const Sphere2& direction, const Point3& bias, + const SharedNoiseModel& model) : + NoiseModelFactor1(model, key), // + measured_(measured), nM_(scale * direction), bias_(bias) { + } + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new MagFactor(*this))); + } + + static Point3 unrotate(const Rot2& R, const Point3& p, + boost::optional HR = boost::none) { + Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); + if (HR) + *HR = HR->col(2); + return q; + } + + /** + * @brief vector of errors + */ + Vector evaluateError(const Rot2& nRb, + boost::optional H = boost::none) const { + // measured bM = nRbÕ * nM + b + Point3 hx = unrotate(nRb, nM_, H) + bias_; + return (hx - measured_).vector(); + } +}; + +/** + * Factor to estimate rotation given magnetometer reading + * This version uses model measured bM = scale * bRn * direction + bias + * and assumes scale, direction, and the bias are given + */ +class MagFactor1: public NoiseModelFactor1 { + + const Point3 measured_; ///< The measured magnetometer values + const Point3 nM_; ///< Local magnetic field (mag output units) + const Point3 bias_; ///< bias + +public: + + /** Constructor */ + MagFactor1(Key key, const Point3& measured, double scale, + const Sphere2& direction, const Point3& bias, + const SharedNoiseModel& model) : + NoiseModelFactor1(model, key), // + measured_(measured), nM_(scale * direction), bias_(bias) { + } + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new MagFactor1(*this))); + } + + /** + * @brief vector of errors + */ + Vector evaluateError(const Rot3& nRb, + boost::optional H = boost::none) const { + // measured bM = nRbÕ * nM + b + Point3 hx = nRb.unrotate(nM_, H) + bias_; + return (hx - measured_).vector(); + } +}; + +/** + * Factor to calibrate local Earth magnetic field as well as magnetometer bias + * This version uses model measured bM = bRn * nM + bias + * and optimizes for both nM and the bias, where nM is in units defined by magnetometer + */ +class MagFactor2: public NoiseModelFactor2 { + + const Point3 measured_; ///< The measured magnetometer values + const Rot3 bRn_; ///< The assumed known rotation from nav to body + +public: + + /** Constructor */ + MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, + const SharedNoiseModel& model) : + NoiseModelFactor2(model, key1, key2), // + measured_(measured), bRn_(nRb.inverse()) { + } + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new MagFactor2(*this))); + } + + /** + * @brief vector of errors + * @param nM (unknown) local earth magnetic field vector, in nav frame + * @param bias (unknown) 3D bias + */ + Vector evaluateError(const Point3& nM, const Point3& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; + if (H2) + *H2 = eye(3); + return (hx - measured_).vector(); + } +}; + +/** + * Factor to calibrate local Earth magnetic field as well as magnetometer bias + * This version uses model measured bM = scale * bRn * direction + bias + * and optimizes for both scale, direction, and the bias. + */ +class MagFactor3: public NoiseModelFactor3 { + + const Point3 measured_; ///< The measured magnetometer values + const Rot3 bRn_; ///< The assumed known rotation from nav to body + +public: + + /** Constructor */ + MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, + const Rot3& nRb, const SharedNoiseModel& model) : + NoiseModelFactor3(model, key1, key2, key3), // + measured_(measured), bRn_(nRb.inverse()) { + } + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new MagFactor3(*this))); + } + + /** + * @brief vector of errors + * @param nM (unknown) local earth magnetic field vector, in nav frame + * @param bias (unknown) 3D bias + */ + Vector evaluateError(const LieScalar& scale, const Sphere2& direction, + const Point3& bias, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, boost::optional H3 = + boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Sphere2 rotated = bRn_.rotate(direction, boost::none, H2); + Point3 hx = scale * rotated.point3() + bias; + if (H1) + *H1 = rotated.point3().vector(); + if (H2) // H2 is 2*2, but we need 3*2 + { + Matrix H; + rotated.point3(H); + *H2 = scale * H * (*H2); + } + if (H3) + *H3 = eye(3); + return (hx - measured_).vector(); + } +}; + +} + diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp new file mode 100644 index 000000000..7e6b940c3 --- /dev/null +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -0,0 +1,113 @@ +/* ---------------------------------------------------------------------------- + + * 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 testMagFactor.cpp + * @brief Unit test for MagFactor + * @author Frank Dellaert + * @date January 29, 2014 + */ + +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace GeographicLib; + +// ************************************************************************* +// Convert from Mag to ENU +// ENU Origin is where the plane was in hold next to runway +// const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; + +// Get field from http://www.ngdc.noaa.gov/geomag-web/#igrfwmm +// Declination = -4.94 degrees (West), Inclination = 62.78 degrees Down +// As NED vector, in nT: +Point3 nM(22653.29982, -1956.83010, 44202.47862); +// Let's assume scale factor, +double scale = 255.0 / 50000.0; +// ...ground truth orientation, +Rot3 nRb = Rot3::yaw(-0.1); +Rot2 theta = nRb.yaw(); +// ...and bias +Point3 bias(10, -10, 50); +// ... then we measure +Point3 scaled = scale * nM; +Point3 measured = nRb.inverse() * (scale * nM) + bias; + +LieScalar s(scale * nM.norm()); +Sphere2 dir(nM); + +SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + +using boost::none; + +// ************************************************************************* +TEST( MagFactor, unrotate ) { + Matrix H; + Point3 expected(22735.5, 314.502, 44202.5); + EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); +} + +// ************************************************************************* +TEST( MagFactor, Factors ) { + + Matrix H1, H2, H3; + + // MagFactor + MagFactor f(1, measured, s, dir, bias, model); + EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + +// MagFactor1 + MagFactor1 f1(1, measured, s, dir, bias, model); + EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + +// MagFactor2 + MagFactor2 f2(1, 2, measured, nRb, model); + EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + H1, 1e-7)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + H2, 1e-7)); + +// MagFactor2 + MagFactor3 f3(1, 2, 3, measured, nRb, model); + EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + H1, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + H2, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + H3, 1e-7)); +} + +// ************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// *************************************************************************