Add a MagPoseFactor for using magnetometer measurements with Pose2/Pose3

gtsam_unstable/slam/MagPoseFactor.h

@@ -0,0 +1,141 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+#include <gtsam/geometry/concepts.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
+ * This version uses the measurement model bM = scale * bRn * direction + bias,
+ * where bRn is the rotation of the body in the nav frame, and scale, direction,
+ * and bias are assumed to be known. If the factor is constructed with a
+ * body_P_sensor, then the magnetometer measurements and bias should be
+ * expressed in the sensor frame.
+ */
+template <class POSE>
+class MagPoseFactor: public NoiseModelFactor1<POSE> {
+ private:
+  using This = MagPoseFactor<POSE>;
+  using Base = NoiseModelFactor1<POSE>;
+  using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
+  using Rot = typename POSE::Rotation;
+
+  const Point measured_; ///< The measured magnetometer data in the body frame.
+  const Point nM_; ///< Local magnetic field (mag output units) in the nav frame.
+  const Point bias_; ///< The bias vector (mag output units) in the body frame.
+  boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
+
+  static const int MeasDim = Point::RowsAtCompileTime;
+  static const int PoseDim = traits<POSE>::dimension;
+  static const int RotDim = traits<Rot>::dimension;
+
+  /// Shorthand for a smart pointer to a factor.
+  using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
+
+  /// Concept check by type.
+  GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
+  GTSAM_CONCEPT_POSE_TYPE(POSE)
+
+ public:
+  ~MagPoseFactor() override {}
+
+  /// Default constructor - only use for serialization.
+  MagPoseFactor() {}
+
+  /**
+   * Construct the factor.
+   * @param pose_key of the unknown pose nPb in the factor graph
+   * @param measured magnetometer reading, a Point2 or Point3
+   * @param scale by which a unit vector is scaled to yield a magnetometer reading
+   * @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
+   * @param bias of the magnetometer, modeled as purely additive (after scaling)
+   * @param model of the additive Gaussian noise that is assumed
+   * @param body_P_sensor an optional transform of the magnetometer in the body frame
+   */
+  MagPoseFactor(Key pose_key,
+                const Point& measured,
+                double scale,
+                const Point& direction,
+                const Point& bias,
+                const SharedNoiseModel& model,
+                const boost::optional<POSE>& body_P_sensor)
+      : Base(model, pose_key),
+        measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
+        nM_(scale * direction.normalized()),
+        bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias),
+        body_P_sensor_(body_P_sensor) {}
+
+  /// @return a deep copy of this factor.
+  NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<NonlinearFactor>(
+        NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// Implement functions needed for Testable.
+
+  // Print out the factor.
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
+    Base::print(s, keyFormatter);
+    gtsam::print(Vector(nM_), "local field (nM): ");
+    gtsam::print(Vector(measured_), "measured field (bM): ");
+    gtsam::print(Vector(bias_), "magnetometer bias: ");
+  }
+
+  /// Equals function.
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
+    const This *e = dynamic_cast<const This*> (&expected);
+    return e != nullptr && Base::equals(*e, tol) &&
+        gtsam::equal_with_abs_tol(this->measured_, e->measured_, tol) &&
+        gtsam::equal_with_abs_tol(this->nM_, e->nM_, tol) &&
+        gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
+  }
+
+  /// Implement functions needed to derive from Factor.
+
+  /**
+   * Return the factor's error h(x) - z, and the optional Jacobian. Note that
+   * the measurement error is expressed in the body frame.
+   */
+  Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
+    // Predict the measured magnetic field h(x) in the *body* frame.
+    // If body_P_sensor was given, bias_ will have been rotated into the body frame.
+    Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
+    const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
+
+    if (H) {
+      // Fill in the relevant part of the Jacobian (just rotation columns).
+      *H = Matrix::Zero(MeasDim, PoseDim);
+      const size_t rot_col0 = nPb.rotationInterval().first;
+      (*H).block(0, rot_col0, MeasDim, RotDim) = H_rot;
+    }
+
+    return (hx - measured_);
+  }
+
+ private:
+  /// Serialization function.
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+         boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(measured_);
+    ar & BOOST_SERIALIZATION_NVP(nM_);
+    ar & BOOST_SERIALIZATION_NVP(bias_);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
+  }
+};  // \class MagPoseFactor
+
+} /// namespace gtsam

gtsam_unstable/slam/tests/testMagPoseFactor.cpp

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam_unstable/slam/MagPoseFactor.h>
+
+using namespace gtsam;
+
+// *****************************************************************************
+// Magnetic field in the nav frame (NED), with units of nT.
+Point3 nM(22653.29982, -1956.83010, 44202.47862);
+
+// Assumed scale factor (scales a unit vector to magnetometer units of nT).
+double scale = 255.0 / 50000.0;
+
+// Ground truth Pose2/Pose3 in the nav frame.
+Pose3 n_P3_b = Pose3(Rot3::Yaw(-0.1), Point3(-3, 12, 5));
+Pose2 n_P2_b = Pose2(Rot2(-0.1), Point2(-3, 12));
+Rot3 n_R3_b = n_P3_b.rotation();
+Rot2 n_R2_b = n_P2_b.rotation();
+
+// Sensor bias (nT).
+Point3 bias3(10, -10, 50);
+Point2 bias2 = bias3.head(2);
+
+Point3 dir3(nM.normalized());
+Point2 dir2(nM.head(2).normalized());
+
+// Compute the measured field in the sensor frame.
+Point3 measured3 = n_R3_b.inverse() * (scale * dir3) + bias3;
+
+// The 2D magnetometer will measure the "NE" field components.
+Point2 measured2 = n_R2_b.inverse() * (scale * dir2) + bias2;
+
+SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.25);
+SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25);
+
+// Make up a rotation and offset of the sensor in the body frame.
+Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0));
+Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3));
+// *****************************************************************************
+
+// *****************************************************************************
+TEST(MagPoseFactor, Constructors) {
+  MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
+  MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
+
+  // Try constructing with a body_P_sensor set.
+  MagPoseFactor<Pose2> f2b = MagPoseFactor<Pose2>(
+      Symbol('X', 0), measured2, scale, dir2, bias2, model2, body_P2_sensor);
+  MagPoseFactor<Pose3> f3b = MagPoseFactor<Pose3>(
+      Symbol('X', 0), measured3, scale, dir3, bias3, model3, body_P3_sensor);
+
+  f2b.print();
+  f3b.print();
+}
+
+// *****************************************************************************
+TEST(MagPoseFactor, JacobianPose2) {
+  Matrix H2;
+
+  // Error should be zero at the groundtruth pose.
+  MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
+  CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
+  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
+      (boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
+}
+
+// *****************************************************************************
+TEST(MagPoseFactor, JacobianPose3) {
+  Matrix H3;
+
+  // Error should be zero at the groundtruth pose.
+  MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
+  CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
+  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
+      (boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
+}
+
+// *****************************************************************************
+TEST(MagPoseFactor, body_P_sensor2) {
+  Matrix H2;
+
+  // Because body_P_sensor is specified, we need to rotate the raw measurement
+  // from the body frame into the sensor frame "s".
+  const Rot2 nRs = n_R2_b * body_P2_sensor.rotation();
+  const Point2 sM = nRs.inverse() * (scale * dir2) + bias2;
+  MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
+  CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
+  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
+      (boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
+}
+
+// *****************************************************************************
+TEST(MagPoseFactor, body_P_sensor3) {
+  Matrix H3;
+
+  // Because body_P_sensor is specified, we need to rotate the raw measurement
+  // from the body frame into the sensor frame "s".
+  const Rot3 nRs = n_R3_b * body_P3_sensor.rotation();
+  const Point3 sM = nRs.inverse() * (scale * dir3) + bias3;
+  MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
+  CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
+  CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
+      (boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
+}
+
+// *****************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+// *****************************************************************************