From 2775bb938118f051b5fbae9c8936d4857b8f3e7e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jan 2014 21:16:43 -0500 Subject: [PATCH 01/12] MagFactor scaffold --- .cproject | 356 ++++++++++++----------- gtsam/navigation/tests/testMagFactor.cpp | 199 +++++++++++++ 2 files changed, 386 insertions(+), 169 deletions(-) create mode 100644 gtsam/navigation/tests/testMagFactor.cpp 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/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp new file mode 100644 index 000000000..18c52edd5 --- /dev/null +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -0,0 +1,199 @@ +/* ---------------------------------------------------------------------------- + + * 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 22, 2014 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/** + * 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. + * Issue with it: expresses nM in units of magnetometer + */ +class MagFactor1: public NoiseModelFactor2 { + + Vector3 measured_; /** The measured magnetometer values */ + Matrix3 bRn_; /** The assumed known rotation from nav to body */ + +public: + + /** Constructor */ + MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, + const SharedNoiseModel& model) : + NoiseModelFactor2(model, key1, key2), // + measured_(measured), bRn_(nRb.transpose()) { + } + + /// @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 + * @param nM (unknown) local earth magnetic field vector, in nav frame + * @param bias (unknown) 3D bias + */ + Vector evaluateError(const LieVector& nM, const LieVector& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Vector3 hx = bRn_ * nM + bias; + if (H1) + *H1 = bRn_; + if (H2) + *H2 = eye(3); + return hx - measured_; + } +}; + +/** + * 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 MagFactor2: public NoiseModelFactor3 { + + Vector3 measured_; /** The measured magnetometer values */ + Rot3 bRn_; /** The assumed known rotation from nav to body */ + +public: + + /** Constructor */ + MagFactor2(Key key1, Key key2, Key key3, const Vector3& 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 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 LieScalar& scale, const Sphere2& direction, + const LieVector& 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); + Vector3 hx = scale * rotated.unitVector() + bias; + if (H1) + *H1 = rotated.unitVector(); + if (H2) // I think H2 is 2*2, but we need 3*2 + { + Matrix H; + rotated.unitVector(H); + *H2 = scale * H * (*H2); + } + if (H3) + *H3 = eye(3); + return hx - measured_; + } +}; + +/** + * @file testMagFactor.cpp + * @brief Unit test for MagFactor + * @author Frank Dellaert + * @date January 22, 2014 + */ + +//#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace GeographicLib; + +// ************************************************************************* +TEST( MagFactor, Constructors ) { + + using boost::none; + + // 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: + Vector3 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); + // ...and bias + Vector3 bias(10, -10, 50); + // ... then we measure + Vector3 scaled = scale * nM; + Vector3 measured = scale * nRb.transpose() * nM + bias; + + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + Matrix expectedH1, expectedH2, expectedH3; + Matrix H1, H2, H3; + + // MagFactor1 + MagFactor1 f1(1, 2, measured, nRb, model); + EXPECT( assert_equal(zero(3),f1.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor1::evaluateError, &f1, _1, bias, none, none), scaled),// + H1, 1e-7)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor1::evaluateError, &f1, scaled, _1, none, none), bias),// + H2, 1e-7)); + + // MagFactor2 + MagFactor2 f2(1, 2, 3, measured, nRb, model); + LieScalar s(scale*nM.norm()); + Sphere2 dir(nM[0], nM[1], nM[2]); + EXPECT(assert_equal(zero(3),f2.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor2::evaluateError, &f2, _1, dir, bias, none, none, none), s),// + H1, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor2::evaluateError, &f2, s, _1, bias, none, none, none), dir),// + H2, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor2::evaluateError, &f2, s, dir, _1, none, none, none), bias),// + H3, 1e-7)); +} + +// ************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// ************************************************************************* From 46c266f8e58579590d304b840348b0adb0eab18f Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jan 2014 21:22:00 -0500 Subject: [PATCH 02/12] Header file --- gtsam/navigation/MagFactor.h | 121 +++++++++++++++++++++++ gtsam/navigation/tests/testMagFactor.cpp | 114 +-------------------- 2 files changed, 123 insertions(+), 112 deletions(-) create mode 100644 gtsam/navigation/MagFactor.h diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h new file mode 100644 index 000000000..82b120fab --- /dev/null +++ b/gtsam/navigation/MagFactor.h @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +using namespace std; +using namespace gtsam; + +/** + * 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. + * Issue with it: expresses nM in units of magnetometer + */ +class MagFactor1: public NoiseModelFactor2 { + + Vector3 measured_; /** The measured magnetometer values */ + Matrix3 bRn_; /** The assumed known rotation from nav to body */ + +public: + + /** Constructor */ + MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, + const SharedNoiseModel& model) : + NoiseModelFactor2(model, key1, key2), // + measured_(measured), bRn_(nRb.transpose()) { + } + + /// @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 + * @param nM (unknown) local earth magnetic field vector, in nav frame + * @param bias (unknown) 3D bias + */ + Vector evaluateError(const LieVector& nM, const LieVector& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Vector3 hx = bRn_ * nM + bias; + if (H1) + *H1 = bRn_; + if (H2) + *H2 = eye(3); + return hx - measured_; + } +}; + +/** + * 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 MagFactor2: public NoiseModelFactor3 { + + Vector3 measured_; /** The measured magnetometer values */ + Rot3 bRn_; /** The assumed known rotation from nav to body */ + +public: + + /** Constructor */ + MagFactor2(Key key1, Key key2, Key key3, const Vector3& 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 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 LieScalar& scale, const Sphere2& direction, + const LieVector& 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); + Vector3 hx = scale * rotated.unitVector() + bias; + if (H1) + *H1 = rotated.unitVector(); + if (H2) // I think H2 is 2*2, but we need 3*2 + { + Matrix H; + rotated.unitVector(H); + *H2 = scale * H * (*H2); + } + if (H3) + *H3 = eye(3); + return hx - measured_; + } +}; + diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 18c52edd5..090ef09f2 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -13,120 +13,10 @@ * @file testMagFactor.cpp * @brief Unit test for MagFactor * @author Frank Dellaert - * @date January 22, 2014 + * @date January 29, 2014 */ -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/** - * 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. - * Issue with it: expresses nM in units of magnetometer - */ -class MagFactor1: public NoiseModelFactor2 { - - Vector3 measured_; /** The measured magnetometer values */ - Matrix3 bRn_; /** The assumed known rotation from nav to body */ - -public: - - /** Constructor */ - MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, - const SharedNoiseModel& model) : - NoiseModelFactor2(model, key1, key2), // - measured_(measured), bRn_(nRb.transpose()) { - } - - /// @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 - * @param nM (unknown) local earth magnetic field vector, in nav frame - * @param bias (unknown) 3D bias - */ - Vector evaluateError(const LieVector& nM, const LieVector& bias, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias - Vector3 hx = bRn_ * nM + bias; - if (H1) - *H1 = bRn_; - if (H2) - *H2 = eye(3); - return hx - measured_; - } -}; - -/** - * 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 MagFactor2: public NoiseModelFactor3 { - - Vector3 measured_; /** The measured magnetometer values */ - Rot3 bRn_; /** The assumed known rotation from nav to body */ - -public: - - /** Constructor */ - MagFactor2(Key key1, Key key2, Key key3, const Vector3& 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 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 LieScalar& scale, const Sphere2& direction, - const LieVector& 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); - Vector3 hx = scale * rotated.unitVector() + bias; - if (H1) - *H1 = rotated.unitVector(); - if (H2) // I think H2 is 2*2, but we need 3*2 - { - Matrix H; - rotated.unitVector(H); - *H2 = scale * H * (*H2); - } - if (H3) - *H3 = eye(3); - return hx - measured_; - } -}; - -/** - * @file testMagFactor.cpp - * @brief Unit test for MagFactor - * @author Frank Dellaert - * @date January 22, 2014 - */ - -//#include +#include #include #include From 2a637e77a72b32d58127fdf07669bcdb88048231 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jan 2014 21:25:33 -0500 Subject: [PATCH 03/12] gtsam namespace --- gtsam/navigation/MagFactor.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 82b120fab..b88d72725 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -21,8 +21,7 @@ #include #include -using namespace std; -using namespace gtsam; +namespace gtsam { /** * Factor to calibrate local Earth magnetic field as well as magnetometer bias @@ -119,3 +118,5 @@ public: } }; +} + From fd6eb2b1291d3ae8b2f24402eb34aaaea33b27a4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 30 Jan 2014 00:58:15 -0500 Subject: [PATCH 04/12] Added MagFactor that solves for rotation, now called 1,2,3, with respectively 1,2,3 arguments to factor. --- gtsam/navigation/MagFactor.h | 58 +++++++++++++++++++++--- gtsam/navigation/tests/testMagFactor.cpp | 30 +++++++----- 2 files changed, 71 insertions(+), 17 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index b88d72725..fa71418b9 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -23,13 +23,59 @@ 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 + */ +class MagFactor1: public NoiseModelFactor1 { + + const Vector3 measured_; /** The measured magnetometer values */ + const double scale_; + const Sphere2 direction_; + const Vector3 bias_; + +public: + + /** Constructor */ + MagFactor1(Key key, const Vector3& measured, const LieScalar& scale, + const Sphere2& direction, const LieVector& bias, + const SharedNoiseModel& model) : + NoiseModelFactor1(model, key), // + measured_(measured), scale_(scale), direction_(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, where b is unknown bias + Sphere2 rotated = nRb.unrotate(direction_, H); + Vector3 hx = scale_ * rotated.unitVector() + bias_; + if (H) // I think H2 is 2*2, but we need 3*2 + { + Matrix U; + rotated.unitVector(U); + *H = scale_ * U * (*H); + } + return hx - measured_; + } +}; + /** * 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. * Issue with it: expresses nM in units of magnetometer */ -class MagFactor1: public NoiseModelFactor2 { +class MagFactor2: public NoiseModelFactor2 { Vector3 measured_; /** The measured magnetometer values */ Matrix3 bRn_; /** The assumed known rotation from nav to body */ @@ -37,7 +83,7 @@ class MagFactor1: public NoiseModelFactor2 { public: /** Constructor */ - MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, + MagFactor2(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, const SharedNoiseModel& model) : NoiseModelFactor2(model, key1, key2), // measured_(measured), bRn_(nRb.transpose()) { @@ -46,7 +92,7 @@ public: /// @return a deep copy of this factor virtual NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( - NonlinearFactor::shared_ptr(new MagFactor1(*this))); + NonlinearFactor::shared_ptr(new MagFactor2(*this))); } /** @@ -72,7 +118,7 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor2: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactor3 { Vector3 measured_; /** The measured magnetometer values */ Rot3 bRn_; /** The assumed known rotation from nav to body */ @@ -80,7 +126,7 @@ class MagFactor2: public NoiseModelFactor3 { public: /** Constructor */ - MagFactor2(Key key1, Key key2, Key key3, const Vector3& measured, + MagFactor3(Key key1, Key key2, Key key3, const Vector3& measured, const Rot3& nRb, const SharedNoiseModel& model) : NoiseModelFactor3(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { @@ -89,7 +135,7 @@ public: /// @return a deep copy of this factor virtual NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( - NonlinearFactor::shared_ptr(new MagFactor2(*this))); + NonlinearFactor::shared_ptr(new MagFactor3(*this))); } /** diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 090ef09f2..721664924 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -51,33 +51,41 @@ TEST( MagFactor, Constructors ) { Vector3 scaled = scale * nM; Vector3 measured = scale * nRb.transpose() * nM + bias; + LieScalar s(scale * nM.norm()); + Sphere2 dir(nM[0], nM[1], nM[2]); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); Matrix expectedH1, expectedH2, expectedH3; Matrix H1, H2, H3; // MagFactor1 - MagFactor1 f1(1, 2, measured, nRb, model); - EXPECT( assert_equal(zero(3),f1.evaluateError(scaled,bias,H1,H2),1e-5)); + 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(&MagFactor1::evaluateError, &f1, _1, bias, none, none), scaled),// + (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor1::evaluateError, &f1, scaled, _1, none, none), bias),// + (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 - MagFactor2 f2(1, 2, 3, measured, nRb, model); - LieScalar s(scale*nM.norm()); - Sphere2 dir(nM[0], nM[1], nM[2]); - EXPECT(assert_equal(zero(3),f2.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + 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(&MagFactor2::evaluateError, &f2, _1, dir, bias, none, none, none), s),// + (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, s, _1, bias, none, none, none), dir),// + (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, s, dir, _1, none, none, none), bias),// + (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } From f6a733fb3e604e12cf629f2f96a17d88f0bd2886 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 00:37:03 -0500 Subject: [PATCH 05/12] Prototype MagFactor --- gtsam/navigation/tests/testMagFactor.cpp | 125 ++++++++++++++++++----- 1 file changed, 98 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 721664924..39a7ccc41 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include @@ -28,44 +29,114 @@ using namespace std; using namespace gtsam; using namespace GeographicLib; +/** + * 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 MagFactor: public NoiseModelFactor1 { + + const Vector3 measured_; /** The measured magnetometer values */ + const double scale_; + const Sphere2 direction_; + const Vector3 bias_; + +public: + + /** Constructor */ + MagFactor(Key key, const Vector3& measured, const LieScalar& scale, + const Sphere2& direction, const LieVector& bias, + const SharedNoiseModel& model) : + NoiseModelFactor1(model, key), // + measured_(measured), scale_(scale), direction_(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 Sphere2 unrotate(const Rot2& R, const Sphere2& p, + boost::optional HR = boost::none) { + Sphere2 q = Rot3::yaw(R.theta()) * p; + if (HR) // 2*3 3*1 + (*HR) = -q.basis().transpose() * q.skew().col(2); + return q; + } + + /** + * @brief vector of errors + */ + Vector evaluateError(const Rot2& nRb, + boost::optional H = boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Sphere2 rotated = unrotate(nRb, direction_, H); + Vector3 hx = scale_ * rotated.unitVector() + bias_; + if (H) // I think H2 is 2*2, but we need 3*2 + { + Matrix U; + rotated.unitVector(U); + *H = scale_ * U * (*H); + } + return hx - measured_; + } +}; + // ************************************************************************* -TEST( MagFactor, Constructors ) { +// 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; - using boost::none; +// 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: +Vector3 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 +Vector3 bias(10, -10, 50); +// ... then we measure +Vector3 scaled = scale * nM; +Vector3 measured = scale * nRb.transpose() * nM + bias; - // 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; +LieScalar s(scale * nM.norm()); +Sphere2 dir(nM[0], nM[1], nM[2]); - // 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: - Vector3 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); - // ...and bias - Vector3 bias(10, -10, 50); - // ... then we measure - Vector3 scaled = scale * nM; - Vector3 measured = scale * nRb.transpose() * nM + bias; +SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - LieScalar s(scale * nM.norm()); - Sphere2 dir(nM[0], nM[1], nM[2]); +using boost::none; + +// ************************************************************************* +TEST( MagFactor, unrotate ) { + Matrix H; + Sphere2 expected(0.457383, 0.00632703, 0.889247); + EXPECT( assert_equal(expected, MagFactor::unrotate(theta,dir,H),1e-5)); + EXPECT( assert_equal(numericalDerivative11 // + (boost::bind(&MagFactor::unrotate, _1, dir, none), theta), H, 1e-7)); +} + +// ************************************************************************* +TEST( MagFactor, Factors ) { - SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - Matrix expectedH1, expectedH2, expectedH3; Matrix H1, H2, H3; - // MagFactor1 + // 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)); + (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); - // MagFactor2 +// 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 // @@ -75,7 +146,7 @@ TEST( MagFactor, Constructors ) { (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); - // MagFactor2 +// 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 // From f6731fe559c01213ad679966ee602f7f8fc2c473 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 00:50:03 -0500 Subject: [PATCH 06/12] More efficient derivative --- gtsam/navigation/tests/testMagFactor.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 39a7ccc41..ad9fbc896 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -60,8 +60,12 @@ public: static Sphere2 unrotate(const Rot2& R, const Sphere2& p, boost::optional HR = boost::none) { Sphere2 q = Rot3::yaw(R.theta()) * p; - if (HR) // 2*3 3*1 - (*HR) = -q.basis().transpose() * q.skew().col(2); + if (HR) { + HR->resize(2, 1); + Point3 Q = q.unitVector(); + Matrix B = q.basis().transpose(); + (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0); + } return q; } From 419de466c01961ff0f4ca2e8f400e6c35bc37b03 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 00:51:52 -0500 Subject: [PATCH 07/12] Moved to header --- gtsam/navigation/MagFactor.h | 58 +++++++++++++++++++++++ gtsam/navigation/tests/testMagFactor.cpp | 59 ------------------------ 2 files changed, 58 insertions(+), 59 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index fa71418b9..672f10e3d 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -17,12 +17,70 @@ */ #include +#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 + */ +class MagFactor: public NoiseModelFactor1 { + + const Vector3 measured_; /** The measured magnetometer values */ + const double scale_; + const Sphere2 direction_; + const Vector3 bias_; + +public: + + /** Constructor */ + MagFactor(Key key, const Vector3& measured, const LieScalar& scale, + const Sphere2& direction, const LieVector& bias, + const SharedNoiseModel& model) : + NoiseModelFactor1(model, key), // + measured_(measured), scale_(scale), direction_(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 Sphere2 unrotate(const Rot2& R, const Sphere2& p, + boost::optional HR = boost::none) { + Sphere2 q = Rot3::yaw(R.theta()) * p; + if (HR) { + HR->resize(2, 1); + Point3 Q = q.unitVector(); + Matrix B = q.basis().transpose(); + (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0); + } + return q; + } + + /** + * @brief vector of errors + */ + Vector evaluateError(const Rot2& nRb, + boost::optional H = boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Sphere2 rotated = unrotate(nRb, direction_, H); + Vector3 hx = scale_ * rotated.unitVector() + bias_; + if (H) { + Matrix U; + rotated.unitVector(U); + *H = scale_ * U * (*H); + } + return hx - measured_; + } +}; + /** * Factor to estimate rotation given magnetometer reading * This version uses model measured bM = scale * bRn * direction + bias diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index ad9fbc896..a29aec3b7 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include @@ -29,64 +28,6 @@ using namespace std; using namespace gtsam; using namespace GeographicLib; -/** - * 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 MagFactor: public NoiseModelFactor1 { - - const Vector3 measured_; /** The measured magnetometer values */ - const double scale_; - const Sphere2 direction_; - const Vector3 bias_; - -public: - - /** Constructor */ - MagFactor(Key key, const Vector3& measured, const LieScalar& scale, - const Sphere2& direction, const LieVector& bias, - const SharedNoiseModel& model) : - NoiseModelFactor1(model, key), // - measured_(measured), scale_(scale), direction_(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 Sphere2 unrotate(const Rot2& R, const Sphere2& p, - boost::optional HR = boost::none) { - Sphere2 q = Rot3::yaw(R.theta()) * p; - if (HR) { - HR->resize(2, 1); - Point3 Q = q.unitVector(); - Matrix B = q.basis().transpose(); - (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0); - } - return q; - } - - /** - * @brief vector of errors - */ - Vector evaluateError(const Rot2& nRb, - boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias - Sphere2 rotated = unrotate(nRb, direction_, H); - Vector3 hx = scale_ * rotated.unitVector() + bias_; - if (H) // I think H2 is 2*2, but we need 3*2 - { - Matrix U; - rotated.unitVector(U); - *H = scale_ * U * (*H); - } - return hx - measured_; - } -}; - // ************************************************************************* // Convert from Mag to ENU // ENU Origin is where the plane was in hold next to runway From d555c017d6f70d614a781cda59613f79d7e44a17 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 09:00:36 -0500 Subject: [PATCH 08/12] Fixed comments --- gtsam/navigation/MagFactor.h | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 672f10e3d..5411280e9 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -31,10 +31,10 @@ namespace gtsam { */ class MagFactor: public NoiseModelFactor1 { - const Vector3 measured_; /** The measured magnetometer values */ - const double scale_; - const Sphere2 direction_; - const Vector3 bias_; + const Vector3 measured_; ///< The measured magnetometer values + const double scale_; ///< Scale factor from direction to magnetometer readings + const Sphere2 direction_; ///< Local magnetic field direction + const Vector3 bias_; ///< bias public: @@ -88,10 +88,10 @@ public: */ class MagFactor1: public NoiseModelFactor1 { - const Vector3 measured_; /** The measured magnetometer values */ - const double scale_; - const Sphere2 direction_; - const Vector3 bias_; + const Vector3 measured_; ///< The measured magnetometer values + const double scale_; ///< Scale factor from direction to magnetometer readings + const Sphere2 direction_; ///< Local magnetic field direction + const Vector3 bias_; ///< bias public: @@ -130,13 +130,12 @@ public: /** * 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. - * Issue with it: expresses nM in units of magnetometer + * and optimizes for both nM and the bias, where nM is in units defined by magnetometer */ class MagFactor2: public NoiseModelFactor2 { - Vector3 measured_; /** The measured magnetometer values */ - Matrix3 bRn_; /** The assumed known rotation from nav to body */ + const Vector3 measured_; ///< The measured magnetometer values + const Matrix3 bRn_; ///< The assumed known rotation from nav to body public: @@ -178,8 +177,8 @@ public: */ class MagFactor3: public NoiseModelFactor3 { - Vector3 measured_; /** The measured magnetometer values */ - Rot3 bRn_; /** The assumed known rotation from nav to body */ + const Vector3 measured_; ///< The measured magnetometer values + const Rot3 bRn_; ///< The assumed known rotation from nav to body public: From 6d16ebf68d0fe7ef71a34ab8ca88936076875a93 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 09:31:22 -0500 Subject: [PATCH 09/12] Moved from Vector3/LieVector to Point3. I have mixed feelings about this. Wondering (again) whether Point3 ought to *be* a Vector3 after all. --- gtsam/geometry/Sphere2.h | 10 ++- gtsam/navigation/MagFactor.h | 82 +++++++++++------------- gtsam/navigation/tests/testMagFactor.cpp | 16 ++--- 3 files changed, 48 insertions(+), 60 deletions(-) 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 index 5411280e9..386a842f7 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -19,7 +19,6 @@ #include #include #include -#include #include namespace gtsam { @@ -31,16 +30,16 @@ namespace gtsam { */ class MagFactor: public NoiseModelFactor1 { - const Vector3 measured_; ///< The measured magnetometer values + const Point3 measured_; ///< The measured magnetometer values const double scale_; ///< Scale factor from direction to magnetometer readings const Sphere2 direction_; ///< Local magnetic field direction - const Vector3 bias_; ///< bias + const Point3 bias_; ///< bias public: /** Constructor */ - MagFactor(Key key, const Vector3& measured, const LieScalar& scale, - const Sphere2& direction, const LieVector& bias, + MagFactor(Key key, const Point3& measured, const LieScalar& scale, + const Sphere2& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), // measured_(measured), scale_(scale), direction_(direction), bias_(bias) { @@ -57,7 +56,7 @@ public: Sphere2 q = Rot3::yaw(R.theta()) * p; if (HR) { HR->resize(2, 1); - Point3 Q = q.unitVector(); + Point3 Q = q.point3(); Matrix B = q.basis().transpose(); (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0); } @@ -71,13 +70,13 @@ public: boost::optional H = boost::none) const { // measured bM = nRbÕ * nM + b, where b is unknown bias Sphere2 rotated = unrotate(nRb, direction_, H); - Vector3 hx = scale_ * rotated.unitVector() + bias_; + Point3 hx = scale_ * rotated.point3() + bias_; if (H) { Matrix U; - rotated.unitVector(U); + rotated.point3(U); *H = scale_ * U * (*H); } - return hx - measured_; + return (hx - measured_).vector(); } }; @@ -88,19 +87,18 @@ public: */ class MagFactor1: public NoiseModelFactor1 { - const Vector3 measured_; ///< The measured magnetometer values - const double scale_; ///< Scale factor from direction to magnetometer readings - const Sphere2 direction_; ///< Local magnetic field direction - const Vector3 bias_; ///< bias + 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 Vector3& measured, const LieScalar& scale, - const Sphere2& direction, const LieVector& bias, + MagFactor1(Key key, const Point3& measured, const LieScalar& scale, + const Sphere2& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), // - measured_(measured), scale_(scale), direction_(direction), bias_(bias) { + measured_(measured), nM_(scale * direction), bias_(bias) { } /// @return a deep copy of this factor @@ -115,15 +113,9 @@ public: Vector evaluateError(const Rot3& nRb, boost::optional H = boost::none) const { // measured bM = nRbÕ * nM + b, where b is unknown bias - Sphere2 rotated = nRb.unrotate(direction_, H); - Vector3 hx = scale_ * rotated.unitVector() + bias_; - if (H) // I think H2 is 2*2, but we need 3*2 - { - Matrix U; - rotated.unitVector(U); - *H = scale_ * U * (*H); - } - return hx - measured_; + Point3 q = nRb.unrotate(nM_, H); + Point3 hx = q + bias_; + return (hx - measured_).vector(); } }; @@ -132,18 +124,18 @@ public: * 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 { +class MagFactor2: public NoiseModelFactor2 { - const Vector3 measured_; ///< The measured magnetometer values - const Matrix3 bRn_; ///< The assumed known rotation from nav to body + 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 Vector3& measured, const Rot3& nRb, + MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor2(model, key1, key2), // - measured_(measured), bRn_(nRb.transpose()) { + NoiseModelFactor2(model, key1, key2), // + measured_(measured), bRn_(nRb.inverse()) { } /// @return a deep copy of this factor @@ -157,16 +149,14 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - Vector evaluateError(const LieVector& nM, const LieVector& 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 - Vector3 hx = bRn_ * nM + bias; - if (H1) - *H1 = bRn_; + Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) *H2 = eye(3); - return hx - measured_; + return (hx - measured_).vector(); } }; @@ -175,17 +165,17 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor3: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactor3 { - const Vector3 measured_; ///< The measured magnetometer values + 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 Vector3& measured, + MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // + NoiseModelFactor3(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { } @@ -201,23 +191,23 @@ public: * @param bias (unknown) 3D bias */ Vector evaluateError(const LieScalar& scale, const Sphere2& direction, - const LieVector& bias, boost::optional H1 = boost::none, + 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); - Vector3 hx = scale * rotated.unitVector() + bias; + Point3 hx = scale * rotated.point3() + bias; if (H1) - *H1 = rotated.unitVector(); - if (H2) // I think H2 is 2*2, but we need 3*2 + *H1 = rotated.point3().vector(); + if (H2) // H2 is 2*2, but we need 3*2 { Matrix H; - rotated.unitVector(H); + rotated.point3(H); *H2 = scale * H * (*H2); } if (H3) *H3 = eye(3); - return hx - measured_; + return (hx - measured_).vector(); } }; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index a29aec3b7..97e4cfea2 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -36,20 +36,20 @@ using namespace GeographicLib; // 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: -Vector3 nM(22653.29982, -1956.83010, 44202.47862); +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 -Vector3 bias(10, -10, 50); +Point3 bias(10, -10, 50); // ... then we measure -Vector3 scaled = scale * nM; -Vector3 measured = scale * nRb.transpose() * nM + bias; +Point3 scaled = scale * nM; +Point3 measured = nRb.inverse() * (scale * nM) + bias; LieScalar s(scale * nM.norm()); -Sphere2 dir(nM[0], nM[1], nM[2]); +Sphere2 dir(nM); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); @@ -84,10 +84,10 @@ TEST( MagFactor, Factors ) { // 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 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); @@ -100,7 +100,7 @@ TEST( MagFactor, Factors ) { EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } From 5369a7bd17d9bb7ffde7149f36648e5f8774824f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 10:29:03 -0500 Subject: [PATCH 10/12] Simplify by storing s*dir --- gtsam/navigation/MagFactor.h | 31 +++++++----------------- gtsam/navigation/tests/testMagFactor.cpp | 8 +++--- 2 files changed, 13 insertions(+), 26 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 386a842f7..eb40513e9 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -31,8 +31,7 @@ namespace gtsam { class MagFactor: public NoiseModelFactor1 { const Point3 measured_; ///< The measured magnetometer values - const double scale_; ///< Scale factor from direction to magnetometer readings - const Sphere2 direction_; ///< Local magnetic field direction + const Point3 nM_; ///< Local magnetic field (mag output units) const Point3 bias_; ///< bias public: @@ -42,7 +41,7 @@ public: const Sphere2& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), // - measured_(measured), scale_(scale), direction_(direction), bias_(bias) { + measured_(measured), nM_(scale * direction), bias_(bias) { } /// @return a deep copy of this factor @@ -51,15 +50,10 @@ public: NonlinearFactor::shared_ptr(new MagFactor(*this))); } - static Sphere2 unrotate(const Rot2& R, const Sphere2& p, + static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Sphere2 q = Rot3::yaw(R.theta()) * p; - if (HR) { - HR->resize(2, 1); - Point3 Q = q.point3(); - Matrix B = q.basis().transpose(); - (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0); - } + Point3 q = Rot3::yaw(R.theta()).rotate(p,HR); + if (HR) *HR = HR->col(2); return q; } @@ -68,14 +62,8 @@ public: */ Vector evaluateError(const Rot2& nRb, boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias - Sphere2 rotated = unrotate(nRb, direction_, H); - Point3 hx = scale_ * rotated.point3() + bias_; - if (H) { - Matrix U; - rotated.point3(U); - *H = scale_ * U * (*H); - } + // measured bM = nRbÕ * nM + b + Point3 hx = unrotate(nRb, nM_, H) + bias_; return (hx - measured_).vector(); } }; @@ -112,9 +100,8 @@ public: */ Vector evaluateError(const Rot3& nRb, boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias - Point3 q = nRb.unrotate(nM_, H); - Point3 hx = q + bias_; + // measured bM = nRbÕ * nM + b + Point3 hx = nRb.unrotate(nM_, H) + bias_; return (hx - measured_).vector(); } }; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 97e4cfea2..6a6954e55 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -58,10 +58,10 @@ using boost::none; // ************************************************************************* TEST( MagFactor, unrotate ) { Matrix H; - Sphere2 expected(0.457383, 0.00632703, 0.889247); - EXPECT( assert_equal(expected, MagFactor::unrotate(theta,dir,H),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor::unrotate, _1, dir, none), theta), H, 1e-7)); + 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)); } // ************************************************************************* From a48c72ff63a7925f8fdf3ef894aae308cd79e898 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 10:33:17 -0500 Subject: [PATCH 11/12] Fixed sign of 2D version --- gtsam/navigation/MagFactor.h | 5 +++-- gtsam/navigation/tests/testMagFactor.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index eb40513e9..61ffd2d5d 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -26,7 +26,8 @@ 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 + * 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 { @@ -52,7 +53,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Point3 q = Rot3::yaw(R.theta()).rotate(p,HR); + Point3 q = Rot3::yaw(R.theta()).unrotate(p,HR); if (HR) *HR = HR->col(2); return q; } diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 6a6954e55..7e6b940c3 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -41,7 +41,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862); double scale = 255.0 / 50000.0; // ...ground truth orientation, Rot3 nRb = Rot3::yaw(-0.1); -Rot2 theta = -nRb.yaw(); +Rot2 theta = nRb.yaw(); // ...and bias Point3 bias(10, -10, 50); // ... then we measure From cfe5a3663ed047c13fe3ea690b99fc3ced42ab20 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 14:03:16 -0500 Subject: [PATCH 12/12] LieScalar does not make sense here: scale is known -> double --- gtsam/navigation/MagFactor.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 61ffd2d5d..6c47523a6 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -38,7 +38,7 @@ class MagFactor: public NoiseModelFactor1 { public: /** Constructor */ - MagFactor(Key key, const Point3& measured, const LieScalar& scale, + MagFactor(Key key, const Point3& measured, double scale, const Sphere2& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), // @@ -53,8 +53,9 @@ public: 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); + Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); + if (HR) + *HR = HR->col(2); return q; } @@ -83,7 +84,7 @@ class MagFactor1: public NoiseModelFactor1 { public: /** Constructor */ - MagFactor1(Key key, const Point3& measured, const LieScalar& scale, + MagFactor1(Key key, const Point3& measured, double scale, const Sphere2& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), //