Added MagFactor that solves for rotation, now called 1,2,3, with respectively 1,2,3 arguments to factor.

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<Rot3> {
+
+  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<Rot3>(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>(
+        NonlinearFactor::shared_ptr(new MagFactor1(*this)));
+  }
+
+  /**
+   * @brief vector of errors
+   */
+  Vector evaluateError(const Rot3& nRb,
+      boost::optional<Matrix&> 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<LieVector, LieVector> {
+class MagFactor2: public NoiseModelFactor2<LieVector, LieVector> {
 
   Vector3 measured_; /** The measured magnetometer values */
   Matrix3 bRn_; /** The assumed known rotation from nav to body */
@@ -37,7 +83,7 @@ class MagFactor1: public NoiseModelFactor2<LieVector, LieVector> {
 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<LieVector, LieVector>(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>(
-        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<LieScalar, Sphere2, LieVector> {
+class MagFactor3: public NoiseModelFactor3<LieScalar, Sphere2, LieVector> {
 
   Vector3 measured_; /** The measured magnetometer values */
   Rot3 bRn_; /** The assumed known rotation from nav to body */
@@ -80,7 +126,7 @@ class MagFactor2: public NoiseModelFactor3<LieScalar, Sphere2, LieVector> {
 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<LieScalar, Sphere2, LieVector>(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>(
-        NonlinearFactor::shared_ptr(new MagFactor2(*this)));
+        NonlinearFactor::shared_ptr(new MagFactor3(*this)));
   }
 
   /**

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);
+  MagFactor1 f1(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(zero(3),f1.evaluateError(scaled,bias,H1,H2),1e-5));
+  EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
+  EXPECT( assert_equal(numericalDerivative11<Rot3> //
+      (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<LieVector> //
-      (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<LieVector> //
-      (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);
+  MagFactor3 f3(1, 2, 3, measured, nRb, model);
-  LieScalar s(scale*nM.norm());
+  EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
-  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<LieScalar> //
-      (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<Sphere2> //
-      (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<LieVector> //
-      (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));
 }