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)); }