Prototype MagFactor

gtsam/navigation/tests/testMagFactor.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/navigation/MagFactor.h>
+#include <gtsam/geometry/Rot2.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
@@ -28,11 +29,61 @@ 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<Rot2> {
+
+  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<Rot2>(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 MagFactor(*this)));
+  }
+
+  static Sphere2 unrotate(const Rot2& R, const Sphere2& p,
+      boost::optional<Matrix&> 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<Matrix&> 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 ) {
-
-  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;
@@ -45,6 +96,7 @@ TEST( MagFactor, Constructors ) {
 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
@@ -55,15 +107,34 @@ TEST( MagFactor, Constructors ) {
 Sphere2 dir(nM[0], nM[1], nM[2]);
 
 SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
-  Matrix expectedH1, expectedH2, expectedH3;
+
+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<Sphere2,Rot2> //
+      (boost::bind(&MagFactor::unrotate, _1, dir, none), theta), H, 1e-7));
+}
+
+// *************************************************************************
+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<Rot2> //
+      (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<Rot3> //
-      (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb),//
-      H1, 1e-7));
+      (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
 
 // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);