From fd6eb2b1291d3ae8b2f24402eb34aaaea33b27a4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 30 Jan 2014 00:58:15 -0500 Subject: [PATCH] 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)); }