Merge pull request #752 from miloknowles/feature/mag_pose_factor
						commit
						554009741c
					
				|  | @ -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
 | ||||
|  | @ -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); | ||||
| } | ||||
| // *****************************************************************************
 | ||||
		Loading…
	
		Reference in New Issue